CE-3巡视器机械臂就位探测需要满足测控、数传、星历计算、光照、指令数最少、避碰、X光谱仪投放高度及方向等约束^[1-3]。

李大生等^[4]提出了基于动力学约束的机械臂规划方法,戈新生和吴永东^[5]使用粒子群优化算法对机械臂进行规划。他们都涉及到动力学方程求解,CE-3巡视器机械臂的长度较小,可以不使用动力学方程求解。Lozano-Perez^[6]提出了基于结构构型空间(configuration space)的规划方法,将环境中的障碍物映射到机械臂关节坐标系中,求得结构空间的补集,在该集合中利用启发式搜索算法进行无碰撞规划,但对于复杂的月面环境并不适用。Cheung和Lumelsky^[7]在机械臂上附加传感器,利用有限的已知信息,可在未知环境中进行无碰撞机械臂运动规划,但CE-3巡视器机械臂上并无传感器,且巡视器前端的避障相机可为运动规划提供足够的环境信息。Khatib^[8]提出基于模拟势场的机械臂规划方法,障碍物和目标点分别为排斥势场和吸引势场,机械臂的运动轨迹由多个势场的合力决定,但该方法不适用于复杂的月面环境。azaz 和 ondu^[9] 在三次样条插值函数的运动规划基础上实现了在线规划,并对运动进行了时间优化设计,但该方法不能满足运动点之间的加速度连续性。以上方法仅仅满足无碰撞约束,不满足CE-3巡视器月面探测任务的其他约束条件。

本文针对机械臂就位探测任务规划问题进行研究,首先说明如何满足各约束条件,并阐述了机械臂就位探测任务总体流程；然后分别介绍探测点的评估和基本星历计算；其次建立机械臂模型,并着重介绍任务规划中机械臂工作空间、机械臂碰撞检测和SA^*算法的实现；最后进行仿真实验评估其性能。

1 机械臂就位探测任务规划概述

CE-3巡视器机械臂执行探测任务时,因质谱仪自身发射X射线,为保证分析工作正常进行,要求质谱仪轴向与太阳反射光有一定夹角。因质谱仪有一定的工作距离,要求探测区域要有一定的平整度,才能保证端接质谱仪与目标接近的程度达到技术指标要求。在此基础上,还要保证机械臂可无碰撞地展开到目标探测点。而且因控制指令数量的限制,机械臂位形切换次数要求最少。

通过星历计算得到太阳光的方向向量,根据点云数据中探测点的地理信息得到太阳反射光方向向量。通过机械臂正逆运动学求解得到机械臂展开到探测点时质谱仪轴向方向向量。当质谱仪轴向方向向量和太阳反射光方向向量夹角大于一定角度时,即可满足光照要求。

对探测点进行评估,用探测点与其临近点的高度平方差评估探测区域的平整度。将机械臂用多层次的包围盒包围,当机械臂位形切换时,通过正运动学得到包围盒在底座坐标系下的表示。若最外层的包围盒没有与点云数据重构的探测环境发生碰撞,则机械臂一定不会与探测环境碰撞。多层包围盒碰撞检测方法可快速地进行机械臂碰撞检测。

本文提出的SA^*算法可生成不与探测环境碰撞且机械臂位形切换次数最少的运动序列,用以满足控制指令数量的限制。

机械臂就位探测任务规划总体流程如图 1所示。

2 探测点评估

探测区域可用点云数据描述。当执行探测任务时,因端接质谱仪有一定的工作距离,要求探测点所在的探测区域要有一定的平整度。

图 2为探测点评估。图中：P_a为探测点。可根据P_a临近点P_b、P_c、P_d、P_e与P_a的高度方差来分析平面凸凹程度。当目标具有一定平整度时,才能保证端接质谱仪与目标接近的程度达到技术指标要求。因点云数据具有噪声是不可避免的,所以当探测区域内节点数目达到一定数量时,才能保证数据的可靠性,只有存在足够的节点数时,才能确定探测区域中是否存在障碍。

3 基本星历计算

CE-3巡视器在执行月面探测任务时要满足光照约束,探测时太阳光照射探测区域后太阳反射光方向向量和质谱仪轴向方向向量的夹角要大于5°。基本星历计算是为了初步解决光照约束。

