CN114046800A - 一种基于双层滤波框架的高精度里程估计方法 - Google Patents
一种基于双层滤波框架的高精度里程估计方法 Download PDFInfo
- Publication number
- CN114046800A CN114046800A CN202111321919.7A CN202111321919A CN114046800A CN 114046800 A CN114046800 A CN 114046800A CN 202111321919 A CN202111321919 A CN 202111321919A CN 114046800 A CN114046800 A CN 114046800A
- Authority
- CN
- China
- Prior art keywords
- imu
- leg
- foot
- observation
- state vector
- 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.)
- Granted
Links
- 238000001914 filtration Methods 0.000 title claims abstract description 44
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000004927 fusion Effects 0.000 claims abstract description 42
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 34
- 238000005259 measurement Methods 0.000 claims abstract description 33
- 230000033001 locomotion Effects 0.000 claims abstract description 26
- 210000003108 foot joint Anatomy 0.000 claims abstract description 9
- 239000013598 vector Substances 0.000 claims description 111
- 239000011159 matrix material Substances 0.000 claims description 67
- 230000001133 acceleration Effects 0.000 claims description 29
- 230000000007 visual effect Effects 0.000 claims description 17
- 238000004364 calculation method Methods 0.000 claims description 8
- 230000005484 gravity Effects 0.000 claims description 6
- 230000010354 integration Effects 0.000 claims description 5
- 230000009466 transformation Effects 0.000 claims description 4
- 230000007704 transition Effects 0.000 claims description 4
- 238000013507 mapping Methods 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 2
- 230000000644 propagated effect Effects 0.000 claims description 2
- 230000009897 systematic effect Effects 0.000 claims description 2
- 230000036962 time dependent Effects 0.000 claims description 2
- 230000001131 transforming effect Effects 0.000 claims description 2
- 238000007796 conventional method Methods 0.000 abstract description 3
- 239000010410 layer Substances 0.000 description 15
- 238000005457 optimization Methods 0.000 description 7
- 230000008878 coupling Effects 0.000 description 6
- 238000010168 coupling process Methods 0.000 description 6
- 238000005859 coupling reaction Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000011160 research Methods 0.000 description 5
- 238000005295 random walk Methods 0.000 description 4
- 238000009795 derivation Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000007500 overflow downdraw method Methods 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 239000002355 dual-layer Substances 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000002035 prolonged effect Effects 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于双层滤波框架的高精度里程估计方法,该方法首先采集双目相机、主体IMU、腿足辅助IMU、腿足关节电机和足端力传感器的传感信息;利用底层腿足滤波器对腿足关节电机的转角和转速、腿足辅助IMU以及足端力传感器的测量信息进行信息融合,得到腿足辅助IMU坐标系下的速度观测;再将速度观测输入上层滤波器,利用上层滤波器对采集的主体IMU、双目相机的观测信息进行融合,完成信息融合并输出机器人的里程估计结果。本发明方法能够有效解决现有方法在四足移动机器人快速运动中,由于剧烈抖动导致的里程估计精度不高甚至算法失效等问题,为四足移动机器人在快速运动中的自主定位提供了有效解决方案。
Description
技术领域
本发明属于移动机器人自主定位技术领域,尤其涉及一种基于双层滤波框架的高精度里程估计方法。
背景技术
随着人工智能的迅速发展,机器人、无人车、虚拟现实、增强现实等领域成为研究热点,尤其是在环境未知且GPS信号无法使用的情况下,实现精确的自主定位并构建地图是该领域的研究前提。
惯性测量单元(IMU)能以很高的频率测量移动平台的加速度和转速,从而在较短时间段内实现较高精度的相对位姿估计。然而,IMU存在零点漂移,且受噪声影响,随着运行时间的增加,通过积分(累加)的方式计算相对位姿产生的累积误差也会逐渐增大,且无法矫正。因此,通常使用其他能够感知外部环境的传感器进行辅助定位,以降低IMU噪声累积和零点漂移等问题对里程估计精度的影响。视觉辅助惯性里程估计方法(Visual-aidedInertial Odometry,VIO)通过相机对IMU的噪声和零漂进行矫正,以其较低的成本、较高的鲁棒性成为最早成功应用在航空航天、VR/AR、自动驾驶等领域中、且较为通用的定位方法之一。然而,在某些极具挑战性的应用场景中,VIO算法框架也有其局限性:图像频率相对较低,在高动态场景中离散化误差、线性化误差导致里程估计精度大幅度下降甚至定位失败;剧烈抖动场景下图像模糊导致常规的特征点匹配算法失效;当移动平台进行平面运动、匀加速运动或绕定轴转动等特殊运动时,VIO定位方法存在严重的退化问题,导致无法正确估计运动尺度。以上场景都可以在实际应用中找到对应场景,甚至某些移动平台同时具备上述多种条件,例如四足移动机器人快速奔跑时,机器人身体剧烈抖动且移动迅速(目前最快的四足移动机器人移动速度可以达到4m/s)。幸运的是,这些问题可以通过融合平台上的其他传感器,通过多传感器融合的方法进行解决。
感知定位问题在近年来重新成为移动机器人领域的研究热点,很大程度上归功于众多新型传感器的发明和应用,例如激光雷达、相机等传感器的广泛应用促成了激光SLAM和视觉SLAM等领域的蓬勃发展,多传感器融合是当前移动机器人感知领域的主要研究方向之一。根据融合框架的区别,又可以分为松耦合与紧耦合。松耦合算法将多种传感器的测量值分别处理获得独立的位姿估计,再将多个位姿估计的结果进行融合;紧耦合框架则利用多种传感器的测量值共同对一组姿态变量进行预测与更新修正,最终得到一个符合所有传感器观测的最优位姿估计。松耦合的方法设计简单,可以很容易地融合多种传感器观测,但是将多个独立的位姿估计进行融合时,会将线性化误差引入观测噪声,导致其残差模型不再服从近似高斯分布,造成位姿估计的较大误差。研究领域普遍认为,紧耦合的多传感器融合算法在精度上要明显高于松耦合方法,然而其算法设计也更加复杂。根据应用平台和应用场景,选择合适的传感器组合,并设计相应的紧耦合算法,是机器人感知定位算法在实际应用中要解决的主要问题之一。
根据所采用的后端融合算法,视觉惯性里程估计算法可以分为基于滤波的算法和基于优化的算法。基于优化的方法以最小化特征点观测的重投影误差为优化目标,对***实体的位姿进行迭代优化,多依赖g2o、Ceres等优化框架,能够获得更为精确的位姿估计,但是迭代优化需要较多的计算资源,实际应用中的移动机器人多为计算资源稀缺的***(单片机、ARM架构的板载芯片等),使用迭代优化的方法难以满足位姿估计实时性的要求。基于改进卡尔曼滤波的算法多进行马尔可夫假设,只需要相对少量的矩阵运算即可完成较为精确的状态估计,对计算资源的要求较低,更能满足实际应用的需要。
目前,较为通用的视觉惯性里程估计算法无法在四足移动机器人快速运动过程中进行高精度相对位姿估计,而基于激光的定位算法以其极其极高的传感器价格难以在实际应用中落地。因此,如何充分考虑四足移动机器人的平台特性,并合理选择额外的辅助传感器进行融合定位,是应用领域亟待解决的技术问题。
发明内容
本发明的目的是针对现有技术的不足,提供一种基于双层滤波框架的高精度里程估计方法,本发明结合IMU紧耦合融合视觉特征、足式里程计等外部传感器,在剧烈抖动、快速移动等极具挑战性的场景下实现了高精度里程估计。
本发明的目的是通过以下技术方案来实现的:一种基于双层滤波框架的高精度里程估计方法,包括以下步骤:
S1,设定双目相机和IMU的内参,设定主体IMU与双目相机之间的外参、主体IMU与足式里程计之间的外参以及腿足辅助IMU与腿足关节电机之间的外参;采集腿足关节电机的反馈信息、足端力传感器的测量信息、主体IMU和腿足辅助IMU的测量信息以及双目视觉观测信息;
S2,利用底层腿足滤波器对步骤S1采集的腿足关节电机的转角和转速、腿足辅助IMU以及足端力传感器的测量信息进行信息融合,首先定义底层腿足滤波的状态向量,计算腿足的平均速观测及其对应的方差,再计算该方差对应的卡尔曼增益,根据该卡尔曼增益更新底层腿足滤波的状态向量,得到腿足辅助IMU坐标系下的速度观测;
S3,将步骤S2得到的速度观测输入上层滤波器,利用上层滤波器对步骤S1采集的主体IMU、相机和足式里程计的观测信息进行融合,首先定义上层滤波器的状态向量,利用主体IMU的运动模型对状态向量进行预测,得到状态向量的先验,再利用双目视觉观测对状态向量的先验进行更新,得到视觉-主体IMU融合状态估计值;最后利用腿足里程计的速度观测对视觉-主体IMU融合状态估计值进行状态更新,得到上层滤波器状态向量的估计值,完成信息融合并输出机器人的里程估计结果。
本发明具有以下有益效果:本发明所述的基于双层滤波框架的高精度里程估计方法,能够高效融合IMU、相机、腿足电机反馈、足端理传感器等多种传感器,有效解决现有方法在移动平台快速移动、剧烈抖动情况下难以获得高精度位姿估计的问题。同时,通过选取多状态约束卡尔曼滤波框架为主体,对多状态进行线性插值实现多传感器的时间对齐,兼顾融合精度和算法效率,可以在计算资源受限的板载平台上进行实时的相对位姿估计。
附图说明
图1为本发明所述的双层滤波融合框架示意图;
图2为本发明应用在四足移动机器人感知***的传感器分布示意图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式。本发明还可以通过另外不同的实施方案加以应用,本说明中的各项细节也可以基于不同的观点和应用,在没有背离本发明核心思想的情况下进行各种改变。
下面结合实施例和附图对本发明进行详细说明。本发明所述的基于双层滤波框架的高精度里程估计方法的发明原理如下:
本发明使用改进的卡尔曼滤波算法将IMU、腿足里程计、双目相机的观测数据进行融合,获得移动平台的高精度位姿估计,是一种融合多种传感器观测以实现高精度位姿估计的方法。其中,IMU测量数据经过数值积分(一阶近似)获得的相对位姿作为预测,相机观测通过多状态约束卡尔曼滤波(MSC-KF)算法的更新策略对状态进行修正,腿足里程计以扩展卡尔曼滤波(EKF)算法的更新策略对状态进行再次修正。***整体框架与MSC-KF算法框架一致,即以动态窗口的形式在状态向量中存储过去一段时间内的相机位姿,以便形成图像特征约束对状态进行更新。足式里程计的观测数据往往频率较高,以足式里程计的观测频率对状态进行更新会显著提高状态估计需要花费的时间,因此本发明采用线性插值的方式对上述动态窗口中的相邻状态进行更新。采用MSC-KF滤波框架为主体,对动态窗口内的状态线性插值用以辅助更新,这种组合框架可以有效提高融合算法的运行效率,保障算法实时性。实施例一:
本实例提供一种基于双层滤波框架的高精度里程估计方法,图1为本发明的双层滤波融合框架示意图;图2为本发明应用在四足移动机器人感知***的传感器分布示意图。主要包括以下步骤:
S1,构建双目视觉惯性里程估计***,输入双目相机、IMU的内参以及双目相机、IMU、足式里程计之间的外参,开始采集双目相机、IMU、腿足辅助IMU、腿足关节电机的转速和转角反馈、以及足端力传感器的信息。
所述四足机器人感知***构建方式具体为:图2为四足机器人感知***的传感器分布示意图。如图2所示,所述四足机器人感知***实体包括足端力传感器1、双目相机2、主体IMU3、腿足辅助IMU4、腿足关节电机5。所述双目相机2固定安装在四足机器人头部;所述主体IMU3安装在四足机器人头部;所述腿足辅助IMU4安装在四足机器人身体中心;所述腿足关节电机5安装在机器人每条腿的第二个活动关节上,可以反馈该关节的转角和转速信息;所述足端力传感器1分别安装在机器人四条腿的末端。
内外参数说明:所述IMU的内参包括加速度计测量噪声的方差、加速度计随机游走噪声的方差、陀螺仪测量噪声的方差、陀螺仪随机游走噪声的方差。所述双目相机的内参包括相机的焦点坐标、焦距和相机的畸变参数。所述双层滤波融合算法所需的外参,包括主体IMU与双目相机之间的外参、主体IMU与足式里程计之间的外参以及腿足辅助IMU与腿足关节之间的外参。所述主体IMU与双目相机之间的外参,包括IMU坐标系到相机坐标系的旋转矩阵和相机坐标系在主体IMU坐标系下的位置。所述主体IMU与足式里程计之间的外参,包括主体IMU坐标系到足式里程计坐标系的旋转矩阵和足式里程计坐标系在主体IMU坐标系下的位置。所述腿足辅助IMU与腿足关节之间的外参,包括腿足辅助IMU坐标系到腿足坐标系的旋转矩阵和腿足坐标系在腿足辅助IMU坐标系下的位置。
传感器测量信息说明:所述传感器信息包括腿足关节电机的反馈信息、足端力传感器的测量信息、主体IMU和腿足辅助IMU的测量信息、以及双目视觉观测信息。所述腿足关节电机反馈信息包括电机转角信息和转速信息,采集频率为1000Hz。所述足端力传感器的测量信息包括足端与地面接触时受力的大小,采集频率为1000Hz。所述主体IMU和所述腿足辅助IMU的测量信息包括沿着IMU坐标系三轴(x,y,z)的加速度和绕着三轴的转速,信号采集频率为400Hz。所述双目视觉观测信息包括在同一时刻有双目相机采集到的左目图片和右目图片。
S2,底层腿足滤波器进行信息融合。本步骤的核心思想在于将四足机器人的触地点看成机械臂的运动末梢,在已有每个关节电机转速的情况下,通过DH参数法求解正运动学,即可求得腿足触地点和机器人身体之间的相对位置。所述上层滤波器有两个工作阶段:预测阶段和更新阶段。所述上层滤波器在所述预测阶段将主体IMU在相邻两帧图像之间测量的加速度和转速进行一阶近似的积分运算,进而得到所述四足机器人在这段时间内的移动位移和转动角度。所述上层滤波器在所述更新阶段采用多状态约束卡尔曼滤波融合方法用相机观测对IMU预测阶段的结果进行融合更新,并采用扩展卡尔曼滤波融合方法将足式里程计的速度观测对相机更新后的里程估计结果做进一步的融合更新。具体包括以下子步骤:
S201,定义底层腿足滤波的状态向量,利用腿足辅助IMU的观测模型得到预测的状态向量,具体为:
定义的底层腿足滤波的状态向量为
xT=[p R v ba bw]
其中,p表示机器人本体在世界坐标系下的位置,R表示机器人本体在世界坐标系下的旋转姿态,v表示机器人本体在世界坐标系下的速度,ba表示腿足辅助IMU的加速度计的零偏误差bias,bw表示腿足辅助IMU陀螺仪的零偏误差bias。
根据腿足辅助IMU的观测模型可以得到:
其中,上标表示腿足辅助IMU在IMU坐标系下的角速度测量值,上标表示腿足辅助IMU在IMU坐标系下的加速度测量值,IωWI和IaWI表示腿足辅助IMU在IMU坐标系下的真实的角速度和加速度,na和nω是加速度和转速观测的噪声,它们都采用零均值的高斯白噪声模型。
将腿足辅助IMU的观测转换到身体坐标系下作为***的控制输入uk,得tk时对应的输入为:
其中,RIB为对应的腿足辅助IMU坐标系到身体坐标系的旋转变换,ωk为第k时刻对应身体坐标系下的角速度,ak第k时刻对应身体坐标系下的加速度。
根据IMU运动模型,得到如下的预测关系:
其中表示t时刻预测的状态向量,下标k-1和k分别表示tk-1和tk时刻对应的变量,Δt=tk-tk-1,表示第K时刻身体坐标系下的角速度向量转换为斜对称矩阵,Rk-1表示k-1时刻机器人本体在世界坐标系下的旋转姿态,exp(·)表示指数映射操作。g为重力加速度。
其中,fk(·)和J(·)为足式机器人动力学模型及其对应的雅可比矩阵;和是腿足第二个关节的关节角和转速;ω为机器人腿足里程计坐标系下的身体转速,由腿足辅助IMU测量得到;ηv表示测量速度噪声,取值为零均值高斯白噪声,方差为1e-3左右。
S203,考虑到同一时刻有多条腿触地,利用步骤S202得到的单条腿的触地点相对于身体坐标系的速度,将每条腿触地的可能性作为其得到速度对应的权重,计算对应时刻身体的速度,即多条腿的加权平均速度值,得到平均速观测为:
其中l(·)表示连续牛顿-欧拉公式,bg表示重力加速度,τl为每条腿的力矩。
S204,计算步骤S203平均速观测对应的方差,再由该方差计算得到对应的卡尔曼增益,再利用该卡尔曼增益更新步骤S201预测的状态向量;得到更新后的状态向量;所述更新后的状态向量中的速度向量即上层滤波器所需的速度观测。具体为:
综合上述公式,可以得到对应的卡尔曼增益Kk为
然后用测量和前面计算得到的卡尔曼增益来更新预测得到对应的更新后的状态向量xk;
其中xk中含有对应的tk时刻的身体速度vk。所述更新后的状态向量中的速度向量即上层滤波器所需的速度观测
S3,将步骤S2得到的速度观测输入上层滤波器,利用上层滤波器进行信息融合并输出相对位姿。首先定义上层滤波器的状态向量,利用主体IMU的运动模型对状态向量进行预测,得到状态向量的先验,再利用双目视觉观测对状态向量的先验进行更新,得到视觉-主体IMU融合状态估计值;最后利用腿足里程计的速度观测对视觉-主体IMU融合状态估计值进行状态更新,得到上层滤波器状态向量的估计值,完成信息融合并输出机器人的里程估计结果。具体为:
其中,xI,k表示tk时刻的主体IMU状态向量;xcl,k为tk时刻保存的历史状态窗口,具体形式为:
其中,表示在tk时刻从世界坐标系到主体IMU坐标系的旋转四元数;表示在tk时刻主体IMU坐标系在世界坐标系中的位置;表示在tk时刻机器人在主体IMU坐标系下表示的速度;bg,k为主体IMU测量转速的偏移量;ba,k为主体IMU测量加速度的偏移量;为历史状态窗口中克隆保存的tk-i时刻的位姿和速度。
S302,利用主体IMU的运动模型对上层滤波器的状态向量进行预测,得到上层滤波器的状态向量的先验和方差,具体为:
S3021,通过主体IMU采集转速和线加速度,构建主体IMU的观测模型,所述主体IMU包括三轴陀螺仪和三轴加速度计;在主体IMU坐标系下,在tk时刻主体IMU的观测模型可以表示为:
其中,ωm,k为tk时刻主体IMU测量得到的角速度;am,k为tk时刻主体IMU测量得到的线加速度在世界坐标系下的表示;为tk时刻主体IMU角速度的真实值;Gak为tk时刻主体IMU线加速度的真实值;Gg为重力加速度在世界坐标系中的表示;本文采用经典方法,将IMU加速度和角速度的偏移量建模成随机游走模型,因此na,k和ng,k为零均值高斯白噪声。
S3022,构建主体IMU的运动模型:
其中,Iω=[ω1 ω2 ω3]T为主体IMU测量得到的角速度;GaI为IMU测量得到的线加速度,表示在世界坐标系下;本文采用经典方法,将IMU加速度和角速度的偏移量建模成随机游走模型,因此nωg和nωa为零均值高斯白噪声;最后其中ω∧表示反对称矩阵。
S3023,利用步骤S3021得到的主体IMU的观测模型和步骤S3022构建的主体IMU的运动模型,计算状态向量的先验:
假设需要使用[tk,tk+1]时间区间内的IMU测量值进行数值积分获得这段时间机器人的位姿变化量,进而获得tk+1时刻的机器人状态向量的先验位姿:
其中,um(tk:tk+1)表示[tk,tk+1]时间区间内IMU的全部测量值,和分别为tk和tk+1时刻的状态估计值。f()函数将IMU的测量值进行离散时间数值积分后,叠加到当前的状态向量上,得到下一时刻状态向量的预测值。
S3024,定义误差状态向量,计算状态向量的协方差矩阵,具体为:
为了将状态向量的先验与视觉观测相结合,还需要计算先验状态向量的协方差矩阵。因此,在进行状态预测的同时,还需要融合上一时刻的协方差矩阵与IMU测量噪声,为此定义误差状态向量:
其中,为***噪声向量,其协方差矩阵为 Fc为误差状态转移矩阵;Gc为输入噪声矩阵。通过线性化可以将IMU动力学模型由非线性时不变模型转化为线性时变模型。定义Φ(tk+1,tk)为线性化模型的状态转移矩阵,用于将状态的协方差矩阵从tk传播到tk+1时刻,Φ(tk+1,tk)可以使用下式求解:
其中求解上式的初值条件为:Φ(tk,tk)=I15+6M。其中,M为历史状态窗口中克隆保存的历史状态数量。使用数值计算或解析求解Φ(tk+1,tk),就可以使用标准的扩展卡尔曼滤波(EKF)框架计算先验位姿的协方差矩阵:
Pk+1|k=Φ(tk+1,tk)Pk|kΦT(tk+1,tk)+Qd,k
其中,Qd,k为离散时间***的噪声协方差矩阵:
MSC-KF会在每次状态预测后,将当前的状态中的位姿分量克隆下来,保存在历史位姿窗口xcl中,用于后续观测更新。
S303,利用双目视觉观测对状态向量的先验进行更新,得到视觉-主体IMU融合状态估计值,具体包括以下子步骤:
S3031,构建视觉观测残差;推导视觉观测的雅可比矩阵;
本发明实施例使用基于特征点的视觉观测,首先对图像进行特征提取并在多帧之间进行特征匹配,被多帧图像观测到的特征点会被三角化,最后三角化后的特征点重投影到图像平面上,与每一帧的实际观测结果之间的误差即为观测残差。
MSC-KF算法会在tk时刻将过去M个图像观测对应的位姿保存在状态向量xcl,k中,同时将特征点保存下来。假设tj(j=k,k-1,…,k-M+1)时刻相机观测到了坐标为该坐标点在的三维坐标与其在图像平面上的二维像素坐标(假设相机已经事先标定过内外参,此处指的是正则化后的相机观测)之间的关系,即视觉观测模型为:
其中,为特征点在相机坐标系下的三维坐标;Gpf为被观测到的特征点在世界坐标系中表示的三维坐标,nz,j为零均值高斯白噪声;为相机和IMU之间的旋转矩阵和相对位置,由离线外参标定得到。如前述状态定义所示,与传统的EKF-SLAM算法不同,MSC-KF算法不会在状态中保存特征点在世界坐标系中的位置,而是利用滑动窗口中的历史图像对同一个特征点的共同观测进行特征点三角化,得到特征点位置的估计值上式中的和被保留在了滑动窗口的历史状态xcl,k中,而和CpI是事先标定好的外参,因此在三角化得到后,就得到了计算观测估计值所需要的所有变量。对于每一个特征点,可以通过三角化以及观测模型计算将该特征点的实际观测记为则视觉特征的观测残差为
其中,最后一步使用了左零空间映射,将方程两边同时乘以Hf矩阵的左零空间,从而在充分利用视觉观测的同时,避免在状态向量中加入特征点位置。通过变量替换,令Hx,k=NTHx,nz,k=NTnz,可以将其转化为的形式,其中为最终的观测残差,Hx,k为最终的视觉观测雅可比矩阵。
S3032,利用多状态约束卡尔曼滤波(MSC-KF)算法融合步骤S3023得到的状态向量的先验,步骤S3024得到的状态向量的协方差矩阵,步骤S3031得到的视觉观测残差和步骤S3032得到的视觉观测的雅可比矩阵,得到视觉-主体IMU融合状态估计值。
S304,利用步骤S2腿足里程计的速度观测对步骤S303得到的视觉-主体IMU融合状态估计值进行状态更新,得到状态向量的估计值,完成信息融合并输出机器人的里程估计结果。
S3041,计算速度观测模型,构建速度观测残差;具体为:
其中,Ovm,k和Oωm,k是在足式里程计坐标系{O}下表示的身体速度和转速,表示从主体IMU坐标系{I}的旋转矩阵,GvI是{I}在{G}坐标系下表示的速度,是从世界坐标系{G}到{I}的旋转矩阵,Iω是身体在{I}坐标系下的速度,Iω∧表示向量Iω对应的反对角矩阵,IPO为{O}在{I}中的位置,nv和nω是速度和转速观测的噪声,它们都采用零均值的高斯白噪声模型。
要使用足式里程计的速度观测更新状态向量中的位姿,需要将其与状态向量中待更新的位姿进行时间对齐。为了提高扩展卡尔曼滤波(EKF)算法的运行效率,本发明对状态向量中的位姿滑窗xcl进行线性插值,且考虑到机器人位姿中的旋转矩阵在三维标准正交流形上,因此线性插值也在对应流形上完成。考虑足式里程计的速度观测采集时间为tk,在位姿滑窗中与其相邻的两个位姿对应时间戳分别为tk1和tk2,且满足tk1<tk<tk2,则对位姿的线性流形插值为:
其中,时间戳tk1和tk2是从图像观测得到的时间戳;λk为时间戳系数;tk为足式里程计的观测时间戳;为tki(i=1,2)时刻世界坐标系到IMU坐标系的旋转矩阵,为tki(i=1,2)时刻IMU在世界坐标系中的位置。因此,在tk时刻获得足式里程计的观测后,就可以根据观测信息和状态向量中的已有信息计算进而使用下式计算速度ovm的估计值
其中,是的反对称矩阵,是IMU坐标系下角速度Iω的估计值。IpO和是事先标定好的IMU与足式里程计坐标系之间的相对位姿变换关系。由此可以获得足式里程计线速度的观测残差:其中,ovm是实际观测到的足式里程计线速度。
对于角速度的估计值,可以由IMU观测与当前状态中的偏移量bias计算得到:
根据EKF的融合更新框架,要使用足式里程计的线速度和转速观测更新状态向量,只需要将观测模型经过线性化,并整理成以下形式即可:
其中,为观测残差;为误差状态向量;Hx为观测模型对状态向量的雅可比矩阵;n为观测噪声。观测残差已经在本小节前半部分内容中完成定义和建模;误差状态向量在IMU的运动模型与状态预测部分完成定义;观测噪声n为外部设置的参数。
S3042,推导速度观测的雅可比矩阵;具体为:
可以利用链式求导法则计算观测模型对状态向量中的待更新分量的雅可比矩阵表达式。用下式表示tk时刻足式里程计的观测向量Γk:
则用tk更新其相邻两个时刻tk1和tk2位姿的雅可比矩阵表达式为:
R=Exp(ω)=exp(ω∧)
ω=Log(R)=log(R)∨
其中,x∧为任意向量x对应的反对称矩阵,X∨是其逆运算,得到反对称矩阵X对应的向量;exp()和log()分别表示矩阵指数运算和矩阵对数运算。
S3042,利用扩展卡尔曼滤波(EKF)算法融合步骤S303得到的视觉-主体IMU融合状态估计值和协方差矩阵、步骤S3041得到的速度观测残差和速度观测的雅可比矩阵,得到视觉-主体IMU融合状态估计值;
综上所述,本发明有效克服了现有技术中的种种缺点而具有高度产业利用价值。本发明所述的基于双层滤波框架的高精度里程估计方法,能够高效融合IMU、相机、腿足电机反馈、足端理传感器等多种传感器,有效解决现有方法在移动平台快速移动、剧烈抖动情况下难以获得高精度位姿估计的问题。同时,通过选取多状态约束卡尔曼滤波框架为主体,对多状态进行线性插值实现多传感器的时间对齐,兼顾融合精度和算法效率,可以在计算资源受限的板载平台上进行实时的相对位姿估计。
上述实施例仅例示性说明本发明的原理及其功效,而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下,对上述实施例进行修饰或改变。因此,举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变,仍应由本发明的权利要求所涵盖。
Claims (10)
1.一种基于双层滤波框架的高精度里程估计方法,其特征在于,包括以下步骤:
S1,设定双目相机和IMU的内参,设定主体IMU与双目相机之间的外参、主体IMU与足式里程计之间的外参以及腿足辅助IMU与腿足关节电机之间的外参;采集腿足关节电机的反馈信息、足端力传感器的测量信息、主体IMU和腿足辅助IMU的测量信息以及双目视觉观测信息;
S2,利用底层腿足滤波器对步骤S1采集的腿足关节电机的转角和转速、腿足辅助IMU以及足端力传感器的测量信息进行信息融合,首先定义底层腿足滤波的状态向量,计算腿足的平均速观测及其对应的方差,再计算该方差对应的卡尔曼增益,根据该卡尔曼增益更新底层腿足滤波的状态向量,得到腿足辅助IMU坐标系下的速度观测;
S3,将步骤S2得到的速度观测输入上层滤波器,利用上层滤波器对步骤S1采集的主体IMU、相机和足式里程计的观测信息进行融合,首先定义上层滤波器的状态向量,利用主体IMU的运动模型对状态向量进行预测,得到状态向量的先验,再利用双目视觉观测对状态向量的先验进行更新,得到视觉-主体IMU融合状态估计值;最后利用腿足里程计的速度观测对视觉-主体IMU融合状态估计值进行状态更新,得到上层滤波器状态向量的估计值,完成信息融合并输出机器人的里程估计结果。
2.根据权利要求1所述的基于双层滤波框架的高精度里程估计方法,其特征在于,所述步骤S2包括以下子步骤:
S201,定义底层腿足滤波的状态向量,利用腿足辅助IMU的观测模型得到预测的状态向量;
S204,计算步骤S203平均速观测对应的方差,再由该方差计算得到对应的卡尔曼增益,再利用该卡尔曼增益更新步骤S201预测的状态向量;得到更新后的状态向量;所述更新后的状态向量中的速度向量即上层滤波器所需的速度观测。
3.根据权利要求2所述的基于双层滤波框架的高精度里程估计方法,其特征在于,所述步骤S201具体为:
定义的底层腿足滤波的状态向量为:
xT=[p R v ba bw]
其中,p表示机器人本体在世界坐标系下的位置,R表示机器人本体在世界坐标系下的旋转姿态,v表示机器人本体在世界坐标系下的速度,ba表示腿足辅助IMU的加速度计的零偏误差bias,bw表示腿足辅助IMU陀螺仪的零偏误差bias;
根据腿足辅助IMU的观测模型可以得到:
其中,表示腿足辅助IMU在IMU坐标系下的角速度测量值, IωWI和IaWI表示腿足辅助IMU在IMU坐标系下的真实的角速度和加速度,na和nω是加速度和转速观测的噪声,它们都采用零均值的高斯白噪声模型;
将腿足辅助IMU的观测转换到身体坐标系下作为***的控制输入uk,得tk时对应的输入为:
其中,RIB为对应的腿足辅助IMU坐标系到身体坐标系的旋转变换,ωk为第k时刻对应身体坐标系下的角速度,ak第k时刻对应身体坐标系下的加速度。
根据IMU运动模型,得到如下的预测关系:
6.根据权利要求2所述的基于双层滤波框架的高精度里程估计方法,其特征在于,所述步骤S204具体为:
得到对应的卡尔曼增益Kk为
其中,H表示为对应速度观测的雅克比矩阵;
然后用测量和前面计算得到的卡尔曼增益来更新预测得到对应的更新后的状态向量xk;
其中xk中含有对应的tk时刻的身体速度vk。所述更新后的状态向量中的速度向量即上层滤波器所需的速度观测。
7.根据权利要求1所述的基于双层滤波框架的高精度里程估计方法,其特征在于,所述步骤S3具体包括以下子步骤:
S301,首先定义上层滤波器的状态向量;
S302,利用主体IMU的运动模型对上层滤波器的状态向量进行预测,得到上层滤波器的状态向量的先验和方差;
S303,利用多状态约束卡尔曼滤波(MSC-KF)算法根据双目视觉观测对上层滤波器状态向量的先验进行更新,得到视觉-主体IMU融合状态估计值;
S304,利用步骤S2腿足里程计的速度观测对步骤S303得到的视觉-主体IMU融合状态估计值进行状态更新,得到上层滤波器状态向量的估计值,利用扩展卡尔曼滤波算法完成信息融合并输出机器人的里程估计结果。
9.根据权利要求7所述的基于双层滤波框架的高精度里程估计方法,其特征在于,所述步骤S302具体包括以下子步骤:
S3021,通过主体IMU采集转速和线加速度,构建主体IMU的观测模型在主体IMU坐标系下,在tk时刻主体IMU的观测模型表示为:
其中,ωm,k为tk时刻主体IMU测量得到的角速度;am,k为tk时刻主体IMU测量得到的线加速度;为tk时刻主体IMU角速度的真实值;Gak为tk时刻主体IMU线加速度的真实值;Gg为重力加速度;na,k和ng,k为零均值高斯白噪声。
S3022,构建主体IMU的运动模型:
S3023,利用步骤S3021得到的主体IMU的观测模型和步骤S3022构建的主体IMU的运动模型,计算状态向量的先验:
计算tk+1时刻的机器人状态向量的先验位姿:
其中,um(tk:tk+1)表示[tk,tk+1]时间区间内IMU的全部测量值,和分别为tk和tk+1时刻的状态估计值;f()函数将IMU的测量值进行离散时间数值积分后,叠加到当前的状态向量上,得到下一时刻状态向量的预测值。
S3024,定义误差状态向量,计算状态向量的协方差矩阵,具体为:
定义误差状态向量:
其中,为***噪声向量,其协方差矩阵为 Fc为误差状态转移矩阵;Gc为输入噪声矩阵。通过线性化可以将IMU动力学模型由非线性时不变模型转化为线性时变模型。定义Φ(tk+1,tk)为线性化模型的状态转移矩阵,用于将状态的协方差矩阵从tk传播到tk+1时刻,Φ(tk+1,tk)可以使用下式求解:
其中上式的初值条件为:Φ(tk,tk)=I15+6M,M为历史状态窗口中克隆保存的历史状态数量。使用标准的扩展卡尔曼滤波框架计算先验位姿的协方差矩阵:
Pk+1|k=Φ(tk+1,tk)Pk|kΦT(tk+1,tk)+Qd,k
其中,Qd,k为离散时间***的噪声协方差矩阵:
10.根据权利要求9所述的基于双层滤波框架的高精度里程估计方法,其特征在于,所述步骤S303具体包括以下子步骤:
S3031,构建视觉观测残差;推导视觉观测的雅可比矩阵;
S3032,利用多状态约束卡尔曼滤波算法融合步骤S3023得到的状态向量的先验、步骤S3024得到的状态向量的协方差矩阵、步骤S3031得到的视觉观测残差和视觉观测的雅可比矩阵,最终得到视觉-主体IMU融合状态估计值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111321919.7A CN114046800B (zh) | 2021-11-09 | 2021-11-09 | 一种基于双层滤波框架的高精度里程估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111321919.7A CN114046800B (zh) | 2021-11-09 | 2021-11-09 | 一种基于双层滤波框架的高精度里程估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114046800A true CN114046800A (zh) | 2022-02-15 |
CN114046800B CN114046800B (zh) | 2023-09-29 |
Family
ID=80207829
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111321919.7A Active CN114046800B (zh) | 2021-11-09 | 2021-11-09 | 一种基于双层滤波框架的高精度里程估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114046800B (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024037155A1 (zh) * | 2022-08-17 | 2024-02-22 | 腾讯科技(深圳)有限公司 | 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质 |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104897158A (zh) * | 2015-06-26 | 2015-09-09 | 中国科学院上海高等研究院 | 一种步行者室内双层定位方法及*** |
CN105865452A (zh) * | 2016-04-29 | 2016-08-17 | 浙江国自机器人技术有限公司 | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 |
CN110455309A (zh) * | 2019-08-27 | 2019-11-15 | 清华大学 | 具备在线时间校准的基于msckf的视觉惯性里程计 |
CN111811506A (zh) * | 2020-09-15 | 2020-10-23 | 中国人民解放军国防科技大学 | 视觉/惯性里程计组合导航方法、电子设备及存储介质 |
CN111896007A (zh) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | 一种补偿足地冲击的四足机器人姿态解算方法 |
CN111949929A (zh) * | 2020-08-12 | 2020-11-17 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
-
2021
- 2021-11-09 CN CN202111321919.7A patent/CN114046800B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104897158A (zh) * | 2015-06-26 | 2015-09-09 | 中国科学院上海高等研究院 | 一种步行者室内双层定位方法及*** |
CN105865452A (zh) * | 2016-04-29 | 2016-08-17 | 浙江国自机器人技术有限公司 | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 |
CN110455309A (zh) * | 2019-08-27 | 2019-11-15 | 清华大学 | 具备在线时间校准的基于msckf的视觉惯性里程计 |
CN111896007A (zh) * | 2020-08-12 | 2020-11-06 | 智能移动机器人(中山)研究院 | 一种补偿足地冲击的四足机器人姿态解算方法 |
CN111949929A (zh) * | 2020-08-12 | 2020-11-17 | 智能移动机器人(中山)研究院 | 一种多传感器融合的四足机器人运动里程计设计方法 |
CN111811506A (zh) * | 2020-09-15 | 2020-10-23 | 中国人民解放军国防科技大学 | 视觉/惯性里程计组合导航方法、电子设备及存储介质 |
Non-Patent Citations (2)
Title |
---|
YONG LIU,ET AL: "Stereo Visual-Inertial Odometry With Multiple Kalman Filters Ensemble", 《IEEE TRANSACTIONS ON INDUSTRIAL ELECTRONICS》, vol. 63, no. 10, pages 620606216 * |
徐之航;欧阳威;武元新;: "基于改进克隆卡尔曼滤波的视觉惯性里程计实现", 信息技术, no. 05, pages 21 - 28 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2024037155A1 (zh) * | 2022-08-17 | 2024-02-22 | 腾讯科技(深圳)有限公司 | 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN114046800B (zh) | 2023-09-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10317214B2 (en) | Inertial odometry with retroactive sensor calibration | |
CN110243358B (zh) | 多源融合的无人车室内外定位方法及*** | |
Lynen et al. | A robust and modular multi-sensor fusion approach applied to MAV navigation | |
CN110986939B (zh) | 一种基于imu预积分的视觉惯性里程计方法 | |
CN109885080B (zh) | 自主控制***及自主控制方法 | |
CN106525049B (zh) | 一种基于计算机视觉的四足机器人本***姿跟踪方法 | |
WO2023082050A1 (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
JP6855524B2 (ja) | 低速特徴からのメトリック表現の教師なし学習 | |
US20130162785A1 (en) | Method and system for fusing data arising from image sensors and from motion or position sensors | |
Eckenhoff et al. | Sensor-failure-resilient multi-imu visual-inertial navigation | |
CN111949929B (zh) | 一种多传感器融合的四足机器人运动里程计设计方法 | |
CN112113582A (zh) | 时间同步处理方法、电子设备及存储介质 | |
WO2021031346A1 (zh) | 基于多传感器混合滤波器的在线机器人运动学校准方法 | |
CN110967017B (zh) | 一种用于双移动机器人刚体协作搬运的协同定位方法 | |
CN111707261A (zh) | 一种微型无人机高速感知和定位方法 | |
Zhang et al. | Vision-aided localization for ground robots | |
CN110207693B (zh) | 一种鲁棒立体视觉惯性预积分slam方法 | |
CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
CN115143954B (zh) | 一种基于多源信息融合的无人车导航方法 | |
Yousuf et al. | Information fusion of GPS, INS and odometer sensors for improving localization accuracy of mobile robots in indoor and outdoor applications | |
CN113483755A (zh) | 一种基于非全局一致地图的多传感器组合定位方法及*** | |
CN115388899A (zh) | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 | |
CN115218906A (zh) | 面向室内slam的视觉惯性融合定位方法及*** | |
CN114046800B (zh) | 一种基于双层滤波框架的高精度里程估计方法 | |
CN112284381B (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |