CN109048890A - 基于机器人的协调轨迹控制方法、系统、设备及存储介质 - Google Patents

Abstract

本发明公开了一种基于双臂空间机器人的协调轨迹控制方法，包括以下步骤：分别获取各机械臂末端点与抓捕点的相对位姿偏差；根据上述所得相对位姿偏差，判断是否满足抓捕条件；若满足，则对翻滚目标进行抓捕；若不满足，则根据所述相对位姿偏差，获得翻滚目标估计的线速度及角速度；根据速度分解法规划双臂末端的线速度及角速度；求得机械臂末端期望的关节角速度；驱动双臂各关节运动，完成所述翻滚目标的抓获。以及基于双臂空间机器人的协调轨迹控制系统、设备及可读存储介质。其实现了有效地抓捕未知空间非合作目标的目的。其较现有技术，提高了轨迹控制的精度，同时也提高了抓获目标的效率，可广泛应用于空间机器人控制领域。

Description

基于机器人的协调轨迹控制方法、系统、设备及存储介质

技术领域

本发明涉及空间机器人控制领域，具体为基于双臂空间机器人的协调轨迹控制方法、系统、设备及可读存储介质。

背景技术

随着空间探索的不断深入，空间失效/失控航天器越来越多，如何保障在轨航天器的正常运行成为航天科技关注的焦点。由于空间失效航天器大都处于翻滚状态，且产生不规则的运动，这对视觉测量以及空间抓捕操作造成了很大的障碍。

空间翻滚目标视觉测量过程中图像处理的速度受到CPU计算的限制，同时测量结果的输出频率不是很高，使得给出的视觉测量信息往往与翻滚目标当前的运动状态不一致，存在时延现象，因而需要有对翻滚目标的运动状态进行实时的预测与估计的有效算法。

针对空间翻滚目标复杂的运动特性，从像平面到三维空间抓捕点的映射关系是非线性的。传统的研究只考虑理想或简单的情况。假定目标围绕主惯性轴旋转，这样只有部分参数能被估计，惯性积常被忽略。在实际应用中，非合作翻滚目标的姿态，角速度和惯性参数对于轨迹规划和控制具有非常重要的意义。相比于UKF，EKF滤波效率会比较高，针对空间姿态动力学强非线性的特点，EKF的估计值与实际值的偏差将会很大，而UKF一定程度上可以避免了扩展卡尔曼滤波精度低以及容易迭代发散等问题。

为了提高参数估计效率的同时又兼顾估计精度，因此该技术有必要进行改进。

发明内容

为了解决上述技术问题中的至少一种，本发明的目的之一是提供一种精度较高的基于双臂空间机器人的协调轨迹控制方法、系统、设备及可读存储介质。

本发明所采用的技术方案是：

本发明提供一种基于双臂空间机器人的协调轨迹控制方法，包括以下步骤：

分别获取各机械臂末端点与抓捕点的相对位姿偏差；

根据上述所得相对位姿偏差，判断是否满足抓捕条件；

若满足，则对翻滚目标进行抓捕；

若不满足，则根据所述相对位姿偏差，获得翻滚目标估计的线速度及角速度；

根据速度分解法规划双臂末端的线速度及角速度；

求得机械臂末端期望的关节角速度；

驱动双臂各关节运动，完成所述翻滚目标的抓获。

作为该技术方案的改进，通过安装在双臂末端的手眼相机获取各机械臂末端点与抓捕点的相对位姿偏差。

作为该技术方案的改进，所述抓捕条件包括：机械臂末端在抓捕面上的投影位于抓捕区域内，且相对位姿偏差在预设阈值范围内。

作为该技术方案的改进，所述方法包括：根据所述相对位姿偏差，通过混合卡尔曼滤波器对所述相对位姿偏差进行参数估计，获得翻滚目标估计的线速度及角速度。

进一步地，所述步骤求得机械臂末端期望的关节角速度，其中，所述关节角速度的求解公式为：

