火车上水机器人运动学分析与路径规划

The Kinematic Analysis and Path Planning of Water Robots on Trains

摘要: 本文选择对自制的非标上水机器人进行运动学分析与仿真。首先利用改进型DH参数法建立各连杆的坐标系，确定D-H参数。接着推导出其运动学方程，在实物上进行运动分析，验证模型的正确性。然后，选择代数法进行逆运动学分析计算，得到运动学逆解，使用Python求解过程封装成函数。通过此函数随机输入给定位姿都能得到给定位姿对应的最优逆解，证实了逆解算法的正确性。最后，本文在Moveit上使用RRT路径规划算法，完成了机器人从初始点到目标点的路径规划。结果表明，对该上水机器人的运动学分析正确，实现了仿真与实物的相互通信。

Abstract: This paper conducts a kinematic analysis and simulation of a self-made non-standard water delivery robot. Firstly, an improved Denavit-Hartenberg (D-H) parameter method is used to establish the coordinate systems for each link and determine the D-H parameters. Then, the kinematic equations are derived, and motion analysis is performed on the physical prototype to validate the model’s accuracy. Next, an algebraic method is chosen for inverse kinematics analysis, resulting in the kinematic inverse solution. A function is encapsulated in Python to solve this process, allowing for random input of specified poses to obtain the corresponding optimal inverse solutions, confirming the correctness of the inverse solution algorithm. Finally, the RRT path planning algorithm is utilized in Moveit to complete the path planning from the initial point to the target point. The results indicate that the kinematic analysis of the water delivery robot is accurate, achieving effective communication between simulation and the physical model.

1. 引言

在工业4.0的推进过程中，火车作为重要的通勤手段，其上水系统的效率和精度直接反映了一个国家的工业化和制造业水平。由于天气条件的影响，尤其是在冬季，人工为火车上水的操作不仅劳动强度高，而且效率低下。因此，迫切需要将生产模式向高速、连续和高精度方向发展。李统中[1]提出使用夜视效果较好的RGB相机进行上水口物体检测，但在雨天、雾天等恶劣天气条件下，该方法效果较差。随着激光雷达价格的下降，本文决定采用激光雷达进行物体识别。在识别完物体后，本文需要控制机器人到达目标位置。首先，需要完成机器人与相机之间的坐标系转换，常用的方法是Z. Zhang [2]标定法。其次，本文对上水机器人进行运动学建模与路径规划。虽然Dijkstra [3]算法通过不断更新距离集合来找到最短路径，但效率较慢，因此本文采用基于采样的RRT算法。总之，本文开发了一套图像识别控制系统，利用机器人替代人工完成火车的智能上水操作。

2. 手眼标定

Figure 1. Hand-eye calibration

图1. 手眼标定

相机校准是视觉捕捉中的重要步骤，帮助机器人转换已识别的视觉信息并完成后续控制任务。当本文通过点云聚类法获得了目标物体的位置后，需要将其转为机器人坐标系下的位置，常用的校准方法包括Zhang的校准方法。如图1所示，预测的姿态估计B需要转换为机器人坐标系下的值。由于手眼标定矩阵是固定的，本文通过不断更换标定板的位置，建立方程1，最终通联立方程组使用最小二乘法求得X的转换关系。

$B_1\ast X\ast A_1=B_2\ast X\ast A_2$ (1)

完成机器人与相机的手眼标定矩阵之后，本文将点云聚类得到的物体中心转换为夹爪末端的中心位置，下一步通过机器人运动学模型，控制机器人到达目标位置进行抓取。

3. 正逆运动学分析

图2所示为本文研究的上水机器人。其有垂直升降和三个水平旋转的关节，并且三个旋转关节的驱动轴相互独立，可以实现灵活运动，其有一个末端执行器，可以实现上水的操作。由图2的上水机器人模型可得改进DH表[4] [5]，其中为i连杆序号；a_i为连杆长度，表示沿X轴，从Z_i−1移动到Z_i的距离；α_i为连杆转角，表示绕X_i轴，从Z_i−1轴旋转到Z_i轴的角度。θ_i为关节角，表示绕Z_i轴，从X_i−1旋转到X_i的角度；d_i为连杆偏距，表示沿Z_i轴，从X_i−1移动到X_i的距离。由此并创建了机器人连杆的坐标系，从而可得到上水机器人的D-H参数表，如表1所示。

Figure 2. Water supply robot 3d model

图2. 上水机器人3d模型

Table 1. The D-H parameters table for the upwater robot

表1. 上水机器人的D-H参数表

i	αi	ai	di	θi
0	0	0	525.5	0
1	0	214	d2	0

续表

