2. 航天飞行动力学技术国家级重点实验室, 陕西 西安 710072
FFSR具备节省燃料, 延长系统寿命等优点, 被广泛用于在轨服务。FFSR构型参数中, 关节角和基座姿态间存在一阶导关系, 需直接考虑微分约束进行运动规划。
FFSR运动规划方法中, 研究较多的包含3类。早期学者研究了系统的操控(steer)问题, 即在不考虑障碍物的情况下, 生成一系列的动作, 让FFSR从初始构型运动到目标构型。如Vafa等基于“虚拟机械臂(virtual manipulator, VM)”[1]的模型, 发展了自校正运动法[1]及“扰动图法(disturbance map, DM)”[2]。Nakamura和Mukherjee[3]基于李雅普诺夫函数设计了双向方法。Fernandes等[4]研究了耦合刚体的运动规划方法，此类方法着眼于利用关节运动操控基座姿态; 但未考虑碰撞, 不能形成完整的运动规划。第二类方法是基于多项式参数化的方法, 即以FFSR的初始关节角和最终关节角为端点, 利用带未知系数的多项式对关节轨迹进行参数化, 并以最终时刻基座姿态扰动为适应度函数, 将规划问题转换为优化问题; 利用遗传算法[5]、粒子群算法[6]、微分进化算法[7]等解优化问题, 可得最终轨迹。该类方法不易控制轨迹的形状, 因此很难考虑碰撞问题; 此外, 由于适应度函数计算需进行积分, 因此效率低下。第三类方法是通过直接法或间接法将运动规划问题转换为非线性优化问题[8-9]。Misra等[10]于2017年基于非线性优化研究了空间机器人考虑任务约束的运动规划问题, 并将问题构造为一个凸二次规划问题。该类方法求解效率高, 但也存在局部最优问题。
基于采样的运动规划方法, 能高效解决有高自由度构型空间的机器人运动规划问题。其中RRT(rapidly-exploring random tree)对有微分约束运动规划有很强的适用性[11]。一般情况下, 运动规划多采用“先求逆运动学, 再规划路径/轨迹”的框架; 但这一框架存在逆运动学所求目标构型与初始构型未必在同一连通域内的隐患。针对这一问题, Bertram、Weghe等[12-13]基于有目标偏置的RRT(RRT with Goal Bias, GB-RRT)给出了解决方案, 即不求逆运动学, 而是直接引入目标末端位姿导引生长。在该方案中, 用于目标末端位姿导引生长的局部规划含一个迭代过程, 每次迭代通过雅克比矩阵的转置, 将末端位姿误差映射到构型空间, 从而产生一系列越来越接近目标末端位姿的构型，但该方案针对固定基座机械臂的路径规划问题, 而FFSR的运动规划需直接考虑微分约束。此外, 固定基座机械臂执行机构自由度大于末端位姿自由度, 但FFSR末端末端位姿导引生长, 除要求具备最终收敛到目标末端位姿的能力外, 还需保证基座姿态扰动不能过大, 因此执行机构自由度不足。
本文以GB-RRT为框架设计规划算法, 解决FFSR从初始构型到目标末端位姿的运动规划问题。为适应FFSR直接考虑微分约束的需求, 在Weghe等[13]提出的方法基础上, 在每次迭代中引入积分机制; 为克服FFSR执行机构自由度不足的问题, 提出适时调节基座姿态的目标末端位姿导引生长局部规划器, 以调节末端位姿为主, 同时在必要时兼顾考虑基座姿态的调节。
1 自由漂浮空间机器人的运动学方程
|
| 图 1 自由漂浮空间机器人的运动学方程 |
定义FFSR的构型参数为
|
(1) |
FFSR的EE姿态角Ψe与IRe对应, 因此
|
(2) |
FFSR末端执行器位置pe为
|
(3) |
FFSR末端执行器角速度ωe为
|
(4) |
末端执行器线速度ve度为
|
(5) |
根据(4)~(5)式, 可得FFSR的末端执行器速度ẋe为
|
(6) |
Jb, Jm的表达式见文献[14]。
1.2 动量守恒方程FFSR的动量方程为
|
(7) |
式中，P与L分别为FFSR的线动量、角动量, 相关矩阵的定义间文献[14]。令
|
FFSR线动量、角动量守恒, 假设系统的初始动量为0, 则可将基座速度ẋb表示为
|
(8) |
取上述方程后3行, 可得基座角速度ωb为
|
(9) |
令bωb为ωb在基座固连坐标系中的表示, bωb基座姿态角一阶导
|
(10) |
式中
|
考虑(9)式及(10)式, 则
|
(11) |
式中，JbΨ为与基座姿态角相关的雅克比矩阵。
1.3.2 状态转移方程考虑(11)式, 则FFSR的状态转移方程为
|
(12) |
式中，Jx为与状态转移相关的雅克比矩阵。
1.3.3 广义雅克比矩阵考虑(6)式以及(8)式, 可得
|
(13) |
Jg为广义雅克比矩阵。
1.3.4 考虑线动量守恒积分形式后的方程FFSR的线动量可积分为位置形式
|
(14) |
考虑到
|
式中, 
|
(15) |
进而
|
(16) |
给出构型参数q, 可计算在惯性空间之中的表示Ibi, Iai, 进而可计算r0, 即q为构型参数q的函数。再考虑(3)式, 可知pe为q的函数
|
(17) |
综合考虑(1)式以及(17)式, 可得端位姿xe与q间的映射关系
|
(18) |
算法伪代码如Algorithm 1所示, 在每次迭代中, 利用随机采样构型qSample或目标末端位姿xed来导引树的生长, 这2种生长分别称为随机导引生长(Extend-Randomly()函数)与目标末端位姿导引生长(Extend-Toward-Goal())。每次迭代生成一个随机数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 | is-reach=extend-toward-goal() |
| 5 | if is-reach=true |
| 6 | break |
| 7 | else |
| 8 | extend-randomly () |
| 9 | return tree |
随机构型导引生长选择离qsample最近的节点作为待扩展点qextended, 并从qextended向qsample进行局部规划, 以生成局部轨迹以及新的节点。
| algorithm 2 extend-randomly | |
| 1 | qsample=random-config() |
| 2 | qextend=nearest-node(tree, qsample) |
| 3 | qpresent=qextend |
| 4 | last-checked-q=qpresent |
| 5 | shall-end=false |
| 6 | while all(shall-end=false) |
| 7 | Jb=Jacobian(qpresent) |
| 8 | Jx=[Jb, eye(n, n)] |
| 9 | Δq=qsample-qpresent |
| 10 | JV=inv(Jx)*Δq |
| 11 | BV=Jb*JV |
| 12 | qpresent=qpresent+[BV; JV]*time-step |
| 13 | If max(qpresent-last-checked-q)>= collision-check-thresh |
| 14 | is-collision=collision-check(qpresent) |
| 15 | last-checked-q=qpresent |
| 17 | shall-end=check-termination() |
| 19 | return |
相应局部规划器的伪代码见Algorithm 2, 其含一个迭代过程, 每次迭代中, 计算当前构型qpresent和qsample间的误差Δq=qsample-qpresent, 并计算qpresent处的Jx; 然后通过Jx的伪逆, 将Δq映射到动作空间, 得到
|
(19) |
FFSR按照
|
(20) |
从而得到构型速度
|
(21) |
通过积分, 更新构型
|
(22) |
对qpresent进行碰撞检测, 以保证所生成的局部轨迹处于自由构型空间。若满足下述任何一个条件, 则停止迭代: ①检测出碰撞; ②构型超出限制, 即基座姿态扰动超出限制, 或者关节角度超出范围; ③构型参数之中最大扩展值达到一定的阈值。
2.2.2 适时调节基座姿态的目标末端位姿导引生长局部规划器| algorithm3 extend-toward-goal | |
| 1 | qextend=nearest-node(tree, Xgoal) |
| 2 | qpresent=qextend |
| 3 | last-checked-q=qpresent |
| 4 | shall-end=false |
| 5 | while all(shall-end=false) |
| 6 | JB=Jacobian(qpresent) |
| 7 | JG=general-Jacobian(qpresent) |
| 8 | ΔX=Xgoal-f(qpresent) |
| 9 | ΔqB=qBgoal-qBpresent |
| 10 | JV-for-EE=pinv(JG)*ΔX |
| 11 | if ΔqB>B-adjust-thresh |
| 12 | JV-for-BA=pinv(JB*null(JG))*ΔqB |
| 13 | else |
| 14 | JV-for-BA=0 |
| 15 | JV=JV-for-EE+null(JG)*JV-for-BA |
| 16 | BV=Jb*JV |
| 17 | qpresent=qpresent+[BV; JV]*time-step |
| 18 | if max(qpresent-last-checked-q)>=C-check-tresh |
| 19 | is-collision=collision-check(qpresent) |
| 20 | last-checked-q=qpresent |
| 21 | shall-end=check-termination() |
| 22 | return |
目标末端位姿导引生长, 即选择离xed最近的点作为待扩展点qextended, 并从qextended向xed进行局部规划; 其要求尽可能消除相对于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对应的JbΨ, Jg。②计算qpresent所对应的末端位姿xe=fe(qpresent)与xed间误差xe=xed-xe。③计算ΔΨb。然后, 通过Jg的伪逆, 将xe映射到动作空间, 得到关节角速度
|
(23) |
FFSR按照
|
(24) |
否则
|
(25) |
获得动作后, 后续其他步骤与2.2.1节中相应的步骤相同。本局部规划器的终止条件包含: ①检测出碰撞; ②构型超出限制, 即基座姿态扰动超出限制, 或者关节角度超出范围; ③到达目标。
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°。
| 刚体 序号 |
质量/ kg |
长/m*宽/m*高/m | 转动惯量 | ||
| Ixx | Iyy | Izz | |||
| 0 | 900 | 1×1×1 | 600 | 1 200 | 800 |
| 1 | 20 | 0.35×0.16×0.16 | 20 | 20 | 30 |
| 2 | 20 | 0.35×0.16×0.16 | 20 | 20 | 30 |
| 3 | 40 | 4×0.16×0.16 | 1 | 40 | 40 |
| 4 | 40 | 4×0.16×0.16 | 1 | 40 | 40 |
| 5 | 20 | 0.35×0.16×0.16 | 20 | 20 | 30 |
| 6 | 20 | 0.35×0.16×0.16 | 20 | 20 | 30 |
| 7 | 40 | 1.2×0.16×0.16 | 10 | 8 | 4 |
| i | αi-1/m | αi-1/(°) | di/m | θi/(°) |
| 1 | 0 | 0 | 2.5 | 600 |
| 2 | 0 | 90 | 0.35 | 20 |
| 3 | 0 | 90 | 0.35 | 20 |
| 4 | 4 | 0 | 0 | 40 |
| 5 | 4 | 0 | 0.35 | 40 |
| 6 | 0 | 90 | 0.35 | 20 |
| 7 | 0 | 90 | 1.2 | 20 |
36次迭代后, FFSR的末端到达目标位置, 规划结束。如图 2a)所示, 图中淡蓝色为初始构型, 红色为最终构型。从初始构型到目标末端位置的运动历程如图 2b)所示, 整个过程中未发生碰撞。关节角、基座姿态随时间的变化分别如图 3所示, 从图 3b)可看出, 基座姿态满足取值范围限制。
|
| 图 2 场景1：初始构型与最终构型，从初始构型向目标末端位置的运动历程 |
|
| 图 3 场景1：关节角及基座姿态变化曲线 |
116次迭代后, 到达目标末端位姿, 规划成功; 图 4a)中, 红色为最终构型, 青色为初始构型。从初始构型向目标末端位姿的运动历程如图 4b)所示, 整个过程未发生碰撞。关节角、基座姿态随时间的变化如图 5所示, 可发现基座姿态处于取值范围内。
|
| 图 4 场景2：初始构型与最终构型，从初始构型向目标末端位姿的运动历程 |
|
| 图 5 场景2：关节角及基座姿态变化曲线 |
本文基于GB-RRT设计了FFSR无逆运动学的运动规划算法, 并提出适时调节基座姿态的目标末端位姿导引局部规划器, 引入参考目标基座姿态、基座姿态误差阈值。在该局部规划器的每一次迭代中, 只有相对于参考目标基座姿态误差达到阈值, 才调节基座姿态, 否则只调节末端位姿。该局部规划器所生成的轨迹能满足基座姿态取值限制, 且具备让末端位姿误差收敛的能力, 克服了FFSR执行机构自由度不足的问题。
| [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 |
| [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. |
| [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 |
| [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. DOI:10.1109/9.280745 |
| [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. |
| [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. DOI:10.1016/j.actaastro.2015.03.008 |
| [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. DOI:10.1016/j.asr.2018.01.011 |
| [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 |
| [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 |
| [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. DOI:10.2514/1.G002405 |
| [11] | LAVALLE S M, KUFFNER J R J J. Randomized kinodynamic planning[J]. The International Journal of Robotics Research, 2001, 20(5): 378-400. DOI:10.1177/02783640122067453 |
| [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 |
| [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 |
| [14] |
徐文福. 空间机器人目标捕获的路径规划与实验研究[D]. 哈尔滨: 哈尔滨工业大学, 2010 XU Wenfu. Path planning and experiment study of space robot for target capturing[D]. Harbin: Harbin Institute of Technology, 2010(in Chinese) |
2. National Key Laboratory of Aerospace Flight Dynamics, Xi'an 710072, China
