CN109093620A - 一种双目相机辅助的空间非合作目标动力学参数辨识方法 - Google Patents

Abstract

本发明公开了一种双目相机辅助的空间非合作目标动力学参数辨识方法，根据本体加速度、角加速度、关节角以及服务卫星的本体几何特征，通过积分、微分，求取在惯性参考系下的本体速度、角速度、关节角速度以及关节位置速度；由动力学公式计算服务卫星在接触点所受外力大小，即为服务卫星对目标施加力的反力；构建目标的模型，在目标固有参考系下构建力与加速度的关系等式；循环进行多次采样，利用最小二乘优化法获得目标的参数，完成空间非合作目标动力学参数辨识。本方法能在非合作目标‑末端执行器接触点存在相对运动的情况下对非合作目标质量特征参数进行精确的辨识，且能够降低噪音的影响，并具有计算量较小的优点。

Description

一种双目相机辅助的空间非合作目标动力学参数辨识方法

技术领域

本发明属于航天技术领域，具体涉及一种双目相机辅助的空间非合作目标动力学参数辨识方法。

背景技术

随着航天技术的发展，在轨服务技术日益受到人们的关注，在轨服务最关键的一项技术是空间机器人对非合作目标的抓捕，它是空间碎片移除、故障卫星维修、失效卫星再利用等一系列任务的基础技术。空间非合作目标抓捕分为三个阶段——抓捕前逼近、碰撞、抓捕后稳定。在第三个阶段，装备有空间机械臂的服务卫星需要完成组合体稳定，而抓捕前对非合作目标的运动学重建以及参数估计已经无法满足精确控制的要求，因此需要在抓捕后对非合作目标完成进一步的动力学参数辨识，从而为服务星-目标星组合体的稳定控制提供良好的基础。

针对空间非合作目标抓捕设计的通用抓捕机构，很难实现对未知的抓捕点的完全稳固抓捕，尤其是利用单臂空间机器人的抓捕，极易出现抓捕后非合作目标相对空间机器人末端执行器滑动的现象。常规的参数辨识方法假定接触点是牢固的，若在出现接触点滑动的情况下继续采用常规的参数辨识方法，估算的参数将会出现极大的误差，从而导致稳定控制器失效，最终产生组合体翻滚的现象。

发明内容

本发明所要解决的技术问题在于针对上述现有技术中的不足，提供一种双目相机辅助的空间非合作目标动力学参数辨识方法，将双目相机的运动重建引入参数辨识中，提高参数辨识的精度。

本发明采用以下技术方案：

一种双目相机辅助的空间非合作目标动力学参数辨识方法，包括以下步骤：

S1、利用星上加速计、陀螺仪测量服务卫星本体的加速度角加速度利用编码器测量机械臂关节角θ_i，i＝1:N,N＝6；

S2、利用双目摄像机计算目标固有坐标系相对末端执行器固有坐标系的方向余弦矩阵，以及某个特征点相对接触点的坐标；

S3、根据本体加速度、角加速度、关节角以及服务卫星的本体几何特征，通过积分、微分，求取在惯性参考系下的本体速度、角速度、关节角速度以及关节位置速度；

S4、由动力学公式计算服务卫星在接触点所受外力大小，即为服务卫星对目标施加力的反力；

S5、构建目标的模型，在目标固有参考系下构建力与加速度的关系等式；

S6、循环进行多次采样，利用最小二乘优化法获得目标的参数，完成空间非合作目标动力学参数辨识。

具体的，步骤S2中，通过旋转矩阵^U(t)R_U(t+Δt)与平移矩阵^U(t)T_U(t+Δt)检验目标与末端执行器是否存在位移，双目相机预先测量在N关节本体坐标系中，接触点相对相机的位置，即^Nr_S,W，并实时计算在N关节本体坐标系中，人工设定非合作目标本体系原点相对相机的位置，即

进一步的，利用左右双相机分别在时刻t和t+Δt同时采集两组图像，通过同一时刻左右两图像间同一特征以及相位坐标分别求取同一点在时刻t和t+Δt的三维坐标P和P′；利用N_p(N_p≥3)个不共面的点在时刻t和t′的坐标(P_i,P_i′)，旋转矩阵^U(t)R_U(t+Δt)与平移矩阵^U(t)T_U(t+Δt)如下：

