一种用于捕获翻滚目标的空间双臂机器人控制方法与流程

本发明涉及空间机器人柔顺控制技术，尤其涉及一种用于捕获翻滚目标的空间双臂机器人控制方法。

【背景技术】

目前，近地轨道中故障和失效卫星的数量逐渐增加，为了最大限度的降低损失，各国正在积极研究以卫星维护、轨道垃圾处理为目的的在轨捕获技术。在利用空间双臂机器人完成翻滚目标在轨捕获任务时，最重要的阶段之一是接触碰撞阶段。捕获过程中，末端执行机构不可避免的会与待捕获目标发生接触碰撞甚至激振等行为，不利于捕获操作任务稳定可靠的进行，严重时甚至可能损坏机器人或捕获机构。同时，为防止目标逃逸，空间双臂机器人需要输出必要的操作力，以抑制末端与目标之间的相对运动，实现对目标的可靠抓捕。如何将空间双臂机器人末端与目标之间的接触力控制在期望范围内，对保证捕获过程平稳安全、提高目标捕获成功率具有十分重要的意义。

现有目标捕获过程的空间机器人柔顺控制算法，一般适用于静态目标的抓捕，要求目标与空间机器人之间保持相对静止，以实现抓捕点跟踪，其控制器设计难度较低。考虑在空间摄动力的作用下，自由漂浮状态下的目标旋转速度通常可以达到6°/s，而对于携带推进系统的目标来说，在控制系统失效的情况下，其旋转速度可能在短期内增加到几十甚至上百°/s。此时，若仍采用静态目标抓捕的算法，为实现抓捕点的快速跟踪，空间机器人可能存在跟踪困难，或跟踪运动幅度过大导致系统失稳、碰撞风险加大等问题。因此现有算法并不适用翻滚目标的抓捕。

技术实现要素：

有鉴于此，本发明实施例提供了一种用于捕获翻滚目标的空间双臂机器人控制方法，通过控制末端接触力在夹紧翻滚目标的同时完成其角速度的有效衰减，以实现空间双臂机器人稳定可靠捕获翻滚目标。

本发明实施例提供了一种用于捕获翻滚目标的空间双臂机器人控制方法，包括：

获得空间双臂机器人动力学方程及摩擦接触方程；

依据所述空间双臂机器人动力学方程及摩擦接触方程，获得空间双臂机器人协调操作系统的统一动力学方程；

依据所述空间双臂机器人协调操作系统的统一动力学方程，获得空间双臂机器人捕获翻滚目标的控制器。

上述方法中，所述依据所述空间双臂机器人动力学方程及摩擦接触方程，获得空间双臂机器人协调操作系统的统一动力学方程为：

其中，m为空间双臂机器人操作空间惯性矩阵，c为空间双臂机器人操作空间科氏力项和离心力项之和，fe为空间双臂机器人末端输出力，f为空间双臂机器人广义驱动力，为空间双臂机器人末端加速度矢量。

上述方法中，利用如下空间双臂机器人末端与目标的运动约束关系公式，获得空间双臂机器人末端加速度矢量

其中，为牵连运动加速度，w＝[e6e6]^t(e6为6×6单位阵)，为翻滚目标质心加速度矢量，为相对运动加速度，qa为科氏加速度；

利用如下空间双臂机器人末端与目标接触表面的力约束关系公式，获得空间双臂机器人末端输出力fe：

fe＝fext+fi

其中，目标的外力矢量fext，用以平衡目标的动力学fo及作用于目标的外界环境力fenv，目标的内力矢量fi，用以提供夹紧内力。

上述方法中，利用如下翻滚目标动力学方程公式：

其中，mo为目标惯性矩阵，co为科氏力/离心力系数矩阵，为翻滚目标质心速度矢量，fo为目标所受合外力，fenv为目标所受外界环境力，jo和jp分别为空间双臂机器人抓持矩阵和外界环境力接触矩阵；

依据上式，并采用零空间理论，获得目标的外力矢量fext及目标的内力矢量fi：

