CN114323011A - 一种适用于相对位姿测量的卡尔曼滤波方法 - Google Patents

Abstract

本发明涉及一种适用于相对位姿测量的卡尔曼滤波方法，其特征在于，包括以下步骤：使用分别布置在运动载体与被测物体的两个IMU分别获取运动载体与被测物体的运动信息，并求解出的被测物体相对于运动载体的姿态数据；基于包括所述姿态数据在内的状态量，建立卡尔曼滤波的状态误差方程；根据误差传播过程，更新卡尔曼滤波的误差状态协方差矩阵；建立卡尔曼滤波的观测方程，依据外部观测传感器的观测量，更新状态协方差矩阵与状态向量；得到滤波后的被测物体与运动载体的相对位姿。本发明通过状态误差卡尔曼滤波器对载体IMU的位姿误差与零偏、相对姿态测量的位姿误差与零偏进行修正，得到高精度的被测物体与运动载体的相对位姿。

Description

一种适用于相对位姿测量的卡尔曼滤波方法

技术领域

本发明涉及位姿测量和卡尔曼滤波技术领域，尤其涉及一种适用于相对位姿测量的卡尔曼滤波方法。

背景技术

利用单个IMU的信息可以解算出惯性系中物体实时位姿。利用被测物体的IMU信息以及被测物体所处于的运动载体的IMU信息，来获得目标坐标系下被测物体相对于参照物(运动载体)的相对位姿成为一种需求。例如移动载体的头戴显示系统的位姿获取。

由于惯性器件的自身特性，纯惯性位姿测量会随着传感器漂移而产生累积误差，需要在基于惯性器件的相对姿态测量中兼容外部观测量对惯性器件进行修正得到准确的测姿结果。

发明内容

鉴于上述的分析，本发明旨在提一种适用于相对位姿测量的卡尔曼滤波方法，实现被测物体相对于运动载体的姿态数据的准确测量。

本发明提供的技术方案是：

本发明公开了一种适用于相对位姿测量的卡尔曼滤波方法，包括以下步骤：

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

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

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

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

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

进一步地，所述角速度差分法，包括：

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

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

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

进一步地，卡尔曼滤波的状态误差方程为：

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角速度零偏噪声。

进一步地，卡尔曼滤波的状态协方差矩阵为

F_X为状态转移矩阵，

其中，Δt为采样时间间隔，

为不含误差的被测物体到运动载体相对旋转矩阵

I为单位矩阵；

F_N为噪声转移矩阵，

进一步地，所述更新卡尔曼滤波的误差状态协方差矩阵的过程包括：

1)获取被测物体、载体的IMU数据；

2)根据被测物体与载体运动模型更新状态向量

3)更新状态转移矩阵F_X、更新协方差矩阵F_NNF_N ^T；

4)更新误差状态协方差矩阵

进一步地，所述外部观测传感器观测量包括为位置观测量和角度观测量。

进一步地，卡尔曼滤波的观测方程中，

基于位置观测量的观测方程为Δz_p＝H_pΔX；其中，误差向量

H_p为位置量测矩阵；

基于角度观测量的观测方程Δz_q＝H_qΔX；其中，误差向量

H_q为位置量测矩阵。

进一步地，所述位置量测矩阵

旋转量测矩阵

式中，p_c-h为位置观测量，[p_c-h]_×为对应叉乘运算矩阵。

进一步地，所述更新状态协方差矩阵与状态向量的过程包括：

1)计算观测残差；

2)计算更新矩阵；

3)计算卡尔曼增益；

4)计算状态修正量；

5)计算状态协方差矩阵的递推结果；

6)将状态更新量与原本的状态向量叠加后得到更新后的状态向量。

本发明的有益效果：

本发明的基于双IMU差分的相对姿态测量方法，使用两个IMU的角速度输出与当前时刻的相对旋转矩阵，获取运动载体坐标系下当前时刻相对角速度，积分得到被测物体与运动载体的相对姿态，通过状态误差卡尔曼滤波器对载体IMU的位姿误差与零偏、相对姿态测量的位姿误差与零偏进行修正，得到高精度的被测物体与运动载体的相对位姿；该方法允许包括视觉测姿、无线电测姿在内的外部测姿结果通过卡尔曼滤波器引入观测，修正惯性测姿误差，具有更高精度。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为本发明实施例中的卡尔曼滤波方法流程图；

图2为本发明实施例中的运动载体中姿态测量的坐标系示意图；

图3为本发明实施例中的角速度差分法流程图；

图4为本发明实施例中的误差传播流程图；

图5为本发明实施例中的滤波器更新流程图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

本实施例所公开的一种适用于相对位姿测量的卡尔曼滤波方法，如图1所示，包括以下步骤：

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

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

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

具体的，在本实施例中，载体IMU与运动载体固连，可在惯性系自由运动，被测物体IMU固连在被测物体上，可以实现与载体的相对运动。

在本实施例中，如图2所示，运动载体IMU坐标系为p，被测物体IMU坐标系h，空间坐标系为i。

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

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

具体的，所述角速度差分法，如图3所示，包括：

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

被测物体坐标系到运动载体坐标系的相对旋转矩阵R_rel可通过对被测物体和运动载体的标定获得。

本实施例给出了被测物体相对运动载体的旋转四元数的递推过程，通过递推可知，通过惯性系中运动载体角速度ω_p(t)与惯性系中被测物体角速度ω_h(t)，以及递推得到被测物体相对运动载体的旋转四元数，其各自角速度解算后得到的姿态结果的差分，等同于他们差分角速度的姿态解算结果。具体的递推过程如下：

1)定义k时刻被测物体坐标系到运动载体坐标系的相对旋转为

得到相对旋转的表达式；

所述相对旋转有以下表达形式：

式中

为被测物体坐标系到空间坐标系的旋转四元数；

运为动载体坐标系到惯性坐标系的旋转四元数；

为四元数乘法，将微小旋转

与

变换运算顺序，在已知上一时刻相对旋转关系

的情况下，整理得到

2)使用

表示运动载体坐标系下观察t_k-1至t_k时刻被测物体IMU的旋转角度，带入式中，将指数函数的积变换为统一坐标系后差分旋转矩阵的指数函数有：

3)根据相对旋转的积分关系，相对旋转角速度的积分形式：

式中，

为t_k-1时刻相对旋转矩阵，相对旋转矩阵R_rel与相对旋转四元数Q_rel之间存在相互转换关系，可以采用现有公开的方法实现。

将

带入整理得到

与角速度相关的递推公式：

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

根据被测物体与运动载体的IMU获取角速度信息ω_h(t)、ω_p(t)与t_k-1时刻相对旋转矩阵

计算当前时刻t的相对角增量为：

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

用当前时刻相对角增量

计算相对旋转四元数

为

步骤S2中，建立的被测物体系统的卡尔曼滤波的状态向量为X＝{p_h-p,v_h-p,q_h-p,b_ha,b_pa,b_hω,b_pω}；

X中p_h-p、v_h-p、q_h-p为双IMU差分获得的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；b_hω、b_ha为被测物体IMU测量角速度与加速度的零偏；b_pω、b_pa为运动载体IMU测量角速度与加速度的零偏。其中，载体坐标系下被测物体相对载体的位置p_h-p、速度v_h-p可采用现有的技术方法获得，在此就不赘述。旋转四元数q_h-p为步骤S1中获得的姿态数据。

忽略杆臂效应时，真实情况下状态方程可以用下式表示：

式中，

为相对旋转矩阵；a_h、b_ha和n_ha为被测物体IMU加速度、加速度零偏和加速度噪声；a_p、b_pa、n_pa为运动载体IMU加速度、加速度零偏和加速度噪声；ω_h、b_hω、n_hω为被测物体IMU角速度、角速度零偏和角速度噪声，ω_p、b_pω、n_pω为运动载体IMU角速度、角速度零偏和角速度噪声；

为被测物体IMU加速度零偏噪声和角速度零偏噪声；

为运动载体IMU加速度零偏噪声和角速度零偏噪声。

为不含误差的状态向量；

定义为不含误差的载体坐标系下被测物体相对载体的位置、速度、旋转四元数；

为不含误差的被测物体IMU测量角速度与加速度的零偏；b_pω、b_pa为不含误差的运动载体IMU测量角速度与加速度的零偏；

则用于表示真实状态X与不含噪声状态

的状态误差向量ΔX为：

其中各项的具体展开如下：

根据建立当前时刻误差状态方程Δx与下一时刻状态误差

的递推关系

Δt为当前时刻到下一时刻的时间差。

得到，系统状态误差递推关系即状态误差方程为：

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

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

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

N为状态噪声向量，

其中，

和

分别为被测物体IMU加速度噪声、运动载体IMU加速度噪声、被测物体IMU角速度噪声、运动载体IMU角速度噪声，被测物体IMU加速度零偏噪声、运动载体IMU加速度零偏噪声、被测物体IMU角速度零偏噪声和运动载体IMU角速度零偏噪声的方差。

进一步地，

所述状态转移矩阵

其中，

为不含误差的被测物体到运动载体相对旋转矩阵；

I为单位矩阵；

噪声转移矩阵

则，系统的状态协方差矩阵为

基于上述过程，所述更新卡尔曼滤波的误差状态协方差矩阵的过程包括：

1)获取被测物体、载体的IMU数据；

2)根据被测物体与载体运动模型更新状态向量

3)更新状态转移矩阵F_X、更新协方差矩阵F_NNF_N ^T；

4)更新误差状态协方差矩阵

步骤S3中的卡尔曼滤波的观测方程中，外部观测传感器观测量包括为位置观测量和角度观测量。

基于位置观测量的观测方程为Δz_q＝H_pΔX；其中，误差向量

H_p为位置量测矩阵；

基于角度观测量的观测方程Δz_q＝H_qΔX；其中，误差向量

H_q为位置量测矩阵。

根据外部观测传感器测量结果，列写更新部分位置测量模型z_p；

其中，位置测量模型

式中，p_c-t代表传感器观测的位移，由视觉测量经过内参变化后得到；p_h-p、

为被测物体相对载体的平移向量与旋转矩阵；p_c-h为传感器相对被测物体的位置观测量，可通过标定得到；n_p为量测噪声。

则误差向量

展开有：

展开后忽略二阶项，得到：

根据观测方程Δz_p＝H_pΔx，位置量测矩阵H_p写作如下：

式中，p_c-h为位置观测量，[p_c-h]_×为对应叉乘运算矩阵。

根据外部观测传感器测量结果，列写更新部分角度测量模型z_q；

其中，角度测量模型

则误差向量

展开有

根据观测方程Δz_q＝H_qΔx，旋转量测矩阵H_q写作如下

由上述推导可以得到量测矩阵

如图5所示，所述更新状态协方差矩阵与状态向量的过程包括：

1)计算观测残差

2)计算更新矩阵S＝HPH^T+R；

3)计算卡尔曼增益K＝PH^TS^-1；

4)计算状态修正量

5)计算状态协方差矩阵的递推结果P←(I_d-KH)P(I_d-KH)^T+KRK^T。

6)将状态更新量

与原本的状态向量

叠加后得到更新后的状态向量。

在本实施例的一个优选方案中，IMU通过积分过程以较高采样频率获取变换矩阵T^imu，同时外部位姿传感器以较低速率获取变换矩阵T^cam。记连续几帧外部位姿传感器测姿更新得到的变换矩阵

与

(依次递推)，选取差分后IMU历史测姿信息中与外部位姿传感器获取时刻最接近的几帧惯性测姿变换矩阵

与

(依次递推)。则计算得到外部位姿传感器得到的相邻两帧相对位姿变换矩阵

相邻两帧最近时刻的惯性测量相对姿态变换矩阵

为求取相邻帧时间内视觉测量与惯性测量的相对偏差，定义

在李代数

上的对数映射的二范数D_k

计算k时刻前一定时间内，n组有效相对变换D_k均方根偏差基准RMSD_k。

判定，若新一帧外部姿传感器测量与惯性测量偏差D_k∈[-2RMSD_k,2RMSD_k]，则认为其是有外部位姿测量，对滤波器状态进行更新，同时该D_k计入均方根偏差基准的更新。否则认为是无效的外部位姿测量，丢弃不用，该D_k也不计入均方根偏差基准。

在本实施例另一个优选方案中，外部位姿传感器的数量包括多个，例如采用视觉测姿、无线电测姿和/或电磁测姿的外部位姿传感器；

每个外部位姿传感器按照上述更新过程接入卡尔曼滤波，修正惯性测姿误差，提高测量精度。

综上所述，本实施例基于双IMU差分的相对姿态测量方法，使用两个IMU的角速度输出与当前时刻的相对旋转矩阵，获取运动载体坐标系下当前时刻相对角速度，积分得到被测物体与运动载体的相对姿态，通过状态误差卡尔曼滤波器对载体IMU的位姿误差与零偏、相对姿态测量的位姿误差与零偏进行修正，得到高精度的被测物体与运动载体的相对位姿；该方法允许包括视觉测姿、无线电测姿在内的外部测姿结果通过卡尔曼滤波器引入观测，修正惯性测姿误差，具有更高精度。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

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为噪声转移矩阵，

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

所述更新卡尔曼滤波的误差状态协方差矩阵的过程包括：

1)获取被测物体、载体的IMU数据；

2)根据被测物体与载体运动模型更新状态向量

3)更新状态转移矩阵F_X、更新协方差矩阵F_NNF_N ^T；

4)更新误差状态协方差矩阵

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

所述外部观测传感器观测量包括为位置观测量和角度观测量。

8.根据权利要求7所述的卡尔曼滤波方法，其特征在于，卡尔曼滤波的观测方程中，

基于位置观测量的观测方程为Δz_p＝H_pΔX；其中，误差向量

H_p为位置量测矩阵；

基于角度观测量的观测方程Δz_q＝H_qΔX；其中，误差向量

H_q为位置量测矩阵。

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

所述位置量测矩阵

旋转量测矩阵

式中，p_c-h为位置观测量，[p_c-h]_×为对应叉乘运算矩阵。

10.根据权利要求2所述的卡尔曼滤波方法，其特征在于，所述更新状态协方差矩阵与状态向量的过程包括：

1)计算观测残差；

2)计算更新矩阵；

3)计算卡尔曼增益；

4)计算状态修正量；

5)计算状态协方差矩阵的递推结果；

6)将状态更新量与原本的状态向量叠加后得到更新后的状态向量。

Images