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

一种多传感器融合位姿估计算法 Download PDF

Info

Publication number
CN115540860A
CN115540860A CN202211172443.XA CN202211172443A CN115540860A CN 115540860 A CN115540860 A CN 115540860A CN 202211172443 A CN202211172443 A CN 202211172443A CN 115540860 A CN115540860 A CN 115540860A
Authority
CN
China
Prior art keywords
matrix
state
value
representing
imu
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211172443.XA
Other languages
English (en)
Inventor
李昊东
李瑞峰
霍光磊
于殿勇
黄小春
温宽昌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Harbin Institute of Technology
Fujian Quanzhou HIT Research Institute of Engineering and Technology
Original Assignee
Harbin Institute of Technology
Fujian Quanzhou HIT Research Institute of Engineering and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Harbin Institute of Technology, Fujian Quanzhou HIT Research Institute of Engineering and Technology filed Critical Harbin Institute of Technology
Priority to CN202211172443.XA priority Critical patent/CN115540860A/zh
Publication of CN115540860A publication Critical patent/CN115540860A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Abstract

本发明涉及传感器位姿估算技术领域，具体是公开一种多传感器融合位姿估计算法算法，惯性导航系统的建模与标定，包括确定坐标系，惯性导航系统的IMU设备输出建模，确定IMU运动学模型；基于卡尔曼滤波将惯性导航系统输出的姿态信息与视觉SLMA得到的姿态信息进行姿态融合，包括输入视觉SLMA的视觉传感器与惯性导航系统的IMU设备各自的自由度信息，通过卡尔曼滤波预测过程通过卡尔曼滤波更新过程；不断迭代更新系统方程实现滤波，包括确定误差量表示状态方程，将下一时刻系统信息通过卡尔曼滤波预测过程，将下一时刻系统信息通过卡尔曼滤波的更新过程，该算法修正了IMU本身存在的累计误差，同时解决视觉相机输出频率低，旋转时跟踪易失败等问题。

Description