其中，表示关节k的期望关节角速度，k为a或b；表示双臂空间机器人广义雅克比矩阵的伪逆；表示在末端坐标系中，机械臂末端的广义速度。

进一步地，t时刻的关节控制量可表示：

其中，Θ^k(t)为t时刻关节k的关节角；为t时刻关节k 的关节角速度。

另一方面，本发明还提供一种基于双臂空间机器人的协调轨迹控制系统，包括：

数据采集单元，用于执行步骤分别获取各机械臂末端点与抓捕点的相对位姿偏差；

抓捕判断单元，用于执行步骤根据上述所得相对位姿偏差，判断是否满足抓捕条件；

若满足，则对翻滚目标进行抓捕；

轨迹控制单元，用于执行步骤若不满足，则根据所述相对位姿偏差，获得翻滚目标估计的线速度及角速度；

根据速度分解法规划双臂末端的线速度及角速度；

求得机械臂末端期望的关节角速度；

驱动双臂各关节运动，完成所述翻滚目标的抓获。

再一方面，本发明还提供一种基于双臂空间机器人的协调轨迹控制设备，包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行所述的方法。

第四方面，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行所述的方法。

本发明的有益效果是：

本发明提供的一种基于双臂空间机器人的协调轨迹控制方法、系统、设备及可读存储介质，通过利用混合卡尔曼结合了EKF滤波速度快以及UKF迭代精度高的优点，使得最终的混合卡尔曼方法具有同 UKF相当的精度，但迭代速度比UKF快很多的优势；根据设置的协方差矩阵判断参数估计过程中基于计算精度与计算效率的考虑，实时切换EKF与UKF滤波器，通过运算处理得到空间非合作目标的运动参数与惯性参数，进一步地根据双臂末端点到抓捕点的相对位姿偏差，并将二者与设定的阈值作对比判断，在不满足阈值时通过计算双臂末端期望的广义速度，进而控制双臂末端的抓捕机构的运动，实现有效地抓捕未知空间非合作目标的目的。其较现有技术，提高了轨迹控制的精度，同时也提高了抓获目标的效率。

附图说明

下面结合附图对本发明的具体实施方式作进一步说明：

图1是本发明一实施例的基于双臂空间机器人的协调轨迹控制方法流程图；

图2是本发明一实施例的双臂末端与抓捕目标相对位姿关系示意图；

图3是本发明一实施例的双臂捕获目标卫星的结构模型示意图；

图4是本发明一实施例的混合卡尔曼滤波算法控制流程图；

图5是本发明一实施例的空间翻滚目标的参数估计示意图；

图6是本发明一实施例的空间失效卫星运动示意图；

图7是本发明一实施例的空间翻滚目标与机械臂末端的运动学关系耦合图；

图8是本发明一实施例的整个视觉测量与运动估计示意图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

参照图1-2，是本发明一实施例的基于双臂空间机器人的协调轨迹控制方法示意图。

基于双臂空间机器人的协调轨迹控制方法，其包括：

S1：根据安装在双臂末端的手眼相机测量，分别得到末端点与抓捕点的相对位姿偏差以及

S2：由S1的相对位姿偏差，对应于Arm-k,机械臂末端在抓捕面上的投影位于抓捕区域内且在阈值范围内即满足抓捕条件，可以对翻滚目标进行抓捕。若不满足条件，双臂的轨迹规划指令进行到下一步。

S3：由S1中得到的相对位姿偏差，通过混合卡尔曼滤波器对其进行参数估计，得到目标估计的线速度与角速度

S4：由S2中得到的相对位姿偏差和S3中得到的目标估计速度，再根据速度分解法规划双臂末端的线速度与角速度

S5：根据S4中得到的Arm-k机械臂末端规划的线速度和角速度，通过广义雅克比矩阵反解，求得Arm-k机械臂末端期望的关节角速度

S6：由S5中得到的关节角速度，驱动双臂各关节的运动，如果运行时间在规定时间以内，不断循环直至符合抓捕的容错范围才实施抓捕，完成对非合作目标卫星的捕获。

为了便于讨论定义了如下符号：

中连接与第i个关节；

的关节变量；

的线速度与角速度；

基座的姿态角；

机械臂末端的姿态角；

基座质心的广义速度；

末端的广义速度；

r₀：基座质心的位置矢量；

r_g:系统质心的位置矢量

ⁱA_j∈R^3×3:相对于的旋转矩阵；

ⁱI_i∈R^3×3:相对于质心的惯量矩阵；

的旋转矢量；

以及以及末端的位置矢量；

到以及到的位置矢量。

参照图3，是本发明一实施例的双臂捕获目标卫星的结构模型示意图。该服务卫星由空间基座B0和搭载基座上的串联机械臂n_a-DoF Arm-a与n_b-DoF Arm-b组成。在机械臂Arm-k(k＝a,b)末端都安装有测量目标位姿信息的手眼相机。

参照图4，是本发明的一个实施例的混合卡尔曼滤波器控制流程图。其中令阈值δ∈[0,1]，当k＝1，当k≥2，同时当前估计的状态可信度不高，采用 UKF完成下一阶段参数估计的迭代任务，否则选用EKF完成下一阶段任务，如此，每迭代一个周期进行一次协方差判断。为了防止初始迭代误差过大，假设第一步估计采用UKF来完成，后面的任务通过当前时刻协方差与初始协方差的判断条件来交叉切换EKF与UKF。

在UKF滤波算法求解过程中，权值系数分别为：

通常，作为分配预测值的先验知识，α,β为常值，通常取β＝2，而常值λ＝α²(n+k)-n，n为状态向量x_k的维数(如果n＝1，k＝2；否则，k＝3-n)。

参照图5，是本发明实施例的空间翻滚目标的参数估计示意图。参数估计主要包含姿态四元数(q₁,q₂,q₃,q₄)、角速度(w_x,w_y,w_z)以及惯量(主惯性参数I_xx,I_yy,I_zz和惯量积I_xy,I_xz,I_yz)。

参照图6，是本发明实施例的空间失效卫星运动示意图。设 O_b-X_bY_bZ_b为卫星本体坐标系，O_bt-X_btY_btZ_bt代表抓捕点的参考坐标系，坐标系从O_b-X_bY_bZ_b到O_bt-X_btY_btZ_bt的坐标变换矩阵^btR_b。

卫星的姿态参数可用轴角来描述，记为(e,φ)。其中，转轴的单位矢量e＝(e_x，e_y，e_z)是相对于参考坐标系O_bt-X_btY_btZ_bt矢量，φ表示绕此转轴的转角。

方向余弦矩阵^btR_b可由欧拉轴/角参数e和φ得到，即：

^btR_b＝I₃cosφ+(1-cosφ)ee^T-e^×sinφ (2)

式中，反对称矩阵

空间失效目标的姿态采用四元数来表示可以避免奇异的问题，即总能根据姿态四元数的变化率计算出姿态角速度。令向量常量为q₄，则姿态四元数

令则四元数与欧拉轴角的转换关系可表示为：

将(3)带入(2)可得到目标的姿态转换矩阵，即有：

假设目标的角速度为ω＝[ω_x ω_y ω_z]^T，则姿态四元数的微分可以表示为：

式中

假设M为航天器系统总质量，R₀为系统的质心矢量。由于 v₀与R₀两矢量的方向相同，整个刚体相对于点O的角动量为：

式中，

根据动量矩定理，外力矩可表示为：

式(7)即为欧拉力矩方程，它建立了作用力矩与失效目标角速度以及相对角动量变化率之间的关系。

将公式(6)代入公式(7)可以得到翻滚目标姿态动力学的一般方程为：

当目标绕着惯量主轴旋转时，既有I_xy＝I_xz＝I_yz＝0，此时I＝diag(I_xx, I_yy,I_zz)，公式(8)可简化为有：

