[发明专利]一种多传感器融合位姿估计方法

权利要求书

1.一种多传感器融合位姿估计方法，其特征在于，包括以下步骤：

步骤1：建立载体坐标系与导航坐标系，对IMU设备输出建模，确定IMU运动学模型；

步骤2：输入激光雷达与IMU各自的自由度信息，基于扩展卡尔曼滤波进行融合姿态估计；

步骤3：不断迭代更新步骤2实现滤波。

2.根据权利要求1所述的一种多传感器融合位姿估计方法，其特征在于，所述步骤1中建立坐标系具体为：

确定坐标系，常用坐标系分为：载体坐标系与导航坐标系，使用载体的质心表示载体坐标系的原点，坐标轴指向载体的正前方，正右方和正下方；导航坐标系指向东北天方向；载体坐标系变换到导航坐标系的旋转采用方向余弦矩阵表示，记为导航坐标系先绕俯仰轴转θ，再绕滚动轴旋转/最后绕偏航角转/实现到载体坐标系的变换，余弦矩阵通过下式表示：

3.根据权利要求2所述的一种多传感器融合位姿估计方法，其特征在于，所述步骤1中对IMU设备输出建模具体为：

对IMU设备输出建模，IMU输出包括加速度计的线加速度以及陀螺仪的角速度，加速度计的实际输出值a_m通过下式表示：

a_m＝a-g+n_a+b_a

其中，a表示加速计在全局结构的准确值，g表示重力加速度；

陀螺仪输出的旋转速度ω_m为：

ω_m＝ω+n_g+b_g

n_a～N(0,N_a)

n_g～N(0,N_g)

其中，ω表示陀螺仪即IMU的旋转速度的理想输出；n_a、n_g分别为加速度计好陀螺仪服从正态分布且均值为0的高斯白噪声，

b_a、b_g分别为加速度计好陀螺仪的零偏，视为由对b_a、b_g所建立的随机游走模型导致，模型噪声服从均值为0的正态分布：

采用视觉与IMU之间的松耦合框架建立包括IMU姿态信息以及陀螺零偏的状态量，姿态使用四元数表示，状态向量x为：

4.根据权利要求3所述的一种多传感器融合位姿估计方法，其特征在于，所述步骤1中运动学模型建立过程具体为：

确定IMU运动学模型，IMU输出的姿态由角速度进行一次积分获得，得到运动学模型：

其中，Ω(ω_m)表示ω的四元数表示形式：

5.根据权利要求4所述的一种多传感器融合位姿估计方法，其特征在于，所述步骤2中输入激光雷达与IMU各自的自由度信息具体为：

采用惯导输出的姿态信息作为先验值，采用激光雷达得到的姿态信息作为观测值，在实现过程中对系统矩阵F_k进行离散化处理，观测到的是径向距离和角度，观测矩阵H_k为非线性的函数，以扩展卡尔曼滤波算法为前提处理得到系统状态方程：

其中x_k、x_k-1分别代表当前时刻k、上一时刻k-1的状态值，f为系统的非线性状态函数，h为系统的测量函数，n_k,u_k分别为零均值。

6.根据权利要求5所述的一种多传感器融合位姿估计方法，其特征在于，所述步骤2中基于扩展卡尔曼滤波进行融合姿态估计具体为：

扩展卡尔曼滤波预测过程，通过k-1时刻的状态估计值预测初始时刻k的状态预测值/

根据初始时刻的状态预测值，确定系统协方差矩阵P_k|k-1：

其中，Q_k为系统噪声协方差矩阵：

系统噪声协方差矩阵Q^k根据系统噪声矩阵与状态转移矩阵进行离散化表示：

其中，Φ为状态转移矩阵离散化，状态转移矩阵通过下式表示为：

根据洛必达法则Θ通过下式表示为：

Ψ通过下式表示为：

Q_c为系统噪声协方差矩阵，表示为：

对于Q_k进行近似处理为矩阵块模式并近似处理到三阶形式：

将系统状态方程中的非线性函数h在状态预测处进行一阶泰勒展开并带入系统状态方程z：

其中，H_k表示雅克比矩阵更新模型，通过预测过程求得的系统协方差矩阵计算卡尔曼滤波增益K_k：

得到卡尔曼滤波增益后可通过初始时刻k的状态预测值得到该时刻的状态估计值

确定协方差矩阵的更新值P_k：

P_k＝(I-K_kH_k)P_k|k-1。