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

权利要求书

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任一项所述方法的步骤。