具体的，步骤S3中，根据已知的基座基点加速度基座角加速度基座基点到质心矢量a₀，基座质心到关节矢量b₀，i关节指向i臂质心矢量a_i，i臂质心指向i+1关节的矢量b_i，i关节的关节角θ_i(i＝1:N,N＝6)，以及相应的初始条件，根据运动学公式求得基座基点坐标p₀，基座基点基座质心坐标r₀，基座质心线速度基座质心线加速度基座相对惯性参考系余弦坐标矩阵^IR₀，基座角速度ω₀，i关节坐标p_i，i关节线速度i关节线加速度i臂质心坐标r_i，i臂线质心速度i臂质心线加速度i臂本体坐标系相对于惯性坐标系的余弦坐标矩阵^IR_i，i关节旋转角速度i关节角加速度接触点坐标p_W，接触点线速度接触点线加速度

具体的，步骤S4中，接触点的力与力矩计算如下：

其中，f_U为非合作目标所受外力，n_U为非合作目标所受外力矩，m_i为i臂质量，为i臂质心线加速度，I_i为i臂转动惯量，ω_i为i臂角速度，p_W为接触点坐标，r_i为i臂质心坐标，N为机械臂关节总数。

具体的，步骤S5中，根据力与加速度的关系等式得到非合作目标质量m_U，在非合作目标本体坐标系下基点指向质心的矢量在非合作目标本体坐标系下目标转动惯量^UI_U的参数，力与加速度的关系等式如下：

其中，矢量左上标有U表示该矢量表示在非合作目标本体坐标系下，r_SW为相机指向接触点的矢量，是相机指向接触点的矢量，为目标本体坐标基点线加速度，向量右上角为叉乘变换，若

则有

具体的，步骤S6中，将时间k时刻带入力与加速度的关系等式，在辨识阶段的采样次数一共为N_k，得到minΔ，最终求出非合作目标的参数minΔ如下：

其中，矢量左上标有U表示该矢量表示在非合作目标本体坐标系下，r_SW为相机指向接触点的矢量，是相机指向接触点的矢量，为目标本体坐标基点线加速度，向量右上角为叉乘变换，m_U为非合作目标质量，f_U为非合作目标所受外力，n_U为非合作目标所受外力矩，

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

本发明针对空间机器人末端执行器-非合作目标之间的相对滑动，提出了双目相机辅助的空间非合作目标的动力学参数辨识方法，对相对滑动的分析符合工程实际，计算具有实时性、快速性，且通过多次取样计算，利用最优化算法降低因测量误差导致的结果误差。

进一步的，利用双目摄像机计算目标固有坐标系相对末端执行器固有坐标系的方向余弦矩阵，以及某个特征点相对接触点的坐标，目的是：1)检测目标与末端执行器是否存在相对滑动或者相对转动；2)确定目标与末端执行器之间存在相对滑动或者相对转动时，利用滤波算法计算得到精确的相对运动信息。该步骤可以避免相对位移对后续的参数辨识带来不可预知的影响，从而大大提高辨识精度。

进一步的，根据本体加速度、角加速度、关节角以及服务卫星的本体几何特征，通过积分、微分，求取在惯性参考系下的本体速度、角速度、关节角速度以及关节位置速度。该步骤目的是利用星敏感器以及空间机器人固有构型得到关节、质心的位置、速度以及加速度。该步骤快速可靠。

进一步的，由动力学公式计算服务卫星在接触点所受外力大小，即为服务卫星对目标施加力的反力。该步骤目的是利用星敏感器以及固有质量参数得到目标所受力与力矩之和。该方法避免使用易导致结构振动的力矩传感器，而利用卫星惯性导航系统的数据，具有很好的稳定性。

进一步的，构建目标的模型，在目标固有参考系下构建力与加速度的关系等式。该步骤目的是通过欧拉牛顿法建立目标的动力学模型，并在动力学方程中提取非合作目标的质量参数。该方法计算精度高，且计算量较小。

进一步的，循环进行多次采样，利用最小二乘优化法获得目标的参数，完成空间非合作目标动力学参数辨识。该方法目的是降低观测误差带来的噪音。优势是采用最小二乘法计算量小，能大幅提高参数准确性。

综上所述，本方法能在非合作目标-末端执行器接触点存在相对运动的情况下对非合作目标质量特征参数进行精确的辨识，且能够降低噪音的影响，并具有计算量较小的优点。

下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。

附图说明

图1为非合作目标-机械臂-卫星基座组合体示意图；

图2为机械臂-卫星基座的在惯性参考系下的运动关系以及受力状态图；

