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

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

【技术实现步骤摘要】
一种多传感器融合位姿估计方法


[0001]本专利技术涉及机器人姿态估计
，特别是涉及一种多传感器融合位姿估计方法。

技术介绍

[0002]现有的姿态估计方法多是通过单独使用激光雷达或惯性传感器为主要手段对机器人位姿进行估计。
[0003]在机器人姿态估计方法中，单纯基于激光雷达或imu的方法在室内环境下进行位姿估计的实时性较差，输出频率低由于自身硬件设备的限制，易受到光照的影响，且在机器人在高速旋转等情况下容易出现跟踪失败的情况。而作为惯性传感器，具有姿态估计的输出频率高，高速旋转情况下输出精度高的优点，但由于其位姿信息的输出在长时间工作的情况下会导致误差积累过大。
[0004]针对上述问题，拟设计一种融合方式的姿态估计方法，以解决惯性传感器自身存在累计误差与激光雷达旋转时跟踪易失败等问题。

技术实现思路

[0005]为了解决惯性传感器自身存在累计误差与激光雷达旋转时跟踪易失败等问题，本专利技术提供一种多传感器融合位姿估计方法，具体方案如下：
[0006]一种多传感器融合位姿估计方法，包括以下步骤：
[0007]步骤1：建立载体坐标系与导航坐标系，对IMU设备输出建模，确定IMU运动学模型；
[0008]步骤2：输入激光雷达与IMU各自的自由度信息，基于扩展卡尔曼滤波进行融合姿态估计；
[0009]步骤3：不断迭代更新步骤2实现滤波。
[0010]进一步地，所述步骤1中建立坐标系具体为：
[0011]确定坐标系，常用坐标系分为：载体坐标系与导航坐标系，使用载体的质心表示载体坐标系的原点，坐标轴指向载体的正前方，正右方和正下方；导航坐标系指向东北天方向；载体坐标系变换到导航坐标系的旋转采用方向余弦矩阵表示，记为导航坐标系先绕俯仰轴转θ，再绕滚动轴旋转最后绕偏航角转实现到载体坐标系的变换，余弦矩阵通过下式表示：
[0012][0013]进一步地，所述步骤1中对IMU设备输出建模具体为：
[0014]对IMU设备输出建模，IMU输出包括加速度计的线加速度以及陀螺仪的角速度，加速度计的实际输出值a
m
通过下式表示：
[0015]a
m
＝a
‑
g+n
a
+b
a
[0016]其中，a表示加速计在全局结构的准确值，g表示重力加速度；
[0017]陀螺仪输出的旋转速度ω
m
为：
[0018]ω
m
＝ω+n
g
+b
g
[0019]n
a
～N(0,N
a
)
[0020]n
g
～N(0,N
g
)
[0021]其中，ω表示陀螺仪即IMU的旋转速度的理想输出；n
a
、n
g
分别为加速度计好陀螺仪服从正态分布且均值为0的高斯白噪声，
[0022]b
a
、b
g
分别为加速度计好陀螺仪的零偏，视为由对b
a
、b
g
所建立的随机游走模型导致，模型噪声服从均值为0的正态分布：
[0023][0024][0025]采用视觉与IMU之间的松耦合框架建立包括IMU姿态信息以及陀螺零偏的状态量，姿态使用四元数表示，状态向量x为：
[0026][0027]进一步地，所述步骤1中运动学模型建立过程具体为：
[0028]确定IMU运动学模型，IMU输出的姿态由角速度进行一次积分获得，得到运动学模型：
[0029][0030]其中，Ω(ω
m
)表示ω的四元数表示形式：
[0031][0032]进一步地，所述步骤2中输入激光雷达与IMU各自的自由度信息具体为：
[0033]采用惯导输出的姿态信息作为先验值，采用激光雷达得到的姿态信息作为观测值，在实现过程中对系统矩阵F
k
进行离散化处理，观测到的是径向距离和角度，观测矩阵H
k
为非线性的函数，以扩展卡尔曼滤波算法为前提处理得到系统状态方程：
[0034][0035]其中x
k
、x
k
‑1分别代表当前时刻k、上一时刻k
‑
1的状态值，f为系统的非线性状态函数，h为系统的测量函数，n
k
,u
k
分别为零均值。
[0036]进一步地，所述步骤2中基于扩展卡尔曼滤波进行融合姿态估计具体为：
[0037]扩展卡尔曼滤波预测过程，通过k
‑
1时刻的状态估计值预测初始时刻k的状态预测值
[0038][0039]根据初始时刻的状态预测值，确定系统协方差矩阵P
k|k
‑1：
[0040][0041]其中，Q
k
为系统噪声协方差矩阵：
[0042]系统噪声协方差矩阵Q
k
根据系统噪声矩阵与状态转移矩阵进行离散化表示：
[0043][0044]其中，Φ为状态转移矩阵离散化，状态转移矩阵通过下式表示为：
[0045][0046]根据洛必达法则Θ通过下式表示为：
[0047][0048]Ψ通过下式表示为：
[0049][0050]Q
c
为系统噪声协方差矩阵，表示为：
[0051][0052]对于Q
k
进行近似处理为矩阵块模式并近似处理到三阶形式：
[0053][0054]将系统状态方程中的非线性函数h在状态预测处进行一阶泰勒展开并带入系统状态方程z：
[0055][0056]其中，H
k
表示雅克比矩阵更新模型，通过预测过程求得的系统协方差矩阵计算卡尔曼滤波增益K
k
：
[0057][0058]得到卡尔曼滤波增益后可通过初始时刻k的状态预测值得到该时刻的状态估计值
[0059][0060][0061]确定协方差矩阵的更新值P
k
：
[0062]P
k
＝(I
‑
K
k
H
k
)P
k|k
‑1[0063]。
[0064]有益效果：
[0065]本专利技术方法利用激光雷达获得的姿态信息作为观测值校正了IMU输出的值使其与真实值更为接近，在误差统计上相比于其他方法精度上有了一定的提高，做到了修正IMU本身存在的累计误差同时解决旋转时跟踪易失败等问题，整个系统的鲁棒性与强健性也相对提升。
附图说明
[0066]图1为本专利技术流程图。
具体实施方式
[0067]下面将结合本专利技术实施例中的附图对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的本文档来自技高网...

【技术保护点】

【技术特征摘要】
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的正态分布：服从均值为0的正态分布：采用视觉与IMU之间的松耦合框架建立包括IMU姿态信息以及陀螺零偏的状态量，姿态使用四元数表示，状态向量x为：4.根据权利要求3所述的一种多传感器融合位姿估计方法，其特征在于，所述步骤1中运动学模型建立过程具体为：确定IMU运动学模型，IM...

【专利技术属性】
技术研发人员：任玉廷，张俊东，李莫，邵文迪，涂武强，葛鎣，张玉才，杨智显，许晓辉，周汝楠，
申请(专利权)人：华能吉林发电有限公司新能源分公司，
类型：发明
国别省市：