CN114046800A - 一种基于双层滤波框架的高精度里程估计方法 - Google Patents

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的观测模型得到预测的状态向量，具体为：

定义的底层腿足滤波的状态向量为

x^T＝[p R v b^a b^w]

其中，p表示机器人本体在世界坐标系下的位置，R表示机器人本体在世界坐标系下的旋转姿态，v表示机器人本体在世界坐标系下的速度，b^a表示腿足辅助IMU的加速度计的零偏误差bias，b^w表示腿足辅助IMU陀螺仪的零偏误差bias。

根据腿足辅助IMU的观测模型可以得到：

其中，

上标表示腿足辅助IMU在IMU坐标系下的角速度测量值，

上标表示腿足辅助IMU在IMU坐标系下的加速度测量值，_Iω_WI和_Ia_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为重力加速度。

S202，计算单条腿

的触地点相对于身体坐标系的速度；

对于每条腿

定义接触的状态

其中1表示腿

在t_k时刻触地。接下来根据串联结构的正运动学模型，同时考虑噪声的干扰,在t_k时刻，得到单条腿

的触地点相对于身体坐标系的速度为：

其中，f_k(·)和J(·)为足式机器人动力学模型及其对应的雅可比矩阵；

和

是腿足第二个关节的关节角和转速；ω为机器人腿足里程计坐标系下的身体转速，由腿足辅助IMU测量得到；η^v表示测量速度噪声，取值为零均值高斯白噪声，方差为1e^-3左右。

S203，考虑到同一时刻有多条腿触地，利用步骤S202得到的单条腿

的触地点相对于身体坐标系的速度，将每条腿触地的可能性作为其得到速度对应的权重，计算对应时刻身体的速度，即多条腿的加权平均速度值，得到平均速观测为：

其中，P_k(·)为通过对应的触地力与触地概率之间的函数关系(可通过实验测试拟合得到)，C为所有腿的集合，

表示在t_k时刻腿

触地时候同地面接触的力，其可通过如下串联机器人的动力学模型得到：

其中_l(·)表示连续牛顿-欧拉公式，_bg表示重力加速度,τ_l为每条腿的力矩。

S204，计算步骤S203平均速观测对应的方差，再由该方差计算得到对应的卡尔曼增益，再利用该卡尔曼增益更新步骤S201预测的状态向量；得到更新后的状态向量；所述更新后的状态向量中的速度向量即上层滤波器所需的速度观测。具体为：

计算步骤S203平均速观测对应的方差

为：

其中，

为零时刻的初始方差，α为经验值决定的归一化系数，I₃为单位矩阵，Δf表示由于触地时候的冲击导致的力的不连续性产生的单位时间内每条腿的力的平均差值，令c_k表示为t_k时触地腿的总数，则：

表示为对应每条腿速度的方差：

综合上述公式，可以得到对应的卡尔曼增益K_k为

其中H表示为对应速度观测的雅克比矩阵，由于这里的速度观测

所以对应的雅克比矩阵为对应线速度的选择矩阵。

然后用测量和前面计算得到的卡尔曼增益来更新预测得到对应的更新后的状态向量x_k；

其中x_k中含有对应的t_k时刻的身体速度v_k。所述更新后的状态向量中的速度向量即上层滤波器所需的速度观测

S3，将步骤S2得到的速度观测输入上层滤波器，利用上层滤波器进行信息融合并输出相对位姿。首先定义上层滤波器的状态向量，利用主体IMU的运动模型对状态向量进行预测，得到状态向量的先验，再利用双目视觉观测对状态向量的先验进行更新，得到视觉-主体IMU融合状态估计值；最后利用腿足里程计的速度观测对视觉-主体IMU融合状态估计值进行状态更新，得到上层滤波器状态向量的估计值，完成信息融合并输出机器人的里程估计结果。具体为：

S301，首先定义上层滤波器的状态向量，在t_k时刻上层滤波器的状态向量

的定义如下：

其中，x_I,k表示t_k时刻的主体IMU状态向量；x_cl,k为t_k时刻保存的历史状态窗口，具体形式为：

其中，

表示在t_k时刻从世界坐标系到主体IMU坐标系的旋转四元数；

表示在t_k时刻主体IMU坐标系在世界坐标系中的位置；

表示在t_k时刻机器人在主体IMU坐标系下表示的速度；b_g,k为主体IMU测量转速的偏移量；b_a,k为主体IMU测量加速度的偏移量；

为历史状态窗口中克隆保存的t_k-i时刻的位姿和速度。

S302，利用主体IMU的运动模型对上层滤波器的状态向量进行预测，得到上层滤波器的状态向量的先验和方差，具体为：

S3021，通过主体IMU采集转速和线加速度，构建主体IMU的观测模型，所述主体IMU包括三轴陀螺仪和三轴加速度计；在主体IMU坐标系下，在t_k时刻主体IMU的观测模型可以表示为：

其中，ω_m,k为t_k时刻主体IMU测量得到的角速度；a_m,k为t_k时刻主体IMU测量得到的线加速度在世界坐标系下的表示；

为t_k时刻主体IMU角速度的真实值；^Ga_k为t_k时刻主体IMU线加速度的真实值；^Gg为重力加速度在世界坐标系中的表示；本文采用经典方法，将IMU加速度和角速度的偏移量建模成随机游走模型，因此n_a,k和n_g,k为零均值高斯白噪声。

S3022，构建主体IMU的运动模型：

其中，^Iω＝[ω₁ ω₂ ω₃]^T为主体IMU测量得到的角速度；^Ga_I为IMU测量得到的线加速度，表示在世界坐标系下；本文采用经典方法，将IMU加速度和角速度的偏移量建模成随机游走模型，因此n_ωg和n_ωa为零均值高斯白噪声；最后

其中ω^∧表示反对称矩阵。

S3023，利用步骤S3021得到的主体IMU的观测模型和步骤S3022构建的主体IMU的运动模型，计算状态向量的先验：

假设需要使用[t_k,t_k+1]时间区间内的IMU测量值进行数值积分获得这段时间机器人的位姿变化量，进而获得t_k+1时刻的机器人状态向量的先验位姿：

其中，u_m(t_k:t_k+1)表示[t_k,t_k+1]时间区间内IMU的全部测量值，

和

分别为t_k和t_k+1时刻的状态估计值。f()函数将IMU的测量值进行离散时间数值积分后，叠加到当前的状态向量上，得到下一时刻状态向量的预测值。

S3024，定义误差状态向量，计算状态向量的协方差矩阵，具体为：

为了将状态向量的先验与视觉观测相结合，还需要计算先验状态向量的协方差矩阵。因此，在进行状态预测的同时，还需要融合上一时刻的协方差矩阵与IMU测量噪声，为此定义误差状态向量：

其中，除了姿态角

的误差状态向量外，其他状态向量x对应的误差状态向量为：

其中

是状态向量x的估计值；姿态的误差向量使用

进行定义。将主体IMU的运动模型在当前状态估计值处线性化：

其中，

为系统噪声向量，其协方差矩阵为

F_c为误差状态转移矩阵；G_c为输入噪声矩阵。通过线性化可以将IMU动力学模型由非线性时不变模型转化为线性时变模型。定义Φ(t_k+1,t_k)为线性化模型的状态转移矩阵，用于将状态的协方差矩阵从t_k传播到t_k+1时刻，Φ(t_k+1,t_k)可以使用下式求解：

其中求解上式的初值条件为：Φ(t_k,t_k)＝I_15+6M。其中，M为历史状态窗口中克隆保存的历史状态数量。使用数值计算或解析求解Φ(t_k+1,t_k)，就可以使用标准的扩展卡尔曼滤波(EKF)框架计算先验位姿的协方差矩阵：

P_k+1|k＝Φ(t_k+1,t_k)P_k|kΦ^T(t_k+1,t_k)+Q_d,k

其中，Q_d,k为离散时间系统的噪声协方差矩阵：

MSC-KF会在每次状态预测后，将当前的状态中的位姿分量克隆下来，保存在历史位姿窗口x_cl中，用于后续观测更新。

S303，利用双目视觉观测对状态向量的先验进行更新，得到视觉-主体IMU融合状态估计值，具体包括以下子步骤：

S3031，构建视觉观测残差；推导视觉观测的雅可比矩阵；

本发明实施例使用基于特征点的视觉观测，首先对图像进行特征提取并在多帧之间进行特征匹配，被多帧图像观测到的特征点会被三角化，最后三角化后的特征点重投影到图像平面上，与每一帧的实际观测结果之间的误差即为观测残差。

MSC-KF算法会在t_k时刻将过去M个图像观测对应的位姿保存在状态向量x_cl,k中，同时将特征点保存下来。假设t_j(j＝k,k-1,…,k-M+1)时刻相机观测到了坐标为

该坐标点在的三维坐标与其在图像平面上的二维像素坐标(假设相机已经事先标定过内外参，此处指的是正则化后的相机观测)之间的关系，即视觉观测模型为：

其中，

为特征点在相机坐标系下的三维坐标；^Gp_f为被观测到的特征点在世界坐标系中表示的三维坐标，n_z,j为零均值高斯白噪声；

为相机和IMU之间的旋转矩阵和相对位置，由离线外参标定得到。如前述状态定义所示，与传统的EKF-SLAM算法不同，MSC-KF算法不会在状态中保存特征点在世界坐标系中的位置，而是利用滑动窗口中的历史图像对同一个特征点的共同观测进行特征点三角化，得到特征点位置的估计值

上式中的

和

被保留在了滑动窗口的历史状态x_cl,k中，而

和^Cp_I是事先标定好的外参，因此在三角化得到

后，就得到了计算观测估计值

所需要的所有变量。对于每一个特征点，可以通过三角化以及观测模型计算

将该特征点的实际观测记为

则视觉特征的观测残差为

从表达式可以看出，特征点在图像上的二维坐标是状态向量和特征点全局坐标的非线性函数，以当前状态的估计值

和三角化得到的特征点位置估计值

为线性化点，可以将观测模型线性化：

其中

为观测残差，H_x,j和H_f,j是观测模型关于状态向量先验值和特征点位置估计值的雅可比矩阵。由于历史窗口中存在多个状态观测到这个特征点，因此会有多个上述方程，将其统一整理成矩阵形式可以得到：

其中，最后一步使用了左零空间映射，将方程两边同时乘以H_f矩阵的左零空间，从而在充分利用视觉观测的同时，避免在状态向量中加入特征点位置。通过变量替换，令

H_x,k＝N^TH_x，n_z,k＝N^Tn_z，可以将其转化为

的形式，其中

为最终的观测残差，H_x,k为最终的视觉观测雅可比矩阵。

S3032，利用多状态约束卡尔曼滤波(MSC-KF)算法融合步骤S3023得到的状态向量的先验，步骤S3024得到的状态向量的协方差矩阵，步骤S3031得到的视觉观测残差和步骤S3032得到的视觉观测的雅可比矩阵，得到视觉-主体IMU融合状态估计值。

S304，利用步骤S2腿足里程计的速度观测对步骤S303得到的视觉-主体IMU融合状态估计值进行状态更新，得到状态向量的估计值，完成信息融合并输出机器人的里程估计结果。

S3041，计算速度观测模型，构建速度观测残差；具体为：

假设足式里程计与主体IMU之间的外参为

则速度观测模型为：

其中，^Ov_m,k和^Oω_m,k是在足式里程计坐标系{O}下表示的身体速度和转速，

表示从主体IMU坐标系{I}的旋转矩阵，^Gv_I是{I}在{G}坐标系下表示的速度，

是从世界坐标系{G}到{I}的旋转矩阵，^Iω是身体在{I}坐标系下的速度，^Iω^∧表示向量^Iω对应的反对角矩阵，^IP_O为{O}在{I}中的位置,n_v和n_ω是速度和转速观测的噪声，它们都采用零均值的高斯白噪声模型。

要使用足式里程计的速度观测更新状态向量中的位姿，需要将其与状态向量中待更新的位姿进行时间对齐。为了提高扩展卡尔曼滤波(EKF)算法的运行效率，本发明对状态向量中的位姿滑窗x_cl进行线性插值，且考虑到机器人位姿中的旋转矩阵在三维标准正交流形上，因此线性插值也在对应流形上完成。考虑足式里程计的速度观测采集时间为t_k，在位姿滑窗中与其相邻的两个位姿对应时间戳分别为t_k1和t_k2，且满足t_k1＜t_k＜t_k2，则对位姿的线性流形插值为：

其中，时间戳t_k1和t_k2是从图像观测得到的时间戳；λ_k为时间戳系数；t_k为足式里程计的观测时间戳；

为t_ki(i＝1,2)时刻世界坐标系到IMU坐标系的旋转矩阵，

为t_ki(i＝1,2)时刻IMU在世界坐标系中的位置。因此，在t_k时刻获得足式里程计的观测后，就可以根据观测信息和状态向量中的已有信息计算

进而使用下式计算速度^ov_m的估计值

其中，

是

的反对称矩阵，

是IMU坐标系下角速度^Iω的估计值。^Ip_O和

是事先标定好的IMU与足式里程计坐标系之间的相对位姿变换关系。由此可以获得足式里程计线速度的观测残差：

其中，^ov_m是实际观测到的足式里程计线速度。

对于角速度的估计值，可以由IMU观测与当前状态中的偏移量bias计算得到：

其中，

是IMU坐标系下角速度^Iω的估计值，

是事先标定好的IMU与足式里程计坐标系之间的相对位姿变换关系。由此可以定义足式里程计转速的观测残差为：

其中，^oω_m是实际观测到的足式里程计转速。

根据EKF的融合更新框架，要使用足式里程计的线速度和转速观测更新状态向量，只需要将观测模型经过线性化，并整理成以下形式即可：

其中，

为观测残差；

为误差状态向量；H_x为观测模型对状态向量的雅可比矩阵；n为观测噪声。观测残差

已经在本小节前半部分内容中完成定义和建模；误差状态向量

在IMU的运动模型与状态预测部分完成定义；观测噪声n为外部设置的参数。

S3042，推导速度观测的雅可比矩阵；具体为：

可以利用链式求导法则计算观测模型对状态向量中的待更新分量的雅可比矩阵表达式。用下式表示t_k时刻足式里程计的观测向量Γ_k：

则用t_k更新其相邻两个时刻t_k1和t_k2位姿的雅可比矩阵表达式为：

要计算表达式

需要使用足式里程计观测方程对状态向量中的所有状态量分别求导，其中大多数求导后的结果为零向量。由于雅可比矩阵的定义是明确的，因此对于矩阵的具体形式，本文不予罗列。这里只给出求导后的非零项：

其中，

和^Ip_o是预先标定好的IMU与足式里程计坐标系之间的外参；

和

由时间戳对齐部分的线性插值算法计算得到；任意向量x对应的反对称矩阵为x^∧。至此，已经完成了

的推导计算。

在计算

和

之前，需要先对两个运算符号进行说明。旋转矩阵

对应的李代数记为

其中，

表示

对应的李代数集合。因此，存在互为逆运算的等式：

R＝Exp(ω)＝exp(ω^∧)

ω＝Log(R)＝log(R)^∨

其中，x^∧为任意向量x对应的反对称矩阵，X^∨是其逆运算，得到反对称矩阵X对应的向量；exp()和log()分别表示矩阵指数运算和矩阵对数运算。

对于

和

同样只列出非零项：

其中，

I₃为三维单位矩阵。

至此，已经完成了对雅可比矩阵中非零项的推导，加上前文对误差状态向量的定义和对观测残差的定义和推导，已经可以将足式里程计观测模型转化为

的形式，进而使用EKF融合框架进行更新。

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的观测模型得到预测的状态向量；

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坐标系下的角速度测量值，

_Iω_WI和_Ia_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(·)表示连续牛顿-欧拉公式，_bg表示重力加速度，τ_l为每条腿的力矩。

6.根据权利要求2所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S204具体为：

计算步骤S203平均速观测对应的方差

为：

其中，

为零时刻的初始方差，α为经验值决定的归一化系数，I₃为单位矩阵，Δf表示由于触地时候的冲击导致的力的不连续性产生的单位时间内每条腿的力的平均差值，令c_k表示为t_k时触地腿的总数，则：

其中，

表示为对应每条腿速度的方差：

得到对应的卡尔曼增益K_k为

其中，H表示为对应速度观测的雅克比矩阵；

然后用测量和前面计算得到的卡尔曼增益来更新预测得到对应的更新后的状态向量x_k；

其中x_k中含有对应的t_k时刻的身体速度v_k。所述更新后的状态向量中的速度向量即上层滤波器所需的速度观测。

7.根据权利要求1所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S3具体包括以下子步骤：

S301，首先定义上层滤波器的状态向量；

S302，利用主体IMU的运动模型对上层滤波器的状态向量进行预测，得到上层滤波器的状态向量的先验和方差；