其中，e12为12×12单位阵。

上述方法中，所述依据所述空间双臂机器人协调操作系统的统一动力学方程，获得空间双臂机器人捕获翻滚目标的控制器，包括：

获得目标操作空间阻抗控制方程：

其中，ho为目标期望惯量，bo为目标期望阻尼，ko为目标期望刚度，为目标运动位姿误差，yo为目标实际位姿，yod为目标期望位姿，和分别为运动位姿误差的一阶导和二阶导；

依据上式，变换获得控制变量目标加速度

采用混合控制理论，获得空间双臂机器人笛卡尔空间混合阻抗控制方程：

其中，he为空间双臂机器人末端期望惯量，be为空间双臂机器人末端期望阻尼，ke为空间双臂机器人末端期望刚度，kif为空间双臂机器人内力控制增益，^cxoed、和表示空间双臂机器人末端与目标之间的期望相对运动位姿、期望相对运动速度和期望相对运动加速度，^cxoe、和表示空间双臂机器人末端与目标之间的实际相对运动位姿、实际相对运动速度和实际相对运动加速度，fid为期望内力，{c}为接触坐标系，s为阻抗控制模式选择矩阵，为力控制模式选择矩阵；

依据上式，变换获得控制变量目标加速度

依据所述目标操作空间阻抗控制方程及空间双臂机器人笛卡尔空间混合阻抗控制方程，获得空间双臂机器人捕获翻滚目标的多空间混合阻抗控制器：

其中，rs为接触坐标系与惯性系的转换矩阵；

依据空间双臂机器人捕获翻滚目标的多空间混合阻抗控制器和空间双臂机器人协调操作系统的统一动力学方程，获得空间双臂机器人广义驱动力f：

由以上技术方案可以看出，本发明实施例具有以下有益效果：

本发明实施例的技术方案中，依据空间双臂机器人动力学方程及摩擦接触方程，获得空间双臂机器人协调操作系统的统一动力学方程，进而依据统一动力学方程，获得空间双臂机器人捕获翻滚目标的控制器，通过控制机器人末端输出力，在夹紧翻滚目标的同时完成其角速度的有效衰减，保证对目标期望运动的稳定跟踪及双臂末端与目标之间的柔顺性，从而实现空间双臂机器人稳定可靠的捕获翻滚目标。

【附图说明】

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单的介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性和劳动性的前提下，还可以根据这些附图获得其它附图。

图1是本发明实施例所提供的用于捕获翻滚目标的空间双臂机器人控制方法的流程示意图；

图2是本发明实施例所提供的空间双臂机器人运动学模型示意图；

图3是利用本发明实施例所提供的方法对初始旋转速度ω0＝0°/s的目标进行控制的多空间混合阻抗控制曲线图；

图4是利用本发明实施例所提供的方法对初始旋转速度ω0＝5°/s的目标进行控制的多空间混合阻抗控制曲线图；

图5是利用本发明实施例所提供的方法对初始旋转速度ω0＝20°/s的目标进行控制的多空间混合阻抗控制曲线图。

【具体实施方式】

为了更好的理解本发明的技术方案，下面结合附图对本发明实施例进行详细描述。

应当明确，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明实施例给出一种用于捕获翻滚目标的空间双臂机器人控制方法，请参考图1，其本发明实施例所提供的用于捕获翻滚目标的空间双臂机器人控制方法的流程示意图，如图1所示，该方法包括以下步骤：

步骤101，获得空间双臂机器人动力学方程及摩擦接触方程。

具体的，首先，获得空间双臂机器人动力学方程。

针对带耦合分支的空间双臂机器人，获得如下运动学方程，用以描述空间双臂机器人末端运动与关节运动的映射关系：

其中，为空间双臂机器人末端速度矢量，分别为左臂和右臂末端速度矢量，为关节角速度矢量，j为空间双臂机器人雅克比矩阵，jl＝[jl1jl2]、jr＝[jr1jr3]分别为与左臂、右臂运动相关的雅克比矩阵。