假设外力矩τ＝[0,0,0]^T，由姿态动力学方程可得：

选取状态量为：

X＝[q^T,ω^T,I^T]^T (11)

令由公式(11)可知，系统的连续状态方程可表示为：

此时，状态方程的雅克比矩阵可表示为：

式中

状态转移矩阵为：

状态方程为：

式中，W_k-1代表系统过程噪声，它是均值为0，方差为Q_k的白噪声。

把姿态四元数和瞬时角速度作为观测值，即有：

h(X)＝[q₁,q₂,q₃,q₄,ω_x,ω_y,ω_z]^T (16)

那么，量测方程可表示为：

式中，V代表系统测量噪声，它是均值为0，方差为R_k的白噪声。

当目标绕惯量主轴旋转时，由姿态动力学方程可解得的表达式为：

选取状态量：

X＝[q^T,ω^T,I^T]^T (21)

状态方程的雅克比矩阵可表示为：

式中

状态转移矩阵可表示为：

离散化后的状态方程为

把姿态四元数和瞬时角速度作为观测值，即有：

h(X)＝[q₁,q₂,q₃,q₄,ω_x,ω_y,ω_z]^T (27)

离散化后的观测方程为

当目标的姿态与角速度估计出来了，被抓捕目标的线速度可以进一步估计出来。目标的几何外形通过三维重构获得，这时，目标本体上的发动机喷嘴、对接环以及太阳帆板的三角支架可作为机械臂进行抓捕的对象。

参照图7，是本发明实施例的空间翻滚目标与机械臂末端的运动学关系耦合图，这里O_t-X_tY_tZ_t是固连在目标上的坐标系。目标上各个轴的指向与O_bt-X_btY_btZ_bt坐标系一致，从O_bt-X_btY_btZ_bt初始坐标系到 O_t-X_tY_tZ_t的位置向量为r_t。

抓捕点相对于惯性系的位置向量可表示为：

根据式(29)，可以求得抓捕点的线速度为：

这里，ω_t是抓捕目标的角速度，它与混合卡尔曼滤波器估计出来的角速度一致，记为考虑到抓捕时间足够短，作用在目标上的外力可以忽略，于是目标的初始线速度接近于0，即v_bt≈0。因此，抓捕点线速度的估计值为：

至此，空间目标的线速度、角速度以及惯性参数都估计出来了，通过双臂的手眼相机实时测量机械臂末端与翻滚目标的相对位姿偏差，结合目标运动参数与惯性参数的初始值与估计值，可以规划出双臂末端期望的轨迹，从而实现双臂协同抓捕翻滚目标。

参照图8，是本发明实施例的整个视觉测量与运动估计示意图。对应于Arm-k，目标抓捕点与机械臂末端的旋转矩阵分别为与待抓捕的目标相对于机械臂末端的位姿向量为相对姿态偏差可表示为：

a)Arm-k末端速度规划

只采用速度进行规划会导致末端位姿的相对位姿偏差过大，期望的末端速度可以通过估计目标的速度进行补偿，即：

式中，K_p、K_v分别为比例、微分参数，分别为估计的目标线速度与角速度。

b)关节运动分解

刚体质心B_i’s的位置矢量为：

由于系统不受外力，系统的质心不变，即有：

将式(34)代入式(35)，可以得到基座质心的位置矢量，即有：

进一步地，Arm-k末端的位置矢量可表示为：

式中，

式(36)又可以表示为:

式中，

根据自由漂浮双臂空间机器人的速度级运动学方程，双臂空间机器人系统中Arm_k(k＝a,b)末端的广义速度可表示为：

式中，

其中，双臂空间机器人广义雅克比的四个分块矩阵分别为

然后，利用Arm-a与Arm-b的逆运动学方程，计算相应的关节角速度为：

c)t时刻关节角的计算

t时刻Arm-a与Arm-b关节控制量为：

通过公式(42)得到Arm-k的关节角数据，驱动双臂各关节的运动，直至机械臂末端与抓捕点的相对位姿同时满足启动抓捕指令，完成对目标卫星的抓捕操作。

本发明还提供一种基于双臂空间机器人的协调轨迹控制设备，包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行所述的方法。

再一方面，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行所述的方法。

本发明提供的一种基于双臂空间机器人的协调轨迹控制方法及系统，通过利用混合卡尔曼结合了EKF滤波速度快以及UKF迭代精度高的优点，使得最终的混合卡尔曼方法具有同UKF相当的精度，但迭代速度比UKF快很多的优势；根据设置的协方差矩阵判断参数估计过程中基于计算精度与计算效率的考虑，实时切换EKF与UKF滤波器，通过运算处理得到空间非合作目标的运动参数与惯性参数，进一步地根据双臂末端点到抓捕点的相对位姿偏差，并将二者与设定的阈值作对比判断，在不满足阈值时通过计算双臂末端期望的广义速度，进而控制双臂末端的抓捕机构的运动，实现有效地抓捕未知空间非合作目标的目的。其较现有技术，提高了轨迹控制的精度，同时也提高了抓获目标的效率。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。

Claims (9)

1.一种基于双臂空间机器人的协调轨迹控制方法，其特征在于，包括以下步骤：

分别获取各机械臂末端点与抓捕点的相对位姿偏差；

根据上述所得相对位姿偏差，判断是否满足抓捕条件；

若满足，则对翻滚目标进行抓捕；

若不满足，则根据所述相对位姿偏差，获得翻滚目标估计的线速度及角速度；

根据速度分解法规划双臂末端的线速度及角速度；

求得机械臂末端期望的关节角速度；

驱动双臂各关节运动，完成所述翻滚目标的抓获。

2.根据权利要求1所述的基于双臂空间机器人的协调轨迹控制方法，其特征在于：通过安装在双臂末端的手眼相机获取各机械臂末端点与抓捕点的相对位姿偏差。

3.根据权利要求1或2所述的基于双臂空间机器人的协调轨迹控制方法，其特征在于，所述抓捕条件包括：机械臂末端在抓捕面上的投影位于抓捕区域内，且相对位姿偏差在预设阈值范围内。

4.根据权利要求3所述的基于双臂空间机器人的协调轨迹控制方法，其特征在于，所述方法包括：根据所述相对位姿偏差，通过混合卡尔曼滤波器对所述相对位姿偏差进行参数估计，获得翻滚目标估计的线速度及角速度。

5.根据权利要求4所述的基于双臂空间机器人的协调轨迹控制方法，其特征在于，所述步骤求得机械臂末端期望的关节角速度，其中，所述关节角速度的求解公式为：

其中，表示关节k的期望关节角速度，k为a或b；表示双臂空间机器人广义雅克比矩阵的伪逆；表示在末端坐标系中，机械臂末端的广义速度。

6.根据权利要求5所述的基于双臂空间机器人的协调轨迹控制方法，其特征在于，t时刻的关节控制量可表示：

其中，Θ^k(t)为t时刻关节k的关节角；为t时刻关节k的关节角速度。

7.一种基于双臂空间机器人的协调轨迹控制系统，其特征在于，包括：

数据采集单元，用于执行步骤分别获取各机械臂末端点与抓捕点的相对位姿偏差；

抓捕判断单元，用于执行步骤根据上述所得相对位姿偏差，判断是否满足抓捕条件；

若满足，则对翻滚目标进行抓捕；

轨迹控制单元，用于执行步骤若不满足，则根据所述相对位姿偏差，获得翻滚目标估计的线速度及角速度；

根据速度分解法规划双臂末端的线速度及角速度；

求得机械臂末端期望的关节角速度；

驱动双臂各关节运动，完成所述翻滚目标的抓获。

8.一种基于双臂空间机器人的协调轨迹控制设备，其特征在于，包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行如权利要求1至6任一项所述的方法。

9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，所述计算机可执行指令用于使计算机执行如权利要求1至6任一项所述的方法。