一种多传感器融合位姿估计算法
技术领域
本发明涉及传感器位姿估算技术领域。
背景技术
在现有的机器人位姿估计算法中，位姿估计算法多数是通过单独使用深度相机或惯性传感器为主要手段对机器人位姿进行估计。单纯基于视觉SLAM算法在室内环境下进行位姿估计的实时性较差，输出频率低由于深度相机自身硬件设备的限制，易受到光照的影响，且在机器人在高速旋转等情况下容易出现跟踪失败的情况。而作为惯性传感器，具有姿态估计的输出频率高，高速旋转情况下输出精度高的优点，但由于其位姿信息的输出在长时间工作的情况下会导致误差积累过大。综上，现有的机器人位姿估计算法中存在惯性传感器自身存在累计误差，深度相机输出频率低，旋转时跟踪易失败等问题
发明内容
本发明的目的在于提供一种多传感器融合位姿估计算法，该算法针对上述问题，是一种融合上述两种姿态估计算法优点的算法。
为实现上述目的，本发明的技术方案是：一种多传感器融合位姿估计算法，算法步骤包括，
步骤1、惯性导航系统的建模与标定；包括下列步骤：步骤1.1、确定坐标系，步骤1.2、惯性导航系统的IMU设备输出建模，步骤1.3、确定IMU运动学模型；
步骤2、基于卡尔曼滤波将惯性导航系统输出的姿态信息与视觉SLMA得到的姿态信息进行姿态融合，包括下列步骤：步骤2.1、输入视觉SLMA的视觉传感器与惯性导航系统的IMU设备各自的自由度信息，步骤2.2、通过卡尔曼滤波预测过程，步骤2.3、通过卡尔曼滤波更新过程；
步骤3、不断迭代更新系统方程实现滤波，包括下列步骤：步骤3.1、确定误差量表示状态方程，步骤3.2、迭代下一时刻姿态信息进行卡尔曼滤波姿态融合。
所述步骤1.1、确定坐标系，使用载体的质心表示载体坐标系的原点，坐标轴一般指向载体的正前方、正右方和正下方，导航坐标系一般指向东、北、天方向，载体坐标系变换到导航坐标系的旋转采用方向余弦矩阵表示，记为
Figure BDA0003862911220000021
假设导航坐标系先绕俯仰轴转θ，再绕滚动轴旋转
Figure BDA0003862911220000022
最后绕偏航角转φ，从而实现到载体坐标系的变换，该方向余弦矩阵表示为：
Figure BDA0003862911220000023
所述步骤1.2、惯性导航系统的IMU设备输出建模，其算法过程包括，
加速度计的实际输出值表示为am＝a-g+na+ba，
陀螺仪输出的旋转速度为ωm＝ω+ng+bg，
其中am表示加速度计的测量值，a表示加速度计在全局结构的准确值，g表示重力加速度，ωm表示在噪声影响下的陀螺仪输出，即IMU的旋转速度的实际输出，ω表示陀螺仪输出，即IMU的旋转速度的理想输出，na、ng分别为加速度计和陀螺仪服从正态分布且均值为0的高斯白噪声，ba、bg分别为加速度计和陀螺仪的零偏，可视为由对ba、bg所建立的随机游走模型导致，其模型噪声
Figure BDA0003862911220000024
服从均值为0的正态分布，
na～N(O，Na)
ng～N(O，Ng)
Figure BDA0003862911220000031
Figure BDA0003862911220000032
采用视觉与IMU之间的松耦合框架建立包括IMU姿态信息以及陀螺零偏的状态量，其中姿态使用四元数
Figure BDA0003862911220000033
表示，状态向量X为
Figure BDA0003862911220000034
bg表示陀螺仪的零偏。
所述步骤1.3、确定IMU运动学模型，其算法过程包括，
根据IMU运动学原理，IMU输出的姿态由角速度进行一次积分获得，考虑中立及噪声因素得到运动学模型：
Figure BDA0003862911220000035
其中，q表示物体的旋转，Ω(ωm)表示ω的四元数，Ω(ωm)表示形式为：
Figure BDA0003862911220000036
其中，ωx为ω四元数表示下的x轴分量，|ωy为ω四元数表示下的y轴分量ωz为ω四元数表示下的z轴分量。
所述步骤2.1、输入视觉SLMA的视觉传感器与惯性导航系统的IMU设备各自的自由度信息，其算法过程包括，
采用惯性导航系统输出的姿态信息作为先验值，采用视觉SLAM得到的姿态信息作为观测值，在实现过程中对系统矩阵Fk进行离散化处理，以卡尔曼滤波算法为前提处理得到状态方程：
Figure BDA0003862911220000037
其中xk表示当前时刻k的状态值、xk-1表示上一时刻k-1的状态值，Fk为系统的状态转移矩阵，Gk为系统的噪声矩阵，Hk为系统的观测矩阵，zk卡尔曼滤波测量方程，n为系统的噪声，u为系统观测噪声。
所述步骤2.2、通过卡尔曼滤波预测过程，其算法过程包括，
通过k-1时刻的状态估计值
Figure BDA0003862911220000041
预测初始时刻k的状态预测值
Figure BDA0003862911220000042
表示为：
Figure BDA0003862911220000043
其中
Figure BDA0003862911220000044
表示状态转移矩阵有了初始时刻的状态预测值，可以求得该时刻的系统协方差矩阵Pk|k-1，
Figure BDA0003862911220000045
其中Qk为系统噪声协方差矩阵，Pk-1表示上一时刻的协方差矩阵更新值，
所述步骤2.3、通过卡尔曼滤波更新过程，其算法过程包括，
通过步骤2.2预测过程求得的系统协方差矩阵Pk|k-1计算卡尔曼滤波增益 Kk，
Figure BDA0003862911220000046
其中，Rk表示观测噪声方差矩阵，
得到卡尔曼滤波增益Kk后可通过初始时刻k的状态预测值
Figure BDA0003862911220000047
得到该时刻的状态估计值
Figure BDA0003862911220000048
Figure BDA0003862911220000049
求得协方差矩阵的更新值Pk，Pk＝(I-KkHk)Pk|k-1。
所述步骤3.1、确定误差量表示状态方程，其算法过程包括，
使用误差四元数δq定义误差：
Figure BDA00038629112200000410
可以用近似小角度误差δq表示为,
Figure BDA0003862911220000051
其中δq为微小旋转的虚部，δqw为微小旋转的实部，δθ表示自定义的小角度误差，
此时，误差状态量δx为，
Figure BDA0003862911220000052
自此惯性导航系统在真实情况下的状态方程可使用误差状态方程近似表示
Figure BDA0003862911220000053
联立上面两式可得状态方程：
Figure BDA0003862911220000054
其中
Figure BDA0003862911220000055
为关于旋转轴的反对称矩阵，
Figure BDA0003862911220000056
所述步骤3.2、迭代下一时刻姿态信息进行卡尔曼滤波姿态融合，其算法过程包括，
步骤3.2.1、将下一时刻系统信息通过卡尔曼滤波预测过程，
步骤3.2.2、将下一时刻系统信息通过卡尔曼滤波的更新过程，
所述步骤3.2.1、将下一时刻系统信息通过卡尔曼滤波预测过程，其算法过程包括，采用惯性导航系统输出的姿态信息作为先验值，采用视觉SLAM得到的姿态信息作为观测值，对系统矩阵Fk进行离散化处理，采用泰勒级数展开并收敛到二阶项：
Figure BDA0003862911220000057
这里通过取了一小段时间Δt为参考进行计算，
联立状态方程可得：
Figure BDA0003862911220000061
其中I表示单位矩阵，O表示零矩阵，
状态转移矩阵可以表示为：
Figure BDA0003862911220000062
其中θ代指此处的矩阵块，ψ代指此处的矩阵块，利用洛必达法则
θ可以表示为
Figure BDA0003862911220000063
ψ可以表示为
Figure BDA0003862911220000064
系统噪声协方差矩阵Qk根据系统噪声矩阵与状态转移矩阵进行离散化表示：
Figure BDA0003862911220000065
其中Qc为系统噪声协方差矩阵，表示为
Figure BDA0003862911220000066
Figure BDA0003862911220000067
为状态转移矩阵
Figure BDA0003862911220000068
离散化表示，
对于系统噪声协方差矩阵Qk进行近似处理为矩阵块模式并近似处理到三阶形式：
Figure BDA0003862911220000069
所述步骤步骤3.2.2、将下一时刻系统信息通过卡尔曼滤波的更新过程，其算法过程包括，
用视觉姿态估计值
Figure BDA00038629112200000610
表示相机坐标系到世界坐标系的姿态变换，同时也表示滤波器的观测值Z联立观测矩阵，可得观测方程：H＝[O3×3 I3×3]
自此，卡尔曼滤波的更新过程为：
Figure BDA0003862911220000071
Figure BDA0003862911220000072
Pk＝(I-KkHk)Pk|k-1
其中Kk表示卡尔曼滤波增益，Rk表示观测噪声方差矩阵，
Figure BDA0003862911220000073
表示k-1 时刻的状态估计值，
Figure BDA0003862911220000074
表示预测初始时刻k的状态预测值，Pk|k-1表示该时刻的系统协方差矩阵。
通过采用上述技术方案，本发明的有益效果是：本发明上述技术方案的位姿估计算法中利用视觉SLAM获得的姿态信息作为观测值校正了惯性导航系统的 IMU输出的值，融合了两种姿态估计算法的优点，使其与真实值更为接近，在误差统计上相比于改进的ORB—SLAM算法(一种基于ORB特征的三维定位与地图构建算法)精度上有了一定的提高，做到了修正惯性导航系统IMU本身存在的累计误差，同时解决视觉SLAM中视觉相机输出频率低，旋转时跟踪易失败等问题，整个系统的鲁棒性与强健性也相对提升，相比于双目相机资源消耗更低。
具体实施方式
为了进一步解释本发明的技术方案，下面通过具体实施例来对本发明进行详细阐述。
本实施例公开的一种多传感器融合位姿估计算法，算法步骤包括步骤1、惯性导航系统的建模与标定，步骤2、基于卡尔曼滤波将惯性导航系统输出的姿态信息与视觉SLMA得到的姿态信息进行姿态融合以及步骤3、不断迭代更新系统方程实现滤波这三大步，具体各步骤中的过程算法下面详细描述。
步骤1、惯性导航系统的建模与标定，包括下列步骤：
步骤1.1、确定坐标系；常用坐标系可分为载体坐标系与导航坐标系，通常使用载体的质心表示载体坐标系的原点，坐标轴一般指向载体的正前方、正右方和正下方，导航坐标系一般指向东、北、天方向，载体坐标系变换到导航坐标系的旋转采用方向余弦矩阵表示，记为
Figure BDA0003862911220000081
假设导航坐标系先绕俯仰轴转θ，再绕滚动轴旋转
Figure BDA0003862911220000082
最后绕偏航角转φ，从而实现到载体坐标系的变换，该方向余弦矩阵表示为：
Figure BDA0003862911220000083
步骤1.2、惯性导航系统的IMU设备输出建模；IMU(Inertial Measurement Unit)即惯性测量单元，IMU设备输出包括加速度计的线加速度以及陀螺仪的角速度，加速度计的实际输出值表示为am＝a-g+na+ba，陀螺仪输出的旋转速度为ωm＝ω+ng+bg，
其中am表示加速度计的测量值，a表示加速度计在全局结构的准确值，g表示重力加速度，ωm表示在噪声影响下的陀螺仪输出，即IMU的旋转速度的实际输出，ω表示陀螺仪输出，即IMU的旋转速度的理想输出，ba、bg分别为加速度计和陀螺仪服从正态分布且均值为0的高斯白噪声，ba、bg分别为加速度计和陀螺仪的零偏，可视为由对ba、bg所建立的随机游走模型导致，其模型噪声
Figure BDA0003862911220000084
服从均值为0的正态分布，
na～N(O，Na)
ng～N(O，Ng)
Figure BDA0003862911220000085
Figure BDA0003862911220000086
采用视觉与IMU之间的松耦合框架建立包括IMU姿态信息以及陀螺零偏的状态量，其中姿态使用四元数
Figure BDA0003862911220000087
表示，状态向量X为
Figure BDA0003862911220000088
bg表示陀螺仪的零偏，
步骤1.3、确定IMU运动学模型；根据IMU运动学原理，IMU输出的姿态由角速度进行一次积分获得，考虑中立及噪声因素得到运动学模型：
Figure BDA0003862911220000091
其中，q表示物体的旋转，Ω(ωm)表示ω的四元数，Ω(ωm)表示形式为：
Figure BDA0003862911220000092
其中，ωx为ω四元数表示下的x轴分量，|ωy为ω四元数表示下的y轴分量ωz为ω四元数表示下的z轴分量，
步骤2、基于卡尔曼滤波将惯性导航系统输出的姿态信息与视觉SLMA得到的姿态信息进行姿态融合，包括下列步骤：
步骤2.1、输入视觉SLMA的视觉传感器与惯性导航系统的IMU设备各自的自由度信息；本发明采用惯性导航系统输出的姿态信息作为先验值，采用视觉 SLAM得到的姿态信息作为观测值，在实现过程中对系统矩阵Fk进行离散化处理，以卡尔曼滤波算法为前提处理得到状态方程：
Figure BDA0003862911220000093
其中xk表示当前时刻k的状态值、xk-1表示上一时刻k-1的状态值，Fk为系统的状态转移矩阵，Gk为系统的噪声矩阵，Hk为系统的观测矩阵，zk卡尔曼滤波测量方程，n为系统的噪声，u为系统观测噪声。
步骤2.2、通过卡尔曼滤波预测过程；通过k-1时刻的状态估计值
Figure BDA0003862911220000094
预测初始时刻k的状态预测值
Figure BDA0003862911220000095
表示为：
Figure BDA0003862911220000101
其中
Figure BDA0003862911220000102
表示状态转移矩阵有了初始时刻的状态预测值，可以求得该时刻的系统协方差矩阵Pk|k-1，
Figure BDA0003862911220000103
其中Qk为系统噪声协方差矩阵，Pk-1表示上一时刻的协方差矩阵更新值，
步骤2.3、通过卡尔曼滤波更新过程；通过步骤2.2预测过程求得的系统协方差矩阵Pk|k-1计算卡尔曼滤波增益Kk，
Figure BDA0003862911220000104
其中，Rk表示观测噪声方差矩阵，
得到卡尔曼滤波增益Kk后可通过初始时刻k的状态预测值
Figure BDA0003862911220000105
得到该时刻的状态估计值
Figure BDA0003862911220000106
Figure BDA0003862911220000107
求得协方差矩阵的更新值Pk，Pk＝(I-KkHk)Pk|k-1。
步骤3、不断迭代更新系统方程实现滤波，包括下列步骤：
步骤3.1、确定误差量表示状态方程；为了系统稳定性使用误差向量表示状态方程，这里是使用误差四元数δq定义误差：
Figure BDA0003862911220000108
可以用近似小角度误差δq表示为,
Figure BDA0003862911220000109
其中δq为微小旋转的虚部，δqw为微小旋转的实部，δθ表示自定义的小角度误差，
此时，误差状态量δx为，
Figure BDA0003862911220000111
自此惯性导航系统在真实情况下的状态方程可使用误差状态方程近似表示
Figure BDA0003862911220000112
此方程出自Trawny《Indirect Kalman filter for 3D attitude estimation》文献的推导结果，
联立上面两式可得状态方程：
Figure BDA0003862911220000113
其中
Figure BDA0003862911220000114
为关于旋转轴的反对称矩阵，
Figure BDA0003862911220000115
步骤3.2、迭代下一时刻姿态信息进行卡尔曼滤波姿态融合，其算法过程包括，步骤3.2.1、将下一时刻系统信息通过卡尔曼滤波预测过程，步骤3.2.2、将下一时刻系统信息通过卡尔曼滤波的更新过程，
所述步骤3.2.1、将下一时刻系统信息通过卡尔曼滤波预测过程，采用惯性导航系统输出的姿态信息作为先验值，采用视觉SLAM得到的姿态信息作为观测值，对系统矩阵Fk进行离散化处理，采用泰勒级数展开并收敛到二阶项：
Figure BDA0003862911220000116
这里通过取了一小段时间Δt为参考进行计算，
联立状态方程可得：
Figure BDA0003862911220000121
其中I表示单位矩阵，O表示零矩阵，
状态转移矩阵可以表示为：
Figure BDA0003862911220000122
其中Θ代指此处的矩阵块，Ψ代指此处的矩阵块，利用洛必达法则
Θ可以表示为
Figure BDA0003862911220000123
ψ可以表示为
Figure BDA0003862911220000124
系统噪声协方差矩阵Qk根据系统噪声矩阵与状态转移矩阵进行离散化表示：
Figure BDA0003862911220000125
其中Qc为系统噪声协方差矩阵，表示为
Figure BDA0003862911220000126
Φ(tk+1，t)为状态转移矩阵
Figure BDA0003862911220000127
离散化表示
对于系统噪声协方差矩阵Qk进行近似处理为矩阵块模式并近似处理到三阶形式：
Figure BDA0003862911220000128
步骤3.2.2、将下一时刻系统信息通过卡尔曼滤波的更新过程；由于IMU输出的姿态估计频率非常高，从而导致容易随着时间的增长而获得较大的姿态累积误差，使用视觉SLAM的低频输出结果作为校正IMU的姿态输出值，可以提高整个系统的强健性。用视觉姿态估计值
Figure BDA0003862911220000129
表示相机坐标系到世界坐标系的姿态变换，同时也表示滤波器的观测值Z联立观测矩阵可得观测方程：H＝[O3×3 I3×3]
自此卡尔曼滤波的更新过程为：
Figure BDA0003862911220000131
Figure BDA0003862911220000132
Pk＝(I-KkHk)Pk|k-1
上述实施例并非限定本发明的产品形态和式样，任何所属技术领域的普通技术人员对其所做的适当变化或修饰，皆应视为不脱离本发明的专利范畴。

