CN112497240B - 非刚性抓捕的目标状态观测方法、计算机设备及存储介质 - Google Patents

Abstract

本发明属于航天技术领域，具体公开了一种非刚性抓捕的目标状态观测方法，计算服务机器人在非刚性抓捕接触点所受外力大小；利用双目相机或者激光雷达对目标进行观测，获取姿态与位置数据；构建目标的模型，构建状态方程、观测方程；基于EKF方法，完成非合作目标状态估计，状态包括目标的位置、姿态、速度、角速度、质量、质心位置及转动惯量。本方法能在非合作目标‑末端执行器接触点存在相对运动的情况下对非合作目标的运动状态和质量惯性参数进行估计，且能够降低测量噪音和过程噪音的影响，并具有计算量较小的优点。

Description

非刚性抓捕的目标状态观测方法、计算机设备及存储介质

技术领域

本发明属于航天技术领域，特别涉及一种非刚性抓捕的目标状态观测方 法、计算机设备及存储介质。

背景技术

在空间任务中，经常采用机器人抓捕非合作目标。然而在抓捕目标的过程 中，存在因抓捕力有限而出现目标转动或者平动的状况，称之为非刚性抓捕。 服务机器人完成对非合作目标抓捕后，其抓捕点一般存在多个自由度的滑动或 转动，需要考虑力约束，基于传统测量信息输入的参数辨识方法误差大，无法 支持后续操控任务开展。同时相机、激光雷达对目标的位置测量有很大的误差， 且基于微分的方法获得的速度级状态误差极大。此时需要依据一定的方法，对 目标的运动状态、质量惯性参数的状态进行观测，从而为控制率设计提供数据 支持。

发明内容

本发明的目的在于提供一种非刚性抓捕的目标状态观测方法、计算机设备 及存储介质，能够在非刚性抓捕过程中，对目标进行质量参数估计、运动估计。

本发明是通过以下技术方案来实现：

一种非刚性抓捕的目标状态观测方法，包括以下步骤：

S1、计算服务机器人在非刚性抓捕接触点所受外力大小；

S2、对目标进行观测，获取目标的姿态与位置数据；

S3、构建目标的模型，构建状态方程和观测方程，得出非合作目标的状态；

S4、基于扩展卡尔曼滤波方法，构造估计方程与更新方程，从而完成非合 作目标状态估计。

进一步，步骤S1中，接触点的力与力矩计算如下：

其中，f_U为非合作目标所受外力，n_U为非合作目标所受外力矩，m_i为服务 机器人基座i臂质量，

为i臂质心线加速度，I_i为i臂转动惯量，ω_i为i臂角速 度，p_W为接触点坐标，r_i为i臂质心坐标，N为机械臂关节总数，f_E为环境外 力，τ_E为环境外力矩。

进一步，在步骤S2中，利用双目相机或激光雷达预先测量在服务机器人基 座的本体坐标系中接触点的位置，实时计算非合作目标相对相机的姿态。

进一步，步骤S3中，构建的观测方程如下：

其中，

为非合作目标相对相机的位置，q₁₃表示3坐标系相对于1坐标系 的姿态四元数，υ为观测误差。

进一步，在步骤S3中，构建非合作目标的状态方程如下：

其中，q₂为目标的姿态四元数，ω₂为目标角速度，p为目标主惯性坐标系的 惯性张量，ε_τ为干扰力矩，ε_f为干扰力，w为两者的向量，r_U为目标在惯性空间 矢量，m_U为目标质量，p_xx为目标在x-轴的惯性分量；

其中，

[·×]为叉乘运算；

其中，

p_xx、p_yy、p_zz分别是非合作目标的转动惯量在惯性主轴的三个分量。

进一步，步骤S3中，非合作目标的初始状态为：

分别为非合作目标的四元数矢量部分、角速度、 位置、速度、转动惯量的倒数及质量倒数，其中，

进一步，在步骤S4中，更新方程如下:

其中1₁₄表示14维的单位矩阵；

估计方程如下：

其中H_k为观测方程的一阶线性化矩阵，其求解方法为：

Q_k为离散状态方程的过程误差矩阵，其求解方法为：

其中，

Φ为状态转移矩阵，求解方法为：

本发明还公开了一种计算机设备，包括存储器、处理器以及存储在所述存 储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程 序时实现所述方法的步骤。

本发明还公开了一种计算机可读存储介质，所述计算机可读存储介质存储 有计算机程序，所述计算机程序被处理器执行时实现所述方法的步骤。

进一步，步骤S1中，所述服务机器人在非刚性抓捕接触点所受外力大小 为服务机器人对目标施加力的反力或者利用力传感器测量并计算获得的服务 机器人在接触点所受外力大小。

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