2	90˚	0	0	−θ
3	0	760	0	0
4	−90˚	352.5	0	0

正运动学求解即在已知各关节变量的条件下，通过计算，最终得到末端执行器的位姿与基座之间的函数关系的过程。根据上述所得出的改进D-H参数表中的数据和图2，图3中的坐标系与关节转角关系，可建立起各关节的位置关系，依次推导出末端执行器相对于基坐标系的齐次变换矩阵，从而建立运动学方程。为了简洁，用C_i和S_i分别代表sinθ_i和cosθ_i，S_ij表示sin(θ_i + θ_j)。结合改进的D-H参数(式1)可得上水机器人各坐标系之间的变换矩阵为

${}_1{}^{0}T=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 525.5\\ 0& 0& 0& 1\end{array}\right]$

${}_2{}^{1}T=\left[\begin{array}{cccc}1& 0& 0& 214\\ 0& 1& 0& 0\\ 0& 0& 1& d_2\\ 0& 0& 0& 1\end{array}\right]$

${}_3{}^{2}T=\left[\begin{array}{cccc}c\theta & s\theta & 0& 0\\ 0& 0& -1& 0\\ -s\theta & c\theta & 1& 0\\ 0& 0& 0& 1\end{array}\right]$

${}_4{}^{3}T=\left[\begin{array}{cccc}c\theta & -s\theta & 0& 760\\ s\theta & c\theta & 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$

${}_5{}^{4}T=\left[\begin{array}{cccc}1& 0& 0& 352.5\\ 0& 0& 1& 0\\ 0& -1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$

根据公式(2)就可以求出最后的正运动学方程。

${}_5{}^{0}T={}_1{}^{0}T{}_2{}^{1}T{}_3{}^{2}T{}_4{}^{3}T{}_5{}^{4}T$ (2)

根据公式(3)联立方程组[6]，就可以求出最后的θ和d。

${}_5{}^{0}T={}_1{}^{0}T\ast {}_2{}^{1}T\ast {}_3{}^{2}T\ast {}_4{}^{3}T\ast {}_5{}^{4}T=\left(\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right)$ (3)

本文通过最小二乘法采集了10组数据，并利用MATLAB方程求解器不断迭代，最终获得了机器人的运动学模型。通过该模型，本文计算出了机器人到达抓取位置时所需的各轴旋转角度。然而，在实现目标位置的过程中，本文需要规划一条最优路径，以避开各种障碍物，并尽可能提高速度。这需要进行路径规划。

4. RRT路径规划

Dijkstra 算法是一种用于寻找图中最短路径的算法，特别适用于具有非负边权的图，但Dijkstra算法仅返回从源节点到目标节点的最短路径，无法直接提供从源节点到所有其他节点的所有最短路径。所以本文最终在三维空间中使用RRT算法[7]，通过随机生成多个关节点的位置进行路径规划。首先，从起点出发，计算与其最近的随机点[7] [8]，并以固定步长向该点移动。接着，本文检查从当前点到目标点的路径是否存在障碍物。如果路径无障碍，则将该点作为新的起点，继续迭代；如果有障碍，则跳过该点，尝试下一个随机点，直至到达终点。本文在Moveit界面中实现了RRT算法的仿真，并将各个关节的转动角度和位移作用在终端输出[9] [10]，通过Modbus TCP协议将值发送给电机，最终完成给火车上水的操作。

Figure 3. Moveit simulation interface

图3. Moveit仿真界面

如图3所示，本文将图2的模型进行了一些简化，去除了一些电机和支撑零件，只保留传动的杆件，本文调用Moveit的RRT算法，在末端抓取姿态栏内输入机器人末端位姿，RRT算法将计算出从当前点到目标点的运动轨迹。橙色的部分是本文的机器人的起始姿态，灰色的是通过规划算法得到的机器人仿真姿态。本文在这个轨迹上，以固定的步长采用10个点输出到终端，并将其通过Modbus TCP协议传递给实物电机，从而实现仿真与实物之间的通讯。

5. 结语

针对上水机器人的运动学问题，本文针对其结构特点，利用改进型MDH参数法得到其运动学方程，针对逆运动学存在多解的问题，本文使用MATLAB求解器求出最优解，对于已知初始位置和终端位置，本文使用RRT路径规划方法规划出一条最优的路径。总之，本文开发了一套图像识别控制系统，通过图像识别技术替代人类视觉，自动寻找列车注水口，并实现机械臂的自动控制，减少人工操作的需求。这为未来在碰撞检测和轨迹规划方面的研究奠定了坚实的理论基础。然而，这仅是一个起步，后续还需针对规划出的路径进行轨迹规划、动力学建模等深入研究与分析。