Claims (10)

1.一种多传感器融合位姿估计算法，其特征在于，算法步骤包括，
步骤1、惯性导航系统的建模与标定；
步骤2、基于卡尔曼滤波将惯性导航系统输出的姿态信息与视觉SLMA得到的姿态信息进行姿态融合，包括下列步骤：步骤2.1、输入视觉SLMA的视觉传感器与惯性导航系统的IMU设备各自的自由度信息，步骤2.2、通过卡尔曼滤波预测过程，步骤2.3、通过卡尔曼滤波更新过程；
步骤3、不断迭代更新系统方程实现滤波，包括下列步骤：步骤3.1、确定误差量表示状态方程，步骤3.2、迭代下一时刻姿态信息进行卡尔曼滤波姿态融合。
2.如权利要求1所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤1包括下列步骤：步骤1.1、确定坐标系，步骤1.2、惯性导航系统的IMU设备输出建模，步骤1.3、确定IMU运动学模型；
所述步骤1.2、惯性导航系统的IMU设备输出建模，其算法过程包括，
加速度计的实际输出值表示为am＝a-g+na+ba，
陀螺仪输出的旋转速度为ωm＝ω+ng+bg，
其中am表示加速度计的测量值，a表示加速度计在全局结构的准确值，g表示重力加速度，ωm表示在噪声影响下的陀螺仪输出，即IMU的旋转速度的实际输出，ω表示陀螺仪输出，即IMU的旋转速度的理想输出，na、ng分别为加速度计和陀螺仪服从正态分布且均值为0的高斯白噪声，ba、bg分别为加速度计和陀螺仪的零偏，可视为由对ba、bg所建立的随机游走模型导致，其模型噪声
Figure FDA0003862911210000011
服从均值为0的正态分布，
na～N(0，Na)
ng～N(0，Ng)
Figure FDA0003862911210000021
Figure FDA0003862911210000022
采用视觉与IMU之间的松耦合框架建立包括IMU姿态信息以及陀螺零偏的状态量，其中姿态使用四元数
Figure FDA0003862911210000023
表示，状态向量X为
Figure FDA0003862911210000024
bg表示陀螺仪的零偏。
3.如权利要求2所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤1.3、确定IMU运动学模型，其算法过程包括，
根据IMU运动学原理，IMU输出的姿态由角速度进行一次积分获得，考虑中立及噪声因素得到运动学模型：
Figure FDA0003862911210000025
其中，q表示物体的旋转，Ω(ωm)表示ω的四元数，Ω(ωm)表示形式为：
Figure FDA0003862911210000026
其中，ωx为ω四元数表示下的x轴分量，ωy为ω四元数表示下的y轴分量ωz为ω四元数表示下的z轴分量。
4.如权利要求3所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤2.1、输入视觉SLMA的视觉传感器与惯性导航系统的IMU设备各自的自由度信息，其算法过程包括，
采用惯性导航系统输出的姿态信息作为先验值，采用视觉SLAM得到的姿态信息作为观测值，在实现过程中对系统矩阵Fk进行离散化处理，以卡尔曼滤波算法为前提处理得到状态方程：
Figure FDA0003862911210000031
其中xk表示当前时刻k的状态值、xk-1表示上一时刻k-1的状态值，Fk为系统的状态转移矩阵，Gk为系统的噪声矩阵，Hk为系统的观测矩阵，zk卡尔曼滤波测量方程，n为系统的噪声，u为系统观测噪声。
5.如权利要求4所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤3.1、确定误差量表示状态方程，其算法过程包括，
使用误差四元数δq定义误差：
Figure FDA0003862911210000032
可以用近似小角度误差δq表示为,
Figure FDA0003862911210000033
其中δq为微小旋转的虚部，δqw为微小旋转的实部，δθ表示自定义的小角度误差，
此时，误差状态量δx为，
Figure FDA0003862911210000034
自此惯性导航系统在真实情况下的状态方程可使用误差状态方程近似表示
Figure FDA0003862911210000035
联立上面两式可得状态方程：
Figure FDA0003862911210000036
其中
Figure FDA0003862911210000037
为关于旋转轴的反对称矩阵，
Figure FDA0003862911210000038
6.如权利要求5所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤3.2、迭代下一时刻姿态信息进行卡尔曼滤波姿态融合，其算法过程包括，
步骤3.2.1、将下一时刻系统信息通过卡尔曼滤波预测过程，
步骤3.2.2、将下一时刻系统信息通过卡尔曼滤波的更新过程，
所述步骤3.2.1、将下一时刻系统信息通过卡尔曼滤波预测过程，其算法过程包括，采用惯性导航系统输出的姿态信息作为先验值，采用视觉SLAM得到的姿态信息作为观测值，对系统矩阵Fk进行离散化处理，采用泰勒级数展开并收敛到二阶项：
Figure FDA0003862911210000041
这里通过取了一小段时间Δt为参考进行计算，
联立状态方程可得：
Figure FDA0003862911210000042
其中I表示单位矩阵，O表示零矩阵，
状态转移矩阵可以表示为：
Figure FDA0003862911210000043
其中
Figure FDA0003862911210000044
代指此处的矩阵块，ψ代指此处的矩阵块，利用洛必达法则
Figure FDA0003862911210000045
可以表示为
Figure FDA0003862911210000046
ψ可以表示为
Figure FDA0003862911210000047
系统噪声协方差矩阵Qk根据系统噪声矩阵与状态转移矩阵进行离散化表示：
Figure FDA0003862911210000048
其中Qc为系统噪声协方差矩阵，表示为
Figure FDA0003862911210000051
Φ(tk+1，t)为状态转移矩阵
Figure FDA0003862911210000052
离散化表示，
对于系统噪声协方差矩阵Qk进行近似处理为矩阵块模式并近似处理到三阶形式：
Figure FDA0003862911210000053
7.如权利要求6所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤3.2.2、将下一时刻系统信息通过卡尔曼滤波的更新过程，其算法过程包括，用视觉姿态估计值
Figure FDA0003862911210000054
表示相机坐标系到世界坐标系的姿态变换，同时也表示滤波器的观测值Z联立观测矩阵，可得观测方程：H＝[O3×3 I3×3]
自此，卡尔曼滤波的更新过程为：
Figure FDA0003862911210000055
Figure FDA0003862911210000056
Pk＝(1-KkHk)Pk|k-1
其中Kk表示卡尔曼滤波增益，Rk表示观测噪声方差矩阵，
Figure FDA0003862911210000057
表示k-1时刻的状态估计值，
Figure FDA0003862911210000058
表示预测初始时刻k的状态预测值，Pk|k-1表示该时刻的系统协方差矩阵。
8.如权利要求4-7任意一项所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤2.2、通过卡尔曼滤波预测过程，其算法过程包括，
通过'-1时刻的状态估计值
Figure FDA0003862911210000059
预测初始时刻k的状态预测值
Figure FDA00038629112100000510
表示为：
Figure FDA00038629112100000511
其中
Figure FDA00038629112100000512
表示状态转移矩阵有了初始时刻的状态预测值，可以求得该时刻的系统协方差矩阵Pk|k-1，
Figure FDA00038629112100000513
其中Qk为系统噪声协方差矩阵，Pk-1表示上一时刻的协方差矩阵更新值。
9.如权利要求8所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤2.3、通过卡尔曼滤波更新过程，其算法过程包括，
通过步骤2.2预测过程求得的系统协方差矩阵Pk|k-1计算卡尔曼滤波增益Kk，
Figure FDA0003862911210000061
其中，Rk表示观测噪声方差矩阵，
得到卡尔曼滤波增益Kk后可通过初始时刻k的状态预测值
Figure FDA0003862911210000062
得到该时刻的状态估计值
Figure FDA0003862911210000063
Figure FDA0003862911210000064
求得协方差矩阵的更新值Pk，Pk＝(I-KkHk)Pk|k-1。
10.如权利要求8所述的一种多传感器融合位姿估计算法，其特征在于，所述步骤1.1、确定坐标系，使用载体的质心表示载体坐标系的原点，坐标轴一般指向载体的正前方、正右方和正下方，导航坐标系一般指向东、北、天方向，载体坐标系变换到导航坐标系的旋转采用方向余弦矩阵表示，记为
Figure FDA0003862911210000065
假设导航坐标系先绕俯仰轴转θ，再绕滚动轴旋转
Figure FDA0003862911210000066
最后绕偏航角转φ，从而实现到载体坐标系的变换，该方向余弦矩阵表示为：
Figure FDA0003862911210000067
CN202211172443.XA 2022-09-26 2022-09-26 一种多传感器融合位姿估计算法 Pending CN115540860A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211172443.XA CN115540860A (zh) 2022-09-26 2022-09-26 一种多传感器融合位姿估计算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211172443.XA CN115540860A (zh) 2022-09-26 2022-09-26 一种多传感器融合位姿估计算法