本发明针对机器人-目标之间的相对滑动，提出了一种非刚性抓捕的目标 状态观测方法，能够同时获得较为精确、可信的非合作目标的状态，输出的状 态包括获取目标的位置、姿态、速度和角速度，以及质量、惯性张量。能在非 合作目标-末端执行器接触点存在相对运动的情况下对非合作目标的运动状态 和质量惯性参数进行估计，本发明考虑测量噪音和过程噪音的影响，并且利用 滤波器降低了两种噪音的影响；本发明使用了EKF，从而保证该发明在高维状 态下计算量较小的优点，能够实时地对状态进行更新，算法鲁棒性更高。

附图说明

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

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

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

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

具体实施方式

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

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

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

本发明提供了一种非刚性抓捕的目标状态观测方法，在抓捕非合作目标之 后，非合作目标与服务机器人的手爪存在相对运动，且非合作目标受到外界干 扰力的影响。本专利提出一种方案，通过对非合作目标进行观测，获取带有噪 音的非合作目标相对运动状态，并基于EKF设计滤波器，从而估算出较为精 确的非合作目标的绝对质量、绝对转动惯量以及位置、姿态、速度、角速度等 状态。

如图1所示，本专利给定了一个非合作目标-机械臂-机器人基座组合体的 模拟结构图，具体包括机器人基座0，非合作目标7和双目相机9，机器人基 座0与非合作目标7之间依次设置有第一机器人臂1、第二机器人臂2、第三 机器人臂3、第四机器人臂4、第五机器人臂5和第六机器人臂6，相邻机器人 臂之间均通过机器人关节8连接，第六机器人臂6通过末端执行器10与非合 作目标7连接，双目相机9设置在第六机器人臂6上。

本发明公开了一种非刚性抓捕的目标状态观测方法，包括以下步骤：

S1、计算服务机器人在非刚性抓捕接触点所受外力大小：

其中，f_U为非合作目标所受外力，n_U为非合作目标所受外力矩，m_i为服务 机器人基座i臂质量，

为i臂质心线加速度，I_i为i臂转动惯量，ω_i为i臂角速 度，p_W为接触点坐标，r_i为i臂质心坐标，N为机械臂关节总数，f_E为环境外 力，τ_E为环境外力矩，同时已知外力干扰为白噪声，且其协方差矩阵为S。

S2、对目标进行观测，获取目标的姿态与位置数据：

如图3所示，相机安装于N臂上，相机需要测量

即相机指向目标基点 的矢量。双目相机利用现有技术，例如ICP技术，测量非合作目标相对于相机 的姿态。考虑服务机器人的观测误差，因此构建的观测方程如下：

为人工设定目标的本体坐标系系原点相对相机的位置，通常将1视作机器人基座的坐标系，2作为视点所固定的本体坐标系，将3作为目标质心的主惯性轴 坐标系，因此q₁₃表示3坐标系相对于1坐标系的姿态四元数，υ为观测误差，观 测误差为白噪声，其协方差矩阵为R。

S3、构建目标的模型，构建状态方程和观测方程，得出非合作目标的状态：

即状态包括目标的四元数矢量部分、角速度、位置、速度、转动惯量的倒 数、质量倒数。其中，

考虑外界干扰力，并构建非合作目标的状态方程如下：

也可以写作

其中，q₂为目标的姿态四元数，ω₂为目标角速度，p为目标主惯性坐标系的 惯性张量，ε_τ为干扰力矩，ε_f为干扰力，w为两者的向量，r_U为目标在惯性空间 矢量，m_U为目标质量，p_xx为目标在x-轴的惯性分量；

其中，

而[·×]为叉乘运算，且有

其中，p_xx、p_yy、p_zz分别是非合作目标在转动惯量在惯性主轴的三个分量。

S4、基于扩展卡尔曼滤波方法(简称EKF方法)，构造估计方程与更新方 程，从而完成非合作目标状态估计：根据EKF构建滤波方法，且有更新方程和 估计方程两部分组成。

更新方程如下:

其中1₁₄表示14维的单位矩阵；

估计方程如下：

其中H_k为观测方程的一阶线性化矩阵，其求解方法为：

Q_k为离散状态方程的过程误差矩阵，其求解方法为：

其中

而Φ为状态转移矩阵，求解方法为：

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

如图2所示，以图1所示的组合体为例展示了卫星各部分本体坐标系基点、 质心、以及固有构型与速度之间的关系。安装于基座的惯性元件可以测量基座 的加速度

与角加速度

而每个关节所安装的电位计可以测量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_S,W在N臂本体坐标系 下是时不变的，因而可以在地面得到精确的测量值。相机需要测量

