2.793

2018影响因子

(CJCR)

  • 中文核心
  • EI
  • 中国科技核心
  • Scopus
  • CSCD
  • 英国科学文摘

留言板

尊敬的读者、作者、审稿人, 关于本刊的投稿、审稿、编辑和出版的任何问题, 您可以本页添加留言。我们将尽快给您答复。谢谢您的支持!

姓名
邮箱
手机号码
标题
留言内容
验证码

空间冗余机械臂路径规划方法研究

齐志刚 黄攀峰 刘正雄 韩冬

齐志刚, 黄攀峰, 刘正雄, 韩冬. 空间冗余机械臂路径规划方法研究. 自动化学报, 2019, 45(6): 1103-1110. doi: 10.16383/j.aas.c180191
引用本文: 齐志刚, 黄攀峰, 刘正雄, 韩冬. 空间冗余机械臂路径规划方法研究. 自动化学报, 2019, 45(6): 1103-1110. doi: 10.16383/j.aas.c180191
QI Zhi-Gang, HUANG Pan-Feng, LIU Zheng-Xiong, HAN Dong. Research on Path Planning Method of Spatial Redundant Manipulator. ACTA AUTOMATICA SINICA, 2019, 45(6): 1103-1110. doi: 10.16383/j.aas.c180191
Citation: QI Zhi-Gang, HUANG Pan-Feng, LIU Zheng-Xiong, HAN Dong. Research on Path Planning Method of Spatial Redundant Manipulator. ACTA AUTOMATICA SINICA, 2019, 45(6): 1103-1110. doi: 10.16383/j.aas.c180191

空间冗余机械臂路径规划方法研究


DOI: 10.16383/j.aas.c180191
详细信息
    作者简介:

    齐志刚  西北工业大学航天学院硕士研究生.主要研究方向为空间机械臂路径规划, 机器视觉.E-mail: zhigangqi@mail.nwpu.edu.cn

    刘正雄  西北工业大学航天学院副教授.主要研究方向为空间机器人, 空间遥操作.E-mail:liuzhengxiong@nwpu.edu.cn

    韩冬  西北工业大学博士研究生.主要研究方向为空间遥操作.E-mail:alexcn@mail.nwpu.edu.cn

    通讯作者: 黄攀峰  西北工业大学航天学院教授.主要研究方向为空间机器人, 绳系空间机器人, 智能控制, 机器视觉, 空间遥操作.本文通信作者.E-mail:pfhuang@nwpu.edu.cn
  • 基金项目:

    国家自然科学基金 61773317

    国家杰出青年科学基金 61725303

Research on Path Planning Method of Spatial Redundant Manipulator

More Information
    Author Bio:

    Master student at the School of Astronautics, Northwestern Polytechnical University. His research interest covers path planning of space manipulator and machine vision

    Associate professor at the School of Astronautics, Northwestern Polytechnical University. His research interest covers space robotics and space teleoperation

    Ph. D. candidate at the Northwestern Polytechnical University. His main research interest is dynamic modeling for space teleoperation

    Corresponding author: HUANG Pan-Feng Professor at the School of Astronautics, Northwestern Polytechnical University. His research interest covers space robotics, tethered space robotics, intelligent control, machine vision and space teleoperation. Corresponding author of this paper
  • Fund Project:

    National Natural Science Foundation of China 61773317

    National Science Fund for Distinguished Young Scholar of China 61725303

图(9) / 表(2)
计量
  • 文章访问数:  309
  • HTML全文浏览量:  106
  • PDF下载量:  277
  • 被引次数: 0
出版历程
  • 收稿日期:  2018-04-04
  • 录用日期:  2018-06-10
  • 刊出日期:  2019-06-20

空间冗余机械臂路径规划方法研究

doi: 10.16383/j.aas.c180191
    作者简介:

    齐志刚  西北工业大学航天学院硕士研究生.主要研究方向为空间机械臂路径规划, 机器视觉.E-mail: zhigangqi@mail.nwpu.edu.cn

    刘正雄  西北工业大学航天学院副教授.主要研究方向为空间机器人, 空间遥操作.E-mail:liuzhengxiong@nwpu.edu.cn

    韩冬  西北工业大学博士研究生.主要研究方向为空间遥操作.E-mail:alexcn@mail.nwpu.edu.cn

    通讯作者: 黄攀峰  西北工业大学航天学院教授.主要研究方向为空间机器人, 绳系空间机器人, 智能控制, 机器视觉, 空间遥操作.本文通信作者.E-mail:pfhuang@nwpu.edu.cn
基金项目:

国家自然科学基金 61773317

国家杰出青年科学基金 61725303

