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

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

Info

Publication number
CN110104216B
CN110104216B CN201910081351.2A CN201910081351A CN110104216B CN 110104216 B CN110104216 B CN 110104216B CN 201910081351 A CN201910081351 A CN 201910081351A CN 110104216 B CN110104216 B CN 110104216B
Authority
CN
China
Prior art keywords
end effector
equation
base
arm
space
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910081351.2A
Other languages
English (en)
Other versions
CN110104216A (zh
Inventor
王明明
罗建军
袁建平
朱战霞
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Shenzhen Institute of Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Shenzhen Institute of Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University, Shenzhen Institute of Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN201910081351.2A priority Critical patent/CN110104216B/zh
Publication of CN110104216A publication Critical patent/CN110104216A/zh
Application granted granted Critical
Publication of CN110104216B publication Critical patent/CN110104216B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J11/00—Manipulators not otherwise provided for
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B25J9/1682—Dual arm manipulator; Coordination of several manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
    • B64G1/00—Cosmonautic vehicles
    • B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
    • B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
    • B64G1/242—Orbits and trajectories
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B64—AIRCRAFT; AVIATION; COSMONAUTICS
    • B64G—COSMONAUTICS; VEHICLES OR EQUIPMENT THEREFOR
    • B64G1/00—Cosmonautic vehicles
    • B64G1/22—Parts of, or equipment specially adapted for fitting in or to, cosmonautic vehicles
    • B64G1/24—Guiding or controlling apparatus, e.g. for attitude control
    • B64G1/244—Spacecraft control systems
    • B64G1/245—Attitude control algorithms for spacecraft attitude control

Abstract

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

Description

一种针对运动学冗余双臂空间机器人的协同路径规划方法
技术领域
本发明属于双臂空间机器人的路径规划技术领域，涉及一种针对运动学冗余双臂空间机器人的协同路径规划方法。
背景技术
空间机器人由自由漂浮基座和机械臂组成。当执行在轨任务时，由于基座与机械臂的动力学耦合特性，机械臂和安装在航天器上的转动机制的操作将引起基座的姿态扰动。有限的燃料主要用于轨道转移机动，基本没有燃料用于抑制基座姿态扰动。因此，如何充分探索动力学耦合效应在调整基座姿态的应用已经引起广泛的关注。
目前针对自由漂浮空间机器人姿态扰动最小化，已经提出了许多方法和策略，很多的研究主要是针对单臂，但是当轨道目标没有抓钩时，单臂机器人的拦截和抓捕会非常困难。多臂机器人可以增加抓捕的可能性和提供灵巧操作，针对多臂机器人姿态扰动最小化的问题，目前有以下解决方法：1)针对双柔性机械臂使用混合位置/力控制和振动抑止控制来抓捕空间旋转目标。2)采用等效平衡臂和相应的动力学平衡控制(DBC)方案来减少由任务臂引起的基座姿态扰动。3)整合以任务优先级为基础的方法框架中的DBC和RNS的概念来最小化飞行器的基座姿态扰动。
但是，上述现有方法中，额外臂主要用于基座姿态的控制，多臂空间机器人抓捕目标的协同运动的问题尚未被合理解决。因此，需要提出多臂空间机器人的协同路径规划。
发明内容
本发明的目的在于克服上述现有技术的缺点，提供一种针对运动学冗余双臂空间机器人的协同路径规划方法。
为达到上述目的，本发明采用以下技术方案予以实现：
一种针对运动学冗余双臂空间机器人的协同路径规划方法，包括以下步骤：
步骤1：建立空间机器人系统的动力学方程和运动学方程，得到末端执行器运动学方程；
步骤2：通过动量守恒方程得到系统非完整约束方程，通过末端执行器的运动学方程求解末端执行器逆运动学冗余解，将末端执行器逆运动学冗余解代入系统非完整约束方程中，得到末端执行器运动和基座姿态关系的任务空间约束方程；
步骤3：根据末端执行器的初始与终端位置、速度和加速度，使用五阶贝塞尔曲线得到任务空间中末端执行器路径规划，通过末端执行器的速度和加速度边界确定路径执行时间；
步骤4：通过末端执行器运动学方程和任务空间约束方程，得到对应不同任务优先级的关节运动轨迹规划。
本发明进一步的改进在于：
方法还包括：
步骤5：跟踪关节运动轨迹规划路径，使用闭环控制系统消除跟踪偏差；闭环控制系统中关节力矩输入采用PD控制。
步骤1的具体方法为：
基于拉格朗日原理建立空间机器人系统的动力学方程：
Figure BDA0001960478040000021
其中，
Figure BDA0001960478040000022
是基座的线加速度和角加速度，
Figure BDA0001960478040000023
表示机械臂a的角加速度，
Figure BDA0001960478040000024
表示机械臂b的角加速度，上标的a和b分别表示机械臂a和机械臂b；Hb为基座的惯量矩阵，
Figure BDA0001960478040000031
和
Figure BDA0001960478040000032
分别为两机械臂的与基座的耦合惯量矩阵，
Figure BDA0001960478040000033
和
Figure BDA0001960478040000034
分别为两机械臂的惯量矩阵，cb为基座的非线性项，
Figure BDA0001960478040000035
和
Figure BDA0001960478040000036
分别为两机械臂的非线性项；fb为施加于基座的广义力，
Figure BDA0001960478040000037
和
Figure BDA0001960478040000038
分别为施加于各关节的力矩；
Figure BDA0001960478040000039
和
Figure BDA00019604780400000310
分别为两机械臂对基座的Jacobian矩阵的转置矩阵；
Figure BDA00019604780400000311
和
Figure BDA00019604780400000312
分别为两机械臂的Jacobian矩阵的转置矩阵；
Figure BDA00019604780400000313
和
Figure BDA00019604780400000314
分别为施加于两机械臂末端的广义力；
根据动量守恒定律，整个空间机器人系统的线动量和角动量的守恒方程：
Figure BDA00019604780400000315
其中，M0为空间机械臂系统的总动量；P0和L0为空间机械臂系统的线动量和角动量；mb和mi为基座和各连杆的质量；
Figure BDA00019604780400000316
和
Figure BDA00019604780400000317
为基座和各连杆质心的速度；Ib为基座的惯量矩阵，ωb和ωi为基座和各连杆的旋转角速度，
Figure BDA00019604780400000318
为基座的线速度与角速度，
Figure BDA00019604780400000319
为机械臂b的关节角速，
Figure BDA00019604780400000320
为机械臂a的关节角速度，rb为基座的位置向量，Ii为机械臂连杆的惯量矩阵，rCi为机械臂连杆的位置向量；
由于Hb可逆；得到基座运动方程：
Figure BDA00019604780400000321
其中，Ja为基座的广义Jacobian矩阵，
Figure BDA00019604780400000322
为机械臂的关节角速度向量；
将基座运动方程代入末端执行器a的运动学链式方程
Figure BDA00019604780400000323
和末端执行器b的运动学链式方程
Figure BDA00019604780400000324
中，得到末端执行器运动学方程：
Figure BDA00019604780400000325
其中，Jd是描述系统初始动量对机械臂运动影响的耦合矩阵，Jg是广义雅克比矩阵，
Figure BDA00019604780400000326
为机械臂a的末端执行器速度，
Figure BDA00019604780400000327
为机械臂a对基座影响的Jacobian矩阵，
Figure BDA00019604780400000328
为机械臂b的末端执行器速度，
Figure BDA00019604780400000329
为机械臂b对基座影响的Jacobian矩阵，
Figure BDA0001960478040000041
为机械臂a的末端执行器Jacobian矩阵，
Figure BDA0001960478040000042
为机械臂b的末端执行器Jacobian矩阵，
Figure BDA0001960478040000043
为机械臂a的广义Jacobian矩阵，
Figure BDA0001960478040000044
为机械臂b的广义Jacobian矩阵。
步骤2的具体方法为：
通过动量守恒方程得到空间机器人系统满足的非完整约束方程：
Figure BDA0001960478040000045
其中，Is为整个机器人系统的惯性矩阵，
Figure BDA0001960478040000046
和
Figure BDA0001960478040000047
为基座和机械臂之间的耦合惯性矩阵；非完整约束方程为关节速度约束方程；
根据末端执行器的运动学方程得到末端执行器的逆运动学冗余解，末端执行器的逆运动学冗余解由伪逆特解和齐次方程
Figure BDA0001960478040000048
的一般解组成：
Figure BDA0001960478040000049
其中，
Figure BDA00019604780400000410
是Jg的Moore-Penrose伪逆，En为单位矩阵，h是任意矢量；
将末端执行器的逆运动学冗余解代入关节速度约束方程，得到末端执行器运动和基座姿态关系的任务空间约束方程：
Figure BDA00019604780400000411
步骤3的具体方法为：
采用五阶贝塞尔曲线描述末端执行器在任务空间的路径，末端执行器在任务空间的路径方程：
Figure BDA00019604780400000412
其中，bj,m(τ)是阶数为m的伯恩斯坦多项式，
Figure BDA00019604780400000413
是二项式系数，Pij是给定的控制点用于构建贝塞尔曲线；
第i维机械臂路径方程：
Figure BDA0001960478040000051
对于末端执行器任务，将初始和终端加速度设置为零，设置终端速度与抓捕点速度一致；将初始时刻的位置
Figure BDA0001960478040000052
和速度
Figure BDA0001960478040000053
以及终端时刻的位置
Figure BDA0001960478040000054
和速度
Figure BDA0001960478040000055
代入第i维机械臂路径方程，得到：
Figure BDA0001960478040000056
其中，路径速度和加速度边界通过执行时间来确定：
Figure BDA0001960478040000057
其中，
Figure BDA0001960478040000058
xmax和xmin分别为速度和加速度的上下边界约束。
步骤4的具体方法为：
自由漂浮空间机械臂执行目标抓捕和基座姿态调整任务描述如下：
Figure BDA0001960478040000059
当末端执行器任务和调整基座姿态任务同时考虑时，末端执行器的逆运动学冗余解通过假定机械臂任务在低优先级得到，末端执行器的逆运动学冗余解：
Figure BDA00019604780400000510
其中，
Figure BDA00019604780400000511
代表Jg到Ibm零空间的映射；
当机械臂任务具有优先级，末端执行器的逆运动学冗余解：
Figure BDA0001960478040000061
其中，
Figure BDA0001960478040000062
代表Ibm到Jg零空间的映射；根据末端执行器路径规划和基座运动方程，得到执行目标抓捕和基座姿态调整任务的协同路径。
步骤5中：
PD控制中的PD控制率方程为：
Figure BDA0001960478040000063
其中，Kp＝diag{kpEn}，Kd＝diag{kdEn}是比例和微分增益的对角阵，θ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：建立系统的运动学方程和动力学方程；
基于拉格朗日原理建立的空间机器人系统的动力学方程如下：
Figure BDA0001960478040000091
其中，
Figure BDA0001960478040000092
是基座的线加速度和角加速度，
Figure BDA0001960478040000093
和
Figure BDA0001960478040000094
代表机械臂a和b的角加速度，上标带有a和b的对应代表机械臂a和b；Hb为基座的惯量矩阵，
Figure BDA0001960478040000095
和
Figure BDA0001960478040000096
分别为两机械臂的与基座的耦合惯量矩阵，
Figure BDA0001960478040000101
和
Figure BDA0001960478040000102
分别为两机械臂的惯量矩阵，cb为基座的非线性项，
Figure BDA0001960478040000103
和
Figure BDA0001960478040000104
分别为两机械臂的非线性项；fb为施加于基座的广义力，
Figure BDA0001960478040000105
和
Figure BDA0001960478040000106
分别为施加于各关节的力矩；
Figure BDA0001960478040000107
和
Figure BDA0001960478040000108
分别为两机械臂对基座的Jacobian矩阵的转置矩阵；
Figure BDA0001960478040000109
和
Figure BDA00019604780400001010
分别为两机械臂的Jacobian矩阵的转置矩阵；
Figure BDA00019604780400001011
和
Figure BDA00019604780400001012
分别为施加于两机械臂末端的广义力。
对于自由漂浮空间机器人系统，不考虑末端执行器和基座上的外力和力矩。机械臂的运动通过关节内力矩产生。根据动量守恒定律，整个机器人系统的线动量和角动量守恒，描述如下：
Figure BDA00019604780400001013
其中，M0为空间机械臂系统的总动量；P0和L0为空间机械臂系统的线动量和和角动量；mb和mi为基座和各连杆的质量；
Figure BDA00019604780400001014
和
Figure BDA00019604780400001015
为基座和各连杆质心的速度；Ib为基座的惯量矩阵，ωb和ωi为基座和各连杆的旋转角速度。
由于Hb可逆，基座的运动可以描述如下：
Figure BDA00019604780400001016
将上述方程代入末端执行器a的运动学链式方程
Figure BDA00019604780400001017
和末端执行器b的运动学链式方程
Figure BDA00019604780400001018
中，可以得到末端执行器的运动方程：
Figure BDA00019604780400001019
其中：Jd是描述系统初始动量对末端执行器运动影响的耦合矩阵，Jg是广义雅克比矩阵(GJM)。
步骤2：系统任务空间约束；
由动量方程可以得到系统满足的非完整约束为：
Figure BDA00019604780400001020
其中：Is为整个机器人系统的惯性矩阵，
Figure BDA0001960478040000111
和
Figure BDA0001960478040000112
为基座和机械臂之间的耦合惯性矩阵。上式是关节速度的函数，为关节速度约束。
从末端执行器的运动学方程可以得到末端执行器的逆运动学冗余解由伪逆特解和齐次方程
Figure BDA0001960478040000113
的一般解组成：
Figure BDA0001960478040000114
其中：
Figure BDA0001960478040000115
是Jg的Moore-Penrose伪逆，En为单位矩阵，h是任意适量。将上式代入关节速度约束方程中可以得到任务空间约束为：
Figure BDA0001960478040000116
上式表示了末端执行器运动和基座姿态之间的关系。可以通过规划合适的末端执行器路径来调整基座姿态，实现空间机器人和基座的协同任务。
步骤3：末端执行器路径规划，确定路径执行时间；
采用五阶Bézier曲线(贝塞尔曲线)来描述末端执行器路径：
Figure BDA0001960478040000117
其中：bj,m(τ)是阶数为m的伯恩斯坦多项式，
Figure BDA0001960478040000118
是二项式系数，Pij是给定的控制点用于构建Bézier曲线。
第i维末端执行器路径为：
Figure BDA0001960478040000119
对于末端执行器任务，初始和终端时刻的位置(
Figure BDA00019604780400001110
和
Figure BDA00019604780400001111
)速度(
Figure BDA00019604780400001112
和
Figure BDA00019604780400001113
)可以事先确定。初始和终端加速度设置为零以最小化关节突变。为了避免高碰撞力，终端速度与抓捕点速度一致。将这些等式约束代入到上式可以得到：
Figure BDA00019604780400001114
由于
Figure BDA0001960478040000121
已知，Bézier曲线的形状可以确定，路径速度和加速度边界可以通过执行时间的确定来满足：
Figure BDA0001960478040000122
其中：
Figure BDA0001960478040000123
一旦执行时间确定，末端执行器在任务空间的路径使用Bézier曲线可以完全地生成。
步骤4：关节运动轨迹规划；
自由漂浮空间机械臂的两种任务可以描述如下：
Figure BDA0001960478040000124
当末端执行器任务和调整基座姿态同时考虑时，机械臂关节运动的解(末端执行器的逆运动学冗余解)可以通过假定末端执行器任务在低优先级得到：
Figure BDA0001960478040000125
其中：
Figure BDA0001960478040000126
代表Jg到Ibm零空间的映射。
如果末端执行器任务
Figure BDA0001960478040000127
具有优先级，机械臂关节运动的解(末端执行器的逆运动学冗余解)是：
Figure BDA0001960478040000128
其中：
Figure BDA0001960478040000129
代表Ibm到Jg零空间的映射。使用之前设计的末端执行器路径和基座运动律，可以得到执行目标抓捕和基座姿态调整任务的协同路径。
步骤5：消除跟踪偏差的控制；
采用闭环控制结构消除跟踪偏差。其中，关节力矩输入采用PD控制，控制率用于动力学仿真，PD控制率方程为：
Figure BDA0001960478040000131
其中：Kp＝diag{kpEn}，Kd＝diag{kdEn}是比例和微分增益的对角阵，θkd和θk分别为期望的关节角位置以及当前的关节角位置。
下面将本发明的方法应用到具体的实例中。
末端执行器速度和加速度边界条件为
Figure BDA0001960478040000132
角速度和角加速度为
Figure BDA0001960478040000133
kp＝120,kd＝32。
实施例1
只有双臂运动，末端执行器a,b从初始位置到达抓捕点；其中，初始位置为：
Figure BDA0001960478040000134
Figure BDA0001960478040000135
Figure BDA0001960478040000136
终端位置为：
Figure BDA0001960478040000137
仿真结果参见图3-6，通过速度和加速度边界，可以确定执行时间为T＝14s。通过仿真结果可以看出，设计的末端执行器路径符合提出的约束，可以完成提出的抓捕任务。当不考虑协同轨迹规划策略时，参见图7-12，可以看出双臂都可以执行跟踪任务，不考虑零空间运动。基座姿态和位置受到影响。最大的姿态和位置扰动到0.34°和0.63m。
实施例2
驱动双臂末端执行器到达实例1中目标抓捕点位置，同时最小化基座姿态扰动。
选取ωb＝0和
Figure BDA0001960478040000141
作为依赖速度。参见图13-18，展示了零反应机械臂的轨迹跟踪结果。可以注意到基座的姿态扰动为O(10-4)，保证了双臂空间机器人的无反应机械臂。抑制基座姿态扰动所导致的末端执行器方向偏差小于0.05°。仿真结果表明了协同路径规划策略的有效性。
实施例3
驱动双臂末端执行器到达实例1中目标抓捕点位置，同时调整基座姿态位置。在这一仿真中要求基座姿态从初始的
Figure BDA0001960478040000142
到达终端值
Figure BDA0001960478040000143
度，基座位置自由。仿真结果参见图19-24，可以看出末端执行器可以以要求的速度到达在轨目标的抓捕点。末端执行器的方向追踪偏差小于0.3°。仿真结果表明采用双臂路径协同来调整基座姿态的可行性和有效性。通过上述三个例子可以看出，采用本发明提出的协同路径规划方法，可以完成末端执行器的抓捕任务，同时保证基座的姿态扰动最小或者调整基座姿态。
以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (6)

1.一种针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，包括以下步骤：
步骤1：建立空间机器人系统的动力学方程和运动学方程，得到末端执行器运动学方程；
步骤2：通过动量守恒方程得到系统非完整约束方程，通过末端执行器的运动学方程求解末端执行器逆运动学冗余解，将末端执行器逆运动学冗余解代入系统非完整约束方程中，得到末端执行器运动和基座姿态关系的任务空间约束方程；
步骤3：根据末端执行器的初始与终端位置、速度和加速度，使用五阶贝塞尔曲线得到任务空间中末端执行器路径规划，通过末端执行器的速度和加速度边界确定路径执行时间；
步骤4：通过末端执行器运动学方程和任务空间约束方程，得到对应不同任务优先级的关节运动轨迹规划；
步骤5：跟踪关节运动轨迹规划路径，使用闭环控制系统消除跟踪偏差；闭环控制系统中关节力矩输入采用PD控制。
2.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤1的具体方法为：
基于拉格朗日原理建立空间机器人系统的动力学方程：
Figure FDA0003769941500000011
其中，
Figure FDA0003769941500000012
是基座的线加速度和角加速度，
Figure FDA0003769941500000013
表示机械臂a的角加速度，
Figure FDA0003769941500000014
表示机械臂b的角加速度，上标的a和b分别表示机械臂a和机械臂b；Hb为基座的惯量矩阵，
Figure FDA0003769941500000015
和
Figure FDA0003769941500000016
分别为两机械臂的与基座的耦合惯量矩阵，
Figure FDA0003769941500000017
和
Figure FDA0003769941500000018
分别为两机械臂的惯量矩阵，cb为基座的非线性项，
Figure FDA0003769941500000019
和
Figure FDA00037699415000000110
分别为两机械臂的非线性项；fb为施加于基座的广义力，
Figure FDA00037699415000000111
和
Figure FDA00037699415000000112
分别为施加于各关节的力矩；
Figure FDA00037699415000000113
和
Figure FDA00037699415000000114
分别为两机械臂对基座的Jacobian矩阵的转置矩阵；
Figure FDA0003769941500000021
和
Figure FDA0003769941500000022
分别为两机械臂的Jacobian矩阵的转置矩阵；
Figure FDA0003769941500000023
和
Figure FDA0003769941500000024
分别为施加于两机械臂末端的广义力；
根据动量守恒定律，整个空间机器人系统的线动量和角动量的守恒方程：
Figure FDA0003769941500000025
其中，M0为空间机械臂系统的总动量；P0和L0为空间机械臂系统的线动量和角动量；mb和mi为基座和各连杆的质量；
Figure FDA0003769941500000026
和
Figure FDA0003769941500000027
为基座和各连杆质心的速度；Ib为基座的惯量矩阵，ωb和ωi为基座和各连杆的旋转角速度，
Figure FDA0003769941500000028
为基座的线速度与角速度，
Figure FDA00037699415000000223
为机械臂b的关节角速，
Figure FDA0003769941500000029
为机械臂a的关节角速度，rb为基座的位置向量，Ii为机械臂连杆的惯量矩阵，rCi为机械臂连杆的位置向量；
由于Hb可逆；得到基座运动方程：
Figure FDA00037699415000000210
其中，Ja为基座的广义Jacobian矩阵，
Figure FDA00037699415000000211
为机械臂的关节角速度向量；
将基座运动方程代入末端执行器a的运动学链式方程
Figure FDA00037699415000000212
和末端执行器b的运动学链式方程
Figure FDA00037699415000000213
中，得到末端执行器运动学方程：
Figure FDA00037699415000000214
其中，Jd是描述系统初始动量对机械臂运动影响的耦合矩阵，Jg是广义雅克比矩阵，
Figure FDA00037699415000000215
为机械臂a的末端执行器速度，
Figure FDA00037699415000000216
为机械臂a对基座影响的Jacobian矩阵，
Figure FDA00037699415000000217
为机械臂b的末端执行器速度，
Figure FDA00037699415000000218
为机械臂b对基座影响的Jacobian矩阵，
Figure FDA00037699415000000219
为机械臂a的末端执行器Jacobian矩阵，
Figure FDA00037699415000000220
为机械臂b的末端执行器Jacobian矩阵，
Figure FDA00037699415000000221
为机械臂a的广义Jacobian矩阵，
Figure FDA00037699415000000222
为机械臂b的广义Jacobian矩阵。
3.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤2的具体方法为：
通过动量守恒方程得到空间机器人系统满足的非完整约束方程：
Figure FDA0003769941500000031
其中，Is为整个机器人系统的惯性矩阵，
Figure FDA0003769941500000032
和
Figure FDA0003769941500000033
为基座和机械臂之间的耦合惯性矩阵；非完整约束方程为关节速度约束方程；
根据末端执行器的运动学方程得到末端执行器的逆运动学冗余解，末端执行器的逆运动学冗余解由伪逆特解和齐次方程
Figure FDA0003769941500000034
的一般解组成：
Figure FDA0003769941500000035
其中，
Figure FDA0003769941500000036
是Jg的Moore-Penrose伪逆，En为单位矩阵，h是任意矢量；
将末端执行器的逆运动学冗余解代入关节速度约束方程，得到末端执行器运动和基座姿态关系的任务空间约束方程：
Figure FDA0003769941500000037
4.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤3的具体方法为：
采用五阶贝塞尔曲线描述末端执行器在任务空间的路径，末端执行器在任务空间的路径方程：
Figure FDA0003769941500000038
其中，bj,m(τ)是阶数为m的伯恩斯坦多项式，
Figure FDA0003769941500000039
是二项式系数，Pij是给定的控制点用于构建贝塞尔曲线；
第i维机械臂路径方程：
Figure FDA0003769941500000041
对于末端执行器任务，将初始和终端加速度设置为零，设置终端速度与抓捕点速度一致；将初始时刻的位置
Figure FDA0003769941500000042
和速度
Figure FDA0003769941500000043
以及终端时刻的位置
Figure FDA0003769941500000044
和速度
Figure FDA0003769941500000045
代入第i维机械臂路径方程，得到：
Figure FDA0003769941500000046
其中，路径速度和加速度边界通过执行时间来确定：
Figure FDA0003769941500000047
其中，
Figure FDA0003769941500000048
xmax和xmin分别为速度和加速度的上下边界约束。
5.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤4的具体方法为：
自由漂浮空间机械臂执行目标抓捕和基座姿态调整任务描述如下：
Figure FDA0003769941500000049
当末端执行器任务和调整基座姿态任务同时考虑时，末端执行器的逆运动学冗余解通过假定机械臂任务在低优先级得到，末端执行器的逆运动学冗余解：
Figure FDA00037699415000000410
其中，
Figure FDA00037699415000000411
代表Jg到Ibm零空间的映射；
当机械臂任务具有优先级，末端执行器的逆运动学冗余解：
Figure FDA0003769941500000051
其中，
Figure FDA0003769941500000052
代表Ibm到Jg零空间的映射；根据末端执行器路径规划和基座运动方程，得到执行目标抓捕和基座姿态调整任务的协同路径。
6.根据权利要求1所述的针对运动学冗余双臂空间机器人的协同路径规划方法，其特征在于，所述步骤5中：
PD控制中的PD控制率方程为：
Figure FDA0003769941500000053
其中，Kp＝diag{kpEn}，Kd＝diag{kdEn}是比例和微分增益的对角阵，θkd和θk分别为期望的关节角位置以及当前的关节角位置。
CN201910081351.2A 2019-01-28 2019-01-28 一种针对运动学冗余双臂空间机器人的协同路径规划方法 Active CN110104216B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910081351.2A CN110104216B (zh) 2019-01-28 2019-01-28 一种针对运动学冗余双臂空间机器人的协同路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910081351.2A CN110104216B (zh) 2019-01-28 2019-01-28 一种针对运动学冗余双臂空间机器人的协同路径规划方法

