[发明专利]一种适用于相对位姿测量的卡尔曼滤波方法

权利要求书

1.一种适用于相对位姿测量的卡尔曼滤波方法，其特征在于，包括以下步骤：

使用分别布置在运动载体与被测物体的两个IMU分别获取运动载体与被测物体的运动信息，并求解出的被测物体相对于运动载体的姿态数据；

基于包括所述姿态数据在内的状态量，建立卡尔曼滤波的状态误差方程；根据误差传播过程，更新卡尔曼滤波的误差状态协方差矩阵；

建立卡尔曼滤波的观测方程，依据外部观测传感器的观测量，更新状态协方差矩阵与状态向量；得到滤波后的被测物体与运动载体的相对位姿。

2.根据权利要求1所述的卡尔曼滤波方法，其特征在于，

在求解出的被测物体相对于运动载体的姿态数据时，采用角速度差分法进行姿态数据的求解；

在角速度差分法中，通过对当前时刻被测物体与运动载体的相对角速度进行积分得到被测物体与运动载体的相对姿态。

3.根据权利要求2所述的卡尔曼滤波方法，其特征在于，

所述角速度差分法，包括：

获取被测物体坐标系到运动载体坐标系的相对旋转矩阵；

根据被测物体与运动载体的IMU获取的角速度和上一时刻的相对旋转矩阵计算当前时刻相对角增量；

使用当前时刻相对角增量计算相对旋转四元数得到被测物体相对于运动载体的姿态数据。

4.根据权利要求2所述的卡尔曼滤波方法，其特征在于，

卡尔曼滤波的状态误差方程为：

F_X为状态转移矩阵；F_N为噪声转移矩阵；

X为系统的状态向量，

X＝[p_h-p，v_h-p，q_h-p，b_ha，b_pa，b_hω，b_pω]^T；

X中p_h-p、v_h-p、q_h-p定义为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；b_hω、b_ha为被测物体IMU测量角速度与加速度的零偏；b_pω、b_pa为运动载体IMU测量角速度与加速度的零偏；

为不含误差的状态向量，

ΔX为状态误差向量，

U为IMU测量向量，

U＝[a_h，ω_h，a_p，ω_p]^T；

a_h、ω_h、a_p、和ω_p分别为被测物体IMU加速度与角速度输出、运动载体IMU的加速度与角速度输出；

N为状态噪声向量，

n_ha、n_pa、n_hω、n_pω、和分别为被测物体IMU加速度噪声、运动载体IMU加速度噪声、被测物体IMU角速度噪声、运动载体IMU角速度噪声，被测物体IMU加速度零偏噪声、运动载体IMU加速度零偏噪声、被测物体IMU角速度零偏噪声、运动载体IMU角速度零偏噪声。

5.根据权利要求4所述的卡尔曼滤波方法，其特征在于，

卡尔曼滤波的状态协方差矩阵为

F_X为状态转移矩阵，

其中，Δt为采样时间间隔，为不含误差的被测物体到运动载体相对旋转矩阵

I为单位矩阵；

F_N为噪声转移矩阵，