基于W-空间的弹药装填机器人避障路径规划

徐 达，夏 祥，苏忠亭

（装甲兵工程学院 兵器工程系，北京 100072）

0 引言

避障路径规划是指在给定的障碍条件以及起始和目标的位姿，选择一条从起始点到达目标点的路径，使运动物体能安全、无碰撞地通过所有的障碍[1]。近年来，在机器人避障路径规划方法研究中，国内外许多学者做了大量工作，根据不同规划空间可分为两种：机器人W-空间(Working Space)规划[2，3]和C-空间（Configuration Space）规划[2，3]。由于在C-空间中障碍物映射表述不够完整，自由空间计算难度较大，运动路径不易确定，有效性不高的缺点，本文针对弹药装填机器人作业环境特殊、运动控制需实时可靠的要求，提出了一种基于工作空间的弹药装填机器人避障路径规划算法。该算法首先对规划路径进行离散化，运用圆弧对避障路径进行插补，同时运用样条函数在关节空间进行第二次插补，最后进行了仿真计算，结果表明，该算法运算简单，能有效满足弹药装填机器人的避障要求，保证弹药装填机器人连续和平稳作业。

1 弹药装填机器人运动构型与作业环境表述

1.1 弹药装填机器人运动构型

弹药装填机器人属于关节型机器人，拥有PRRR-S型结构，其中，P为移动副，R为转动副，S为球面关节[4]，由D-H法则建立弹药装填机器人连杆坐标系，其结构简图与连杆坐标系如图1所示，其中l1、l2为弹药装填机器人大臂和小臂的长度。机器人PRRR关节用来确定末端执行器的空间位置，S关节用来确定末端执行器的空间姿态。

图1 弹药装填机器人结构及连杆坐标系图

1.2 作业环境概述

作业环境分析是进行避障路径规划的基础。弹药装填机器人的作业环境主要是指装甲车辆的车内环境，由于坦克、自行火炮车内空间狭窄、结构复杂，且作业过程车体颠簸振动幅度较大，与工业机器人相比，弹药装填机器人作业环境表现为狭窄、封闭、多障碍的特点，如炮塔中的炮尾，在训练与射击过程中，随着炮塔俯仰而不断变化，在W-空间中具有几何不规则性，因此需对其进行一定简化。本文采用球形包围盒法对其进行简化与建模，同时将末端执行器、机械臂的大小、形状因素考虑进去，得到最终简化模型如图2所示。其中为考虑机械臂宽度后球体包围盒当量半径，为包容盒球体的半径，d为机械臂最大宽度。

图2 障碍物包围盒示意图

2 避障路径规划

2.1 避障路径设计

建立当量包围盒后，障碍物几何中心即已知，设圆心坐标为 O (x0,y0,z0)，弹药装填机器人末端执行器参考点坐标为 P ( px, py, pz)，当式（1）成立时，弹药装填机器人即与障碍物发生碰撞，此可作为弹药装填机器人避碰判定式。

图3 弹药装填机器人避障路径示意图

由于空间曲线基本可以分割为多段直线或圆弧的组合，因此在机器人W-空间路径规划过程中，通常采用空间直线和空间圆弧两种基本曲线。

如图3所示，设障碍物圆心为o，半径为r，弹药装填机器人末端执行器运动起始点为A，目标点为B。在点A与点B之间利用空间直线与空间圆弧进行规划。避障规划过程中，路径有多种选择，即空间曲线ACDEB、空间曲线和空间曲线等。根据路径最短和机器人末端执行器可达空间最大化原则，选取空间圆弧ADB作为避障过程路径，其中D点由末端执行器和障碍物的空间尺寸及避碰安全系数决定。

图4 障碍物包围盒示意图

2.2 避障算法步骤与流程

根据式（1）及选取的路径ADB，可设计弹药装填机器人避障步骤，流程如图4所示。具体如下：

1）确定弹药装填机器人工作空间，求解障碍物中心与末端执行器空间坐标；

2）将空间圆弧ADB离散化若干控制点，确定避障路径起点与终点；

3）运用运动学逆解求得起点、终点与离散控制点对应各关节变量；

4）对步骤3中所得关节变量进行二次插值，得到各关节运动函数；

5）取各关节运动函

数极值对应点，进行运动学正解，利用式（1）判断是否与障碍物相碰，检验二次插值精度和离散点数目是否符合要求；

6）利用符合要求的关节运动函数驱动控制电机，实现弹药装填机器人避障路径。

2.3 空间圆弧插补

对于弹药装填机器人工作空间路径规划，由于作业环境的特殊性，只考虑位置变化。设给定的三个点位置分别为，如图5所示，由此确定平面M，方程为：

其中：1a，1b，1c，1d分别为式（2）中关于x，y，z，1的余子式。

图5 弹药装填机器人空间圆弧路径示意图

过AD的中点E并垂直于AD，可以唯一确定一个平面T，则该平面上的任何直线均垂直于AD，从而得平面T的方程为：

同理，过DB中点并垂直于DB的平面S的方程可写为：

设弹药装填机器人末端执行器沿圆弧匀速运动，运动时间为ft，得圆弧上任意时刻t点的位置坐标为：

3 避障路径仿真与分析

根据第三节设计的路径以及算法步骤，采用空间圆弧进行插补，得到弹药装填机器人末端执行器轨迹如图6所示，其运动速度与圆弧插补速度相关，当圆弧插补为匀速时，其末端执行器在x、y、z方向速度矢量如图7所示。

从图6可知，末端执行器空间轨迹与所规划曲线吻合，符合弹药装填机器人供弹过程避障要求，从图7可知，其各个轴向速度曲线具有单调性，速度变量在约束条件内，各轴最大速度满足弹药装填机器人综合特性，末端执行器各轴速度曲线变化平滑和连续，这表明弹药装填机器人末端执行器在避障过程中不会产生振动，能够保证作业中平稳控制与运动。

图6 弹药装填机器人末端执行器空间轨迹

图7 末端执行器各轴速度曲线

4 结束语

本文以弹药装填机器人为对象，从最短路径与易控制性能角度出发，研究了弹药装填机器人作业过程中的避障问题。在分析了弹药装填机器人工作环境与障碍物特性基础上，提出了一种基于W-空间圆弧插补的避障轨迹方法，并规划了该算法的步骤与流程，具体给出了该算法的实现过程，并对所研究的算法进行了仿真与分析，结果表明：该算法运算简单，能有效满足弹药装填机器人的避障要求，保证弹药装填机器人末端执行器按照预设路径进行运动，且满足各个轴向速度、加速度连续平稳的要求。

[1] 施琴,马履中,刘翊,王君雄.六自由度机器人避障问题的MATLAB仿真[J].机床与液压,2010,38(9):89-91.

[2] 孙树栋.工业机器人技术基础[M].西安:西北工业大学出版社,2006(5):92－119.

[3] 戈志勇.番茄采摘机械手运动学仿真与避障算法研究[D].江苏大学,2007.

[4] 徐达,王中盛,刘广洋,等.基于串并联结构的弹药装填机器人设计[J].装甲兵工程学院学报,2008,22(5):45－50.

[5] 徐达,帅元,郝琢,侯润峰.弹药装填机器人自适应PD控制算法[J].装甲兵工程学院学报,2011,25(1):49-53.