图3为末端执行器与非合作目标的几何关系图。

其中：0.卫星基座；1.第一机器人臂；2.第二机器人臂；3.第三机器人臂；4.第四机器人臂；5.第五机器人臂；6.第六机器人臂；7.非合作目标；8.机器人关节；9.双目相机；10.末端执行器。

具体实施方式

在本发明的描述中，需要理解的是，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

本发明以安装在N臂上的双目相机为例，但实际不仅限于安装在N臂，可以安装在任一一个臂上，或者卫星基座，原因是无论安装在卫星哪个位置，其辨识原理与该方法具有一致性，仅需对给出算法做简单修改。

本发明提供了一种双目相机辅助的空间非合作目标动力学参数辨识方法，空间机器人完成对非合作目标抓捕后，其抓捕点一般存在多个自由度的滑动或转动，需要考虑力约束，基于传统测量信息输入的参数辨识方法误差大，无法支持后续操控任务开展。针对这个问题，本发明基于双目视觉信息估算抓捕完成后目标滑动和转动造成的相对位姿扰动，与星敏测量参数融合后，作为运动参数输入参数辨识算法，解算得到精确、可信的非合作目标质量特性参数。

请参阅图1，非合作目标-机械臂-卫星基座组合体包括卫星基座0，非合作目标7和双目相机9，卫星基座0与非合作目标7之间依次设置有第一机器人臂1、第二机器人臂2、第三机器人臂3、第四机器人臂4、第五机器人臂5和第六机器人臂6，每个机器人臂之间均通过机器人关节8连接，第六机器人臂6通过末端执行器10与非合作目标7连接，双目相机9设置在第六机器人臂6上。

本发明一种双目相机辅助的空间非合作目标动力学参数辨识方法，包括以下步骤：

S1、利用星上加速计、陀螺仪测量服务卫星本体的加速度角加速度利用编码器测量机械臂关节角θ_i，i＝1:N,N＝6；

S2、利用双目摄像机计算目标固有坐标系相对末端执行器固有坐标系的方向余弦矩阵，以及某个特征点相对接触点的坐标；

双目相机于固定在服务卫星上，本发明以固定在末端执行器上的双目相机为例，利用双目相机计算目标固有坐标系相对末端执行器固有坐标系的方向余弦矩阵，以及选取某一特征点，测量其三维坐标。

^Co1R_Co2表示Co1坐标系相对于Co2坐标系的余弦坐标矩阵，^Co1T_Co2表示Co1坐标系相对于Co2坐标系的平移向量，向量左上标为I即为在惯性坐标系下表述，左上标为i即为在i臂本体坐标系下表述，左上标为N即为在N臂本体坐标系下表述，左上标为U即为在目标本体坐标系下表述。

利用左右双相机分别在时刻t和t+Δt同时采集两组图像，通过同一时刻左右两图像间同一特征以及它们的相位坐标分别求取同一点在时刻t和t+Δt的三维坐标P和P′；利用N_p(N_p≥3)个不共面的点在时刻t和t′的坐标(P_i,P_i′)，可以通过下式找到旋转矩阵R与平移矩阵T。

P_i′＝^U(t)R_U(t+Δt)P_i+^U(t)T_U(t+Δt) (1)

则通过

获得上述公式旋转矩阵^U(t)R_U(t+Δt)与平移向量^U(t)T_U(t+Δt)，通过这两个矩阵可以检验目标与末端执行器是否存在位移。

同时，如图3所示，双目相机预先测量^Nr_S,W，即接触点相对相机的相对位置，并实时计算

本步骤可检验抓捕目标与末端执行器之间是否存在相对转动、平动。

S3、根据本体加速度、角加速度、关节角以及服务卫星的本体几何特征，通过积分、微分，求取在惯性参考系下的本体速度、角速度、关节角速度以及关节位置速度；

由已知的基座基点加速度基座角加速度基座基点到质心矢量a₀，基座质心到关节矢量b₀，i关节指向i臂质心矢量a_i，i臂质心指向i+1关节的矢量b_i，i关节的关节角θ_i(i＝1:N,N＝6)，以及相应的初始条件，根据运动学公式可以求得基座基点坐标p₀，基座基点基座质心坐标r₀，基座质心线速度基座质心线加速度基座相对惯性参考系余弦坐标矩阵^IR₀，基座角速度ω₀，i关节坐标p_i，i关节线速度i关节线加速度i臂质心坐标r_i，i臂线质心速度i臂质心线加速度i臂本体坐标系相对于惯性坐标系的余弦坐标矩阵^IR_i，i关节旋转角速度i关节角加速度接触点坐标p_W，接触点线速度接触点线加速度