摘要: 针对空间站遥操作7DOF冗余机械臂路径规划的安全性、可靠性问题,提出了基于臂型角逆运动学的优化A*路径规划算法.本文根据臂型角参数化完善了逆运动学方法,得到了32组完备逆解集,增加了路径规划时逆解选择的灵活性;通过臂型角搜索和最小奇异值优化A*路径规划算法,提高机械臂避障、避奇异能力,机械臂操作的灵活性和路径的安全可靠性;同时根据路径优化策略,有效平滑了路径,减少了机械臂的磨损.仿真结果说明了该方法的有效性.

本文责任编委 倪茂林

English Abstract

  • 随着我国航天事业的不断发展, 空间在轨服务和维护技术已经成为空间技术重要的研究方向.空间机械臂代替宇航员完成复杂的任务, 不仅可以减少宇航员在舱外执行任务的风险, 而且可以完成宇航员无法完成的大跨度操作任务, 如货物搬运, 目标捕获和大型设备更换等; 因此对长期在轨服务空间机械臂的操作灵活性、自主性、安全性和可靠性的要求不断提高[1-3].在此背景下许多学者针对冗余机械臂的运动学和路径规划问题进行相关研究, 其中大量学者对非偏置(Spherical-revolution-spherical, SRS)[4-5]机械臂进行研究, 对偏置机械臂(Space station remote manipulator, SSRMS)的研究学者相对较少, 其中Singh等[6]对仅肘部有偏置的7DOF机械臂逆运动学进行相关研究, Yu等[7]对仅腕部有偏置的机械臂进行了研究.对于肩部、肘部、腕部都有偏置的7DOF机械臂的逆运动学更加复杂, 目前对该方向研究的大概分为两大分支, 一种以Crane等[8]为代表的基于空间多边形几何投影的逆运动学求解方法, 该方法涉及复杂的空间几何投影理论, 几何意义不明显; 另一种是以徐文福等[9]为代表的基于臂型角的7DOF机械臂逆运动学求解方法[10-12], 此方法几何意义明显, 适合臂型控制任务.在逆运动学的基础上, 笛卡尔空间的路径规划也显的尤其重要, 路径规划不仅需要找到一条从起始点到终止点的最优路径, 同时也需要满足机械臂与障碍物不发生碰撞的要求.常用的路径规划算法为A*算法、蚁群算法[13]、遗传因子算法[14]、随机树法和人工势场法等, 由于A*算法具有很强的自主性和避障能力, 受到许多学者的青睐, 汪首坤等[15]和Yao等[16]通过改变搜索步长和优化A*算法来减少路径搜索时间, 避免使算法进入死循环; 贾庆轩等[17]在关节空间内运用A*算法, 该方法可以避免机械臂求逆解的问题, 但由于维度过大, 在搜索路径过程中将消耗大量的时间; Ge等[18]提出了基于NEWTON-RAPHSON逆运动学方法的RRT路径规划方法, 由于RRT在笛卡尔空间随机采样, 同时NEWTON-RAPHSON逆运动学方法需要进行数值迭代计算, 降低了路径规划速度, 且该方法没有考虑逆解不存在的可能性及机械臂操作的灵活性.王瞾等[19]提出了基于深度Q学习的机械臂路径规划方法, 该方法存在收敛速度慢以及存储空间等问题, 并且无法对机械臂的臂型进行控制.宗成星等[20]利用A*算法针对6自由度机械臂在笛卡尔空间进行路径规划, 但是没有考虑机械臂操作的灵活性以及臂型控制.徐文福[21]针对PUMA机器人提出了一种奇异分离加阻尼倒数方法, 较好地解决了机器人奇异回避问题.张浩杰等[22]利用变维度状态空间的增量启发式路径规划方法, 通过以前的规划结果, 可以保证算法的增量性及实时性.

    本文在徐文福等[9]、闫磊[12]、王瞾等[19]和宗成星等[20]的基础上, 对空间站肩、轴和腕部均有偏置的7DOF机械臂进行逆运动学改善, 根据臂型角[9]的方法得到机械臂32组完备逆解集, 从而得到了逆运动学的解析解, 该方法比Ge等[18]提出的基于NEWTON-RAPHSON逆运动学方法计算速度快, 且通过臂型角实现对机械臂的臂型控制, 几何意义明显.结合逆运动学方法, 通过把机械臂的雅克比矩阵最小奇异值作为代价项加入到A*算法代价函数中, 在臂型角参数空间和笛卡尔空间进行搜索, 寻找一组合适的空间路径和机械臂逆解来实现对机械臂的避障, 以及避奇异路径规划, 解决了王瞾等[19]中Q-learning收敛速度慢以及所需存储空间大的问题; 与宗成星等[20]的方法相比, 本文方法在逆运动学求解时, 若机械臂与障碍物发生碰撞, 可以通过在臂型角空间搜索合适的臂型角来规避与障碍物的碰撞, 增加路径规划的可靠性与安全性.并通过计算仿真, 证明逆运动学求解的正确性, 路径规划的安全可靠性.

    • 下面表 1为某机械臂的D-H参数, 参考霍伟[23]和Craig[24]书中模型建立方法, 图 1表 1参数下的机械臂的D-H坐标系图, 其中每个关节为旋转关节, 由此可得如式(1)所示的坐标旋转矩阵, 式中, ${}^{i-1}{{{T}}_{i}}$代表第$i$坐标系相对于第$i-1$坐标系的齐次变换矩阵.

      表 1  机械臂D-H参数

      Table 1.  The D-H parameters of manipulator

      杆$i$ ${{\theta }_{i}} \, ({}^\circ )$ ${{\alpha }_{i}} \, ({}^\circ )$ ${{a}_{i}} \, (\text{m)}$ ${{d}_{i}}\, (\text{m)}$
      10900 ${{d}_{1}}$
      20900 ${{d}_{2}}$
      300 ${{a}_{3}}$ ${{d}_{3}}$
      400 ${{a}_{4}}$ ${{d}_{4}}$
      50900 ${{d}_{5}}$
      60900 ${{d}_{6}}$
      70900 ${{d}_{7}}$

      图  1  表 1的机械臂几何模型

      Figure 1.  Manipulator's geometry model of Table 1

      由此可得式(2)机械臂的正运动学方程.

      $$ \begin{align} &{}^{i-1}{{{T}}_{{i}}}=\nonumber\\&\qquad \begin{subarray}{l}\begin{bmatrix} \cos{{\theta }_{i}} \!\!&\!\! -\sin{{\theta }_{i}}\cos{{\alpha }_{i}} \!\!&\!\! \sin{{\theta }_{i}}\sin{{\alpha }_{i}} \!\!&\!\! {{a}_{i}}\cos{{\theta }_{i}} \\ \sin{{\theta }_{i}} \!\!&\!\! \cos{{\theta }_{i}}\cos{{\alpha }_{i}} \!\!&\!\! -\cos{{\theta }_{i}}\sin{{\alpha }_{i}} \!\!&\!\! {{a}_{i}}\sin{{\theta }_{i}} \\ 0 \!\!&\!\! \sin{{\alpha }_{i}} \!\!&\!\! \cos{{\alpha }_{i}} \!\!&\!\! {{d}_{i}} \\ 0 \!\!&\!\! 0 \!\!&\!\! 0 \!\!&\!\! 1 \\ \end{bmatrix}\end{subarray} \end{align} $$ (1)
      $$ \begin{equation} ^0{T_7} = \mathop {\mathop \prod \limits_{i = 1} {\mkern 1mu} }\limits^7 {{\mkern 1mu} ^{i - 1}}{T_i} \end{equation} $$ (2)

      当${{d}_{2}}$与${{d}_{6}}$都为零时得到SRS机械臂模型, 当${{d}_{2}}$与${{d}_{6}}$都不为零时其模型为SSRMS模型.首先求解SRS机械臂模型的${}^{0}{R}_{3}^{\psi =0}$ (3号坐标系在臂型角为0时, 相对于基座的旋转矩阵), 再根据臂型角求解各个关节, 然后利用机械臂模型SRS与SSRMS的逆解关系求解SSRMS模型的逆解.

    • 本文选取关节$1$和关节$2$旋转轴线的交点作为$S$点, $3$号坐标系的原点为$E$点, 关节$6$和关节$7$旋转轴线的交点为$W$点, 点${{S}_{\bot}}$为点$S$沿关节3 $({{z}_{2}})$轴线延长${{d}_{3}}$长度的点, 与$EW$直线构成一个平面.根据该机械臂的臂型特征可知, 直线$S{{S}_{\bot}}$与面${{S}{\bot}}EW$垂直, 本文用符号$\left| SW \right|$表示线段$SW$的长度, 如图 2所示.根据图 2得肘部关节为

      $$ \begin{equation} \theta_{_{4}}^{'}=\pm\operatorname{arccos}\left(\frac{{{\left|{SW}\right|}^{2}}-d_{3}^{2}-a_{3}^{2}-a_{4}^{2}}{2{{a}_{3}}{{a}_{4}}} \right) \end{equation} $$ (3)

      图  2  $\rm SRS$机械臂几何模型

      Figure 2.  The geometry model of SRS's manipulator

      图 3图 2的简化示意图, 图中的$\psi$为臂型角, 臂型角的具体定义, 请参考徐文福等[9]文章.本文规定3号坐标系的原点在参考平面上时(设为初始状态), 各量的右上标取为0加以区分, 参考系标识在左上角, 机械臂处于初始状态关系图如图 4所示.在图 4中, 经计算分析可知, 当$\alpha$取负值时对应的臂型与$\alpha$为正值时旋转${180}^\circ$对应得臂型是不同的两种情况, 所以把$\alpha$取负值时的臂型也算初始臂型的一种情况.下面将对${}^{0}{R}_{3}^{\psi =0}$进行求解.

      图  3  臂型角示意图

      Figure 3.  The diagram of arm angle

      图  4  初始状态下各向量关系图

      Figure 4.  The relationship among vectors in initial state

      图 2及DH坐标系建系规则可知:

      $$ \begin{equation} {{d}_{3}}{}^{0}\boldsymbol{z}_{3}^{0}+{{a}_{3}}{}^{0}\boldsymbol{x}_{3}^{0} \, = \, {}^{0}{{\boldsymbol{e}}^{0}} \end{equation} $$ (4)

      式(4)中的${}^{0}{{\boldsymbol{e}}^{0}}$为已知量, 证明如下:

      图 4可知, 向量$\boldsymbol{e}$可以看作是由向量$\boldsymbol{w}$绕向量$\boldsymbol{l}$旋转$\alpha$角度所得.此时则有

      $$ \begin{equation} {}^{\text{0}}{{\boldsymbol{e}}^{0}}={}^{\text{0}}\boldsymbol{e}={R}\left( \boldsymbol{l}, \alpha \right)\cdot \boldsymbol{w} \end{equation} $$ (5)

      其中:

      $$ \begin{equation} \boldsymbol{l=v}\text{ }\!\!\times\!\!\text{ }\boldsymbol{w} \end{equation} $$ (6)
      $$ \begin{equation} {R}\left( \boldsymbol{l}, \alpha \right)={{{I}}_{3}}+\left[ {{\boldsymbol{u}}_{\boldsymbol{l}}}\times \right]\sin \alpha +{{\left[ {{\boldsymbol{u}}_{\boldsymbol{l}}}\times \right]}^{2}}\left( 1-\cos \alpha \right) \end{equation} $$ (7)

      $\left[ {{\boldsymbol{u}}_{\boldsymbol{l}}}\times \right]$表示向量$\boldsymbol{l}$单位化后的叉乘因子, 式(5)中只有$\alpha$是未知的, 其他均已知, $\alpha$的求解过程如下, 结合图 2图 4, 在$\vartriangle{S}{{{E}}^{0}}{W}$中可得:

      $$ \begin{equation} {{c}_{\alpha }}=\frac{{{a}_{3}}^{2}+{{d}_{3}}^{2}+{{\left|SW \right|}^{2}}-{{a}_{4}}^{2}}{2{\left| SW \right|}\sqrt{{{a}_{3}}^{2}+{{d}_{3}}^{2}}} \end{equation} $$ (8)

      在(8)中, 由于$\sqrt{{{a}_{3}}^{2}+{{d}_{3}}^{2}}>{{a}_{4}}$, 所以$\left|\alpha\right|\le{{90}^{\circ }}$, 故有:

      $$ \begin{equation} \alpha =\pm \text{arccos}(\frac{{{a}_{3}}^{2}+{{d}_{3}}^{2}+{{\left| SW \right|}^{2}}-{{a}_{4}}^{2}}{2\left| SW \right|\sqrt{{{a}_{3}}^{2}+{{d}_{3}}^{2}}}) \end{equation} $$ (9)

      至此, $\alpha$求解完毕, 即${}^{0}{{\boldsymbol{e}}^{0}}$为已知量证明完毕.由图 2及DH坐标系建系规则可知:

      $$ \begin{equation} \boldsymbol{w}-\boldsymbol{e}={{a}_{4}}{{\boldsymbol{x}}_{4}} \end{equation} $$ (10)

      由方向余弦表示的姿态变换关系可知:

      $$ \begin{equation} {}^{3}{{\boldsymbol{x}}_{4}}={{\left[ \begin{matrix} {{c}_{4}} & {{s}_{4}} & 0 \\ \end{matrix} \right]}^{\text{T}}} \end{equation} $$ (11)
      $$ \begin{equation} {}^{0}{{\boldsymbol{x}}_{4}}={}^{0}{{{R}}_{3}}\cdot {}^{3}{{\boldsymbol{x}}_{4}}=\left[ \begin{matrix} {}^{0}{{\boldsymbol{x}}_{3}} & {}^{0}{{\boldsymbol{y}}_{3}} & {}^{0}{{\boldsymbol{z}}_{3}} \\ \end{matrix} \right]{}^{3}{{\boldsymbol{x}}_{4}} \end{equation} $$ (12)

      由式(10)$\sim$(12)可得:

      $$ \begin{equation} {{c}_{4}}{}^{0}\boldsymbol{x}_{3}^{0}+{{s}_{4}}{}^{0}\boldsymbol{y}_{3}^{0}={}^{0}\boldsymbol{x}_{4}^{0} \end{equation} $$ (13)
      $$ \begin{equation} {}^{0}\boldsymbol{x}_{4}^{0}=\frac{\boldsymbol{w}-{}^{0}{{\boldsymbol{e}}^{0}}}{{{a}_{4}}} \end{equation} $$ (14)

      由于$\boldsymbol{w}$、${{a}_{4}}$和${}^{0}{{\boldsymbol{e}}^{\mathsf{0}}}$都是已知量, 故${}^{0}\boldsymbol{x}_{4}^{0}$也是已知量.式(4)和式(13)左右两边分别叉乘且结合三轴单位正交矢量的关系可得:

      $$ \begin{equation} \begin{aligned} {}^{0}\boldsymbol{x}_{4}^{0}\times&{}^{{0}}{{\boldsymbol{e}}^{{0}}}=\left( {{c}_{4}}{}^{0}\boldsymbol{x}_{3}^{0}+{{s}_{4}}{}^{0}\boldsymbol{y}_{3}^{0} \right)\times \left({{d}_{3}}{}^{{0}}\boldsymbol{z}_{{3}}^{{0}}+{{a}_{3}}{}^{{0}}\boldsymbol{x}_{{3}}^{{0}} \right) =\\&{{d}_{3}}{{c}_{4}}{}^{0}\boldsymbol{x}_{3}^{0}\times {}^{{0}}\boldsymbol{z}_{{3}}^{{0}}+{{a}_{3}}{{c}_{4}}{}^{0}\boldsymbol{x}_{3}^{0}\times {}^{{0}}\boldsymbol{x}_{{3}}^{{0}}+\\&{{d}_{3}}{{s}_{4}}{}^{0}\boldsymbol{y}_{3}^{0}\times {}^{{0}}\boldsymbol{z}_{{3}}^{{0}}+{{a}_{3}}{{s}_{4}}{}^{0}\boldsymbol{y}_{3}^{0}\times {}^{{0}}\boldsymbol{x}_{{3}}^{{0}} =\\&-{{d}_{3}}{{c}_{4}}{}^{0}\boldsymbol{y}_{3}^{0}+{{d}_{3}}{{s}_{4}}{}^{{0}} \boldsymbol{x}_{{3}}^{{0}}-{{a}_{3}}{{s}_{4}}{}^{{0}}{\boldsymbol z}_{{3}}^{{0}} \end{aligned} \end{equation} $$ (15)

      解由式(4)、式(13)和式(15)组成的方程组:

      $$ \begin{equation} \left\{\begin{aligned} & {{c}_{4}}{}^{0}\boldsymbol{x}_{3}^{0}+{{s}_{4}}{}^{0}\boldsymbol{y}_{3}^{0}={}^{0}\boldsymbol{x}_{4}^{0} \\ &{{d}_{3}}{}^{{0}}\boldsymbol{z}_{{3}}^{{0}}+{{a}_{3}}{}^{{0}}\boldsymbol{x}_{{3}}^{{0}}={}^{{0}}{{\boldsymbol{e}}^{{0}}} \\ &-{{d}_{3}}{{c}_{4}}{}^{0}\boldsymbol{y}_{3}^{0}+{{d}_{3}}{{s}_{4}}{}^{{0}}\boldsymbol{x}_{{3}}^{{0}}-{{a}_{3}}{{s}_{4}}{}^{{0}}\boldsymbol{z}_{{3}}^{{0}}={}^{0}\boldsymbol{x}_{4}^{0}\times {}^{{0}}{{\boldsymbol{e}}^{{0}}} \\ \end{aligned} \right. \end{equation} $$ (16)

      由式(16)可得:

      $$ \begin{equation} \begin{cases} {}^{0}\boldsymbol x_{3}^{0}=\displaystyle\dfrac{({}^{0}\boldsymbol x_{4}^{0}\times {}^{0}{{\boldsymbol e}^{0}}){{d}_{3}}{{s}_{4}}+d_{3}^{2}{{c}_{4}}{}^{0}\boldsymbol x_{4}^{0}+{{a}_{3}}s_{4}^{2}{}^{0}{{\boldsymbol e}^{0}}}{d_{3}^{2}+{{({{ a}_{3}}{{s}_{4}})}^{2}}} \\ \displaystyle{}^{0}\boldsymbol z_{3}^{0}=\dfrac{{}^{0}{{\boldsymbol e}^{0}}-{{ a}_{3}}{}^{0}\boldsymbol x_{3}^{0}}{{{d}_{3}}} \\ {}^{0}\mathsf{\boldsymbol y}_{3}^{0}={}^{0}\mathsf{\boldsymbol z}_{3}^{0}\times {}^{0}\mathsf{\boldsymbol x}_{3}^{0} \end{cases} \end{equation} $$ (17)

      故:

      $$ \begin{equation} {}^{0}{R}_{3}^{\psi =0}=\left[ \begin{matrix} {}^{0}\boldsymbol{x}_{3}^{0} & {}^{0}\boldsymbol{y}_{3}^{0} & {}^{0}\boldsymbol{z}_{3}^{0} \\ \end{matrix} \right] \end{equation} $$ (18)

      因为$\theta _{4}^{'}$和$\alpha$可取两种情况, 即${}^{0}{R}_{3}^{\psi =0}$有四种情况.

    • 由于SRS机械臂的逆解不是本文重点, 不做详细描述, 具体方法请参考徐文福等[9]文章, 下面将直接给出臂型角为时SRS机械臂逆解的结论.

      当末端位姿与臂型角确定后可得到SRS机械臂的关节角为$\theta _{1}^{'}, \theta _{2}^{'}, \theta _{3}^{'}, \theta _{4}^{'}$+$\theta_{5}^{'}, \theta _{6}^{'}, \theta _{7}^{'}$, 因为$\theta _{4}^{'}, \alpha , \theta _{2}^{'}, \theta _{6}^{'}$都有两种情况, 固$\theta _{1}^{'}, \theta _{2}^{'}, \theta _{3}^{'}, \theta _{4}^{'}$+$\theta _{5}^{'}, \theta _{6}^{'}, \theta _{7}^{'}$共有16种情况.

      本文$\theta _{i}^{'}$代表SRS机械臂的逆解关节角, ${{\theta }_{i}}$代表SSRMS机械臂的逆解关节角, 由SSRMS与SRS关系得${{\theta }_{1}}=\theta _{1}^{'}, {{\theta }_{2}}=\theta _{2}^{'}, {{\theta }_{6}}=\theta _{6}^{'}, {{\theta }_{7}}=\theta _{7}^{'}, {{\theta }_{345}}=\theta _{345}^{'}$,

      根据式(2)得到:

      $$ \begin{equation} {}^{2}{{T}_{5}}={{({}^{0}{{T}_{1}}{}^{1}{{T}_{2}})}^{-1}}({}^{0}{{T}_{7}}){{({}^{5}{{T}_{6}}{}^{6}{{T}_{7}})}^{-1}} \end{equation} $$ (19)
      $$ \begin{equation} {}^{2}{{T}_{5}}={}^{2}{{T}_{3}}{}^{3}{{T}_{4}}{}^{4}{{T}_{5}} \end{equation} $$ (20)

      由于SSRMS机械臂的${{\theta }_{1}}, {{\theta }_{2}}, {{\theta }_{6}}, {{\theta }_{7}}$已知, 设矩阵${}^{2}{{{T}}_{5}}$中(1, 4)和(2, 4)元素分别为$A$、$B$, 令式(20)两边第(1, 4)和(2, 4)元素相等, 则有

      $$ \begin{equation} \left\{ \begin{aligned} & {{a}_{3}}{{s}_{3}}+{{a}_{4}}{{s}_{4}}=A \\ & {{a}_{3}}{{c}_{3}}+{{a}_{4}}{{c}_{34}}=B \\ \end{aligned} \right. \end{equation} $$ (21)

      可得${{\theta }_{4}}$、${{\theta }_{3}}$的值

      $$ \begin{align} {{\theta}_{4}}=\, &\pm \arccos \frac{{{A}^{2}}+{{B}^{2}}-a_{3}^{2}-a_{4}^{2}}{2{{a}_{3}}{{a}_{4}}} \end{align} $$ (22)
      $$ \begin{align} {{\theta }_{3}}=\, & \arctan(A({{a}_{3}}+{{a}_{4}}{{c}_{4}})- B{{a}_{4}}{{s}_{4}}, \nonumber\\ & B({{a}_{3}}+{{a}_{4}}{{c}_{4}})+A{{a}_{4}}{{s}_{4}}) \end{align} $$ (23)

      由${{\theta }_{5}}=\theta _{3}^{'}+\theta _{4}^{'}+\theta _{5}^{'}-{{\theta }_{3}}-{{\theta }_{4}}$得关节角5的值.综上偏置7DOF机械臂(SSRMS) ${{\theta }_{4}}$有两种情况, 固偏置7DOF机械臂(SSRMS)逆解有32种情况.

    • 为实现A*算法的路径规划, 需要对机械臂工作空间进行栅格化处理并建模.为了使机械臂有效避开奇异空间和障碍物, 本文优化的A*路径规划算法分为两步; 第一步为:先把最小奇异值作为代价项加入到代价函数中实现在笛卡尔空间避奇异搜索, 然后在臂型角空间搜索合适的臂型, 从而实现对机械臂避障控制.由于第一步搜索的路径缺乏光滑性, 因此通过第二步优化策略进行光滑处理, 使路径更加平滑, 减少机械臂的磨损.

      当机械臂末端位姿确定时, 根据本文提到的逆运动学求解方法, 可求得关节角序列为${{q}_{i}}~(i=1, 2, 3, \cdots, 32)$, 根据式(24)和(25)可得到最小奇异值, 为了安全考虑, 由式(26)可得$s(n)$, 若对应的位置无逆解存在, 则令$s(n)$为一个较小值$\zeta$.

      $$ \begin{equation} J({{q}_{i}})={U}\sum {{{V}}^{{\rm T}}}={U}\left[ \begin{matrix} {{\lambda }_{1}} & {} & {} \\ {} & \ddots & {} \\ {} & {} & {{\lambda }_{k}} \\ \end{matrix} \right]{{{V}}^{{\rm T}}} \end{equation} $$ (24)
      $$ \begin{equation} {{\sigma }_{\min \_i}}=\{\min \{{{\sigma }_{j}}|{{\sigma }_{j}}=\sqrt{{{\lambda }_{j}}}\}\}, ~j=1, 2, \cdots, k \end{equation} $$ (25)
      $$ \begin{equation} s(n)=\min \{{{\sigma }_{\min \_i}}\}, ~i=1, 2, \cdots, 32 \end{equation} $$ (26)
    • 传统的A*算法为启发式路径规划算法, 其算法代价函数如式(27)所示

      $$ \begin{equation} f(n)=g(n)+h(n) \end{equation} $$ (27)

      其中, $f(n)$为实际代价, $g(n)$为起始位置到当前位置的代价, $h(n)$为当前位置到目标位置代价.当路径距离奇异位置较近时有可能导致任务失败, 为避免此情况发生, 本文将代价函数改为式(28)所示, 可以有效避免因奇异原因导致机械臂路径规划失败.

      $$ \begin{equation} f(n)=g(n)+h(n)+\frac{\rho}{s(n)} \end{equation} $$ (28)

      上式中$\rho$为比例因子, 有调节路径距离奇异空间远近的作用.

      图 5算法流程图中:

      图  5  优化A*算法第一步流程图

      Figure 5.  First step flow chart of optimization A* algorithm

      1) $F=G+H+S$, 其中$F$、$G$、$H$和$S$分别代表当前点$f(n)$、$g(n)$、$h(n)$和$\displaystyle{\rho}/{s(n)}$的值.

      2) 图中close列表中的节点为${{P}_{i}}$集合, 即为规划路径, 集合$I$为集合${{P}_{i}}$中对应节点的逆解序列集合, 临域节点为周围26临域可到达点.

      3) 当臂型角增加到${360}^\circ$时, 臂型角重新复位到${0}^\circ $.

      4) 第一步算法在笛卡尔空间进行搜索可以有效避开空间奇异位置, 同时在臂型角空间进行搜索, 可以选择合适的臂型使机械臂避免与障碍物发生碰撞.

      图 6算法流程图中:

      图  6  优化A*算法第二步流程图

      Figure 6.  The second step flow chart of optimization A* algorithm

      1) $\mu$值为集合${{P}_{i}}$中节点最小奇异值的均值.

      2) 通过减少路径中不必要的节点, 进行路径光滑处理, 得到集合${{P}_{o}}$和${{I}_{o}}$.本文算法得到的${{P}_{i}}$集合与${{P}_{o}}$集合如式(29)所示, ${{I}_{o}}$为${{P}_{o}}$中节点对应的逆解序列.

      $$ \begin{equation} \left\{ \begin{aligned} & {{P}_{i}}\{x_{1}^{i}, x_{2}^{i}, x_{3}^{i}, \cdots, x_{j}^{i}, \cdots, x_{a}^{i}\} \\ & {{P}_{o}}\{x_{1}^{o}, x_{2}^{o}, x_{3}^{o}, \cdots, x_{k}^{o}, \cdots, x_{b}^{o}\} \\ \end{aligned} \right. \end{equation} $$ (29)
    • 本文以表 2所示的机械臂参数为模型进行仿真验证, 为了证明逆解的正确性, 当机械臂末端位姿为[1.5 m, 2.5 m, $-$1.0 m, ${0}^\circ$, ${0}^\circ $, ${0}^\circ$], 臂型角分别为${30}^\circ$、${120}^\circ$时, 进行32组逆解求解, 然后在Unity3D平台模型中验证了逆解的正确性, 由于逆解数目过多, 本文只展示臂型角分别为${30}^\circ$、${120}^\circ$时32组逆解中的两种臂型情况, 如图 7所示.图中a、b为臂型角${30}^\circ$时两种逆解臂型, c、d为臂型角为${120}^\circ$时两种逆解臂型.当臂型角为${30}^\circ$时, 有16种逆解与障碍物发生碰撞, 16种逆解与障碍物未发生碰撞; 当臂型角为${120}^\circ$时, 32种逆解都未与障碍物发生碰撞.

      表 2  仿真机械臂D-H参数

      Table 2.  Simulation parameter of manipulator's DH

      杆$i$ ${{\theta }_{i}} ({}^\circ )$ ${{\alpha }_{i}} ({}^\circ )$ ${{a}_{i}} (\text{m)}$ ${{d}_{i}} (\text{m)}$
      1909000.25
      2909000.25
      39002.00.6
      418002.00
      5909000
      609000.25
      709000.35

      图  7  逆解的臂型

      Figure 7.  The arm type of the inverse solution

      图 8为按机械臂末端姿态固定(姿态按照路径规划要求选取, 本文按照固定值仿真), 位置进行均匀采样得到散点云图, 其中越亮的位置对应的最小奇异值越小, 越临近奇异空间; 并在工作空间[3.5 m, 0.5 m, 0.5 m], [2.5 m, 1.5 m, 0.5 m]和[1.5 m, 1.5 m, 1.5 m]处有1 m×1 m×1 m大小的立方体障碍物; 在散点云空间进行从位置[1.0 m, 1.0 m, 0.0 m]到位置[1.5 m, 1.5 m, 3.0 m]的路径规划来进行仿真验证.其中的虚线路径为传统的A*算法搜索到的路径, 点划线路径为优化A*算法第一步搜索到的路径, 黑色实线为优化算法第二步得到的路径, 即为本文所求路径.

      图  8  在可达空间内$s(n)$的散点云图

      Figure 8.  The scatter gram of $s(n)$ in reachable space

      图 9图 8中规划的三条路径最小奇异值变化曲线, 点划线路径的最小奇异值均值为0.735;黑色实线路径最小奇异值均值为0.6747, 虽然最小奇异值稍有降低, 但路径更加平缓; 虚线路径最小奇异值均值为0.223, 路径经过奇异值为0的位置, 这将导致路径规划的失败.本次路径仿真参数$\rho$、$\zeta$分别取10.0、0.0001.

      图  9  路径中最小奇异值曲线

      Figure 9.  The curve of minimum singular value in path

    • 图 7分析可知, 当臂型角为${30}^\circ$时, 有一半逆解与障碍物发生碰撞, 另一半逆解与障碍物未发生碰撞; 当臂型角为${120}^\circ$时, 32种逆解都未与障碍物发生碰撞, 因此在臂型角空间进行搜索规划路径可以有效实现机械臂避障功能; 同时也证明逆运动学算法的正确性.

      机械臂的雅克比矩阵最小奇异值越大代表机械臂在该状态下离奇异空间越远, 路径越安全, 因此路径中节点的最小奇异值的平均值可以代表路径规划的质量.从图 8可知, 算法第一步规划的路径为点画线, 该路径避开较亮的奇异位置, 路径中最小奇异值的平均值为0.735, 并未出现最小奇异值为0的位置, 从而保证了规划任务的安全可靠性; 通过算法第二步, 可以得到图 8中黑色实线平滑路径, 路径中最小奇异值的平均值为0.6747, 也未出现最小奇异值为0的位置, 虽然最小奇异值的均值稍有下降, 但路径变得更加平滑, 有效降低机械臂的磨损; 图 8中虚线路径为传统A*路径规划算法, 该路径中最小奇异值的平均值为0.223, 并且有部分节点经过奇异位置, 这将降低路径的安全可靠性, 导致规划任务失败.综上所述, 仿真结果证明本文的算法有效性.

    • 本文改善了基于臂型角的逆运动学方法, 可以得到32组逆解的完备集, 该方法不仅可以增加逆解选择的灵活性, 而且可以通过臂型角实现对臂型的控制, 为机械臂的避障规划提供了一种方法.在基于逆运动学的基础上, 本文提到的优化A*路径规划算法, 通过引入最小奇异值作为代价项和搜索合适的臂型角方法, 使机械臂实现了避障、避奇异功能, 保证路径规划任务的安全可靠性; 并通过优化策略有效的平滑了路径, 减小了机械臂的磨损.本文提到的算法也适用地面机械臂路径规划任务.

参考文献 (24)

目录

    /

    返回文章
    返回