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

1.本发明涉及六足机器人技术领域，特别是涉及一种基于多传感器融合的六足机器人定位方法。


背景技术：

2.移动机器人具有良好的灵活性与发展前景，其中，足式机器人相比其他类型结构的移动机器人更具有良好的环境适应能力以及运动灵活性，六足机器人作为一种典型的足式机器人，具有较强的环境适应性、良好的运动稳定性和容错性以及多变的运动方式，但其定位技术方面的研究局限了其应用场景和发展。
3.多传感器融合技术是机器人定位与导航的重要研究方向之一，单一传感器定位各有缺陷，多传感器融合定位具有互补性，通过安装在机器人身上的多类传感器来采集机器人周围的环境信息，相互融合估算出机器人的实时位姿信息以及周围环境的地图信息。视觉传感器具有体积小以及信息量丰富等特点，其中因单目相机低成本的优势，使基于单目视觉信息实现定位的算法研究成为热门，但视觉定位在弱纹理以及光照变化强烈等环境下定位精度急剧下降，且纯单目视觉定位存在尺度信息不明确的问题，使其常常需要引入其他传感器来恢复尺度信息。
4.单目相机与imu(惯性传感器)之间的传感器融合是目前机器人定位技术方面的常见手段，称为视觉惯性定位方法，其imu的引入在一定程度使尺度信息弱可观以及解决机器人短时间快速运动下纯视觉定位误差过大的不足，其视觉的引入也解决了imu长时间运动定位误差随时间扩大的问题；但当机器人运动阶段出现加速度激励不足情况，将失去对尺度的约束，导致尺度不确定性逐渐增加，造成定位误差；并且机器人长期运动在无纹理以及弱光照环境下，相机无法获取有效信息，此时视觉惯性定位将退化为仅基于惯性的航位推算，因imu采集数据的误差问题，使定位的位姿误差随时间快速增长。
5.比如，公开号为cn113608556a的发明公开了一种基于多传感器融合的多机器人相对定位方法，所采用的技术方案包括以下步骤：单机对视觉可见范围内其他无人机的视觉位姿估计；imu信息与视觉估计和uwb测距信息融合；多机间定位结果融合。由于采用了确定尺寸的合作标识，使得视觉定位的鲁棒性、精度和计算速度均得以提高，加快观测更新频率；采用了imu预测 uwb测距 视觉图像观测的多传感器组合解算，使得多机器人间的定位精度得以进一步提高，并且可以在不依靠gps信息的情况下保持系统相对构型。采用了环形网络拓扑进行阵型估计，使得系统在部分观测失效的情况下仍能保持运作，具有一定的冗余性。该发明虽然在视觉惯性定位方法上引入了uwb测距信息，但并没有针对于应用的机器人本体传感器进行充分利用，其成本相对较高，工程适用性具有一定局限。
6.再比如，公开号为cn111739063a的发明公开了一种基于多传感器融合的电力巡检机器人定位方法，其步骤为：首先对相机、imu和里程计采集的数据进行预处理以及系统的标定，完成机器人系统的初始化；其次，提取关键帧，利用关键帧的实时视觉位姿对机器人的位置、速度、角度和imu中的陀螺仪的偏置进行后端优化，得到机器人的实时位姿；然后，
构建关键帧数据库，并计算当前帧图像与关键帧数据库中的所有关键帧的相似度；最后，对形成闭环的关键帧数据库中的关键帧进行闭环优化，输出闭环优化后的位姿，完成对机器人的定位。本发明提出的后端优化方法提高了定位的精度；且在视觉定位过程中增加了闭环优化，有效的消除了定位过程中存在的累计误差，保证了长时间运行情况下的准确性。该发明在视觉惯性定位方法上引入了轮式机器人的轮速传感器，但并不适用于足式机器人。


技术实现要素：