即相 机指向目标基点的矢量，双目相机利用现有技术，例如ICP技术，测量非合作 目标相对于相机的姿态。

本发明公开的一种非刚性抓捕的目标状态观测方法如果以软件功能单元 的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存 储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程， 也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于 一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个 方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机 程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。 计算机可读存储介质包括永久性和非永久性、可移动和非可移动媒体可以由任 何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序 的模块或其他数据。需要说明的是，所述计算机可读介质包含的内容可以根据 司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖 区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

在示例性实施例中，还提供了一种计算机可读存储介质，所述计算机可读 存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述彩色图 像可逆信息隐藏方法的步骤。其中，所述计算机存储介质可以是计算机能够存 取的任何可用介质或数据存储设备，包括但不限于磁性存储器(例如软盘、硬 盘、磁带、磁光盘(MO)等)、光学存储器(例如CD、DVD、BD、HVD等)、以 及半导体存储器(例如ROM、EPROM、EEPROM、非易失性存储器(NANDFLASH)、固态硬盘(SSD))等。

在示例性实施例中，还提供了一种计算机设备，包括存储器、处理器以及 存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行 所述计算机程序时实现所述彩色图像可逆信息隐藏方法的步骤。处理器可能是 中央处理单元(CentralProcessingUnit，CPU)，还可以是其他通用处理器、数字 信号处理器(DigitalSignalProcessor，DSP)、专用集成电路 (ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列 (Field-ProgrammableGateArray，FPGA)或者其他可编程逻辑器件、分立门或者 晶体管逻辑器件、分立硬件组件等。

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

Claims (7)

1.一种非刚性抓捕的目标状态观测方法，其特征在于，包括以下步骤：

S1、计算服务机器人在非刚性抓捕接触点所受外力大小；

S2、对目标进行观测，获取目标的姿态与位置数据；

S3、构建目标的模型，构建状态方程和观测方程，得出非合作目标的状态；

S4、基于扩展卡尔曼滤波方法，构造估计方程与更新方程，从而完成非合作目标状态估计；

在步骤S3中，构建非合作目标的状态方程如下：

其中，q₂为目标的姿态四元数，ω₂为目标角速度，p为目标主惯性坐标系的惯性张量，ε_τ为干扰力矩，ε_f为干扰力，w为两者的向量，r_U为目标在惯性空间矢量，m_U为目标质量，p_xx为目标在x-轴的惯性分量，f_U是非合作目标所受的外力，v_U是目标质心的速度，p_W为接触点的坐标；

其中，

[·×]为叉乘运算；

其中，

p_xx、p_yy、p_zz分别是非合作目标的转动惯量在惯性主轴的三个分量；

步骤S3中，非合作目标的状态为：

分别为非合作目标的四元数矢量部分、角速度、位置、速度、转动惯量的倒数及质量倒数，其中，

在步骤S4中，更新方程如下:

其中1₁₄表示14维的单位矩阵；

估计方程如下：

其中H_k为观测方程的一阶线性化矩阵，其求解方法为：

Q_k为离散状态方程的过程误差矩阵，其求解方法为：

其中，G(x)的形式为：

Φ为状态转移矩阵，求解方法为：

2.根据权利要求1所述的一种非刚性抓捕的目标状态观测方法，其特征在于，步骤S1中，接触点的力与力矩计算如下：

其中，f_U为非合作目标所受外力，n_U为非合作目标所受外力矩，m_i为服务机器人基座i臂质量，

为i臂质心线加速度，I_i为i臂转动惯量，ω_i为i臂角速度，p_W为接触点坐标，r_i为i臂质心坐标，N为机械臂关节总数，f_E为环境外力，τ_E为环境外力矩。

3.根据权利要求1所述的一种非刚性抓捕的目标状态观测方法，其特征在于，在步骤S2中，利用双目相机或激光雷达预先测量在服务机器人基座的本体坐标系中接触点的位置，实时计算非合作目标相对相机的姿态。

4.根据权利要求1所述的一种非刚性抓捕的目标状态观测方法，其特征在于，步骤S3中，构建的观测方程如下：

其中，

为非合作目标相对相机的位置，q₁₃表示3坐标系相对于1坐标系的姿态四元数，υ为观测误差。

5.根据权利要求1所述的一种非刚性抓捕的目标状态观测方法，其特征在于，步骤S1中，所述服务机器人在非刚性抓捕接触点所受外力大小为服务机器人对目标施加力的反力或者利用力传感器测量并计算获得的服务机器人在接触点所受外力大小。

6.一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至5任一项所述方法的步骤。

7.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述方法的步骤。

Images