S4、由动力学公式计算服务卫星在接触点所受外力大小，即为服务卫星对目标施加力的反力；

由于卫星基座、机械臂的质量、质心位置、惯性参数均为已知参数，则由动力学方程求接触点的力与力矩

其中，f_U为非合作目标所受外力，n_U为非合作目标所受外力矩，m_i为i臂质量，为i臂质心线加速度，I_i为i臂转动惯量，ω_i为i臂角速度，p_W为接触点坐标，r_i为i臂质心坐标，N为机械臂关节总数。

S5、构建目标的模型，在目标固有参考系下构建力与加速度的关系等式；

由

其中，r_U为非合作目标质心在惯性系下的坐标向量，有

r_U＝p_W-r_CW (5)

其中

考虑到坐标变换，有

^IR_U(t)＝^IR_N(t) ^N(t)R_U(t) (7)

ω_E,U为目标相对于末端执行器的角速度，则

^Uω_U＝^UR_I ^Iω_N+^Uω_N,U (10)

^Iω_N即为N臂在惯性空间中的角速度，在上文中写作ω_N。

则^Ip_W、^Nr_SW、及其对时间的一二阶导数均可推算，通过^IR_U(t)、^NR_U(t)的坐标变换到Σ_U(t)中，从而可得

其中

且有

则根据公式(11)(13)可以得到m_U， ^UI_U的参数。

其中，矢量左上标有U表示该矢量表示在非合作目标本体坐标系下，r_SW为相机指向接触点的矢量，是相机指向接触点的矢量，为目标本体坐标基点线加速度。另外，向量右上角为叉乘变换，即

S6、循环进行多次采样，利用最小二乘优化法获得目标的参数。

将在时间为k时刻的(11)(13)写作

A_k＝B_kC (14)

在辨识阶段的采样次数一共为N_k，则有

最终求出非合作目标的参数。

本发明首次考虑到空间机器人与非合作目标之间的相对滑动进行动力学参数辨识的研究，并将双目相机的运动重建引入参数辨识算法中，接触点滑动的本质是空间机器人末端执行器与非合作目标之间不再是单一的位置约束，而是变为位置/力混合约束甚至单一力约束，这个是目前的研究所忽略的方面。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。通常在此处附图中的描述和所示的本发明实施例的组件可以通过各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图2，图2为卫星基座-机械臂结构示意图，展示了卫星各部分本体坐标系基点、质心、以及固有构型与速度之间的关系。在基座的惯性元件可以测量基座基点加速度基座角加速度而每个关节所安装的电位计可以测量i关节的关节角θ_i(i＝1:N,N＝6)，根地面测量的基座基点到质心矢量a₀，基座质心到关节矢量b₀，i关节指向i臂质心矢量a_i，i臂质心指向i+1关节的矢量b_i，以及位置和速度的初始条件，根据经典的D-H建模方法的运动学公式可以求得基座基点坐标p₀，基座基点基座质心坐标r₀，基座质心线速度基座质心线加速度基座相对惯性参考系余弦坐标矩阵^IR₀，基座角速度ω₀，i关节坐标p_i，i关节线速度i关节线加速度i臂质心坐标r_i，i臂线质心速度i臂质心线加速度i臂本体坐标系相对于惯性坐标系的余弦坐标矩阵^IR_i，i关节旋转角速度i关节角加速度接触点坐标p_W，接触点线速度接触点线加速度

请参阅图3，图3为目标-相机-N臂位置关系。考虑到相机安装于N臂上，因而，相机与N臂之间余弦坐标矩阵为单位矩阵，r_SW在N臂本体坐标系下是时不变的，因而可以在地面得到精确的测量值。相机需要精确地测量即相机指向目标基点的矢量，并结合双目相机对目标姿态的变化，得到目标相对N臂的姿态角变化或者余弦坐标矩阵随时间变化的情况。从而补偿因滑动导致的运动信息损失的情况。

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

Claims (7)

1.一种双目相机辅助的空间非合作目标动力学参数辨识方法，其特征在于，包括以下步骤：

S1、利用星上加速计、陀螺仪测量服务卫星本体的加速度角加速度利用编码器测量机械臂关节角θ_i，i＝1:N,N＝6；