图 3为星历矢量关系。图中：O为太阳系统质心;J_2k、s^*、e、m、s和n分别为国际天球参考系、太阳参考系、地球参考系、月球参考系、各测控站参考系和着陆器定点系。根据国际天文联合会发布的星历数据(earth_assoc_itrf93,DE42),给定天文时,得到基础星历矢量^J_2kr_Om、^J_2kr_Oe和^J_2kr_Os^*,以及星体间旋转变换关系^J_2kQ_m和^J_2kQ_e,其中:^jr_i为j坐标系原点到空间中一点i的距离矢量在j坐标系下的投影,^i-1Q_i为体系i-1至体系i的旋转变换阵在体系i-1下的表示。本文都遵循以上标记法。

给定站经纬度、地球半长轴及扁率时,可得^er_Os和^eQ_s。给定器经纬度、月球半长轴及扁率时,可得^mr_On和^mQ_n。因此,站器矢量在站坐标系下表示、器站矢量在器导航坐标系下表示及器日矢量在器导航坐标系下表示分别为

根据站器矢量在站坐标系下表示、器站矢量在器导航坐标系下表示及器日矢量在器导航坐标系下表示,可以分别计算站观测器的高度及方位角、器观测站的高度及方位角和器观测日的高度及方位角。

4 机械臂建模

CE-3巡视器机械臂系统如图 4所示,其由底座、肩、臂、腕和端接质谱仪设备组成,可用Denavit-Harterberg(D-H)方法对其建模。

4.1 D-H坐标系建模

CE-3巡视器机械臂D-H坐标系如图 5所示。图中：、2和3为机械臂各关节坐标系；4为虚拟第四关节坐标系；a_i、c_i分别为体系i到体系i+1的轴距、轴偏距；θ_i为各轴的旋转角度。

根据D-H参数规范确定D-H参数。机械臂处于零位时,θ₁=θ₂=θ₃=0。机械臂模型的 D-H参数如表 1所示。

4.2 机械臂逆运动学

定位方程^[10-12]为

式中：¹r_C=(x_C,y_C,z_C)为光谱仪中心C在基座坐标系的期望位置。

由式(1)求解得三轴角度θ₁、θ₂和θ₃^[13]：

式中：C=2a₂a₃；D=2a₂c₄；E=[W]a²₂+a²₃+c²₃+c²₄-x²_C-y²_C-(z_C-c₁)²+2c₂c₃；A₁₁=a₂+a₃cosθ₃+c₄sinθ₃；A₁₂=-a₃sinθ₃-c₄cosθ₃。

4.3 机械臂正逆运动学检验

机械臂逆运动行为过程的正确性可以通过机械臂正逆行为对比校验(见表 2),可知,机械臂逆运动行为过程计算是正确和可靠的。

5 机械臂工作空间与探测空间

机械臂工作空间计算是为了提高机械臂展开规划的速度。机械臂探测空间计算是为了提供灵活的人机交互功能,引导用户进行有效的探测目标选择,提高机械臂探测的效率。

机械臂的工作空间是机械臂拾取点运动的空间范围。机械臂各轴工作范围为:θ₁∈(θ¹d₁,θ¹u₁),θ₂∈(θ¹d₂,θ¹u₂),θ₃∈(θ¹d₃,θ¹u₃),θ¹d_i、θ¹u_i为各轴上下极限角度。按各轴角度粒度δθ₁、δθ₂和δθ₃逐次通过正运动学计算拾取点位置,并绘制拾取点可达的工作空间。

机械臂拾取点工作空间范围是评定机械臂功能实现的一个重要评价指标。一方面,由图 6可知,该机械臂工作空间与机械臂期望探测的空间交集相对较小,导致机械臂探测目标区域较小。另一方面,由图 7可知,当被探测物位于机械臂后侧时,则无法探测。

机械臂工作空间与探测对象空间(可由点云数据描述)的交集称为探测空间。由于机械臂工作空间较小及机械臂构型不合理,使得岩石上探测空间较小(见图 7),导致频繁移动巡视器,从而降低月面探测效率。机械臂的构形及各轴工作范围设计对其应用有着重要影响,需要根据机械臂应用目的进行优化,才能保证机械臂优良的行为能力。

6 机械臂碰撞检测

机械臂碰撞检测解决的是机械臂和由点云数据重构的探测环境之间是否发生碰撞的问题。对于一次探测任务而言,探测环境是不变的,变化的只是机械臂的位置。针对此特点,采用层次包围盒的方法进行机械臂碰撞检测。因为点云数据非常庞大,要想实现实时仿真,必须选择计算量较小的包围盒,因此选择轴对齐包围盒(Axis-Aligned Bounding Boxes,AABB)^[14-15]。

层次包围盒的核心思想是用多层包围盒描述几何物体(见图 8),最顶层使用一个略大的包围盒描述整个几何物体,随着层数的增加,使用多个更加贴合几何物体的包围盒描述。在进行碰撞检测时,若最顶层包围盒没有重叠,则肯定不会发生碰撞；若发生重叠,进行下一层检测,对物体分区块的进行快速碰撞检测。

AA包围盒定义为各边都平行于坐标轴且包含几何物体的最小长方体。可用组成几何物体的所有顶点坐标的最小值(x_min,y_min,z_min) 和最大值(x_max,y_max,z_max)表示。AA树是一棵完全二叉树,构建流程如图 9所示。

对于一次探测任务而言,探测环境的包围盒不需要更新,只要更新机械臂的包围盒即可。当机械臂平移时,对包围盒做同样的操作即可完成更新,当机械臂旋转时,用正运动学计算出机械臂特征点的新坐标值,求得其包围盒完成更新。

2个几何物体是否发生碰撞,可用2个物体的包围盒在各坐标轴上的投影是否重叠来判断。

图 10为包围盒碰撞检测。图中：C₁、C₂为2个包围盒的几何中心；P₁、P₂分别为C₁、C₂在Z轴上的投影；D₁、D₂为包围盒投影长度的一半。2个包围盒是否发生碰撞可以根据D₁、D₂、 的长度关系来判断,当时,2个包围盒发生碰撞。

当2个AABB包围盒在3个坐标轴上的投影均重叠时,2个包围盒描述的几何物体才发生碰撞。其他种类的包围盒,每个包围盒有各自的轴,而同一坐标系下的所有AABB包围盒坐标轴是一样的,因此只需要做3次投影,运算量大大减少。

7 SA^*算法

在智能机器人领域,A^*启发式搜索算法应用十分广泛,使用启发函数对每个子节点进行评估,选择最优子节点再扩展,不断接近目标状态。但应用A^*启发式搜索算法规划出的机械臂运动路径,从起点到终点,需遍历该条路径上所有子节点,而相邻子节点即产生一次机械臂位形切换,故产生多条遥测指令,无法满足任务要求中的机械臂位形切换次数最少的约束条件。SA^*算法是A^*启发式搜索算法的一种改进算法,通过对相邻节点进行代价削减,合并相邻节点,从而得到机械臂位形切换次数最少的运动序列。

SA^*算法的核心思想是代价削减。设当前节点i位形为Θ_i={θ_i1,θ_i2,θ_i3},其父节点和祖父节点可以确定机械臂连续2次位形变更,即{Δθ_(i－1)1,Δθ_(i－1)2,Δθ_(i－1)3},{Δθ_i1,Δθ_i2,Δθ_i3},是否可以等效为次位形变更,可根据二者绕三轴旋转方向是否一致来判断。图 11为机械臂关节运动约束。可知,Δθ_i2=0,Δθ_i3=0,故2次位形变更可视为次位形变更{Δθ_i1+Δθ_(i－1)1,Δθ_i2,Δθ_i3}。

因规划节点位形与初始位形的Hamming距离表示为,故启发函数设计为

式中：k为常数。确定常数k,使h(θ_i1,θ_i2,θ_i3)≤3。

假定机械臂三轴角度运动范围为θ_X、θ_Y和θ_Z。按机械臂角度运动范围将角度空间分为N_X、N_Y和N_Z等份,即分为N_X、N_Y和N_Z个单元。每个单元索引号映射为一个机械臂位形,θ_i={θ_i,θ_i2,θ_i3}为算法的状态空间。

算法的算子为

机械臂收拢位形为算法的初始状态,即θ_o={θ_o1,θ_o2,θ_o3}。

由评估之后的目标位置,应用机械臂逆运动学计算原理可计算出机械臂投放的目标位形θ_g={θ_g1,θ_g2,θ_g3},其作为基于SA^*的机械臂就位探测任务规划算法的目标状态。

算法的约束条件为：{θ_i1,θ_i2,θ_i3}∈{θ_X,θ_Y,θ_Z},且机械臂不与巡视器本体和探测区域发生碰撞。

基于SA^*的机械臂就位探测任务规划算法流程如图 12所示。算法的具体步骤如下：

1) 选择探测点。

2) 评估探测点是否有效,若有效进行步骤3)；否则,转至步骤)。

3) 通过星历计算和探测点的地理信息求得太阳反射光方向向量,通过探测点在世界坐标系下的坐标,应用机械臂逆运动学求得机械臂目标位形θ_g。

4) 由机械臂正运动学和机械臂目标位形θ_g求得机械臂展开到探测点时质谱仪轴向方向向量。

5) 判断太阳反射光方向向量和质谱仪轴向方向向量夹角,若大于5°,则进行步骤6)；否则,探测点不满足光照约束,转至步骤1)重新选择探测点。

6) 将机械臂初始位形θ_o放入Open表。

7) 若Open表为空,则规划失败,无法产生机械臂到达目标探测点θ_g的无碰撞运动序列,转至步骤)重新选取探测点；否则,进行步骤8)。

8) 根据启发函数h(θ)选取值最小的节点存入Unique树。

9) 若该节点可与其Unique树中的祖父节点合并,则进行代价削减；否则,转至步骤10)。

10) 扩展后的机械臂位形为θ_g,则规划成功,转至步骤3)；否则,进行步骤)。

11) 扩展该节点,并对扩展后的每一个子节点进行碰撞检测。

12) 若子节点发生碰撞,丢弃该无效子节点；否则,存入Open表,转至步骤7)。

13) 遍历Unique树,生成机械臂位形变换序列。

14) 若该序列满足机械臂位形切换次数要求,则算法完成退出；否则,转至步骤)重新选择探测点。

8 实验设置

为了验证基于SA^*的机械臂就位探测任务规划算法的正确性,使用Visual C++和开源三维仿真库搭建仿真系统,基于CE-3巡视器机械臂开展仿真验证工作,根据点云数据仿真地面环境。

8.1 实验：探测石块

根据本文提出的基于SA^*的机械臂就位探测任务规划算法原理,可以得出满足机械臂位形切换次数最少且不与环境发生干涉的机械臂运动序列,具体过程如图 13所示,黑色点为选取的探测点。表 3为基于SA^*的机械臂就位探测任务规划实验参数设置。

探测点在巡视器体系下的坐标为(0.762,-0.76,0.098),得到运动序列如表 4所示。

8.2 实验2：探测平面

更换点云数据,重建新的探测环境,机械臂展开过程如图 14所示。表 5为基于SA^*的机械臂就位探测任务规划实验2参数设置。

探测点在巡视器体系下的坐标为(0.692,-0.043,0.334),得到运动序列如表 6所示。

8.3 实验总结

通过以上2次仿真实验,可得结果如表 7所示,证明基于SA^*的机械臂就位探测任务规划算法可以实现对探测点的准确展开,并具备产生的运动序列数量少、不与环境发生干涉以及精度高的特点。

9 结 论

本文针对CE-3巡视器机械臂就位探测任务的约束特点,提出了一种改进型机械臂任务规划算法,经机械臂内场实验验证表明：

1) 月面探测任务的光照约束可被星历计算解决。

2) 通过在机械臂的探测空间中选择探测点,并对探测点进行评估,可提升探测成功率并提高探测效率。

3) 算法的机械臂碰撞检测方法具有快速和精度高的特点,在0.02 m的安全距离约束下可无碰撞地展开到预定探测点,完全满足探测需求。

4) 算法规划后的运动序列满足机械臂位形切换次数最少约束条件,在三维仿真图像中机械臂也正确地展开到探测点,证明了规划结果的正确性。