CN115855048A - 一种多传感器融合位姿估计方法 - Google Patents

Abstract

本发明提供了一种多传感器融合位姿估计方法，本发明通过基于动态贝叶斯网络和断裂力学，本发明建立载体坐标系与导航坐标系，对IMU设备输出建模，确定IMU运动学模型；输入激光雷达与IMU各自的自由度信息，基于扩展卡尔曼滤波进行融合姿态估计；不断迭代更新实现滤波。本发明利用激光雷达获得的姿态信息作为观测值校正了IMU输出的值使其与真实值更为接近，在误差统计上相比于其他方法精度上有了一定的提高，做到了修正IMU本身存在的累计误差同时解决旋转时跟踪易失败等问题，整个系统的鲁棒性与强健性也相对提升。

Description

一种多传感器融合位姿估计方法

技术领域

本发明涉及机器人姿态估计技术领域，特别是涉及一种多传感器融合位姿估计方法。

背景技术

现有的姿态估计方法多是通过单独使用激光雷达或惯性传感器为主要手段对机器人位姿进行估计。

在机器人姿态估计方法中，单纯基于激光雷达或imu的方法在室内环境下进行位姿估计的实时性较差，输出频率低由于自身硬件设备的限制，易受到光照的影响，且在机器人在高速旋转等情况下容易出现跟踪失败的情况。而作为惯性传感器，具有姿态估计的输出频率高，高速旋转情况下输出精度高的优点，但由于其位姿信息的输出在长时间工作的情况下会导致误差积累过大。

针对上述问题，拟设计一种融合方式的姿态估计方法，以解决惯性传感器自身存在累计误差与激光雷达旋转时跟踪易失败等问题。

发明内容

为了解决惯性传感器自身存在累计误差与激光雷达旋转时跟踪易失败等问题，本发明提供一种多传感器融合位姿估计方法，具体方案如下：

一种多传感器融合位姿估计方法，包括以下步骤：

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

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

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

进一步地，所述步骤1中建立坐标系具体为：

确定坐标系，常用坐标系分为：载体坐标系与导航坐标系，使用载体的质心表示载体坐标系的原点，坐标轴指向载体的正前方，正右方和正下方；导航坐标系指向东北天方向；载体坐标系变换到导航坐标系的旋转采用方向余弦矩阵表示，记为

导航坐标系先绕俯仰轴转θ，再绕滚动轴旋转/>

最后绕偏航角转/>

实现到载体坐标系的变换，余弦矩阵通过下式表示：

进一步地，所述步骤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为：

进一步地，所述步骤1中运动学模型建立过程具体为：

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

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

进一步地，所述步骤2中输入激光雷达与IMU各自的自由度信息具体为：

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

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

进一步地，所述步骤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

。

有益效果：

本发明方法利用激光雷达获得的姿态信息作为观测值校正了IMU输出的值使其与真实值更为接近，在误差统计上相比于其他方法精度上有了一定的提高，做到了修正IMU本身存在的累计误差同时解决旋转时跟踪易失败等问题，整个系统的鲁棒性与强健性也相对提升。

附图说明

图1为本发明流程图。

具体实施方式

下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

结合图1所示，本发明提供一种多传感器融合位姿估计方法，所述方法包括以下步骤：

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

常用坐标系可分为：载体坐标系与导航坐标系。通常使用载体的质心表示载体坐标系的原点，坐标轴一般指向载体的正前方，正右方和正下方；导航坐标系一般指向东北天方向。载体坐标系变换到导航坐标系的旋转采用方向余弦矩阵表示，记为

假设导航坐标系先绕俯仰轴转θ，再绕滚动轴旋转/>

最后绕偏航角转/>

从而实现到载体坐标系的变换，该方向余弦矩阵表示为：

IMU设备输出建模

IMU输出包括加速度计的线加速度以及陀螺仪的角速度，加速度计的实际输出值a_m表示为：a_m＝a-g+n_a+b_a

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

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

ω_m＝ω+n_g+b_g

其中ω表示陀螺仪即IMU的旋转速度的理想输出，

n_a、n_g分别为加速度计好陀螺仪服从正态分布且均值为0的高斯白噪声：

n_a～N(0,N_a)

n_g～N(0,N_g)

b_a、b_g分别为加速度计好陀螺仪的零偏，可视为由对b_a、b_g所建立的随机游走模型导致，其模型噪声

服从均值为0的正态分布：

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

表示，状态向量x为：

确定IMU运动学模型

根据IMU运动学原理，IMU输出的姿态由角速度进行一次积分获得，考虑中立及噪声因素得到运动学模型：

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

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

2.1、输入激光雷达与IMU各自的自由度信息

本发明采用惯导输出的姿态信息作为先验值，采用激光雷达得到的姿态信息作为观测值，在实现过程中对系统矩阵F_k进行离散化处理。解决非线性问题，如极坐标系的雷达，观测到的是径向距离和角度，这个时候的观测矩阵H_k就是非线性的函数

以扩展卡尔曼滤波算法为前提处理得到系统状态方程：

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

2.2、扩展卡尔曼滤波预测过程

通过k-1时刻的状态估计值

预测初始时刻k的状态预测值/>

有了初始时刻的状态预测值，可以求得该时刻的系统协方差矩阵P_k|k-1

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

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

其中Φ为状态转移矩阵

离散化，状态转移矩阵可以表示为：

利用洛必达法则Θ可以表示为：

Ψ可以表示为：

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

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

2.3、扩展卡尔曼滤波更新过程

将系统状态方程中的非线性函数h在状态预测

处进行一阶泰勒展开并带入系统状态方程z：

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

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

得到该时刻的状态估计值/>

求得协方差矩阵的更新值P_k：

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

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

本发明利用激光雷达获得的姿态信息作为观测值校正了IMU输出的值使其与真实值更为接近，在误差统计上相比于其他方法精度上有了一定的提高，做到了修正IMU本身存在的累计误差同时解决旋转时跟踪易失败等问题，整个系统的鲁棒性与强健性也相对提升。

以上对本发明所提供的一种多传感器融合位姿估计方法，进行了详细介绍，本发明应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (6)

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。

Images