7.鉴于背景技术所述的诸多不足问题，本发明提供了一种基于多传感器融合的六足机器人定位方法，其目的在于解决现有六足机器人定位方法在实际场景下尺度信息弱可观、定位精度以及鲁棒性较差的技术问题。
8.一种基于多传感器融合的六足机器人定位方法，六足机器人包括机身和六条腿，每条腿分为多节，每条腿与机身之间以及每条腿的相邻两节之间均通过关节连接，每条腿的底部为足端，六足机器人定位方法包括以下步骤：
9.s1：在六足机器人上安置多款传感器，根据所述传感器采集的原数据进行数据预处理工作，传感器包括相机、imu和带有位置反馈的舵机，利用相机采集图像信息，提取与跟踪图像特征点；利用imu采集加速度与角速度，处理获取imu预积分；利用带有位置反馈的舵机采集关节角度，处理获取六足机器人运动里程计预积分；
10.s2：根据步骤s1中图像特征点的提取与跟踪，利用sfm算法进行纯视觉估计滑窗内关键帧以及非关键帧的视觉位姿以及3d点坐标信息；
11.s3：分别将视觉位姿、imu预积分、六足机器人运动里程计预积分两两结合进行对齐求解初始化参数，初始化参数包括陀螺仪零偏校正、重力以及速度初始化和尺度参数的初始化；
12.s4：针对滑窗中的关键帧，将视觉重投影残差、imu预积分残差、六足机器人运动里程计预积分残差以及先验误差约束量联合构建残差函数，对残差函数进行非线性优化求解，得到六足机器人的实时位姿信息；
13.s5：基于dbow2词袋算法对六足机器人的位姿信息进行回环检测，检测成功后进行重定位，再对整个相机轨迹进行回环优化，进而对滑窗内的位姿进行修正，最后完成基于多传感器融合的六足机器人定位。
14.本发明中使用的相机优选使用单目相机。滑窗，也叫滑动窗口，用于固定同时处理的图像帧数量。
15.优选的，步骤s1中利用相机采集图像信息，处理获得图像特征点的提取与跟踪，具体包括如下步骤：
16.s111：将相机采集到的原图像数据均衡化处理后，利用基于金字塔分层的光流法跟踪相邻两帧，并通过检测跟踪状态位、对极约束满足情况等方法来剔除异常点，并使用shi-tomasi角点检测算法对图像提取特征点信息加以补给；
17.s112：在保证每帧图像总特征点数不小于设定阈值基础上，对特征点提取过程进行四叉树均匀化处理算法，使图像特征点分布均匀。
18.优选的，步骤s1中imu采集三轴加速度与角速度，处理获取imu预积分，具体包括如下步骤：
19.s121：imu包括加速度计与陀螺仪，分别用于读取imu坐标系下的加速度和角速度测量信息；将相邻两帧图像第k帧与第k 1帧间的所有imu数据进行积分，计算得到第k 1帧在世界坐标系下的位置、速度以及姿态信息；
20.s122：将步骤s121得到的第k 1帧位置、速度以及姿态信息的参考坐标系从世界坐标系w变换为第k个关键帧时刻的imu坐标系bk，从而提取出积分结果为bk 1对于bk的相对运动量，即imu预积分；imu预积分项为：
[0021][0022]
其中，为连续两帧之间imu预积分提供的位移增量，为连续两帧之间imu预积分提供的速度增量，为连续两帧之间imu预积分提供的旋转增量(四元数)，ba为imu加速度计的零偏量，bw为imu陀螺仪的零偏量。
[0023]
优选的，步骤s1中带有位置反馈的舵机采集关节角度，处理获取六足机器人运动里程计预积分，具体包括如下步骤：
[0024]
s131：六足机器人运动里程计：
[0025]
s1311：六足机器人运动里程计位姿求解成立条件：(1)任何时刻，至少三条非共线足端的腿支撑，(2)求解位姿时，与地面接触的腿没有足端滑动，(3)在六足机器人行走过程中，一定存在足端构成的支撑三角形，即单站相，可定义出某个足端三脚架坐标系τ作为参考坐标系，(4)在相邻两个连续单站相之间一定存在双站相，即六足机器人所有脚支撑于地面，且相邻两个连续单站相定义的足端三角架坐标系与同一个机体坐标系b相关，(5)足够的双站相时间周期，两单站相足以完成位姿求解；
[0026]
s1312：带有位置反馈的舵机，直接获得当前时刻各关节的角度信息，通过建立d-h坐标系，正运动学建模，求得当前时刻各足末端在机体坐标系bn下的位置信息；
[0027]
s1313：在单站相，定义了当前时刻某个足末端三脚架坐标系τn，τn系原点及其余两足末端s2、s3在当前时刻机体坐标系bn下的位置可知，从而换算出在机体坐标系bn下以为向量起始点的两条有向单位向量e1和e2，即，即
[0028]
s1314：获得足末端三角坐标系τn的三轴关系为：的三轴关系为：
[0029]
s1315：足末端三角坐标系τn的三轴均为当前时刻机体坐标系bn下的有向单位向量，转换成三角坐标系τn相对于机体坐标系bn的旋转矩阵，即为
[0030]
s1316：通过建立d-h坐标系，正运动学建模，获得三角坐标系τn原点在机体坐标系下的位置通过向量计算与投影变换，获得三角坐标系τn相对于机体坐标系bn的旋转矩阵即获得当前时刻三角坐标系τn相对于机体坐标系bn的变换矩阵为
即当前时刻三脚架坐标系τn与机体坐标系bn的变换矩阵为
[0031]
s1317：两个连续单站相各自定义有三角架坐标系分别为τk、τ
k 1
；各自解算出三脚架坐标系与机体坐标系的变换矩阵分别为为建立两个连续单站相的三角架坐标系变换关系，单站相间一定存在双站相，其计算出的变换关系为：
[0032]
s1318：足端处设有触底反馈装置，触底反馈装置反馈出各腿足端是否触底的二值信号，用于判断双站相的存在情况；
[0033]
s1319：若假设第一单站相位的三脚架坐标系τ0对应的机体坐标系b0定义为里程计的世界坐标系w，为表示出六足机器人运动里程计的位姿信息，通过解算出变动的机体坐标系bn与世界坐标系w之间的变换矩阵，递归地定义为：
[0034][0035][0036]
s132：六足机器人运动里程计预积分：
[0037]
s1321：六足机器人运动里程计估算出位姿信息为其中包含位移量以及旋转量；在非线性优化过程中，在两个关键帧图像间，六足机器人运动里程计预积分中位移部分来源于六足机器人运动里程计，旋转部分来源于imu预积分，即：
[0038][0039][0040]
s1322：六足机器人运动里程计预积分项为：
[0041][0042]
其中，为连续两帧之间里程计预积分提供的位移增量，为连续两帧之间imu预积分坐标变换得到的旋转增量，bw为imu陀螺仪的零偏量。
[0043]
上述进一步方案的有益效果是：在视觉特征点提取方面，采用shi-tomasi角点检测算法，其相对于常见的harris角点更具有稳定性，确保特征跟踪的鲁棒性；并且在特征点均匀化处理方面，采用四叉树均匀化处理算法，其能更为有效的避免特征点聚集情况,更好的提高了特征点的匹配精度；针对六足机器人，充分利用机体自身带有位置反馈的舵机，通过角度信息建立d-h坐标系，正运动学建模，以及向量计算与投影变换，获取足端与机体之间的转换矩阵，通过有效的运动相位变化以及坐标变换关系分析，计算出六足机器人运动里程计，输出机体相对于世界坐标的位姿信息。
[0044]
优选的，步骤s2包括以下子步骤：
[0045]
s21：通过计算当前帧与滑窗中关键帧的平均视差与匹配特征点数，设定其平均视差大于某视差阈值且匹配特征点数最多的滑窗内关键帧作为枢纽帧；
[0046]
s22：利用对极几何约束计算当前帧与枢纽帧之间的本质矩阵e＝t^r，利用本质矩阵计算出当前帧相对于枢纽帧的转换矩阵
[0047]
s23：设定枢纽帧的位姿信息为故当前帧的位姿信息为已知两帧位姿信息以及两帧共视的匹配特征点，即可通过三角化这些特征点，得到当前帧与枢纽帧之间均能观测到的3d路标点；
[0048]
s24：当前帧与枢纽帧之间的滑窗内关键帧均能观测到步骤s23三角化出的3d路标点，即能通过3d点对应到这些关键帧提取和跟踪到的2d特征点，利用pnp法求解出当前帧与枢纽帧之间滑窗内所有关键帧的位姿信息；
[0049]
s25：遍历枢纽帧到当前帧的所有关键帧，三角化枢纽帧与其他关键帧的特征点，得到更多的3d路标点；
[0050]
s26：对于滑窗中的第一关键帧到枢纽帧之间的所有关键帧，同样基于pnp法求解出关键帧位姿信息，再将共视的匹配特征点三角化出更多的3d路标点；
[0051]
s27：基于得到的滑窗内所有关键帧的位姿信息以及大量的3d路标点，通过pnp法求解出滑窗内的所有非关键帧位姿信息；
[0052]
s28：每个3d点在相应滑窗帧上有观测的2d特征点，从而构成一次重投影误差，基于滑窗中所有关键帧和非关键帧位姿信息以及大量的3d路标点，将对应的重投影误差求和，构造一个关于重投影误差的非线性最小二乘问题，其目标函数为：
[0053][0054]
其中，t为待求解的滑窗内所有帧的位姿信息，pi为第i个3d路标点坐标，n为3d路标点对应的特征点数量，[ui,vi]
t
为3d路标点投影到的特征点像素坐标，zi为第i个3d路标点在相机坐标系下的z轴坐标，k为相机内参矩阵；
[0055]
s29：步骤s24-s27所述的滑窗中所有关键帧和非关键帧位姿信息作为s28所述目标函数的初始值，利用最小二乘法非线性优化进一步求解出滑窗中所有关键帧和非关键帧位姿信息。
[0056]
优选的，步骤s3中基于imu预积分与六足机器人运动里程计对齐求解陀螺仪零偏校正，具体方法为：
[0057]
陀螺仪零偏量bw是imu预积分旋转部分的自变量，六足机器人运动里程计提供两帧间的相对旋转在初始化过程中，假设滑窗内所有帧的陀螺仪零偏量相等，为估计出陀螺仪零偏量bw，将帧间与的误差构建为最小二乘问题，目标函数为：
[0058]
[0059]
其中，是imu坐标系相对于机体坐标系的旋转外参，a为滑窗中所有关键帧与非关键帧；在陀螺仪偏置误差变化时，利用和两者间的一阶近似关系进行更新；为第k帧时刻下imu预积分关于零偏量的雅可比。
[0060]
优选的，步骤s3中基于imu预积分与六足机器人运动里程计对齐求解重力以及速度初始化，具体方法为：
[0061]
待优化的变量为：
[0062]
其中，均为imu坐标系下的速度，为机体坐标系下的重力加速度，
[0063]
相邻两帧图像k
→
k 1之间，定义imu预积分项中与增量，计算这两个增量分别与第k和k 1帧图像对应的imu位置、速度之间的增量预测值的误差，并对各项进行参考坐标系变换；化简为形式：
[0064][0065]
其中，δtk为相邻两帧图像k
→
k 1之间的时间间隔，为第一帧机体坐标系相对于第k帧时刻imu坐标系的旋转矩阵，为第k 1帧时刻imu坐标系相对于第一帧机体坐标系的旋转矩阵，为机体坐标系相对于imu坐标系的旋转外参，为机体坐标系相对于imu坐标系的旋转外参，为机体坐标系相对于imu坐标系的平移外参，
[0066]
基于滑窗内的所有关键帧与非关键帧作为约束，构造多组最终组合成关于待优化变量χi的最小二乘问题：
[0067][0068]
重力初始化后往往需要对重力方向修正处理，其目的在于使初始化得到的重力更为准确。
[0069]
优选的，步骤s3中尺度参数的初始化，采用条件加权计算法，结合视觉位姿分别与imu预积分、六足机器人运动里程计预积分对齐求解结果加权处理，s321：视觉位姿与六足机器人运动里程计预积分对齐：
[0070]
相邻两帧图像k
→
k 1之间，六足机器人运动里程计预积分增量与纯视觉计算两帧图像位姿信息间存在误差：
[0071][0072]
上式转换为hx＝b形式为：
[0073][0074]
其中，为第一帧相机坐标系相对于第k帧机体坐标系的旋转矩阵，为第k、k 1帧时刻相机坐标系分别相对于第一帧相机坐标系的位置，为相机坐标系相对于机体坐标系的平移外参，为第k 1帧时刻机体坐标系相对于第一帧相机坐标系的旋转矩阵，
[0075]
基于滑窗内的所有关键帧与非关键帧作为约束，构造多组hx＝b，最终组合成关于待优化变量s1的最小二乘问题：
[0076][0077]
s322：视觉位姿与imu预积分对齐：
[0078]
待优化的变量为：
[0079]
相邻两帧图像k
→
k 1之间，定义imu预积分项中与增量，计算这两个增量分别与第k和k 1帧图像对应的imu位置、速度之间的增量预测值的误差，并对各项进行合理的参考坐标系变换；化简为形式：
[0080][0081]
其中，δtk为相邻两帧图像k
→
k 1之间的时间间隔，为第一帧相机坐标系相对于第k帧时刻imu坐标系的旋转矩阵，为第k、k 1帧时刻相机坐标系分别相对于第一帧相机坐标系的位置，为第k 1帧时刻imu坐标系相对于第一帧相机坐标系的旋转矩阵，为相机坐标系相对于imu坐标系的平移外参，
[0082]
基于滑窗内的所有关键帧与非关键帧作为约束，构造多组最终组合成关于待优化变量χⅱ的最小二乘问题：
[0083][0084]
s323：条件加权计算法完成尺度参数初始化：
[0085]
s3231：纯视觉求解位姿采用有光流跟踪，其正常工作前提是灰度假设不变，而在实际应用中由于光照条件的变化，常常拍摄的图像质量参差不齐；灰度图像的一元灰度熵
代表了图像灰度分布的聚类特征，也是图像信息不确定性的度量，其公式为：
[0086][0087]
其中，h(x)为灰度图像的一元灰度熵，p(xi)为某一灰度xi在图像中出现的概率，若h(x)越小，则图像的像素梯度越小，图像纹理信息越差；若连续两帧图像k
→
k 1之间的熵变||hk(x)-h
k 1
(x)||越大，图像的运动存在异常，估计的位姿可靠性越低；
[0088]
基于滑窗内的所有关键帧与非关键帧，计算其灰度熵均值与熵变均值来综合判断纯视觉估计位姿信息的可靠性：
[0089]
灰度熵均值：
[0090]
熵变均值：
[0091]
其中，当灰度熵均值不大于某设定阈值或熵变均值不小于某设定阈值，即hⅰ≤m1||hⅱ≥m2，则认为纯视觉求解的位姿信息可靠性较低；
[0092]
s3232：基于滑窗内的所有关键帧与非关键帧，通过比较某设定阈值与该段时间内两帧图像间加速度变化量的标准差的大小关系，来参数化因六足机器人在运动过程中出现匀速或匀加速运动所导致的加速度激励不足而降低imu求解出的位姿信息可靠性的影响，标准差计算公式为：
[0093][0094]
其中，为连续两帧间的加速度变化量，为滑窗内所有连续关键帧与非关键帧之间的加速度变化量均值，当标准差不大于设定阈值，即u≤m3，则认为imu求解的位姿信息可靠性低；
[0095]
s3233：六足机器人运动里程计在正常工作情况下，其单站相至少存在3条腿足端落地，且连续两个单站相之间一定存在双站相；而双站相的存在是为了使里程计求解出连续状态变化的位姿信息，且最佳状态下要求6条腿足端同时落地；实际过程中双站相时刻不一定6条腿触地，如果前后两次单站相所涉及到的腿在双站相均触地也可以连贯位姿求解，所以认为6条同时触地最佳，其他情况可靠性下降。为参数化里程计求解出的位姿信息可靠性，在六足机器人足端设置有触底反馈装置，其反馈信息为是否触底的二值信号，其六条腿的信号表示为{(l1,l2,l3,l4,l5,l6)|li＝0||1}，设定某阈值与六条腿的信号总值比较，从而参数化里程计求解出的位姿信息可靠性的影响，其六条腿的信号总值为：
[0096][0097]
其中，l为六足机器人足端触底信号总值，当信号总值不等于6，即l≠m4，则认为六
足机器人运动里程计求解的位姿信息可靠性低；
[0098]
s3234：条件加权计算法：
[0099]
在滑窗内，imu的采样频率最高，其次是六足机器人运动里程计，纯视觉最小；通过对各传感器求解的位姿信息可靠性综合分析，设定不同权重系数来动态调控尺度参数，其表达式为：
[0100][0101]
其中，纯视觉的异常判断条件hⅰ≤m1||hⅱ≥m2，设定权重系数w1；imu的异常判断条件u≤m3，设定权重系数w2；六足机器运动里程计的异常判断条件l≠m4，设定权重系数w3；s为加权优化后的尺度参数，其计算结果s＞0时，尺度参数初始化完成，则整个初始化过程完成。
[0102]
上述进一步方案的有益效果是：在初始化阶段，陀螺仪零偏校正、重力以及速度初始化、重力初始化后进行的重力方向修正问题均基于imu预积分与六足机器人运动里程计对齐求解，该两者的数据采样频率更高，受环境影响较小，能求解出更可靠的初始值；而尺度参数的初始化采用条件加权计算法，结合视觉位姿分别与imu预积分、六足机器人运动里程计预积分对齐求解结果加权处理，其充分考虑并参数化三种传感器的数据可靠性问题，使主要影响相机重投影误差的尺度参数物理量可以估算出更为接近最优解的初始值，提高整个初始化过程的有效性，确保非线性优化阶段位姿信息输出的实时性。
[0103]
优选的，步骤s4具体包括以下步骤：
[0104]
s41：基于滑窗内关键帧的非线性优化问题，其待优化变量为：
[0105][0106][0107]
其中，χ为待优化变量，xk为世界坐标系下第k帧的六足机器人位姿信息pvq及零偏量，λm为第m个路标点首次被某帧观测到的逆深度信息；
[0108]
s42：六足机器人运动里程计预积分残差项：
[0109]
s421：利用带有位置反馈的舵机采集关节角度，处理获取六足机器人运动里程计预积分，其角度信息受噪声nb的影响，假设该类噪声服从高斯分布：
[0110][0111]
s422：预积分误差量状态方程：
[0112][0113]
将误差量状态方程写为矩阵形式：
[0114][0115][0116]
由上式可以得到雅可比矩阵和协方差矩阵的递推方程：
[0117][0118]
其中，通过递推获得相邻两个关键帧之间里程计观测噪声的协方差矩阵初始时刻系统状态的雅可比矩阵和协方差矩阵分别为单位阵和零矩阵；v为噪声ni的协方差矩阵，且各分量相互独立，故
[0119]
s423：六足机器人运动里程计预积分残差约束：
[0120][0121]
s424：imu预积分残差约束：
[0122][0123]
其中，[]
vec
为提取四元数的虚部，为四元数误差的三维表达式，为四元数误差的三维表达式，为第k关键帧到第k 1关键帧时间间隔内，使用含有噪声的加速度计和陀螺仪数据计算的imu预积分测量项；
[0124]
s424：视觉重投影残差约束：
[0125][0126][0127]
其中，为第l个路标点在第j个相机归一化相机坐标系中横纵坐标值，为估计第l个路标点在第j个相机归一化相机坐标系中的可能坐标；
[0128]
s425：将视觉重投影残差、imu预积分残差、六足机器人运动里程计预积分残差以及先验误差约束量联合构建残差目标函数为：
[0129][0130]
其中，四种残差均以马氏距离表示，ρ为核函数，r
p-j
p
χ为边缘化形成的先验误差约束，通过舒尔补方法获得；b为六足机器人运动里程计预积分的第k帧时刻关键帧的集合，i为imu预积分的第k帧时刻关键帧的集合，c为第l路标点被第j帧时刻关键帧观测的集合；相邻两个关键帧之间六足机器人运动里程计观测噪声的协方差矩阵，相邻两个关键帧之间imu观测噪声的协方差矩阵，视觉重投影观测噪声的协方差矩阵。
[0131]
上述进一步方案的有益效果是：以非线性优化的方式，充分融合三种传感器，其定位精度以及鲁棒性优于视觉惯性定位方法；同时，引入六足机器人运动里程计预积分带来的新约束时，通过只添加里程计的位移增量约束，在提高定位精度的基础上，减少了后端优化的计算量，有效地确保了优化求解位姿信息实时性的要求。
[0132]
优选的，步骤s5包括以下子步骤：
[0133]
s51：当前帧与关键帧数据库中所有关键帧进行比较，即计算当前帧与词袋的相似度分数：
[0134][0135]
其中，s(m,ni)为当前帧与关键帧数据库中第i个关键帧的相似度，w为关键帧数据库大小，m为当前帧的描述子向量，ni为关键帧数据库中第i个关键帧的描述子向量，||||1为l1范数，即个元素绝对值之和；
[0136]
s52：检测到当前帧与回环帧之间有回环，进行重定位，将回环帧的位姿和相关特征点的观测与当前帧之间构成重投影误差，加入到s425所述的目标函数中，优化求解出当前帧与回环帧之间的相对位姿关系：
[0137][0138]
其中，
ψ
为第l路标点被第v帧闭环帧观测的集合，为当前帧与闭环帧间的重投影误差，为第v闭环帧相对于当前帧的旋转四元数，为第v闭环帧相对于当前帧的位移，视觉重投影观测噪声的协方差矩阵；
[0139]
s53：当前帧与回环帧之间的相机轨迹进行回环优化，计算相邻关键帧之间的残差为：
[0140][0141]
其中，r
i,i 1
()为相邻第i关键帧与第i 1关键帧之间的残差，分别为第i关键帧的翻滚角、俯仰角、偏航角，分别为第i关键帧与第i 1关键帧的位置，为在第i关键帧坐标系下第i关键帧与第i 1关键帧间的相对位移，为第i关键帧与第i 1关键帧间的相对偏航角；
[0142]
s54：根据步骤s53的残差项，构造整体目标函数为：
[0143][0144]
其中，ε为序列边，l为回环边。
[0145]
本发明有益效果是：本发明的定位方法借助于多传感器融合以及非线性优化求解的思想充分利用机器人便携的以及本体的传感器数据，将imu、单目相机以及带有位置反馈的舵机有效的融合，提高了六足机器人定位的鲁棒性和精度，并且在视觉惯性定位方法的应用优势基础上，进一步解决了机器人运动阶段加速度激励不足造成定位误差加大以及机器人长期运动于无纹理以及弱光照环境定位误差随时间快速增长等实际场景应用的问题；同时，基于非线性优化求解位姿信息的初始化阶段，将三类传感器数据相互对齐，充分利用各自数据优势，得到更为接近最优解的初始值，减少优化求解计算量，加快优化求解速度，使位姿信息实时性得到保障。最后，在融合定位的基础上增添了回环检测优化，有效的降低机器人定位过程的累积误差，完善了整个机器人定位方法，进一步提高了本发明在实际长时间执行下的定位精度。
附图说明
[0146]
图1为本发明基于多传感器融合的六足机器人定位方法的流程图。
[0147]
图2为本发明六足机器人结构及各坐标系示意图。
[0148]
图3为本发明六足机器人运动里程计位姿求解示意图。
[0149]
图4为本发明初始化的流程图。
[0150]
图5为本发明条件加权计算法的流程图。
具体实施方式
[0151]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
[0152]
如图1所示，一种基于多传感器融合的六足机器人定位方法，一种基于多传感器融合的六足机器人定位方法，六足机器人包括机身和六条腿，每条腿分为多节，每条腿与机身之间以及每条腿的相邻两节之间均通过关节连接，每条腿的底部为足端，六足机器人定位方法包括以下步骤：
[0153]
s1：在六足机器人机体安置多款传感器，根据所述传感器采集的原数据进行数据预处理工作，传感器包括相机、imu和带有位置反馈的舵机，利用相机采集图像信息，提取与跟踪图像特征点；利用imu采集加速度与角速度，处理获取imu预积分；利用带有位置反馈的舵机采集关节角度，处理获取六足机器人运动里程计预积分；
[0154]
s2：根据s1中图像特征点的提取与跟踪，利用sfm算法进行纯视觉估计滑窗内关键帧以及非关键帧的视觉位姿以及3d点坐标信息；
[0155]
s3：分别将视觉位姿、imu预积分、六足机器人运动里程计预积分两两结合进行对齐求解初始化参数，初始化参数包括陀螺仪零偏校正、重力以及速度初始化和尺度参数的初始化；
[0156]
s4：针对滑窗中的关键帧，将视觉重投影残差、imu预积分残差、六足机器人运动里程计预积分残差以及先验误差约束量联合构建残差函数，对残差函数进行非线性优化求解，得到六足机器人的实时位姿信息；
[0157]
s5：基于dbow2词袋算法对六足机器人的位姿信息进行回环检测，检测成功后进行重定位，再对整个相机轨迹进行回环优化，进而对滑窗内的位姿进行修正，最后完成基于多传感器融合的六足机器人定位。
[0158]
如图2所示，本实施例中六足机器人安装有单目相机、六轴imu以及带位置反馈的舵机等其他传感器，六条腿均为根关节、髋关节、膝关节以及足端结构，其中每个关节连接处均设置一个带位置反馈的舵机，每个足端均设置触底反馈装置；各部分刚性连接，通过外参标定估算出相机坐标系、imu坐标系、机体坐标系相互之间的变换矩阵；利用舵机的角度测量值，通过建立d-h坐标系，正运动学建模，估算出足端三脚架坐标系相对于机体坐标系的变换矩阵。
[0159]
在本发明实施例中，如图1所示，步骤s1中利用相机采集图像信息，处理获得图像特征点的提取与跟踪，具体包括如下步骤：
[0160]
s111：原图像数据均衡化处理后，利用基于金字塔分层的光流法跟踪相邻两帧，并通过检测跟踪状态位、对极约束满足情况等方法来剔除异常点，为保证每帧图像总特征点
数达到设定阈值，使用shi-tomasi角点检测算法对图像提取特征点信息加以补给；
[0161]
s112：在保证每帧图像总特征点数不小于设定阈值基础上，为充分利用图像信息，对特征点提取过程进行四叉树均匀化处理算法，使图像特征点分布均匀；
[0162]
在本发明实施例中，如图1所示，步骤s1中imu采集三轴加速度与角速度，处理获取imu预积分，具体包括如下步骤：
[0163]
s121：六轴imu包括加速度计与陀螺仪，分别读取imu坐标系下的加速度和角速度测量信息；将相邻两帧图像第k帧与第k 1帧间的所有imu数据进行积分，计算得到第k 1帧在世界坐标系下的位置、速度以及姿态信息；
[0164]
s122：将步骤s121得到的第k 1帧位置、速度以及姿态信息的参考坐标系从世界坐标系w变换为第k个关键帧时刻的imu坐标系bk，从而提取出积分结果为bk 1对于bk的相对运动量，即imu预积分；imu预积分项为：
[0165][0166]
其中，为连续两帧之间imu预积分提供的位移增量，为连续两帧之间imu预积分提供的速度增量，为连续两帧之间imu预积分提供的旋转增量(四元数)，ba为imu加速度计的零偏量，bw为imu陀螺仪的零偏量；
[0167]
如图3所示，在实施方式中，针对六足机器人，充分利用机体自身带有位置反馈的舵机，通过角度信息建立d-h坐标系，正运动学建模，以及向量计算与投影变换，获取足端与机体之间的转换矩阵，通过有效的运动相位变化以及坐标变换关系分析，计算出六足机器人运动里程计，输出机体相对于世界坐标的位姿信息。
[0168]
在本发明实施例中，如图1、3所示，步骤s1中带有位置反馈的舵机采集关节角度，处理获取六足机器人运动里程计预积分，具体包括如下步骤：
[0169]
s131：六足机器人运动里程计：
[0170]
s1311：六足机器人运动里程计位姿求解成立条件：(1)任何时刻，至少三条非共线足端的腿支撑(2)求解位姿时，与地面接触的腿没有足端滑动(3)在六足机器人行走过程中，一定存在足端构成的支撑三角形，即单站相，可定义出某个足端三脚架坐标系τ作为参考坐标系(4)在相邻两个连续单站相之间一定存在双站相，即六足机器人所有脚支撑于地面，且相邻两个连续单站相定义的足端三角架坐标系与同一个机体坐标系b相关(5)足够的双站相时间周期，两单站相足以完成位姿求解；
[0171]
s1312：带有位置反馈的舵机，直接获得当前时刻各关节的角度信息，通过建立d-h坐标系，正运动学建模，求得当前时刻各足末端在机体坐标系bn下的位置信息；
[0172]
s1313：在单站相，如步骤s1311条件(1)、(3)所述，定义了当前时刻某个足末端三脚架坐标系τn，如步骤s1312所述，τn系原点及其余两足末端s2、s3在当前时刻机体坐标系bn下的位置可知，从而换算出在机体坐标系bn下以为向量起始点的两条有向单位向量e1和e2，即
[0173]
s1314：如s1313所述，可以获得足末端三角坐标系τn的三轴关系为：
[0174]
s1315：如s1313、s1314所述，足末端三角坐标系τn的三轴均为当前时刻机体坐标系bn下的有向单位向量，故可以转换成三角坐标系τn相对于机体坐标系bn的旋转矩阵，即为
[0175]
s1316：如1312、s1313、s1315所述，通过建立d-h坐标系，正运动学建模，获得三角坐标系τn原点在机体坐标系下的位置通过向量计算与投影变换，获得三角坐标系τn相对于机体坐标系bn的旋转矩阵即获得当前时刻三角坐标系τn相对于机体坐标系bn的变换矩阵为即当前时刻三脚架坐标系τn与机体坐标系bn的变换矩阵为
[0176]
s1317：如步骤s1316所述，两个连续单站相各自定义有三角架坐标系分别为τk、τ
k 1
；各自解算出三脚架坐标系与机体坐标系的变换矩阵分别为为建立两个连续单站相的三角架坐标系变换关系，单站相间一定存在双站相，其计算出的变换关系为：
[0177]
s1318：如步骤s1317所述，双站相的存在具有必要性，故为检测双站相状态，本发明设定足端处存在触底反馈装置，该机构反馈出各腿足端是否触底的二值信号，用于判断双站相的存在情况；
[0178]
s1319：如步骤s1316、s1317所述，若假设第一单站相位的三脚架坐标系τ0对应的机体坐标系b0定义为里程计的世界坐标系w，为表示出六足机器人运动里程计的位姿信息，需要解算出变动的机体坐标系bn与世界坐标系w之间的变换矩阵，即可以递归地定义为：
[0179][0180][0181]
s132：六足机器人运动里程计预积分：
[0182]
s1321：如步骤s1316所述，六足机器人运动里程计可以估算出位姿信息为其中包含位移量以及旋转量；在非线性优化过程中，为满足实时性需求，希望引入的新约束在提高求解结果精度的情况下尽量不加重算力，故在两个关键帧图像间，针对该阶段的六足机器人运动里程计预积分，其位移部分来源于六足机器人运动里程计，其旋转部分来源于imu预积分，即：
[0183]
[0184][0185]
s1322：如步骤s1321所述，六足机器人运动里程计预积分项为：
[0186][0187]
其中，为连续两帧之间里程计预积分提供的位移增量，为连续两帧之间imu预积分坐标变换得到的旋转增量(四元数)，bw为imu陀螺仪的零偏量。
[0188]
在本发明实施例中，所述步骤s2包括以下子步骤：
[0189]
s21：通过计算当前帧与滑窗中关键帧的平均视差与匹配特征点数，设定其平均视差大于某视差阈值且匹配特征点数最多的滑窗内关键帧作为枢纽帧；
[0190]
s22：利用对极几何约束计算当前帧与枢纽帧之间的本质矩阵e＝t
^
r，利用本质矩阵计算出当前帧相对于枢纽帧的转换矩阵
[0191]
s23：设定枢纽帧的位姿信息为故当前帧的位姿信息为已知两帧位姿信息以及两帧共视的匹配特征点，即可通过三角化这些特征点，得到当前帧与枢纽帧之间均能观测到的3d路标点；
[0192]
s24：当前帧与枢纽帧之间的滑窗内关键帧均能观测到步骤s23三角化出的3d路标点，即能通过3d点对应到这些关键帧提取和跟踪到的2d特征点，利用pnp法求解出当前帧与枢纽帧之间滑窗内所有关键帧的位姿信息；
[0193]
s25：遍历枢纽帧到当前帧的所有关键帧，三角化枢纽帧与其他关键帧的特征点，得到更多的3d路标点；
[0194]
s26：对于滑窗中的第一关键帧到枢纽帧之间的所有关键帧，同样基于pnp法求解出关键帧位姿信息，再将共视的匹配特征点三角化出更多的3d路标点；
[0195]
s27：基于得到的滑窗内所有关键帧的位姿信息以及大量的3d路标点，通过pnp法求解出滑窗内的所有非关键帧位姿信息；
[0196]
s28：每个3d点在相应滑窗帧上有观测的2d特征点，从而构成一次重投影误差，基于滑窗中所有关键帧和非关键帧位姿信息以及大量的3d路标点，将对应的重投影误差求和，构造一个关于重投影误差的非线性最小二乘问题，其目标函数为：
[0197][0198]
其中，t为待求解的滑窗内所有帧的位姿信息，pi为第i个3d路标点坐标，n为3d路标点对应的特征点数量，[ui,vi]
t
为3d路标点投影到的特征点像素坐标，zi为第i个3d路标点在相机坐标系下的z轴坐标，k为相机内参矩阵；
[0199]
s29：步骤s24-s27所述的滑窗中所有关键帧和非关键帧位姿信息作为s28所述目标函数的初始值，利用最小二乘法非线性优化进一步求解出滑窗中所有关键帧和非关键帧位姿信息。
[0200]
在本发明实施例中，如图4所示，步骤s3中基于imu预积分与六足机器人运动里程
计对齐求解陀螺仪零偏校正，具体方法为：
[0201]
陀螺仪零偏量bw是imu预积分旋转部分的自变量，六足机器人运动里程计本身能够提供两帧间的相对旋转在初始化过程中，假设滑窗内所有帧的陀螺仪零偏量相等，故为估计出陀螺仪零偏量bw，将帧间与的误差构建为最小二乘问题，其目标函数为：
[0202][0203]
其中，是imu坐标系相对于机体坐标系的旋转外参，a为滑窗中所有关键帧与非关键帧；在陀螺仪偏置误差变化时，利用两者间的一阶近似关系进行更新；为第k帧时刻下imu预积分关于零偏量的雅可比；
[0204]
在本发明实施例中，如图4所示，步骤s3中基于imu预积分与六足机器人运动里程计对齐求解重力以及速度初始化，具体方法为：
[0205]
待优化的变量为：
[0206]
其中，为imu坐标系下的速度，为机体坐标系下的重力加速度(矢量)；
[0207]
相邻两帧图像k
→
k 1之间，定义imu预积分项中与增量，计算这两个增量分别与第k和k 1帧图像对应的imu位置、速度之间的增量预测值的误差，并对各项进行合理的参考坐标系变换；为便于求解，化简为形式：
[0208][0209]
其中，δtk为相邻两帧图像k
→
k 1之间的时间间隔，为第一帧机体坐标系相对于第k帧时刻imu坐标系的旋转矩阵，为第k 1帧时刻imu坐标系相对于第一帧机体坐标系的旋转矩阵，为机体坐标系相对于imu坐标系的旋转外参，为机体坐标系相对于imu坐标系的旋转外参，为机体坐标系相对于imu坐标系的平移外参；
[0210]
基于滑窗内的所有关键帧与非关键帧作为约束，构造多组最终组合成关于待优化变量χi的最小二乘问题：
[0211][0212]
重力初始化后往往需要对重力方向修正处理，其目的在于使初始化得到的重力更
为准确。
[0213]
如图5所示，在实施方式中，尺度参数的初始化采用条件加权计算法，结合视觉位姿分别与imu预积分、六足机器人运动里程计预积分对齐求解结果加权处理，充分考虑并参数化三种传感器的数据可靠性问题，使主要影响相机重投影误差的尺度参数物理量可以估算出更为接近最优解的初始值；
[0214]
在本发明实施例中，如图4、5所示，步骤s3中尺度参数的初始化，采用条件加权计算法，结合视觉位姿分别与imu预积分、六足机器人运动里程计预积分对齐求解结果加权处理，
[0215]
s321：视觉位姿与六足机器人运动里程计预积分对齐：
[0216]
相邻两帧图像k
→
k 1之间，其六足机器人运动里程计预积分增量与纯视觉计算两帧图像位姿信息间存在误差：
[0217][0218]
上式转换为hx＝b形式为：
[0219][0220]
其中，为第一帧相机坐标系相对于第k帧机体坐标系的旋转矩阵，为第k、k 1帧时刻相机坐标系分别相对于第一帧相机坐标系的位置，为相机坐标系相对于机体坐标系的平移外参，为第k 1帧时刻机体坐标系相对于第一帧相机坐标系的旋转矩阵；
[0221]
基于滑窗内的所有关键帧与非关键帧作为约束，构造多组hx＝b，最终组合成关于待优化变量s1的最小二乘问题：
[0222][0223]
s322：视觉位姿与imu预积分对齐：
[0224]
待优化的变量为：
[0225]
相邻两帧图像k
→
k 1之间，定义imu预积分项中与增量，计算这两个增量分别与第k和k 1帧图像对应的imu位置、速度之间的增量预测值的误差，并对各项进行合理的参考坐标系变换；为便于求解，化简为形式：
[0226][0227]
其中，δtk为相邻两帧图像k
→
k 1之间的时间间隔，为第一帧相机坐标系相对
于第k帧时刻imu坐标系的旋转矩阵，为第k、k 1帧时刻相机坐标系分别相对于第一帧相机坐标系的位置，为第k 1帧时刻imu坐标系相对于第一帧相机坐标系的旋转矩阵，为相机坐标系相对于imu坐标系的平移外参；
[0228]
基于滑窗内的所有关键帧与非关键帧作为约束，构造多组最终组合成关于待优化变量χⅱ的最小二乘问题：
[0229][0230]
s323：条件加权计算法完成尺度参数初始化：
[0231]
s3231：纯视觉求解位姿采用有光流跟踪，其正常工作前提是灰度假设不变，而在实际应用中由于光照条件的变化，常常拍摄的图像质量参差不齐；灰度图像的一元灰度熵代表了图像灰度分布的聚类特征，也是图像信息不确定性的度量，其公式为：
[0232][0233]
其中，h(x)为灰度图像的一元灰度熵，p(xi)为某一灰度xi在图像中出现的概率，若h(x)越小，则图像的像素梯度越小，图像纹理信息越差；若连续两帧图像k
→
k 1之间的熵变||hk(x)-h
k 1
(x)||越大，图像的运动存在异常，估计的位姿可靠性越低；
[0234]
基于滑窗内的所有关键帧与非关键帧，计算其灰度熵均值与熵变均值来综合判断纯视觉估计位姿信息的可靠性：
[0235]
灰度熵均值：
[0236]
熵变均值：
[0237]
其中，当灰度熵均值不大于某设定阈值或熵变均值不小于某设定阈值，即hⅰ≤m1||hⅱ≥m2，则认为纯视觉求解的位姿信息可靠性较低；
[0238]
s3232：六足机器人在运动过程中，不可避免地会出现匀速或匀加速运动，导致加速度激励不足，使imu求解出的位姿信息可靠性较低，基于滑窗内的所有关键帧与非关键帧，通过比较某设定阈值与该段时间内两帧图像间加速度变化量的标准差的大小关系，来参数化该情况带来的影响，其标准差计算公式为：
[0239][0240]
其中，为连续两帧间的加速度变化量，为滑窗内所有连续关键帧与非关键帧之间的加速度变化量均值，当标准差不大于设定阈值，
即u≤m3，则认为imu求解的位姿信息可靠性较低；
[0241]
s3233：六足机器人运动里程计在正常工作情况下，其单站相至少存在3条腿足端落地，且连续两个单站相之间一定存在双站相；而双站相的存在是为了使里程计求解出连续状态变化的位姿信息，且最佳状态下要求6条腿足端同时落地；实际过程中双站相时刻不一定6条腿触地，如果前后两次单站相所涉及到的腿在双站相均触地也可以连贯位姿求解，所以认为6条同时触地最佳，其他情况可靠性下降。为参数化里程计求解出的位姿信息可靠性，在六足机器人足端设置有触底反馈装置，其反馈信息为是否触底的二值信号，其六条腿的信号表示为{(l1,l2,l3,l4,l5,l6)|li＝0||1}，设定某阈值与六条腿的信号总值比较，从而参数化该情况带来的影响，其六条腿的信号总值为：
[0242][0243]
其中，l为六足机器人足端触底信号总值，当信号总值不等于6，即l≠m4，则认为六足机器人运动里程计求解的位姿信息可靠性较低；
[0244]
s3234：条件加权计算法：
[0245]
在滑窗内，imu的采样频率最高，其次是六足机器人运动里程计，纯视觉最小；如步骤s3231-s3233所述，通过对各传感器求解的位姿信息可靠性综合分析，设定不同权重系数来动态调控尺度参数，其表达式为：
[0246][0247]
其中，纯视觉的异常判断条件hⅰ≤m1||hⅱ≥m2，设定权重系数w1；imu的异常判断条件u≤m3，设定权重系数w2；六足机器运动里程计的异常判断条件l≠m4，设定权重系数w3；s为加权优化后的尺度参数，其计算结果s＞0时，尺度参数初始化完成，则整个初始化过程完成。
[0248]
在本发明实施例中，如图1所示，所述步骤s4具体包括以下子步骤：
[0249]
s41：基于滑窗内关键帧的非线性优化问题，其待优化变量为：
[0250][0251][0252]
其中，χ为待优化变量，xk为世界坐标系下第k帧的六足机器人位姿信息pvq及零偏量，λm为第m个路标点首次被某帧观测到的逆深度信息；
[0253]
s42：六足机器人运动里程计预积分残差项：
[0254]
s421：如步骤s1所述，利用带有位置反馈的舵机采集关节角度，处理获取六足机器人运动里程计预积分，其角度信息受噪声nb的影响，假设该类噪声服从高斯分布：
[0255]
s422：预积分误差量状态方程：
[0256][0257]
将误差量状态方程写为矩阵形式：
[0258][0259][0260]
由上式可以得到雅可比矩阵和协方差矩阵的递推方程：
[0261][0262]
其中，通过递推获得相邻两个关键帧之间里程计观测噪声的协方差矩阵初始时刻系统状态的雅可比矩阵和协方差矩阵分别为单位阵和零矩阵；v为噪声ni的协方差矩阵，且各分量相互独立，故
[0263]
s423：所述六足机器人运动里程计预积分残差约束：
[0264][0265]
s424：所述imu预积分残差约束：
[0266][0267]
其中，[]
vec
为提取四元数的虚部，为四元数误差的三维表达式，
为第k关键帧到第k 1关键帧时间间隔内，使用含有噪声的加速度计和陀螺仪数据计算的imu预积分测量项；
[0268]
s424：所述视觉重投影残差约束：
[0269][0270][0271]
其中，为第l个路标点在第j个相机归一化相机坐标系中横纵坐标值，为估计第l个路标点在第j个相机归一化相机坐标系中的可能坐标；
[0272]
s425：如步骤s423-s424所述，将视觉重投影残差、imu预积分残差、六足机器人运动里程计预积分残差以及先验误差等约束量联合构建残差目标函数为：
[0273][0274]
其中，四种残差均以马氏距离表示，ρ为核函数，r
p-j
p
χ为边缘化形成的先验误差约束，通过舒尔补方法获得；b为六足机器人运动里程计预积分的第k帧时刻关键帧的集合，i为imu预积分的第k帧时刻关键帧的集合，c为第l路标点被第j帧时刻关键帧观测的集合；相邻两个关键帧之间六足机器人运动里程计观测噪声的协方差矩阵，相邻两个关键帧之间imu观测噪声的协方差矩阵，视觉重投影观测噪声的协方差矩阵。
[0275]
在本发明实施例中，所述步骤s5包括以下子步骤：
[0276]
s51：当前帧与关键帧数据库中所有关键帧进行比较，即计算当前帧与词袋的相似度分数：
[0277][0278]
其中，s(m,ni)为当前帧与关键帧数据库中第i个关键帧的相似度，w为关键帧数据库大小，m为当前帧的描述子向量，ni为关键帧数据库中第i个关键帧的描述子向量，||||1为l1范数，即个元素绝对值之和；
[0279]
s52：检测到当前帧与回环帧之间有回环，进行重定位，将回环帧的位姿和相关特征点的观测与当前帧之间构成重投影误差，加入到s425所述的目标函数中，优化求解出当前帧与回环帧之间的相对位姿关系：
[0280][0281]
其中，
ψ
为第l路标点被第v帧闭环帧观测的集合，为当前帧与闭环帧间的重投影误差，为第v闭环帧相对于当前帧的旋转四元数，为第v闭环帧相对于当前帧的位移，视觉重投影观测噪声的协方差矩阵。
[0282]
s53：当前帧与回环帧之间的相机轨迹进行回环优化，计算相邻关键帧之间的残差为：
[0283][0284]
其中，r
i,i 1
()为相邻第i关键帧与第i 1关键帧之间的残差，分别为第i关键帧的翻滚角、俯仰角、偏航角，分别为第i关键帧与第i 1关键帧的位置，为在第i关键帧坐标系下第i关键帧与第i 1关键帧间的相对位移，为第i关键帧与第i 1关键帧间的相对偏航角；
[0285]
s54：根据步骤s53的残差项，构造整体目标函数为：
[0286][0287]
其中，ε为序列边，l为回环边。
[0288]
本发明的工作原理及过程为：本发明提供了一种基于多传感器融合的六足机器人定位方法，其包括：首先利用相机、imu、六足机器人运动里程计采集的数据进行数据预处理并进一步计算出视觉位姿、imu预积分、六足机器人运动里程计预积分；再者采用条件加权计算法完善并完成整个六足机器人的初始化；然后基于滑窗内所有关键帧进行非线性优化，获得六足机器人的实时位姿；最后增添回环检测优化，对滑窗内的位姿修正，完成基于多传感器融合的六足机器人定位。本发明提出的定位方法，提高了六足机器人的定位精度以及鲁棒性，在视觉惯性定位方法的应用优势基础上，解决了常见的实际场景应用问题，且在融合定位的基础上增添了回环检测优化，有效的降低六足机器人的累积误差，完善了整个六足机器人定位方法，提高了长时间执行下的定位精度。
[0289]
本发明的有益效果是：本发明的定位方法借助于多传感器融合以及非线性优化求解的思想充分利用六足机器人便携的、低成本的以及本体的传感器数据，将imu、单目相机以及带有位置反馈的舵机有效的融合，提高了六足机器人定位的鲁棒性和精度，并且在视觉惯性定位方法的应用优势基础上，进一步解决了六足机器人运动阶段加速度激励不足造
成定位误差加大以及六足机器人长期运动于无纹理以及弱光照环境定位误差随时间快速增长等实际场景应用的问题；同时，基于非线性优化求解位姿信息的初始化阶段，将三类传感器数据相互对齐，充分利用各自数据优势，得到更为接近最优解的初始值，减少优化求解计算量，加快优化求解速度，使位姿信息实时性得到保障。最后，在融合定位的基础上增添了回环检测优化，有效的降低六足机器人定位过程的累积误差，完善了整个六足机器人定位方法，进一步提高了本发明在实际长时间执行下的定位精度。
[0290]
本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。