利用拉格朗日方程，获得如下空间双臂机器人关节空间动力学方程，用以描述空间双臂机器人关节运动与关节驱动力矩的映射关系：

其中，a(q)为空间双臂机器人关节空间惯性矩阵，为科式力项，为离心力项，为关节角加度矢量，fe为末端输出力，τ为关节驱动力矩。

将运动学方程(1)微分后代入关节空间动力学方程(2)中，获得如下空间双臂机器人操作空间动力学方程，用以描述空间双臂机器人末端运动与广义驱动力的映射关系：

其中，m＝j^+ta(q)j⁺为空间双臂机器人操作空间惯性矩阵，为空间双臂机器人操作空间科氏力项和离心力项之和，f为空间双臂机器人广义驱动力，为空间双臂机器人末端加速度矢量。

然后，获得摩擦接触方程。

考虑空间双臂机器人与翻滚目标的接触情况，获得机械臂末端输出力：

fe＝fn+ff(4)

其中，fn为法向正压力，ff为切向摩擦力。

采用弹簧-阻尼模型，获得接触情况中的法向正压力fn：

其中，k为弹簧刚度，d为阻尼系数，δp为弹簧的变形量，为δp的一阶导数，||fn||为法向正压力fn的模值，fn的方向为接触面的法线方向。

采用lugre模型，获得接触情况中的切向摩擦力ff：

其中，σ0为接触表面的刚度系数，σ1为阻尼系数，σ2为粘性阻尼系数，z为接触表面的形变量，为接触表面的形变率，v为末端与目标表面的相对速度，||ff||为切向摩擦力ff的模值，ff的方向为相对运动方向的反方向。

其中

g(v)＝fc+(fs-fc)exp(-|v/vs|²)

g(v)为库仑摩擦和stribeck(粘滞力)效应，fc、fs分别为接触面之间的库伦摩擦力及粘滞力，vs为系统的stribeck速度。

当接触摩擦为干摩擦时，反映流体的内摩擦力的粘滞力fs＝0，vs＝0，σ2＝0，有g(v)＝fc，获得接触表面的形变率为：

获得接触情况中的切向摩擦力ff为：

步骤102，依据所述空间双臂机器人动力学方程及摩擦接触方程，获得空间双臂机器人协调操作系统的统一动力学方程。

具体的，考虑翻滚目标捕获过程中目标的动态特性，准确建立空间双臂机器人末端与目标接触表面的运动约束关系及力约束关系，代入空间双臂机器人动力学方程及摩擦接触方程中，获得空间双臂机器人协调操作系统的统一动力学方程，用以描述空间双臂机器人广义驱动力与目标运动、目标外力矢量、目标内力矢量之间的映射关系。

首先，获得空间双臂机器人末端与目标接触表面的运动约束关系。

根据翻滚目标捕获过程中目标的动态特性，获得如下空间双臂机器人末端与目标接触表面的速度约束方程：

其中，为牵连运动速度，w＝[e6e6]^t(e6为6×6单位阵)，为翻滚目标质心速度矢量，为相对运动速度。

其中，

ωo为惯性系下翻滚目标质心角速度矢量，^opoa、^opob分别为翻滚目标质心系下目标质心到左右臂末端位置矢量，对任意向量r＝[x,y,z]，其反对称矩阵s(r)为

将速度约束方程(10)两边对时间进行求导，获得如下空间双臂机器人末端与目标接触表面的加速度约束方程：

其中，为牵连运动加速度，为翻滚目标质心加速度矢量，为相对运动加速度，qa为科氏加速度。

其中，

αo为惯性系下翻滚目标质心角加速度矢量。

然后，获得空间双臂机器人末端与目标接触表面的力约束关系。

利用如下翻滚目标动力学方程公式：

其中，mo为目标惯性矩阵，co为科氏力/离心力系数矩阵，fo为目标所受合外力，fenv为目标所受外界环境力，jo和jp分别为空间双臂机器人抓持矩阵和外界环境力接触矩阵。

其中，

其中，mo、io分别为目标质量和目标惯性张量，jol、jor分别为空间双臂机器人左右臂抓持矩阵，ⁱpoa、ⁱpob、ⁱpop分别为惯性系下目标质心到左右臂末端及环境接触点的位置矢量。

依据翻滚目标动力学方程公式(12)，并采用零空间理论，获得空间双臂机器人末端输出力fe：

其中，fext目标的外力矢量，用以平衡目标的动力学fo及作用于目标的外界环境力fenv，fi目标的内力矢量，用以提供夹紧内力；表示抓持矩阵的零空间矩阵，因此无论内力作用与否，目标的运动都不受影响，从而实现空间双臂机器人协调操作系统的内力、外力分解。

最后，依据空间双臂机器人末端与目标接触表面的运动约束关系和力约束关系，代入空间双臂机器人动力学方程中，获得空间双臂机器人协调操作系统的统一动力学方程。

将空间双臂机器人末端与目标接触表面的运动约束关系(11)和力约束关系(13)，代入空间双臂机器人操作空间动力学方程(3)中，获得空间双臂机器人协调操作系统的统一动力学方程：

步骤103，依据所述空间双臂机器人协调操作系统的统一动力学模型，获得空间双臂机器人捕获翻滚目标的控制器。

具体的，将目标操作空间阻抗作为控制外环，获得目标运动及环境作用力间的阻抗关系；将空间双臂机器人笛卡尔空间混合阻抗作为控制内环，获得接触表面相对运动及目标内力间的阻抗关系；结合目标操作空间阻抗控制器和空间双臂机器人笛卡尔空间混合阻抗控制器，获得空间双臂机器人捕获翻滚目标的控制器。

本发明实施例中，依据所述空间双臂机器人协调操作系统的统一动力学模型，获得空间双臂机器人捕获翻滚目标的控制器，包括：

1)获得目标操作空间阻抗控制方程。

目标的阻抗方程为：

其中，ho为目标期望惯量，bo为目标期望阻尼，ko为目标期望刚度，为目标运动位姿误差，yo为目标实际位姿，yod为目标期望位姿，和分别为运动位姿误差的一阶导和二阶导。

考虑环境作用力难以直接获得，将翻滚目标动力学方程(12)代入目标阻抗方程(15)中，获得：

由式(16)可获得目标操作空间阻抗控制器的控制输入为：

其中，为目标期望加速度。

2)获得空间双臂机器人笛卡尔空间混合阻抗控制方程。

采用混合控制理论，将任务空间分解成阻抗控制子空间和力控制子空间。在阻抗控制子空间对空间双臂机器人系统位置-力之间的阻抗关系进行调整，在力控制子空间则进行期望的力跟踪。空间双臂机器人笛卡尔空间混合阻抗控制方程为：

其中，he为空间双臂机器人末端期望惯量，be为空间双臂机器人末端期望阻尼，ke为空间双臂机器人末端期望刚度，kif为空间双臂机器人内力控制增益，^cxoed、和表示空间双臂机器人末端与目标之间的期望相对运动位姿、期望相对运动速度和期望相对运动加速度，^cxoe、和表示空间双臂机器人末端与目标之间的实际相对运动位姿、实际相对运动速度和实际相对运动加速度，fid为期望内力，{c}为接触坐标系，s为阻抗控制模式选择矩阵，为力控制模式选择矩阵。

式(18)中控制子空间的选取通过调整选择矩阵s和实现：当s对应元素为1时，在接触坐标系{c}的对应方向上采用阻抗控制模式，当对应元素为1时，在对应方向上采用力控制模式。特别的，由于可操作维度的限制，某些方向不进行任何控制，s和对应元素均为0。

其中，期望相对运动通过以下方法获得：

当不考虑外界环境干扰的情况下，根据式(12)和式(13)获得期望摩擦力ffd为：

其中，期望摩擦力ffd＝[ffldffrd]^t，ffld为左臂期望摩擦力，ffrd为右臂期望摩擦力，摩擦矩阵jof＝[s(ⁱpoa)s(ⁱpob)]^t，惯性矩阵mi＝io，科氏力/离心力系数矩阵ci＝-s(ioωo)，αod为惯性系下翻滚目标质心期望角加速度矢量，ωod为惯性系下翻滚目标质心期望角速度矢量。

对式(8)经laplace变换可计算获得变形量z，代入式(9)，可获得接触情况中的切向摩擦力ff：

||ff||＝(fc-σ1|v|)exp(-σ0|x|/fc)+σ1v(20)

由式(19)和式(20)可得期望运动vd为：

其中，vd为接触运动方向上机械臂末端与目标表面相对运动速度的大小，x为机械臂末端与目标表面相对运动位移xoed的大小，库伦摩擦力fc＝μ||fnd||。

将式(21)代入式(18)中，即可实现通过摩擦力的换算，建立期望相对运动与期望目标运动、期望法向正压力间的关系。

由式(18)可获得空间双臂机器人笛卡尔空间混合阻抗控制器的控制输入为：

其中，rs为接触坐标系与惯性系的转换矩阵。

3)获得空间双臂机器人捕获翻滚目标的多空间混合阻抗控制器。

依据所述目标操作空间阻抗控制方程及空间双臂机器人笛卡尔空间混合阻抗控制方程，代入加速度约束方程(11)中，获得空间双臂机器人捕获翻滚目标的多空间混合阻抗控制器的控制输入为：

上式(23)代入空间双臂机器人操作空间动力学方程中，即可获得空间双臂机器人关节驱动力矩。

依据本发明实施例提供的上述方法，对空间双臂机器人的控制进行了仿真，分别针对静止、低速、中速3种不同旋转状态的翻滚目标的夹紧消旋过程中的柔顺控制开展仿真实验研究。

请参考图2，其为空间双臂机器人的运动学模型，其中，tk、lk、rk分别表示耦合分支、左臂、右臂上第k个关节，有zr1为∑r1的z轴单位向量(即空间双臂机器人右臂第1关节轴线方向的单位向量)，xr1为∑r1的x轴单位向量，yr1为∑r1的y轴单位向量，其余符号类似。其对应的d-h参数如表1所示。

表1空间双臂机器人d-h参数表

设置目标初始旋转速度分别为ω0＝0°/s、ω0＝5°/s、ω0＝20°/s，使用本发明

实施例的技术方案对上述任务进行仿真，仿真结果如图3～图5所示。

请参考图3，其为目标初始旋转速度为ω0＝0°/s的控制效果图。设置目期望内力为fi＝[0,10,0]n，目标操作空间的阻抗参数分别为ho＝100e6，bo＝10e6，ko＝0.5e6，空间双臂机器人笛卡尔空间的阻抗参数分别为he＝20e12，be＝250e12，ke＝0.8e12，kif＝0.4e12。请参考图(a)，其为目标角速度控制曲线，实线为目标实际角速度曲线，虚线为目标期望角速度曲线，如图(a)所示，目标运动状态始终维持初始状态。请参考图(b)，其为目标内力控制曲线，实线为目标实际内力曲线，虚线为目标期望内力曲线，如图(b)所示，目标内力在t＝2.6s时即完成了对期望值内力的跟踪(内力误差为0.1n)，最终维持在10^-12n的数量级。使用本发明实施例提供的上述方法实现对静止目标的快速精确夹持。

请参考图4，其为目标初始旋转速度为ω0＝5°/s的控制效果图。设置目标期望内力为fi＝[0,10,0]，n目标操作空间的阻抗参数分别为ho＝100e6，bo＝10e6，ko＝0.5e6，空间双臂机器人笛卡尔空间的阻抗参数分别为he＝60e12，be＝300e12，ke＝0.8e12，kif＝0.4e12，库伦摩擦力系数μ＝0.1，接触表面的刚度系数σ0＝1000n/m，阻尼系数为σ1＝1n/(m/s)，混合阻抗控制中阻抗控制子空间选择矩阵为s＝diag{[0,0,1,0,0,0,0,0,1,0,0,0]}，力控制子空间选择矩阵为请参考图(a)，其为目标角速度控制曲线，实线为目标实际角速度曲线，虚线为目标期望角速度曲线，如图(a)所示，目标角速度能完成较好跟踪，跟踪误差维持在10^-6°/s的数量级。请参考图(b)，其为目标内力控制曲线，实线为目标实际内力曲线，虚线为目标期望内力曲线，如图(b)所示，目标的内力在t＝12.9s时即完成了对期望值内力的跟踪(内力误差为0.1n)，内力误差最终维持在10^-6n的数量级。使用本发明实施例提供的上述方法实现对目标的有效夹紧消旋操作。

请参考图5，其为目标初始旋转速度为ω0＝20°/s的控制效果图。设置目标期望内力为fi＝[0,0.5,0]n，目标操作空间的阻抗参数分别为ho＝100e6，bo＝10e6，ko＝0.5e6，空间双臂机器人笛卡尔空间的阻抗参数分别为he＝20e12，be＝diag{[100,200,100,100,100,100,100,200,100,100,100,100]}，ke＝0.8e12，kif＝diag{[0.8,6,0.8,0.8,0.8,0.8,0.8,6,0.8,0.8,0.8,0.8]}，库伦摩擦力系数μ＝0.1，接触表面的刚度系数σ0＝1000n/m，阻尼系数为σ1＝1n/(m/s)，混合阻抗控制中阻抗控制子空间选择矩阵为s＝diag{[0,0,1,0,0,0,0,0,1,0,0,0]}，力控制子空间选择矩阵为请参考图(a)，其为目标角速度控制曲线，实线为目标实际角速度曲线，虚线为目标期望角速度曲线，如图(a)所示，目标角速度的跟踪误差维持在10^-4°/s的数量级。请参考图(b)，其为目标内力控制曲线，实线为目标实际内力曲线，虚线为目标期望内力曲线，如图(b)所示，内力误差最终维持在10^-6n的数量级。使用本发明实施例提供的上述方法实现对目标的有效夹紧消旋操作。

表2三组实验结果对比

由表2中的数据可以看出，当要求在相同时间内完成翻滚目标角速度的衰减时，随着目标旋转速度的提高，最终角速度跟踪误差和力跟踪误差数量级逐渐增大。当初始旋转速度较小时(需求的加速度较小)，静摩擦力足以实现目标预期的角速度衰减，此时机械臂末端与翻滚目标表面无相对滑动，可以实现对抓捕点的实时稳定的跟踪；当初始旋转速度较大时，如ω0＝20°/s，若仍采取上述策略，机械臂将跟随目标转动达300.5°，此时双臂运动范围过大，可能直接导致双臂之间发生干涉，或是基座侧翻。采用相对运动产生的滑动摩擦力实现目标角速度衰减时，机械臂末端仅需运动0.17m，运动范围大幅下降，大大保障了抓捕过程的稳定性与可靠性。

无论机械臂末端与目标表面是否存在相对滑动，均可采用统一的多空间混合阻抗控制算法进行规划求解，最终速度误差均控制在10^-4°/s量级内，内力误差均控制在10^-6n量级内，能够较好满足目标捕获过程中的柔顺控制需求，证明了本发明实施例提供的多空间混合阻抗控制算法鲁棒性和普适性较强。

本发明实施例的技术方案具有以下有益效果：

建立的空间双臂机器人协调操作模型，综合考虑了末端与目标间的相对运动以及摩擦接触情况，能更加真实的反映实际情况；提出的多空间混合阻抗控制算法，可实现不同旋转状态的翻滚目标稳定可靠的消旋及夹紧，为故障卫星回收方法提供了新思路；提出的控制方法可进一步应用于其他在轨操作任务及研究领域中。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明保护的范围之内。

本发明说明书中未作详细描述的内容属本领域技术人员的公知技术。