S303，利用多状态约束卡尔曼滤波(MSC-KF)算法根据双目视觉观测对上层滤波器状态向量的先验进行更新，得到视觉-主体IMU融合状态估计值；

S304，利用步骤S2腿足里程计的速度观测对步骤S303得到的视觉-主体IMU融合状态估计值进行状态更新，得到上层滤波器状态向量的估计值，利用扩展卡尔曼滤波算法完成信息融合并输出机器人的里程估计结果。

8.根据权利要求7所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S301具体为：

定义在t_k时刻上层滤波器的状态向量：

其中，x_I，k表示t_k时刻的主体IMU状态向量；x_cl，k为t_k时刻保存的历史状态窗口，具体形式为：

其中，

表示在t_k时刻从世界坐标系到主体IMU坐标系的旋转四元数；

表示在t_k时刻主体IMU坐标系在世界坐标系中的位置；

表示在t_k时刻机器人在主体IMU坐标系下表示的速度；b_g，k为主体IMU测量转速的偏移量；b_a，k为主体IMU测量加速度的偏移量；

为历史状态窗口中克隆保存的t_k-i时刻的位姿和速度。

9.根据权利要求7所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S302具体包括以下子步骤：

S3021，通过主体IMU采集转速和线加速度，构建主体IMU的观测模型在主体IMU坐标系下，在t_k时刻主体IMU的观测模型表示为：

其中，ω_m，k为t_k时刻主体IMU测量得到的角速度；a_m,k为t_k时刻主体IMU测量得到的线加速度；

为t_k时刻主体IMU角速度的真实值；^Ga_k为t_k时刻主体IMU线加速度的真实值；^Gg为重力加速度；n_a，k和n_g，k为零均值高斯白噪声。

S3022，构建主体IMU的运动模型：

其中，^Iω＝[ω₁ ω₂ ω₃]^T为主体IMU测量得到的角速度；^Ga_I为IMU测量得到的线加速度；最后

其中ω^表示反对称矩阵。

S3023，利用步骤S3021得到的主体IMU的观测模型和步骤S3022构建的主体IMU的运动模型，计算状态向量的先验：

计算t_k+1时刻的机器人状态向量的先验位姿：

其中，u_m(t_k：t_k+1)表示[t_k，t_k+1]时间区间内IMU的全部测量值，

和

分别为t_k和t_k+1时刻的状态估计值；f()函数将IMU的测量值进行离散时间数值积分后，叠加到当前的状态向量上，得到下一时刻状态向量的预测值。

S3024，定义误差状态向量，计算状态向量的协方差矩阵，具体为：

定义误差状态向量：

其中，除了姿态角

的误差状态向量外，其他状态向量x对应的误差状态向量为：

其中

是状态向量x的估计值；姿态的误差向量使用

进行定义；将主体IMU的运动模型在当前状态估计值处线性化：

其中，

为系统噪声向量，其协方差矩阵为

F_c为误差状态转移矩阵；G_c为输入噪声矩阵。通过线性化可以将IMU动力学模型由非线性时不变模型转化为线性时变模型。定义Φ(t_k+1，t_k)为线性化模型的状态转移矩阵，用于将状态的协方差矩阵从t_k传播到t_k+1时刻，Φ(t_k+1，t_k)可以使用下式求解：

其中上式的初值条件为：Φ(t_k，t_k)＝I_15+6M，M为历史状态窗口中克隆保存的历史状态数量。使用标准的扩展卡尔曼滤波框架计算先验位姿的协方差矩阵：

P_k+1|k＝Φ(t_k+1，t_k)P_k|kΦ^T(t_k+1，t_k)+Q_d，k

其中，Q_d，k为离散时间系统的噪声协方差矩阵：

10.根据权利要求9所述的基于双层滤波框架的高精度里程估计方法，其特征在于，所述步骤S303具体包括以下子步骤：

S3031，构建视觉观测残差；推导视觉观测的雅可比矩阵；

S3032，利用多状态约束卡尔曼滤波算法融合步骤S3023得到的状态向量的先验、步骤S3024得到的状态向量的协方差矩阵、步骤S3031得到的视觉观测残差和视觉观测的雅可比矩阵，最终得到视觉-主体IMU融合状态估计值。

Images