CN110104216A - 一种针对运动学冗余双臂空间机器人的协同路径规划方法 - Google Patents

Abstract

本发明公开了一种针对运动学冗余双臂空间机器人的协同路径规划方法，包括以下步骤：建立空间机器人系统的动力学方程和运动学方程；求解末端执行器逆运动学方程冗余解，通过动量守恒方程得到系统非完整约束方程；通过系统非完整约束方程，得到末端执行器运动和基座姿态关系的任务空间约束方程；使用五阶贝塞尔曲线得到任务空间中末端执行器路径规划，通过末端执行器的速度和加速度边界确定路径执行时间；通过末端执行器运动方程和任务空间约束方程，得到对应不同任务优先级的关节运动轨迹规划。实现了空间双臂机器人的协同路径规划方法，可以根据任务的优先级执行各类不同的任务，多臂协同任务，动态平衡任务等，大大拓展了空间机械臂的操作能力。

Description

一种针对运动学冗余双臂空间机器人的协同路径规划方法

技术领域

本发明属于双臂空间机器人的路径规划技术领域，涉及一种针对运动学冗余双臂空间机器人的协同路径规划方法。

背景技术

空间机器人由自由漂浮基座和机械臂组成。当执行在轨任务时，由于基座与机械臂的动力学耦合特性，机械臂和安装在航天器上的转动机制的操作将引起基座的姿态扰动。有限的燃料主要用于轨道转移机动，基本没有燃料用于抑制基座姿态扰动。因此，如何充分探索动力学耦合效应在调整基座姿态的应用已经引起广泛的关注。

目前针对自由漂浮空间机器人姿态扰动最小化，已经提出了许多方法和策略，很多的研究主要是针对单臂，但是当轨道目标没有抓钩时，单臂机器人的拦截和抓捕会非常困难。多臂机器人可以增加抓捕的可能性和提供灵巧操作，针对多臂机器人姿态扰动最小化的问题，目前有以下解决方法：1)针对双柔性机械臂使用混合位置/力控制和振动抑止控制来抓捕空间旋转目标。2)采用等效平衡臂和相应的动力学平衡控制(DBC)方案来减少由任务臂引起的基座姿态扰动。3)整合以任务优先级为基础的方法框架中的DBC和RNS的概念来最小化飞行器的基座姿态扰动。

但是，上述现有方法中，额外臂主要用于基座姿态的控制，多臂空间机器人抓捕目标的协同运动的问题尚未被合理解决。因此，需要提出多臂空间机器人的协同路径规划。

发明内容

本发明的目的在于克服上述现有技术的缺点，提供一种针对运动学冗余双臂空间机器人的协同路径规划方法。

为达到上述目的，本发明采用以下技术方案予以实现：

一种针对运动学冗余双臂空间机器人的协同路径规划方法，包括以下步骤：

步骤1：建立空间机器人系统的动力学方程和运动学方程，得到末端执行器运动学方程；

步骤2：通过动量守恒方程得到系统非完整约束方程，通过末端执行器的运动学方程求解末端执行器逆运动学冗余解，将末端执行器逆运动学冗余解代入系统非完整约束方程中，得到末端执行器运动和基座姿态关系的任务空间约束方程；

步骤3：根据末端执行器的初始与终端位置、速度和加速度，使用五阶贝塞尔曲线得到任务空间中末端执行器路径规划，通过末端执行器的速度和加速度边界确定路径执行时间；

步骤4：通过末端执行器运动学方程和任务空间约束方程，得到对应不同任务优先级的关节运动轨迹规划。

本发明进一步的改进在于：

方法还包括：

步骤5：跟踪关节运动轨迹规划路径，使用闭环控制系统消除跟踪偏差；闭环控制系统中关节力矩输入采用PD控制。

步骤1的具体方法为：

基于拉格朗日原理建立空间机器人系统的动力学方程：

其中，是基座的线加速度和角加速度，表示机械臂a的角加速度，表示机械臂b的角加速度，上标的a和b分别表示机械臂a和机械臂b；H_b为基座的惯量矩阵，和分别为两机械臂的与基座的耦合惯量矩阵，和分别为两机械臂的惯量矩阵，c_b为基座的非线性项，和分别为两机械臂的非线性项；f_b为施加于基座的广义力，和分别为施加于各关节的力矩；和分别为两机械臂对基座的Jacobian矩阵的转置矩阵；和分别为两机械臂的Jacobian矩阵的转置矩阵；和分别为施加于两机械臂末端的广义力；

根据动量守恒定律，整个空间机器人系统的线动量和角动量的守恒方程：

其中，M₀为空间机械臂系统的总动量；P₀和L₀为空间机械臂系统的线动量和角动量；m_b和m_i为基座和各连杆的质量；和为基座和各连杆质心的速度；I_b为基座的惯量矩阵，ω_b和ω_i为基座和各连杆的旋转角速度，为基座的线速度与角速度，为机械臂b的关节角速，为机械臂a的关节角速度，r_b为基座的位置向量，I_i为机械臂连杆的惯量矩阵，r_Ci为机械臂连杆的位置向量；

由于H_b可逆；得到基座运动方程：

其中，J_a为基座的广义Jacobian矩阵，为机械臂的关节角速度向量；

将基座运动方程代入末端执行器a的运动学链式方程和末端执行器b的运动学链式方程中，得到末端执行器运动学方程：

其中，J_d是描述系统初始动量对机械臂运动影响的耦合矩阵，J_g是广义雅克比矩阵，为机械臂a的末端执行器速度，为机械臂a对基座影响的Jacobian矩阵，为机械臂b的末端执行器速度，为机械臂b对基座影响的Jacobian矩阵，为机械臂a的末端执行器Jacobian矩阵，为机械臂b的末端执行器Jacobian矩阵，为机械臂a的广义Jacobian矩阵，为机械臂b的广义Jacobian矩阵。

步骤2的具体方法为：

通过动量守恒方程得到空间机器人系统满足的非完整约束方程：

其中，I_s为整个机器人系统的惯性矩阵，和为基座和机械臂之间的耦合惯性矩阵；非完整约束方程为关节速度约束方程；

根据末端执行器的运动学方程得到末端执行器的逆运动学冗余解，末端执行器的逆运动学冗余解由伪逆特解和齐次方程的一般解组成：

其中，是J_g的Moore-Penrose伪逆，E_n为单位矩阵，h是任意矢量；

将末端执行器的逆运动学冗余解代入关节速度约束方程，得到末端执行器运动和基座姿态关系的任务空间约束方程：

步骤3的具体方法为：

采用五阶贝塞尔曲线描述末端执行器在任务空间的路径，末端执行器在任务空间的路径方程：

其中，b_j,m(τ)是阶数为m的伯恩斯坦多项式，是二项式系数，P_ij是给定的控制点用于构建贝塞尔曲线；

第i维机械臂路径方程：

对于末端执行器任务，将初始和终端加速度设置为零，设置终端速度与抓捕点速度一致；将初始时刻的位置和速度以及终端时刻的位置和速度代入第i维机械臂路径方程，得到：

其中，路径速度和加速度边界通过执行时间来确定：

其中，x_max和x_min分别为速度和加速度的上下边界约束。

步骤4的具体方法为：

自由漂浮空间机械臂执行目标抓捕和基座姿态调整任务描述如下：

当末端执行器任务和调整基座姿态任务同时考虑时，末端执行器的逆运动学冗余解通过假定机械臂任务在低优先级得到，末端执行器的逆运动学冗余解：

其中，代表J_g到I_bm零空间的映射；

当机械臂任务具有优先级，末端执行器的逆运动学冗余解：

其中，代表I_bm到J_g零空间的映射；根据末端执行器路径规划和基座运动方程，得到执行目标抓捕和基座姿态调整任务的协同路径。

步骤5中：

PD控制中的PD控制率方程为：

其中，K_p＝diag{k_pE_n}，K_d＝diag{k_dE_n}是比例和微分增益的对角阵，θ^kd和θ^k分别为期望的关节角位置以及当前的关节角位置。

与现有技术相比，本发明具有以下有益效果：

通过设计末端执行器的运动路径，采用时间归一化的5阶贝塞尔曲线对机械臂末端的位姿分别进行描述，建立空间多臂机器人末端执行器的运动路径，所设计的末端路径与时间无关，可以将末端的运动约束引入路径的设计过程中，得到最优的末端路径执行时间。基于任务优先级设计了机械臂各关节的运动轨迹，实现了空间双臂机器人的协同路径规划方法，可以根据任务的优先级执行各类不同的任务，如空间机器人基座调整任务，多臂协同任务，动态平衡任务等，在完成末端执行器的抓捕任务的同时保证基座的姿态扰动最小，大大拓展了空间机械臂的操作能力。

附图说明

图1为本发明的自由漂浮空间机器人系统示意图；

图2为本发明的闭环控制构架示意图；

图3为本发明的实例1考虑协同轨迹规划策略时的左端执行器路径仿真结果示意图；

图4为本发明的实例1考虑协同轨迹规划策略时的右端执行器路径仿真结果示意图；

图5为本发明的实例1考虑协同轨迹规划策略时的左端执行器速度仿真结果示意图；

图6为本发明的实例1考虑协同轨迹规划策略时的右端执行器速度仿真结果示意图；

图7为本发明的实例1不考虑协同轨迹规划策略时的左臂关节运动仿真结果示意图；

图8为本发明的实例1不考虑协同轨迹规划策略时的右臂关节运动仿真结果示意图；

图9为本发明的实例1不考虑协同轨迹规划策略时的左末端执行器跟踪误差仿真结果示意图；

图10为本发明的实例1不考虑协同轨迹规划策略时的右末端执行器跟踪误差仿真结果示意图；

图11为本发明的实例1不考虑协同轨迹规划策略时的基座姿态干扰仿真结果示意图；

图12为本发明的实例1不考虑协同轨迹规划策略时的基座位置干扰仿真结果示意图；

图13为本发明的实例2左臂关节运动仿真结果示意图；

图14为本发明的实例2右臂关节运动仿真结果示意图；

图15为本发明的实例2左末端执行器跟踪误差仿真结果示意图；

图16为本发明的实例2右末端执行器跟踪误差仿真结果示意图；

图17为本发明的实例2基座姿态干扰仿真结果示意图；

图18为本发明的实例2基座位置干扰仿真结果示意图；

图19为本发明的实例3左臂关节运动仿真结果示意图；

图20为本发明的实例3右臂关节运动仿真结果示意图；

图21为本发明的实例3左末端执行器跟踪误差仿真结果示意图；

图22为本发明的实例3右末端执行器跟踪误差仿真结果示意图；

图23为本发明的实例3基座姿态干扰仿真结果示意图；

图24为本发明的实例3基座位置干扰仿真结果示意图。

其中：1-基座质心；2-系统质心；3-目标。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

下面结合附图对本发明做进一步详细描述：

参见图1和2，本发明一种针对运动学冗余双臂空间机器人的协同路径规划方法，包括以下步骤：

步骤1：建立空间机器人系统的动力学方程和运动学方程，得到末端执行器运动学方程；

步骤2：通过动量守恒方程得到系统非完整约束方程，通过末端执行器的运动学方程求解末端执行器逆运动学冗余解，将末端执行器逆运动学冗余解代入系统非完整约束方程中，得到末端执行器运动和基座姿态关系的任务空间约束方程；

步骤3：根据末端执行器的初始与终端位置、速度和加速度，使用五阶贝塞尔曲线得到任务空间中末端执行器路径规划，通过末端执行器的速度和加速度边界确定路径执行时间；

步骤4：通过末端执行器运动学方程和任务空间约束方程，得到对应不同任务优先级的关节运动轨迹规划；

步骤5：跟踪关节运动轨迹规划路径，使用闭环控制系统消除跟踪偏差；闭环控制系统中关节力矩输入采用PD控制。

具体的方法如下：

步骤1：建立系统的运动学方程和动力学方程；

基于拉格朗日原理建立的空间机器人系统的动力学方程如下：

其中，是基座的线加速度和角加速度，和代表机械臂a和b的角加速度，上标带有a和b的对应代表机械臂a和b；H_b为基座的惯量矩阵，和分别为两机械臂的与基座的耦合惯量矩阵，和分别为两机械臂的惯量矩阵，c_b为基座的非线性项，和分别为两机械臂的非线性项；f_b为施加于基座的广义力，和分别为施加于各关节的力矩；和分别为两机械臂对基座的Jacobian矩阵的转置矩阵；和分别为两机械臂的Jacobian矩阵的转置矩阵；和分别为施加于两机械臂末端的广义力。

对于自由漂浮空间机器人系统，不考虑末端执行器和基座上的外力和力矩。机械臂的运动通过关节内力矩产生。根据动量守恒定律，整个机器人系统的线动量和角动量守恒，描述如下：

其中，M₀为空间机械臂系统的总动量；P₀和L₀为空间机械臂系统的线动量和和角动量；m_b和m_i为基座和各连杆的质量；和为基座和各连杆质心的速度；I_b为基座的惯量矩阵，ω_b和ω_i为基座和各连杆的旋转角速度。

由于H_b可逆，基座的运动可以描述如下：

将上述方程代入末端执行器a的运动学链式方程和末端执行器b的运动学链式方程中，可以得到末端执行器的运动方程：

其中：J_d是描述系统初始动量对末端执行器运动影响的耦合矩阵，J_g是广义雅克比矩阵(GJM)。

步骤2：系统任务空间约束；

由动量方程可以得到系统满足的非完整约束为：

其中：I_s为整个机器人系统的惯性矩阵，和为基座和机械臂之间的耦合惯性矩阵。上式是关节速度的函数，为关节速度约束。

从末端执行器的运动学方程可以得到末端执行器的逆运动学冗余解由伪逆特解和齐次方程的一般解组成：

其中：是J_g的Moore-Penrose伪逆，E_n为单位矩阵，h是任意适量。将上式代入关节速度约束方程中可以得到任务空间约束为：

上式表示了末端执行器运动和基座姿态之间的关系。可以通过规划合适的末端执行器路径来调整基座姿态，实现空间机器人和基座的协同任务。

步骤3：末端执行器路径规划，确定路径执行时间；

采用五阶Bézier曲线(贝塞尔曲线)来描述末端执行器路径：

其中：b_j,m(τ)是阶数为m的伯恩斯坦多项式，是二项式系数，P_ij是给定的控制点用于构建Bézier曲线。

第i维末端执行器路径为：

对于末端执行器任务，初始和终端时刻的位置(和)速度(和)可以事先确定。初始和终端加速度设置为零以最小化关节突变。为了避免高碰撞力，终端速度与抓捕点速度一致。将这些等式约束代入到上式可以得到：

由于已知，Bézier曲线的形状可以确定，路径速度和加速度边界可以通过执行时间的确定来满足：

其中：一旦执行时间确定，末端执行器在任务空间的路径使用Bézier曲线可以完全地生成。

步骤4：关节运动轨迹规划；

自由漂浮空间机械臂的两种任务可以描述如下：

当末端执行器任务和调整基座姿态同时考虑时，机械臂关节运动的解(末端执行器的逆运动学冗余解)可以通过假定末端执行器任务在低优先级得到：

其中：代表J_g到I_bm零空间的映射。

