一种基于双层滤波框架的高精度里程估计方法技术

本发明专利技术公开了一种基于双层滤波框架的高精度里程估计方法，该方法首先采集双目相机、主体IMU、腿足辅助IMU、腿足关节电机和足端力传感器的传感信息；利用底层腿足滤波器对腿足关节电机的转角和转速、腿足辅助IMU以及足端力传感器的测量信息进行信息融合，得到腿足辅助IMU坐标系下的速度观测；再将速度观测输入上层滤波器，利用上层滤波器对采集的主体IMU、双目相机的观测信息进行融合，完成信息融合并输出机器人的里程估计结果。本发明专利技术方法能够有效解决现有方法在四足移动机器人快速运动中，由于剧烈抖动导致的里程估计精度不高甚至算法失效等问题，为四足移动机器人在快速运动中的自主定位提供了有效解决方案。的自主定位提供了有效解决方案。的自主定位提供了有效解决方案。

【技术实现步骤摘要】
一种基于双层滤波框架的高精度里程估计方法


[0001]本专利技术属于移动机器人自主定位
，尤其涉及一种基于双层滤波框架的高精度里程估计方法。

技术介绍

[0002]随着人工智能的迅速发展，机器人、无人车、虚拟现实、增强现实等领域成为研究热点，尤其是在环境未知且GPS信号无法使用的情况下，实现精确的自主定位并构建地图是该领域的研究前提。
[0003]惯性测量单元(IMU)能以很高的频率测量移动平台的加速度和转速，从而在较短时间段内实现较高精度的相对位姿估计。然而，IMU存在零点漂移，且受噪声影响，随着运行时间的增加，通过积分(累加)的方式计算相对位姿产生的累积误差也会逐渐增大，且无法矫正。因此，通常使用其他能够感知外部环境的传感器进行辅助定位，以降低IMU噪声累积和零点漂移等问题对里程估计精度的影响。视觉辅助惯性里程估计方法(Visual
‑
aided Inertial Odometry,VIO)通过相机对IMU的噪声和零漂进行矫正，以其较低的成本、较高的鲁棒性成为最早成功应用在航空航天、VR/AR、自动驾驶等领域本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于双层滤波框架的高精度里程估计方法，其特征在于，包括以下步骤：S1，设定双目相机和IMU的内参，设定主体IMU与双目相机之间的外参、主体IMU与足式里程计之间的外参以及腿足辅助IMU与腿足关节电机之间的外参；采集腿足关节电机的反馈信息、足端力传感器的测量信息、主体IMU和腿足辅助IMU的测量信息以及双目视觉观测信息；S2，利用底层腿足滤波器对步骤S1采集的腿足关节电机的转角和转速、腿足辅助IMU以及足端力传感器的测量信息进行信息融合，首先定义底层腿足滤波的状态向量，计算腿足的平均速观测及其对应的方差，再计算该方差对应的卡尔曼增益，根据该卡尔曼增益更新底层腿足滤波的状态向量，得到腿足辅助IMU坐标系下的速度观测；S3,将步骤S2得到的速度观测输入上层滤波器，利用上层滤波器对步骤S1采集的主体IMU、相机和足式里程计的观测信息进行融合，首先定义上层滤波器的状态向量，利用主体IMU的运动模型对状态向量进行预测，得到状态向量的先验，再利用双目视觉观测对状态向量的先验进行更新，得到视觉
‑
主体IMU融合状态估计值；最后利用腿足里程计的速度观测对视觉
‑
主体IMU融合状态估计值进行状态更新，得到上层滤波器状态向量的估计值，完成信息融合并输出机器人的里程估计结果。2.根据权利要求1所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S2包括以下子步骤：S201，定义底层腿足滤波的状态向量，利用腿足辅助IMU的观测模型得到预测的状态向量；S202，计算单条腿的触地点相对于身体坐标系的速度；S203，利用步骤S202得到的单条腿的触地点相对于身体坐标系的速度，将每条腿触地的可能性作为其得到速度对应的权重，计算对应时刻身体的速度，即多条腿的加权平均速度值，得到平均速观测；S204，计算步骤S203平均速观测对应的方差，再由该方差计算得到对应的卡尔曼增益，再利用该卡尔曼增益更新步骤S201预测的状态向量；得到更新后的状态向量；所述更新后的状态向量中的速度向量即上层滤波器所需的速度观测。3.根据权利要求2所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S201具体为：定义的底层腿足滤波的状态向量为：x
T
＝[p R v b
a b
w
]其中，p表示机器人本体在世界坐标系下的位置，R表示机器人本体在世界坐标系下的旋转姿态，v表示机器人本体在世界坐标系下的速度，b
a
表示腿足辅助IMU的加速度计的零偏误差bias，b
w
表示腿足辅助IMU陀螺仪的零偏误差bias；根据腿足辅助IMU的观测模型可以得到：根据腿足辅助IMU的观测模型可以得到：其中，表示腿足辅助IMU在IMU坐标系下的角速度测量值，
I
ω
WI
和
I
a
WI
表示腿足辅助IMU在IMU坐标系下的真实的角速度和加速度，n
a
和n
ω
是加速度和转速观测的噪声，它
们都采用零均值的高斯白噪声模型；将腿足辅助IMU的观测转换到身体坐标系下作为系统的控制输入u
k
，得t
k
时对应的输入为：其中，R
IB
为对应的腿足辅助IMU坐标系到身体坐标系的旋转变换，ω
k
为第k时刻对应身体坐标系下的角速度，a
k
第k时刻对应身体坐标系下的加速度。根据IMU运动模型，得到如下的预测关系：其中表示t时刻预测的状态向量，下标k
‑
1和k分别表示t
k
‑1和t
k
时刻对应的变量，Δt＝t
k
‑
t
k
‑1，表示第K时刻身体坐标系下的角速度向量转换为斜对称矩阵，R
k
‑1表示k
‑
1时刻机器人本体在世界坐标系下的旋转姿态，exp(
·
)表示指数映射操作。g为重力加速度。4.根据权利要求2所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S202具体为：对于每条腿定义接触的状态其中1表示腿在t
k
时刻触地；在t
k
时刻，得到单条腿的触地点相对于身体坐标系的速度为：其中，f
k
(
·
)和J(
·
)为足式机器人动力学模型及其对应的雅可比矩阵；和是腿足第二个关节的关节角和转速；ω为机器人腿足里程计坐标系下的身体转速；η
v
表示测量速度噪声。5.根据权利要求2所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S203具体为：利用步骤S202得到的单条腿的触地点相对于身体坐标系的速度，将每条腿触地的可能性作为其得到速度对应的权重，计算对应时刻身体的速度，即多条腿的加权平均速度值，得到平均速观测为：其中，P
k
(
·
)为通过对应的触地力与触地概率之间的关系，C为所有腿的集合，表示在t
k
时刻腿触地时候同地面接触的力，其通过如下串联机器人的动力学模型得到：其中，h
l
(
·
)表示连续牛顿
‑
欧拉公式，
b
g表示重力加速度，τ
l
为每条腿的力矩。6.根据权利要求2所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S204具体为：计算步骤S203平均速观测对应的方差为：
其中，为零时刻的初始方差，α为经验值决定的归一化系数，I3为单位矩阵，Δf表示由于触地时候的冲击导致的力的不连续性产生的单位时间内每条腿的力的平均差值，令c
k
表示为t
k
时触地腿的总数，则：其中，表示为对应每条腿速度的方差：得到对应的卡尔曼增益K
k
为其中，H表示为对应速度观测的雅克比矩阵；然后用测量和前面计算得到的卡尔...

【专利技术属性】
技术研发人员：王越，贾慎涵，韩福长，熊蓉，
申请(专利权)人：浙江大学，
类型：发明
国别省市：