S2、利用双目摄像机计算目标固有坐标系相对末端执行器固有坐标系的方向余弦矩阵，以及某个特征点相对接触点的坐标；

S3、根据本体加速度、角加速度、关节角以及服务卫星的本体几何特征，通过积分、微分，求取在惯性参考系下的本体速度、角速度、关节角速度以及关节位置速度；

S4、由动力学公式计算服务卫星在接触点所受外力大小，即为服务卫星对目标施加力的反力；

S5、构建目标的模型，在目标固有参考系下构建力与加速度的关系等式；

S6、循环进行多次采样，利用最小二乘优化法获得目标的参数，完成空间非合作目标动力学参数辨识。

2.根据权利要求1所述的一种双目相机辅助的空间非合作目标动力学参数辨识方法，其特征在于，步骤S2中，通过旋转矩阵^U(t)R_U(t+Δt)与平移矩阵^U(t)T_U(t+Δt)检验目标与末端执行器是否存在位移，双目相机预先测量在N关节本体坐标系中，接触点相对相机的位置，即^Nr_S,W，并实时计算在N关节本体坐标系中，人工设定非合作目标本体系原点相对相机的位置，即

3.根据权利要求2所述的一种双目相机辅助的空间非合作目标动力学参数辨识方法，其特征在于，利用左右双相机分别在时刻t和t+Δt同时采集两组图像，通过同一时刻左右两图像间同一特征以及相位坐标分别求取同一点在时刻t和t+Δt的三维坐标P和P′；利用N_p(N_p≥3)个不共面的点在时刻t和t′的坐标(P_i,P_i′)，旋转矩阵^U(t)R_U(t+Δt)与平移矩阵^U(t)T_U(t+Δt)如下：

4.根据权利要求1所述的一种双目相机辅助的空间非合作目标动力学参数辨识方法，其特征在于，步骤S3中，根据已知的基座基点加速度基座角加速度基座基点到质心矢量a₀，基座质心到关节矢量b₀，i关节指向i臂质心矢量a_i，i臂质心指向i+1关节的矢量b_i，i关节的关节角θ_i(i＝1:N,N＝6)，以及相应的初始条件，根据运动学公式求得基座基点坐标p₀，基座基点基座质心坐标r₀，基座质心线速度基座质心线加速度基座相对惯性参考系余弦坐标矩阵^IR₀，基座角速度ω₀，i关节坐标p_i，i关节线速度i关节线加速度i臂质心坐标r_i，i臂线质心速度i臂质心线加速度i臂本体坐标系相对于惯性坐标系的余弦坐标矩阵^IR_i，i关节旋转角速度i关节角加速度接触点坐标p_W，接触点线速度接触点线加速度

5.根据权利要求1所述的一种双目相机辅助的空间非合作目标动力学参数辨识方法，其特征在于，步骤S4中，接触点的力与力矩计算如下：

其中，f_U为非合作目标所受外力，n_U为非合作目标所受外力矩，m_i为i臂质量，为i臂质心线加速度，I_i为i臂转动惯量，ω_i为i臂角速度，p_W为接触点坐标，r_i为i臂质心坐标，N为机械臂关节总数。

6.根据权利要求1所述的一种双目相机辅助的空间非合作目标动力学参数辨识方法，其特征在于，步骤S5中，根据力与加速度的关系等式得到非合作目标质量m_U，在非合作目标本体坐标系下基点指向质心的矢量在非合作目标本体坐标系下目标转动惯量^UI_U的参数，力与加速度的关系等式如下：

其中，矢量左上标有U表示该矢量表示在非合作目标本体坐标系下，r_SW为相机指向接触点的矢量，是相机指向接触点的矢量，为目标本体坐标基点线加速度，向量右上角为叉乘变换，若

则有

7.根据权利要求1所述的一种双目相机辅助的空间非合作目标动力学参数辨识方法，其特征在于，步骤S6中，将时间k时刻带入力与加速度的关系等式，在辨识阶段的采样次数一共为N_k，得到minΔ，最终求出非合作目标的参数minΔ如下：

其中，矢量左上标有U表示该矢量表示在非合作目标本体坐标系下，r_SW为相机指向接触点的矢量，是相机指向接触点的矢量，为目标本体坐标基点线加速度，向量右上角为叉乘变换，m_U为非合作目标质量，f_U为非合作目标所受外力，n_U为非合作目标所受外力矩，