如果末端执行器任务具有优先级，机械臂关节运动的解(末端执行器的逆运动学冗余解)是：

其中：代表I_bm到J_g零空间的映射。使用之前设计的末端执行器路径和基座运动律，可以得到执行目标抓捕和基座姿态调整任务的协同路径。

步骤5：消除跟踪偏差的控制；

采用闭环控制结构消除跟踪偏差。其中，关节力矩输入采用PD控制，控制率用于动力学仿真，PD控制率方程为：

其中：K_p＝diag{k_pE_n}，K_d＝diag{k_dE_n}是比例和微分增益的对角阵，θ^kd和θ^k分别为期望的关节角位置以及当前的关节角位置。

下面将本发明的方法应用到具体的实例中。

末端执行器速度和加速度边界条件为角速度和角加速度为k_p＝120,k_d＝32。

实施例1

只有双臂运动，末端执行器a,b从初始位置到达抓捕点；其中，初始位置为：

终端位置为：

仿真结果参见图3-6，通过速度和加速度边界，可以确定执行时间为T＝14s。通过仿真结果可以看出，设计的末端执行器路径符合提出的约束，可以完成提出的抓捕任务。当不考虑协同轨迹规划策略时，参见图7-12，可以看出双臂都可以执行跟踪任务，不考虑零空间运动。基座姿态和位置受到影响。最大的姿态和位置扰动到0.34°和0.63m。

实施例2

驱动双臂末端执行器到达实例1中目标抓捕点位置，同时最小化基座姿态扰动。

选取ω_b＝0和作为依赖速度。参见图13-18，展示了零反应机械臂的轨迹跟踪结果。可以注意到基座的姿态扰动为O(10^-4)，保证了双臂空间机器人的无反应机械臂。抑制基座姿态扰动所导致的末端执行器方向偏差小于0.05°。仿真结果表明了协同路径规划策略的有效性。

实施例3

驱动双臂末端执行器到达实例1中目标抓捕点位置，同时调整基座姿态位置。在这一仿真中要求基座姿态从初始的到达终端值度，基座位置自由。仿真结果参见图19-24，可以看出末端执行器可以以要求的速度到达在轨目标的抓捕点。末端执行器的方向追踪偏差小于0.3°。仿真结果表明采用双臂路径协同来调整基座姿态的可行性和有效性。通过上述三个例子可以看出，采用本发明提出的协同路径规划方法，可以完成末端执行器的抓捕任务，同时保证基座的姿态扰动最小或者调整基座姿态。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (7)

1.一种针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，包括以下步骤：

步骤1：建立空间机器人系统的动力学方程和运动学方程，得到末端执行器运动学方程；

步骤2：通过动量守恒方程得到系统非完整约束方程，通过末端执行器的运动学方程求解末端执行器逆运动学冗余解，将末端执行器逆运动学冗余解代入系统非完整约束方程中，得到末端执行器运动和基座姿态关系的任务空间约束方程；

步骤3：根据末端执行器的初始与终端位置、速度和加速度，使用五阶贝塞尔曲线得到任务空间中末端执行器路径规划，通过末端执行器的速度和加速度边界确定路径执行时间；

步骤4：通过末端执行器运动学方程和任务空间约束方程，得到对应不同任务优先级的关节运动轨迹规划。

2.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述方法还包括：

步骤5：跟踪关节运动轨迹规划路径，使用闭环控制系统消除跟踪偏差；闭环控制系统中关节力矩输入采用PD控制。

3.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤1的具体方法为：

基于拉格朗日原理建立空间机器人系统的动力学方程：

其中，是基座的线加速度和角加速度，表示机械臂a的角加速度，表示机械臂b的角加速度，上标的a和b分别表示机械臂a和机械臂b；H_b为基座的惯量矩阵，和分别为两机械臂的与基座的耦合惯量矩阵，和分别为两机械臂的惯量矩阵，c_b为基座的非线性项，和分别为两机械臂的非线性项；f_b为施加于基座的广义力，和分别为施加于各关节的力矩；和分别为两机械臂对基座的Jacobian矩阵的转置矩阵；和分别为两机械臂的Jacobian矩阵的转置矩阵；和分别为施加于两机械臂末端的广义力；

根据动量守恒定律，整个空间机器人系统的线动量和角动量的守恒方程：

其中，M₀为空间机械臂系统的总动量；P₀和L₀为空间机械臂系统的线动量和角动量；m_b和m_i为基座和各连杆的质量；和为基座和各连杆质心的速度；I_b为基座的惯量矩阵，ω_b和ω_i为基座和各连杆的旋转角速度，为基座的线速度与角速度，为机械臂b的关节角速，为机械臂a的关节角速度，r_b为基座的位置向量，I_i为机械臂连杆的惯量矩阵，r_Ci为机械臂连杆的位置向量；

由于H_b可逆；得到基座运动方程：

其中，J_a为基座的广义Jacobian矩阵，为机械臂的关节角速度向量；

将基座运动方程代入末端执行器a的运动学链式方程和末端执行器b的运动学链式方程中，得到末端执行器运动学方程：

其中，J_d是描述系统初始动量对机械臂运动影响的耦合矩阵，J_g是广义雅克比矩阵，为机械臂a的末端执行器速度，为机械臂a对基座影响的Jacobian矩阵，为机械臂b的末端执行器速度，为机械臂b对基座影响的Jacobian矩阵，为机械臂a的末端执行器Jacobian矩阵，为机械臂b的末端执行器Jacobian矩阵，为机械臂a的广义Jacobian矩阵，为机械臂b的广义Jacobian矩阵。

4.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤2的具体方法为：

通过动量守恒方程得到空间机器人系统满足的非完整约束方程：

其中，I_s为整个机器人系统的惯性矩阵，和为基座和机械臂之间的耦合惯性矩阵；非完整约束方程为关节速度约束方程；

根据末端执行器的运动学方程得到末端执行器的逆运动学冗余解，末端执行器的逆运动学冗余解由伪逆特解和齐次方程的一般解组成：

其中，是J_g的Moore-Penrose伪逆，E_n为单位矩阵，h是任意矢量；

将末端执行器的逆运动学冗余解代入关节速度约束方程，得到末端执行器运动和基座姿态关系的任务空间约束方程：

5.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤3的具体方法为：

采用五阶贝塞尔曲线描述末端执行器在任务空间的路径，末端执行器在任务空间的路径方程：

其中，b_j,m(τ)是阶数为m的伯恩斯坦多项式，是二项式系数，P_ij是给定的控制点用于构建贝塞尔曲线；

第i维机械臂路径方程：

对于末端执行器任务，将初始和终端加速度设置为零，设置终端速度与抓捕点速度一致；将初始时刻的位置和速度以及终端时刻的位置和速度代入第i维机械臂路径方程，得到：

其中，路径速度和加速度边界通过执行时间来确定：

其中，x_max和x_min分别为速度和加速度的上下边界约束。

6.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤4的具体方法为：

自由漂浮空间机械臂执行目标抓捕和基座姿态调整任务描述如下：

当末端执行器任务和调整基座姿态任务同时考虑时，末端执行器的逆运动学冗余解通过假定机械臂任务在低优先级得到，末端执行器的逆运动学冗余解：

其中，代表J_g到I_bm零空间的映射；

当机械臂任务具有优先级，末端执行器的逆运动学冗余解：

其中，代表I_bm到J_g零空间的映射；根据末端执行器路径规划和基座运动方程，得到执行目标抓捕和基座姿态调整任务的协同路径。

7.根据权利要求2所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤5中：

PD控制中的PD控制率方程为：

其中，K_p＝diag{k_pE_n}，K_d＝diag{k_dE_n}是比例和微分增益的对角阵，θ^kd和θ^k分别为期望的关节角位置以及当前的关节角位置。