一种基于多传感器融合的六足机器人定位方法与流程

技术特征：
1.一种基于多传感器融合的六足机器人定位方法，六足机器人包括机身和六条腿，每条腿分为多节，每条腿与机身之间以及每条腿的相邻两节之间均通过关节连接，每条腿的底部为足端，其特征在于，六足机器人定位方法包括以下步骤：s1：在六足机器人上安置多款传感器，根据所述传感器采集的原数据进行数据预处理工作，传感器包括相机、imu和带有位置反馈的舵机，利用相机采集图像信息，提取与跟踪图像特征点；利用imu采集加速度与角速度，处理获取imu预积分；利用带有位置反馈的舵机采集关节角度，处理获取六足机器人运动里程计预积分；s2：根据步骤s1中图像特征点的提取与跟踪，利用sfm算法进行纯视觉估计滑窗内关键帧以及非关键帧的视觉位姿以及3d点坐标信息；s3：分别将视觉位姿、imu预积分、六足机器人运动里程计预积分两两结合进行对齐求解初始化参数，初始化参数包括陀螺仪零偏校正、重力以及速度初始化和尺度参数的初始化；s4：针对滑窗中的关键帧，将视觉重投影残差、imu预积分残差、六足机器人运动里程计预积分残差以及先验误差约束量联合构建残差函数，对残差函数进行非线性优化求解，得到六足机器人的实时位姿信息；s5：基于dbow2词袋算法对六足机器人的位姿信息进行回环检测，检测成功后进行重定位，再对整个相机轨迹进行回环优化，进而对滑窗内的位姿进行修正，最后完成基于多传感器融合的六足机器人定位。2.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s1中利用相机采集图像信息，处理获得图像特征点的提取与跟踪，具体包括如下步骤：s111：将相机采集到的原图像数据均衡化处理后，利用基于金字塔分层的光流法跟踪相邻两帧，剔除异常点，并使用shi-tomasi角点检测算法对图像提取特征点信息加以补给；s112：在保证每帧图像总特征点数不小于设定阈值基础上，对特征点提取过程进行四叉树均匀化处理算法，使图像特征点分布均匀。3.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s1中imu采集三轴加速度与角速度，处理获取imu预积分，具体包括如下步骤：s121：imu包括加速度计与陀螺仪，分别用于读取imu坐标系下的加速度和角速度测量信息；将相邻两帧图像第k帧与第k 1帧间的所有imu数据进行积分，计算得到第k 1帧在世界坐标系下的位置、速度以及姿态信息；s122：将步骤s121得到的第k 1帧位置、速度以及姿态信息的参考坐标系从世界坐标系w变换为第k个关键帧时刻的imu坐标系bk，从而提取出积分结果为bk 1对于bk的相对运动量，即imu预积分；imu预积分项为：其中，为连续两帧之间imu预积分提供的位移增量，为连续两帧之间imu预积分提供的速度增量，为连续两帧之间imu预积分提供的旋转增量(四元数)，b
a
为imu加速度计的零偏量，b
w
为imu陀螺仪的零偏量。4.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s1中带有位置反馈的舵
机采集关节角度，处理获取六足机器人运动里程计预积分，具体包括如下步骤：s131：六足机器人运动里程计：s1311：六足机器人运动里程计位姿求解成立条件：(1)任何时刻，至少三条非共线足端的腿支撑，(2)求解位姿时，与地面接触的腿没有足端滑动，(3)在六足机器人行走过程中，一定存在足端构成的支撑三角形，即单站相，可定义出某个足端三脚架坐标系τ作为参考坐标系，(4)在相邻两个连续单站相之间一定存在双站相，即六足机器人所有脚支撑于地面，且相邻两个连续单站相定义的足端三角架坐标系与同一个机体坐标系b相关，(5)足够的双站相时间周期，两单站相足以完成位姿求解；s1312：带有位置反馈的舵机，直接获得当前时刻各关节的角度信息，通过建立d-h坐标系，正运动学建模，求得当前时刻各足末端在机体坐标系b
n
下的位置信息；s1313：在单站相，定义了当前时刻某个足末端三脚架坐标系τ
n
，τ
n
系原点及其余两足末端s2、s3在当前时刻机体坐标系b
n
下的位置可知，从而换算出在机体坐标系b
n
下以为向量起始点的两条有向单位向量e1和e2，即，即s1314：获得足末端三角坐标系τ
n
的三轴关系为：的三轴关系为：s1315：足末端三角坐标系τ
n
的三轴均为当前时刻机体坐标系b
n
下的有向单位向量，转换成三角坐标系τ
n
相对于机体坐标系b
n
的旋转矩阵，即为s1316：通过建立d-h坐标系，正运动学建模，获得三角坐标系τ
n
原点在机体坐标系下的位置通过向量计算与投影变换，获得三角坐标系τ
n
相对于机体坐标系b
n
的旋转矩阵即获得当前时刻三角坐标系τ
n
相对于机体坐标系b
n
的变换矩阵为即当前时刻三脚架坐标系τ
n
与机体坐标系b
n
的变换矩阵为s1317：两个连续单站相各自定义有三角架坐标系分别为τ
k
、τ
k 1
；各自解算出三脚架坐标系与机体坐标系的变换矩阵分别为为建立两个连续单站相的三角架坐标系变换关系，单站相间一定存在双站相，其计算出的变换关系为：s1318：足端处设有触底反馈装置，触底反馈装置反馈出各腿足端是否触底的二值信号，用于判断双站相的存在情况；s1319：若假设第一单站相位的三脚架坐标系τ0对应的机体坐标系b0定义为里程计的世界坐标系w，为表示出六足机器人运动里程计的位姿信息，通过解算出变动的机体坐标系b
n
与世界坐标系w之间的变换矩阵，递归地定义为：
s132：六足机器人运动里程计预积分：s1321：六足机器人运动里程计估算出位姿信息为其中包含位移量以及旋转量；在非线性优化过程中，在两个关键帧图像间，六足机器人运动里程计预积分中位移部分来源于六足机器人运动里程计，旋转部分来源于imu预积分，即：来源于imu预积分，即：s1322：六足机器人运动里程计预积分项为：其中，为连续两帧之间里程计预积分提供的位移增量，为连续两帧之间imu预积分坐标变换得到的旋转增量，b
w
为imu陀螺仪的零偏量。5.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s2包括以下子步骤：s21：通过计算当前帧与滑窗中关键帧的平均视差与匹配特征点数，设定其平均视差大于某视差阈值且匹配特征点数最多的滑窗内关键帧作为枢纽帧；s22：利用对极几何约束计算当前帧与枢纽帧之间的本质矩阵e＝t^r，利用本质矩阵计算出当前帧相对于枢纽帧的转换矩阵s23：设定枢纽帧的位姿信息为故当前帧的位姿信息为已知两帧位姿信息以及两帧共视的匹配特征点，即可通过三角化这些特征点，得到当前帧与枢纽帧之间均能观测到的3d路标点；s24：当前帧与枢纽帧之间的滑窗内关键帧均能观测到步骤s23三角化出的3d路标点，即能通过3d点对应到这些关键帧提取和跟踪到的2d特征点，利用pnp法求解出当前帧与枢纽帧之间滑窗内所有关键帧的位姿信息；s25：遍历枢纽帧到当前帧的所有关键帧，三角化枢纽帧与其他关键帧的特征点，得到更多的3d路标点；s26：对于滑窗中的第一关键帧到枢纽帧之间的所有关键帧，同样基于pnp法求解出关键帧位姿信息，再将共视的匹配特征点三角化出更多的3d路标点；s27：基于得到的滑窗内所有关键帧的位姿信息以及大量的3d路标点，通过pnp法求解出滑窗内的所有非关键帧位姿信息；s28：每个3d点在相应滑窗帧上有观测的2d特征点，从而构成一次重投影误差，基于滑窗中所有关键帧和非关键帧位姿信息以及大量的3d路标点，将对应的重投影误差求和，构
造一个关于重投影误差的非线性最小二乘问题，其目标函数为：其中，t为待求解的滑窗内所有帧的位姿信息，p
i
为第i个3d路标点坐标，n为3d路标点对应的特征点数量，[u
i
,v
i
]
t
为3d路标点投影到的特征点像素坐标，z
i
为第i个3d路标点在相机坐标系下的z轴坐标，k为相机内参矩阵；s29：步骤s24-s27所述的滑窗中所有关键帧和非关键帧位姿信息作为s28所述目标函数的初始值，利用最小二乘法非线性优化进一步求解出滑窗中所有关键帧和非关键帧位姿信息。6.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s3中基于imu预积分与六足机器人运动里程计对齐求解陀螺仪零偏校正，具体方法为：陀螺仪零偏量b
w
是imu预积分旋转部分的自变量，六足机器人运动里程计提供两帧间的相对旋转在初始化过程中，假设滑窗内所有帧的陀螺仪零偏量相等，为估计出陀螺仪零偏量b
w
，将帧间与的误差构建为最小二乘问题，目标函数为：其中，是imu坐标系相对于机体坐标系的旋转外参，a为滑窗中所有关键帧与非关键帧；在陀螺仪偏置误差变化时，利用和两者间的一阶近似关系进行更新；为第k帧时刻下imu预积分关于零偏量的雅可比。7.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s3中基于imu预积分与六足机器人运动里程计对齐求解重力以及速度初始化，具体方法为：待优化的变量为：其中，均为imu坐标系下的速度，为机体坐标系下的重力加速度，相邻两帧图像k
→
k 1之间，定义imu预积分项中与增量，计算这两个增量分别与第k和k 1帧图像对应的imu位置、速度之间的增量预测值的误差，并对各项进行参考坐标系变换；化简为形式：
其中，δt
k
为相邻两帧图像k
→
k 1之间的时间间隔，为第一帧机体坐标系相对于第k帧时刻imu坐标系的旋转矩阵，为第k 1帧时刻imu坐标系相对于第一帧机体坐标系的旋转矩阵，为机体坐标系相对于imu坐标系的旋转外参，为机体坐标系相对于imu坐标系的旋转外参，为机体坐标系相对于imu坐标系的平移外参，基于滑窗内的所有关键帧与非关键帧作为约束，构造多组最终组合成关于待优化变量χ
i
的最小二乘问题：8.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s3中尺度参数的初始化，采用条件加权计算法，结合视觉位姿分别与imu预积分、六足机器人运动里程计预积分对齐求解结果加权处理，s321：视觉位姿与六足机器人运动里程计预积分对齐：相邻两帧图像k
→
k 1之间，六足机器人运动里程计预积分增量与纯视觉计算两帧图像位姿信息间存在误差：上式转换为hx＝b形式为：其中，为第一帧相机坐标系相对于第k帧机体坐标系的旋转矩阵，为第k、k 1帧时刻相机坐标系分别相对于第一帧相机坐标系的位置，为相机坐标系相对于机体坐标系的平移外参，为第k 1帧时刻机体坐标系相对于第一帧相机坐标系的旋转矩阵，基于滑窗内的所有关键帧与非关键帧作为约束，构造多组hx＝b，最终组合成关于待优化变量s1的最小二乘问题：s322：视觉位姿与imu预积分对齐：待优化的变量为：相邻两帧图像k
→
k 1之间，定义imu预积分项中与增量，计算这两个增量分别与第k和k 1帧图像对应的imu位置、速度之间的增量预测值的误差，并对各项进行合理的参考坐标系变换；化简为形式：
其中，δt
k
为相邻两帧图像k
→
k 1之间的时间间隔，为第一帧相机坐标系相对于第k帧时刻imu坐标系的旋转矩阵，为第k、k 1帧时刻相机坐标系分别相对于第一帧相机坐标系的位置，为第k 1帧时刻imu坐标系相对于第一帧相机坐标系的旋转矩阵，为相机坐标系相对于imu坐标系的平移外参，基于滑窗内的所有关键帧与非关键帧作为约束，构造多组最终组合成关于待优化变量χⅱ的最小二乘问题：s323：条件加权计算法完成尺度参数初始化：s3231：纯视觉求解位姿采用有光流跟踪，灰度图像的一元灰度熵代表了图像灰度分布的聚类特征，也是图像信息不确定性的度量，其公式为：其中，h(x)为灰度图像的一元灰度熵，p(x
i
)为某一灰度x
i
在图像中出现的概率，若h(x)越小，则图像的像素梯度越小，图像纹理信息越差；若连续两帧图像k
→
k 1之间的熵变||h
k
(x)-h
k 1
(x)||越大，图像的运动存在异常，估计的位姿可靠性越低；基于滑窗内的所有关键帧与非关键帧，计算其灰度熵均值与熵变均值来综合判断纯视觉估计位姿信息的可靠性：灰度熵均值：熵变均值：其中，当灰度熵均值不大于某设定阈值或熵变均值不小于某设定阈值，即hⅰ≤m1||hⅱ≥m2，则认为纯视觉求解的位姿信息可靠性较低；s3232：基于滑窗内的所有关键帧与非关键帧，通过比较某设定阈值与该段时间内两帧图像间加速度变化量的标准差的大小关系，来参数化因六足机器人在运动过程中出现匀速或匀加速运动所导致的加速度激励不足而降低imu求解出的位姿信息可靠性的影响，标准差计算公式为：
其中，为连续两帧间的加速度变化量，为滑窗内所有连续关键帧与非关键帧之间的加速度变化量均值，当标准差不大于设定阈值，即u≤m3，则认为imu求解的位姿信息可靠性低；s3233：在六足机器人足端设置有触底反馈装置，其反馈信息为是否触底的二值信号，其六条腿的信号表示为{(l1,l2,l3,l4,l5,l6)|l
i
＝0||1}，设定某阈值与六条腿的信号总值比较，从而参数化里程计求解出的位姿信息可靠性的影响，其六条腿的信号总值为：其中，l为六足机器人足端触底信号总值，当信号总值不等于6，即l≠m4，则认为六足机器人运动里程计求解的位姿信息可靠性低；s3234：条件加权计算法：在滑窗内，imu的采样频率最高，其次是六足机器人运动里程计，纯视觉最小；通过对各传感器求解的位姿信息可靠性综合分析，设定不同权重系数来动态调控尺度参数，其表达式为：其中，纯视觉的异常判断条件hⅰ≤m1||hⅱ≥m2，设定权重系数w1；imu的异常判断条件u≤m3，设定权重系数w2；六足机器运动里程计的异常判断条件l≠m4，设定权重系数w3；s为加权优化后的尺度参数，其计算结果s＞0时，尺度参数初始化完成，则整个初始化过程完成。9.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s4具体包括以下步骤：s41：基于滑窗内关键帧的非线性优化问题，其待优化变量为：s41：基于滑窗内关键帧的非线性优化问题，其待优化变量为：其中，χ为待优化变量，x
k
为世界坐标系下第k帧的六足机器人位姿信息pvq及零偏量，λ
m
为第m个路标点首次被某帧观测到的逆深度信息；s42：六足机器人运动里程计预积分残差项：s421：利用带有位置反馈的舵机采集关节角度，处理获取六足机器人运动里程计预积
分，其角度信息受噪声n
b
的影响，假设该类噪声服从高斯分布：s422：预积分误差量状态方程：将误差量状态方程写为矩阵形式：将误差量状态方程写为矩阵形式：由上式可以得到雅可比矩阵和协方差矩阵的递推方程：其中，通过递推获得相邻两个关键帧之间里程计观测噪声的协方差矩阵初始时刻系统状态的雅可比矩阵和协方差矩阵分别为单位阵和零矩阵；v为噪声n
i
的协方差矩阵，且各分量相互独立，故s423：六足机器人运动里程计预积分残差约束：s424：imu预积分残差约束：
其中，[]
vec
为提取四元数的虚部，为四元数误差的三维表达式，为四元数误差的三维表达式，为第k关键帧到第k 1关键帧时间间隔内，使用含有噪声的加速度计和陀螺仪数据计算的imu预积分测量项；s424：视觉重投影残差约束：觉重投影残差约束：其中，为第l个路标点在第j个相机归一化相机坐标系中横纵坐标值，为估计第l个路标点在第j个相机归一化相机坐标系中的可能坐标；s425：将视觉重投影残差、imu预积分残差、六足机器人运动里程计预积分残差以及先验误差约束量联合构建残差目标函数为：其中，四种残差均以马氏距离表示，ρ为核函数，r
p-j
p
χ为边缘化形成的先验误差约束，通过舒尔补方法获得；b为六足机器人运动里程计预积分的第k帧时刻关键帧的集合，i为imu预积分的第k帧时刻关键帧的集合，c为第l路标点被第j帧时刻关键帧观测的集合；相邻两个关键帧之间六足机器人运动里程计观测噪声的协方差矩阵，相邻两个关键帧之间imu观测噪声的协方差矩阵，视觉重投影观测噪声的协方差矩阵。10.根据权利要求1所述六足机器人定位方法，其特征在于，步骤s5包括以下子步骤：s51：当前帧与关键帧数据库中所有关键帧进行比较，即计算当前帧与词袋的相似度分数：其中，s(m,n
i
)为当前帧与关键帧数据库中第i个关键帧的相似度，w为关键帧数据库大小，m为当前帧的描述子向量，n
i
为关键帧数据库中第i个关键帧的描述子向量，|| ||1为l1范数，即个元素绝对值之和；s52：检测到当前帧与回环帧之间有回环，进行重定位，将回环帧的位姿和相关特征点的观测与当前帧之间构成重投影误差，加入到s425所述的目标函数中，优化求解出当前帧与回环帧之间的相对位姿关系：
其中，
ψ
为第l路标点被第v帧闭环帧观测的集合，为当前帧与闭环帧间的重投影误差，为第v闭环帧相对于当前帧的旋转四元数，为第v闭环帧相对于当前帧的位移，视觉重投影观测噪声的协方差矩阵；s53：当前帧与回环帧之间的相机轨迹进行回环优化，计算相邻关键帧之间的残差为：其中，r
i,i 1
()为相邻第i关键帧与第i 1关键帧之间的残差，ψ
i
分别为第i关键帧的翻滚角、俯仰角、偏航角，p
iw
、分别为第i关键帧与第i 1关键帧的位置，为在第i关键帧坐标系下第i关键帧与第i 1关键帧间的相对位移，为第i关键帧与第i 1关键帧间的相对偏航角；s54：根据步骤s53的残差项，构造整体目标函数为：其中，ε为序列边，l为回环边。

技术总结
本发明公开了一种基于多传感器融合的六足机器人定位方法，先利用相机、IMU、六足机器人运动里程计采集的数据进行数据预处理并计算出视觉位姿、IMU预积分、六足机器人运动里程计预积分；再采用条件加权计算法完善并完成初始化；然后基于滑窗内所有关键帧进行非线性优化，获得六足机器人的实时位姿；最后增添回环检测优化，对滑窗内的位姿修正，完成六足机器人定位。本发明提出的定位方法，提高了机器人的定位精度以及鲁棒性，在视觉惯性定位方法的应用优势基础上，解决了常见的实际场景应用问题，且在融合定位的基础上增添了回环检测优化，有效的降低机器人的累积误差，完善了整个机器人定位方法，提高了长时间执行下的定位精度。度。度。


技术研发人员：金波 吕俊忠 王谦
受保护的技术使用者：浙江大学
技术研发日：2021.12.16
技术公布日：2022/3/25