无人装备弹药装填机器人轨迹规划技术研究

徐达，王小闯，王兆阳

（100072 北京市 陆军装甲兵学院）

0 引言

随着智能化、无人化技术的发展，战争模式已然改变，武器装备智能化、无人化的趋势明显。现役的自动装弹机已经无法满足无人化装备需求，必须突破现有弹药装填技术，机器人完成弹药装填任务已成为未来武器装备智能化、无人化发展的趋势[1]。未来作战中，配备大中口径火炮的无人化作战平台，将作为陆上重要突击和火力打击力量遂行作战任务。目前无人弹药自动装填技术还处于理论探索阶段，并未出现满足实用要求的弹药装填机器人[2]。因此，对无人装备弹药装填机器人轨迹规划技术进行研究，进一步验证无人装备采用机器人装填弹药的可行性。

1 弹药装填机器人运动学

弹药装填机器人的运动学分析是进行机器人弹药装填轨迹规划的基础，包括机器人正向运动学和逆向运动学[3]。

2.1 弹药装填机器人正向运动学

根据机器人齐次变换矩阵的定义，从基坐标到末端执行器的齐次变换矩阵为

式中：ci=cos（θi）；si=sin（θi）。

由式（1）可知，末端关节相对于基坐标的变换矩阵为：

式中：（nx，ny，nz）T——机器人末端关节的x 轴在基坐标系中的方向；（ox，oy，oz）T——机器人末端关节的y 轴在基坐标系中的方向；（ax，ay，az）T——机器人末端关节的z 轴在基坐标系中的方向；（px，py，pz）T——机器人末端关节在基坐标系中的位置坐标。

由于a1=a4=a5=d2=d3=0，机器人关节位置的表达式为

式中：cij=cos（θi+θj）；sij=sin（θi+θj）。

2.2 弹药装填机器人逆向运动学

弹药装填机器人的逆运动学是已知末端关节的位姿，求出使机械臂末端到达这个位置每个关节需要转的角度大小，它描述的是机械臂位姿运动的反向求解问题，是从工作空间到关节空间的变化[4]。

在式（2）两边作矩阵的逆变换

式（4）左边

式中：A=（nxc1+nys1），B=（oxc1+oys1），C=（axc1+ays1），D=（oyc1-oxs1），E=（nyc1+nxs1），F=（ayc1-axs1）。

式（4）右边

由式（5）、式（6）中两个矩阵对应元素相等，且关节末端位姿已知，可求出6 个关节的角度值。在6 个关节解中，θ1，θ5，θ6为2 个解；θ2，θ3，θ4为一个平面3R 机构，其运动学解为2 个。当给定机器人的末端位姿，其运动学逆解有8 组，根据实际工作环境选取合适的解。

3 机器人弹药装填轨迹规划

机器人弹药装填轨迹规划是指机器人在运动学基础上的弹药装填轨迹生成方法，是在任何给定的限制下，由关于时间的足够平滑的函数构成装填轨迹函数，实现轨迹规划[5]。三次B 样条曲线具有连接处1 阶、2 阶导数连续、位移变化率小及计算方便等特点[6]，因此弹药装填机器人轨迹规划采用三次B 样条插值法可以做到良好的平滑性和局部支撑性，使得轨迹平滑，速度、加速度连续，各关节运行平稳[7]。

3.1 弹药装填轨迹规划算法

在装弹过程中，需要进行避障，在进行弹药装填轨迹规划时，需要考虑避障问题[8]。在避障区域内设置n 个避障关键点，要求末端执行器不仅要在起始点和终止点满足位置、速度要求，而且必须经过所有避障关键点，轨迹平滑连续且速度、加速度不能产生突变[9]。首先在工作空间采用三次B 样条插值法进行轨迹规划。

令弹药装填机器人轨迹的第i 段三次B 样条曲线为

式中：t——时间（0 ≤t ≤1）；N（t）——三次B 样条曲线的基函数；Pi——三次B 样条曲线的控制点。

由于三次B 样条曲线轨迹是连续的，相邻曲线连接处的一阶、二阶导数相等，可求得三次B样条曲线的基函数：

已知弹药装填机器人轨迹速度和加速度连续，轨迹的起始点和终止速度为0，可求出三次B 样条曲线的控制点

式中：M1，M2，…，Mi——关节轨迹的中间关键点；P0，P1…Pi-1，Pi——三次B 样条轨迹的控制点。

将控制点代入式（7），结合式（8）可得三次B 样条曲线

3.2 三次B 样条轨迹规划

基于装弹作业环境平台，由机器人将弹药由储弹位运送至装填位，完成弹药的无人化装填。其中，以机器人底座为坐标原点，建立空间笛卡尔坐标系，坐标系遵守右手定则，如图1 所示。

图1 装填作业环境平台Fig.1 Loading operation environment platform

储弹位为轨迹起点P1（P1=M1），到达装填位为终止点P2（P2=M5）。为了使末端执行器能够进行有效避障，在轨迹中间设置3 个避障关键点（M2，M3，M4）。关键点的空间笛卡尔坐标如表1 所示。

表1 弹药装填机器人关键点坐标Tab.1 Coordinates of key points of ammunition loading robot

将关键点坐标分别代入式（9），可解得三次B 样条曲线的控制点，见表2。

表2 三次B 样条轨迹控制点Tab.2 Cubic B-spline trajectory control points

由式（10）可解得末端关节的三次B样条轨迹：

式（11）—式（13）分别为弹药装填机器人在笛卡尔空间中x 轴、y 轴、z 轴上的轨迹，弹药装填机器人笛卡尔空间供弹轨迹如图2 所示。

图2 机器人笛卡尔空间供弹轨迹Fig.2 Trajectory of the robot's Cartesian space feed

4 仿真验证

根据弹药装填机器人运动学逆解[10]，由关键点的空间坐标求解机器人各关节角度，如表3所示。

表3 机器人关键点的关节变量Tab.3 Joint variables of key points of the robot

将关节角度代入式（9）和式（10）中解得弹药装填机器人关节的三次B 样条轨迹，并进行角度、角速度和角加速度的仿真，得到机器人关节角度-时间关系，如图3 所示；机器人关节角速度-时间关系，如图4 所示；机器人关节角加速度-时间关系，如图5 所示。

由图3 可知，利用三次B 样条曲线规划出的弹药装填机器人关节轨迹是连续、平滑的，并且经过了所有避障关键点，无位置突变。由图4 可知，关节角速度曲线也保持了平滑连续，且无速度突变。由图5 可知，弹药装填机器人关节角加速度值连续，在关节通过关键点时角加速度出现转折，造成一定的关节冲击，但对关节轨迹影响不大。综上，弹药装填机器人关节空间轨迹不仅满足了弹药装填平台的弹药装填要求，而且还保证了其良好的运动学特性。

图3 机器人关节角度-时间关系图Fig.3 Relationship between robot joint angle and time

图4 机器人关节角速度-时间关系图Fig.4 Relationship between robot joint angular velocity and time

图5 机器人关节角加速度-时间关系图Fig.5 Relationship between angular acceleration of robot joint and time

5 结语

依托无人弹药装填机器人弹药装填平台，采用三次B 样条对机器人弹药装填进行轨迹规划，根据弹药装填过程关键点求出三次B 样条分段函数，并对弹药装填机器人关节运动轨迹进行仿真验证，得到关节角度、角速度和角加速度曲线。结果表明，规划的轨迹运动学特性良好，能够连续无突变完成弹药装填工作，验证了无人弹药装填的可行性，为武器装备无人化提供了技术支持。