Publications (1)

Publication Number Publication Date
CN115540860A true CN115540860A (zh) 2022-12-30

Family

ID=84728786

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211172443.XA Pending CN115540860A (zh) 2022-09-26 2022-09-26 一种多传感器融合位姿估计算法

Country Status (1)

Country Link
CN (1) CN115540860A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116592896A (zh) * 2023-07-17 2023-08-15 山东水发黄水东调工程有限公司 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法
CN117451043A (zh) * 2023-12-25 2024-01-26 武汉大学 一种数-模混合估计的多源融合定位方法及系统

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116592896A (zh) * 2023-07-17 2023-08-15 山东水发黄水东调工程有限公司 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法
CN116592896B (zh) * 2023-07-17 2023-09-29 山东水发黄水东调工程有限公司 基于卡尔曼滤波和红外热成像的水下机器人导航定位方法
CN117451043A (zh) * 2023-12-25 2024-01-26 武汉大学 一种数-模混合估计的多源融合定位方法及系统
CN117451043B (zh) * 2023-12-25 2024-03-15 武汉大学 一种数-模混合估计的多源融合定位方法及系统

Similar Documents

Publication Publication Date Title
WO2020253854A1 (zh) 移动机器人姿态角解算方法
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN107607113B (zh) 一种两轴姿态倾角测量方法
KR102017404B1 (ko) 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법
CN110986939B (zh) 一种基于imu预积分的视觉惯性里程计方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
WO2017063388A1 (zh) 一种惯导装置初始对准方法
CN115540860A (zh) 一种多传感器融合位姿估计算法
CN106056664A (zh) 一种基于惯性和深度视觉的实时三维场景重构系统及方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN109141475B (zh) 一种dvl辅助sins鲁棒行进间初始对准方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN107621266B (zh) 基于特征点跟踪的空间非合作目标相对导航方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
JP2012173190A (ja) 測位システム、測位方法
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN114216456B (zh) 一种基于imu与机器人本体参数融合的姿态测量方法
CN115855048A (zh) 一种多传感器融合位姿估计方法
Cucci et al. Position tracking and sensors self-calibration in autonomous mobile robots by gauss-newton optimization
CN110207647B (zh) 一种基于互补卡尔曼滤波器的臂环姿态角计算方法
CN116644264A (zh) 一种非线性偏置-不变卡尔曼滤波方法
CN106595669B (zh) 一种旋转体姿态解算方法
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination