Open Access
Issue
JNWPU
Volume 39, Number 5, October 2021
Page(s) 1005 - 1011
DOI https://doi.org/10.1051/jnwpu/20203951005
Published online 14 December 2021

© 2021 Journal of Northwestern Polytechnical University. All rights reserved.

Licence Creative CommonsThis is an Open Access article distributed under the terms of the Creative Commons Attribution License (http://creativecommons.org/licenses/by/4.0), which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

FFSR具备节省燃料, 延长系统寿命等优点, 被广泛用于在轨服务。FFSR构型参数中, 关节角和基座姿态间存在一阶导关系, 需直接考虑微分约束进行运动规划。

FFSR运动规划方法中, 研究较多的包含3类。早期学者研究了系统的操控(steer)问题, 即在不考虑障碍物的情况下, 生成一系列的动作, 让FFSR从初始构型运动到目标构型。如Vafa等基于“虚拟机械臂(virtual manipulator, VM)”1的模型, 发展了自校正运动法1及“扰动图法(disturbance map, DM)”2。Nakamura和Mukherjee3基于李雅普诺夫函数设计了双向方法。Fernandes等4研究了耦合刚体的运动规划方法,此类方法着眼于利用关节运动操控基座姿态; 但未考虑碰撞, 不能形成完整的运动规划。第二类方法是基于多项式参数化的方法, 即以FFSR的初始关节角和最终关节角为端点, 利用带未知系数的多项式对关节轨迹进行参数化, 并以最终时刻基座姿态扰动为适应度函数, 将规划问题转换为优化问题; 利用遗传算法5、粒子群算法6、微分进化算法7等解优化问题, 可得最终轨迹。该类方法不易控制轨迹的形状, 因此很难考虑碰撞问题; 此外, 由于适应度函数计算需进行积分, 因此效率低下。第三类方法是通过直接法或间接法将运动规划问题转换为非线性优化问题89。Misra等10于2017年基于非线性优化研究了空间机器人考虑任务约束的运动规划问题, 并将问题构造为一个凸二次规划问题。该类方法求解效率高, 但也存在局部最优问题。

基于采样的运动规划方法, 能高效解决有高自由度构型空间的机器人运动规划问题。其中RRT(rapidly-exploring random tree)对有微分约束运动规划有很强的适用性11。一般情况下, 运动规划多采用“先求逆运动学, 再规划路径/轨迹”的框架; 但这一框架存在逆运动学所求目标构型与初始构型未必在同一连通域内的隐患。针对这一问题, Bertram、Weghe等1213基于有目标偏置的RRT(RRT with Goal Bias, GB-RRT)给出了解决方案, 即不求逆运动学, 而是直接引入目标末端位姿导引生长。在该方案中, 用于目标末端位姿导引生长的局部规划含一个迭代过程, 每次迭代通过雅克比矩阵的转置, 将末端位姿误差映射到构型空间, 从而产生一系列越来越接近目标末端位姿的构型,但该方案针对固定基座机械臂的路径规划问题, 而FFSR的运动规划需直接考虑微分约束。此外, 固定基座机械臂执行机构自由度大于末端位姿自由度, 但FFSR末端末端位姿导引生长, 除要求具备最终收敛到目标末端位姿的能力外, 还需保证基座姿态扰动不能过大, 因此执行机构自由度不足。

本文以GB-RRT为框架设计规划算法, 解决FFSR从初始构型到目标末端位姿的运动规划问题。为适应FFSR直接考虑微分约束的需求, 在Weghe等13提出的方法基础上, 在每次迭代中引入积分机制; 为克服FFSR执行机构自由度不足的问题, 提出适时调节基座姿态的目标末端位姿导引生长局部规划器, 以调节末端位姿为主, 同时在必要时兼顾考虑基座姿态的调节。

1 自由漂浮空间机器人的运动学方程

FFSR如图 1所示, 符号定义与文献[14]相同。

thumbnail 图1

自由漂浮空间机器人的运动学方程

1.1 仅考虑几何关系的运动学方程

1.1.1 姿态方程

定义FFSR的构型参数为, 其中Ψb为基座姿态角, Θ为关节角向量。FFSR末端执行器(End-Effector, 简写为EE)姿态变换矩阵与q间的映射关系为

FFSR的EE姿态角ΨeIRe对应, 因此

1.1.2 位置方程

FFSR末端执行器位置pe

1.1.3 速度方程

FFSR末端执行器角速度ωe

末端执行器线速度ve度为

根据(4)~(5)式, 可得FFSR的末端执行器速度e

Jb, Jm的表达式见文献[14]。

1.2 动量守恒方程

FFSR的动量方程为

式中,PL分别为FFSR的线动量、角动量, 相关矩阵的定义间文献[14]。令

1.3 考虑动量守恒后的运动学方程

1.3.1 基座姿态角一阶导方程

FFSR线动量、角动量守恒, 假设系统的初始动量为0, 则可将基座速度b表示为的函数

取上述方程后3行, 可得基座角速度ωb

bωbωb在基座固连坐标系中的表示, bωb基座姿态角一阶导间存在如下关系

式中

考虑(9)式及(10)式, 则

式中,J为与基座姿态角相关的雅克比矩阵。

1.3.2 状态转移方程

考虑(11)式, 则FFSR的状态转移方程为

式中,Jx为与状态转移相关的雅克比矩阵。

1.3.3 广义雅克比矩阵

考虑(6)式以及(8)式, 可得

Jg为广义雅克比矩阵。

1.3.4 考虑线动量守恒积分形式后的方程

FFSR的线动量可积分为位置形式

考虑到, 可得

式中, 表示各连杆质心相对于基座质心的位置矢量, 因此上式又可以写成

进而

给出构型参数q, 可计算在惯性空间之中的表示Ibi, Iai, 进而可计算r0, 即q为构型参数q的函数。再考虑(3)式, 可知peq的函数

综合考虑(1)式以及(17)式, 可得端位姿xeq间的映射关系

2 FFSR无逆运动学基于采样的运动规划

算法伪代码如Algorithm 1所示, 在每次迭代中, 利用随机采样构型qSample或目标末端位姿xed来导引树的生长, 这2种生长分别称为随机导引生长(ExtendRandomly()函数)与目标末端位姿导引生长(ExtendTowardGoal())。每次迭代生成一个随机数p∈[0, 1], 若p < pg, 则进行目标末端位姿导引生长。

algorithm 1 GB-RRT for FFSR
1 tree.V.init(qI)

2   for i=1 to K do

3     if rand()<pg

4       isreach=extendtowardgoal()

5       if isreach=true

6         break

7     else

8       extendrandomly ()

9 return tree

2.1 局部规划器设计

2.2.1 随机构型导引生长局部规划器

随机构型导引生长选择离qsample最近的节点作为待扩展点qextended, 并从qextendedqsample进行局部规划, 以生成局部轨迹以及新的节点。

相应局部规划器的伪代码见Algorithm 2, 其含一个迭代过程, 每次迭代中, 计算当前构型qpresentqsample间的误差Δq=qsampleqpresent, 并计算qpresent处的Jx; 然后通过Jx的伪逆, 将Δq映射到动作空间, 得到

FFSR按照运动能够减小Δq。利用J左乘, 可得到与所对应的

从而得到构型速度

通过积分, 更新构型

qpresent进行碰撞检测, 以保证所生成的局部轨迹处于自由构型空间。若满足下述任何一个条件, 则停止迭代: ①检测出碰撞; ②构型超出限制, 即基座姿态扰动超出限制, 或者关节角度超出范围; ③构型参数之中最大扩展值达到一定的阈值。

algorithm 2 extendrandomly
1 qsample=randomconfig()

2 qextend=nearestnode(tree, qsample)

3 qpresent=qextend

4 lastcheckedq=qpresent

5 shallend=false

6 while all(shallend=false)

7  Jb=Jacobian(qpresent)

8  Jx=[Jb, eye(n, n)]

9  Δq=qsampleqpresent

10  JV=inv(Jx)*Δq

11  BV=Jb*JV

12  qpresent=qpresent+[BV; JV]*timestep

13  If max(qpresent-lastcheckedq)>=
collisioncheckthresh

14   iscollision=collisioncheck(qpresent)

15   lastcheckedq=qpresent

17   shallend=checktermination()

19 return

2.2.2 适时调节基座姿态的目标末端位姿导引生长局部规划器

目标末端位姿导引生长, 即选择离xed最近的点作为待扩展点qextended, 并从qextendedxed进行局部规划; 其要求尽可能消除相对于xed的误差, 同时保证Ψb不超出取值范围。本文提出适时调节基座姿态的目标末端位姿导引生长其局部规划器, 其伪代码如Algorithm 3所示。作为执行机构的FFSR操作臂关节仅有7个自由度, 而末端位姿任务为六自由度, 基座姿态为三自由度。相对于到达xed以及保持Ψb处于一定取值范围的要求, 执行机构自由度不足; 因此若在局部规划的每一次迭代中, 同时调节xeΨb, 则末端位姿误差无法收敛。

本文引入参考目标基座姿态Ψbr, 以及基座姿态误差阈值向量ΔΨbr。其中Ψbr根据Ψb取值区间[ΨbminΨbmax]设计, 且选在区间的中心; ΔΨbr小于[ΨbminΨbmax]区间长度的一半, 且为正值。在每次迭中, 计算当前基座姿态Ψbpresent与参考目标基座姿态Ψbr间的误差ΔΨb=ΨbrΨbpresent; 若|ΔΨb|的全部分量小于误差阈值, 则不调节Ψb, 而若|ΔΨb|有分量达到阈值, 但是又离Ψb取值边界还有一定距离时, 则启动Ψb调节。这样既能克服执行机构自由度不足的问题, 保证末端位姿误差收敛; 又能在Ψb还未到达界限时就对其进行调节, 保证其处于取值范围。

对于Algorithm 3的每次迭代, 首先进行以下计算: ①计算当前构型qpresent对应的J, Jg。②计算qpresent所对应的末端位姿xe=fe(qpresent)与xed间误差xe=xedxe。③计算ΔΨb。然后, 通过Jg的伪逆, 将xe映射到动作空间, 得到关节角速度

FFSR按照运动可以减小xe。判断|ΔΨb|是否有分量达到调节阈值, 若有则触发基座姿态调节, 即在Jg的零空间内, 通过J的伪逆产生减小ΔΨb的动作

否则。最后, 将融合产生动作

获得动作后, 后续其他步骤与2.2.1节中相应的步骤相同。本局部规划器的终止条件包含: ①检测出碰撞; ②构型超出限制, 即基座姿态扰动超出限制, 或者关节角度超出范围; ③到达目标。

algorithm3 extendtowardgoal
1 qextend=nearestnode(tree, Xgoal)
2 qpresent=qextend
3 lastcheckedq=qpresent
4 shallend=false
5 while all(shallend=false)
6  JB=Jacobian(qpresent)
7  JG=generalJacobian(qpresent)
8  ΔX=Xgoal-f(qpresent)
9  ΔqB=qBgoalqBpresent
10  JVforEE=pinv(JG)*ΔX
11  if ΔqB>Badjustthresh
12  JVforBA=pinv(JB*null(JG))*ΔqB
13  else
14   JVforBA=0
15  JV=JVforEE+null(JG)*JVforBA
16  BV=Jb*JV
17  qpresent=qpresent+[BV; JV]*timestep
18  if max(qpresent-lastcheckedq)>=Cchecktresh
19   iscollision=collisioncheck(qpresent)
20   lastcheckedq=qpresent
21  shallend=checktermination()
22 return

3 仿真

所选对象为带七自由度关节操作臂的FFSR, 其参数间表 4, D-H参数见表 5。设置3个矩形障碍, 质心位置、尺寸分别为: (1, 1, 2.8)m, 2 m×1 m×1 m; (4, 0, -1)m, 1 m×2 m×2 m; (6, 1.5, 2.5)m, 1 m×2 m×2 m。仿真分2种场景: ①从初始构型到目标末端位置; ②从初始构型到目标末端位姿。对①以及②, 初始构型: 0°; 系统质心位置: rg[0 0 0]m;关节角范围: ±300°; Ψbr=[0° 0° 0°]。其中, 对场景1, pe=[0 2 1]m; Ψb范围: ±20°; ΔΨbr=15°。对场景2:pe=[4 3 1]m, Ψe=[50° 60° 70°]; Ψb范围: ±30°; ΔΨbr=25°。

表1

FFSR的动力学以及几何参数

表2

FFSR的D-H参数

3.1 场景1的结果

36次迭代后, FFSR的末端到达目标位置, 规划结束。如图 2a)所示, 图中淡蓝色为初始构型, 红色为最终构型。从初始构型到目标末端位置的运动历程如图 2b)所示, 整个过程中未发生碰撞。关节角、基座姿态随时间的变化分别如图 3所示, 从图 3b)可看出, 基座姿态满足取值范围限制。

thumbnail 图2

场景1:初始构型与最终构型,从初始构型向目标末端位置的运动历程

thumbnail 图3

场景1:关节角及基座姿态变化曲线

3.2 场景2的结果

116次迭代后, 到达目标末端位姿, 规划成功; 图 4a)中, 红色为最终构型, 青色为初始构型。从初始构型向目标末端位姿的运动历程如图 4b)所示, 整个过程未发生碰撞。关节角、基座姿态随时间的变化如图 5所示, 可发现基座姿态处于取值范围内。

thumbnail 图4

场景2:初始构型与最终构型,从初始构型向目标末端位姿的运动历程

thumbnail 图5

场景2:关节角及基座姿态变化曲线

4 结论

本文基于GB-RRT设计了FFSR无逆运动学的运动规划算法, 并提出适时调节基座姿态的目标末端位姿导引局部规划器, 引入参考目标基座姿态、基座姿态误差阈值。在该局部规划器的每一次迭代中, 只有相对于参考目标基座姿态误差达到阈值, 才调节基座姿态, 否则只调节末端位姿。该局部规划器所生成的轨迹能满足基座姿态取值限制, 且具备让末端位姿误差收敛的能力, 克服了FFSR执行机构自由度不足的问题。

References

  1. Vafa Z, Dubowsky S. On the dynamics of manipulators in space using the virtual manipulator approach[C]//1987 IEEE International Conference on Robotics and Automation, 1987: 579–585 [Google Scholar]
  2. Vafa Z, Dubowsky S. On the dynamics of space manipulators using the virtual manipulator, with applications to path planning[M]. Springer, Boston, MA, 1993: 45–76 [Google Scholar]
  3. Nakamura Y, Mukherjee R. Non-holonomic path planning of space robots via bi-directional approach[C]//IEEE International Conference on Robotics and Automation, 1990: 1764–1769 [Google Scholar]
  4. Fernandes C, Gurvits L, Li Z. Near-optimal nonholonomic motion planning for a system of coupled rigid bodies[J]. IEEE Trans on Automatic Control, 1994, 39(3): 450–463 [Article] [Google Scholar]
  5. Xu W, Li C, Wang X, et al. Study on non-holonomic cartesian path planning of a free-floating space robotic system[J]. Advanced Robotics, 2009, 23(1/2): 113–143 [Google Scholar]
  6. Wang M, Luo J, Walter U. Trajectory planning of free-floating space robot using particle swarm optimization(PSO)[J]. Acta Astronautica, 2015, 112: 77–88 [Article] [Google Scholar]
  7. Wang M, Luo J, Fang J, et al. Optimal trajectory planning of free-floating space manipulator using differential evolution algorithm[J]. Advances in Space Research, 2018, 61(6): 1525–1536 [Article] [Google Scholar]
  8. Yamada K. Arm path planning for a space robot[C]//Proceedings of 1993 IEEE/RSJ International Conference on Intelligent Robots and Systems, 1993: 2049–2055 [Google Scholar]
  9. Suzuki T, Nakamura Y. Planning spiral motion of non-holonomic space robots[C]//Proceedings of IEEE International Conference on Robotics and Automation, 1996: 718–725 [Google Scholar]
  10. Misra G, Bai X. Task-constrained trajectory planning of free-floating space-robotic systems using convex optimization[J]. Journal of Guidance, Control, and Dynamics, 2017, 40(11): 2857–2870 [Article] [Google Scholar]
  11. Lavalle S M, Kuffner J R J J. Randomized kinodynamic planning[J]. The International Journal of Robotics Research, 2001, 20(5): 378–400 [Article] [Google Scholar]
  12. Bertram D, Kuffner J, Dillmann R, et al. An integrated approach to inverse kinematics and path planning for redundant manipulators[C]//2006 IEEE International Conference on Robotics and Automation, 2006: 1874–1879 [Google Scholar]
  13. Weghe M V, Ferguson D, Srinivasa S S. Randomized path planning for redundant manipulators without inverse kinematics[C]//7th IEEE-RAS International Conference on Humanoid Robots, 2007: 477–482 [Google Scholar]
  14. Xu Wenfu. Path planning and experiment study of space robot for target capturing[D]. Harbin: Harbin Institute of Technology, 2010 (in Chinese) [Google Scholar]

All Tables

表1

FFSR的动力学以及几何参数

表2

FFSR的D-H参数

All Figures

thumbnail 图1

自由漂浮空间机器人的运动学方程

In the text
thumbnail 图2

场景1:初始构型与最终构型,从初始构型向目标末端位置的运动历程

In the text
thumbnail 图3

场景1:关节角及基座姿态变化曲线

In the text
thumbnail 图4

场景2:初始构型与最终构型,从初始构型向目标末端位姿的运动历程

In the text
thumbnail 图5

场景2:关节角及基座姿态变化曲线

In the text

Current usage metrics show cumulative count of Article Views (full-text article views including HTML views, PDF and ePub downloads, according to the available data) and Abstracts Views on Vision4Press platform.

Data correspond to usage on the plateform after 2015. The current usage metrics is available 48-96 hours after online publication and is updated daily on week days.

Initial download of the metrics may take a while.