Publications (2)

Publication Number Publication Date
CN110104216A CN110104216A (zh) 2019-08-09
CN110104216B true CN110104216B (zh) 2022-09-06

Family

ID=67483678

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910081351.2A Active CN110104216B (zh) 2019-01-28 2019-01-28 一种针对运动学冗余双臂空间机器人的协同路径规划方法

Country Status (1)

Country Link
CN (1) CN110104216B (zh)

Families Citing this family (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110502010B (zh) * 2019-08-15 2021-06-04 同济大学 一种基于贝塞尔曲线的移动机器人室内自主导航控制方法
CN110948482A (zh) * 2019-11-06 2020-04-03 江苏信息职业技术学院 一种冗余机器人轨迹规划方法
CN110900604B (zh) * 2019-12-02 2021-01-05 浙江大学 基于双机械臂协作打磨系统的动力学模型设计方法
CN113119100B (zh) * 2019-12-30 2022-05-13 深圳市优必选科技股份有限公司 冗余机械臂控制方法、装置、可读存储介质及设备
CN111251297B (zh) * 2020-02-20 2023-02-07 西北工业大学 一种基于随机采样的双臂空间机器人协调路径规划方法
CN111687833B (zh) * 2020-04-30 2023-06-02 广西科技大学 机械手逆优先级阻抗控制系统及控制方法
CN111687832B (zh) * 2020-04-30 2023-06-02 广西科技大学 空间机械手冗余机械臂逆优先级阻抗控制系统及控制方法
CN111687834B (zh) * 2020-04-30 2023-06-02 广西科技大学 移动机械手冗余机械臂逆优先级阻抗控制系统及控制方法
CN111687835B (zh) * 2020-04-30 2023-06-02 广西科技大学 水下机械手冗余机械臂逆优先级阻抗控制系统及控制方法
CN111702753B (zh) * 2020-04-30 2023-02-14 广西科技大学 冗余机械臂逆优先级阻抗控制系统及控制方法
CN111687840B (zh) * 2020-06-11 2021-10-29 清华大学 一种对空间目标实施抓捕的方法、装置和存储介质
CN112171660B (zh) * 2020-08-18 2021-11-23 南京航空航天大学 一种基于深度强化学习的空间双臂系统约束运动规划方法
CN112207835B (zh) * 2020-09-18 2021-11-16 浙江大学 一种基于示教学习实现双臂协同作业任务的方法
CN112451089B (zh) * 2020-10-13 2023-01-20 北京工业大学 一种用于胫骨畸形矫正的位姿空间等间距轨迹规划方法
CN112549025B (zh) * 2020-11-27 2022-06-21 北京工业大学 一种基于融合人体运动学约束的协调图双臂协作控制方法
CN112591153B (zh) * 2020-12-08 2022-06-10 北京航空航天大学 基于抗干扰多目标h2/h∞滤波的空间机械臂末端定位方法
CN112847359B (zh) * 2020-12-31 2022-03-01 西北工业大学 针对大尺度故障航天器的多独立超冗余机械臂协同抓捕方法
CN112975965B (zh) * 2021-02-24 2022-07-29 深圳市优必选科技股份有限公司 仿人机器人的解耦控制方法、装置和仿人机器人
CN114083537B (zh) * 2021-11-30 2023-07-07 深圳市优必选科技股份有限公司 机械臂夹持控制方法、装置、机器人和可读存储介质
CN114800496B (zh) * 2022-04-18 2023-11-03 中国科学院空间应用工程与技术中心 空间机器人接触操作的风险判定方法、系统、介质及设备
CN115958596B (zh) * 2022-12-05 2023-09-29 广州工程技术职业学院 双冗余机械臂运动规划方法及装置、设备、存储介质
CN117075525B (zh) * 2023-10-12 2023-12-19 纳博特南京科技有限公司 基于约束型模型预测控制的移动机器人控制方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104750116B (zh) * 2014-12-31 2017-07-07 中国航天科技集团公司第五研究院第五一三研究所 一种双臂机器人协调操作的路径规划方法
CN106945020B (zh) * 2017-05-18 2019-10-22 哈尔滨工业大学 一种空间双机械臂系统运动协调控制方法
CN107490965B (zh) * 2017-08-21 2020-02-07 西北工业大学 一种空间自由漂浮机械臂的多约束轨迹规划方法
CN108132601B (zh) * 2017-12-06 2020-04-28 西北工业大学 一种利用机械臂抑制航天器基座姿态干扰的方法
CN108621163A (zh) * 2018-05-10 2018-10-09 同济大学 一种面向汇榫工艺的冗余双臂机器人协作控制方法

Also Published As

Publication number Publication date
CN110104216A (zh) 2019-08-09

Similar Documents

Publication Publication Date Title
CN110104216B (zh) 一种针对运动学冗余双臂空间机器人的协同路径规划方法
Seraji An on-line approach to coordinated mobility and manipulation
Moosavian et al. Multiple impedance control for space free-flying robots
CN111251297B (zh) 一种基于随机采样的双臂空间机器人协调路径规划方法
Caccavale et al. Kinematic control of redundant free-floating robotic systems
Rastegari et al. Multiple impedance control of space free-flying robots via virtual linkages
CN108132601B (zh) 一种利用机械臂抑制航天器基座姿态干扰的方法
Stolfi et al. A parametric analysis of a controlled deployable space manipulator for capturing a non-cooperative flexible satellite
Oki et al. Time-optimal detumbling maneuver along an arbitrary arm motion during the capture of a target satellite
Caccavale et al. Quaternion-based kinematic control of redundant spacecraft/manipulator systems
Giglio et al. Selective compliance control for an unmanned aerial vehicle with a robotic arm
CN111268182A (zh) 空间双臂机器人约束柔顺稳定控制方法
Cataldi et al. Set-based inverse kinematics control of an anthropomorphic dual arm aerial manipulator
CN110561441B (zh) 一种冗余度机械臂位姿控制的单94lvi迭代算法
Yang et al. Coordinated control of dual-arm robot on space structure for capturing space targets
CN112698574B (zh) 一种基于混合任务优先级的双臂空间机器人协调控制方法
Ivanovic et al. Exploiting null space in aerial manipulation through model-in-the-loop motion planning
Yu et al. Dynamic modeling and control of a free-flying space robot with flexible-link and flexible-joints
Zhang et al. A multi-priority control of asymmetric coordination for redundant dual-arm robot
Bettez-Bouchard et al. Development and experimental validation of a reorientation algorithm for a free-floating serial manipulator
Yuguchi et al. Analysis on motion control based on reaction null space for ground grip robot on an asteroid
Yi et al. Modeling and control of a fully-actuated quadrotor manipulator with tiltable rotors
Alipour et al. Point-to-point stable motion planning of wheeled mobile robots with multiple arms for heavy object manipulation
CN111975770B (zh) 一种采用空间双臂机器人的自旋目标抓捕方法
Mathew et al. Tracking control of a mobile manipulator with external torque disturbances using computed torque control

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant