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

说明书

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

技术领域

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

背景技术

利用单个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；