一种四足机器人冲击感知与退化互补融合定位方法及系统

摘要: 本发明涉及机器人定位,更确切地说,它涉及一种四足机器人冲击感知与退化互补融合定位方法及系统。背景技术:1、四足机器人可在楼梯、坡道、长直走廊、隧道及非结构化地面...
一种四足机器人冲击感知与退化互补融合定位方法及系统

本发明涉及机器人定位,更确切地说,它涉及一种四足机器人冲击感知与退化互补融合定位方法及系统。

背景技术:

1、四足机器人可在楼梯、坡道、长直走廊、隧道及非结构化地面中运动。为了获得连续的六自由度位姿,通常在机器人机身设置惯性测量单元和激光雷达,并结合关节编码器、足端运动学或本体控制器输出的机体速度构成腿部里程计。

2、惯性测量单元具有频率高、短时连续性好的特点,但四足机器人在小跑、奔跑或刚性落足时会产生持续时间短、幅值高且呈非高斯分布的冲击。若状态估计器始终采用固定的加速度计噪声、陀螺仪噪声以及重力方向观测噪声,容易将触地冲击解释为机体真实加速度或重力方向变化,进而造成姿态、速度和位置估计偏差。

3、接触辅助的足式机器人状态估计方法通常利用足端接触开关、足端力、关节力矩或预先给定的接触相位提高估计精度。然而,部分商用四足机器人不开放上述信号,或者不同平台输出接口差异较大,使依赖特定接触信号的方法难以直接迁移。

4、激光惯性里程计能够利用环境几何结构抑制惯性积分漂移,但在长直走廊、隧道、单一平面或结构重复区域中,点到面约束在一个或多个方向上的可观测性显著下降。现有方法若仅对全部激光残差进行统一降权,会同时削弱非退化方向上仍然有效的约束;若不进行降权,则病态方向上的配准增量可能引起错误修正。

5、此外,现有多传感器方案通常分别调节惯性、激光或辅助机体速度观测的权重,缺少由步态冲击程度和几何退化方向共同驱动的协同机制。尤其是在激光沿特定方向退化时,简单提高辅助机体速度观测的整体权重可能把触地滑移或运动学误差同时放大到其它方向,不能形成方向一致的互补约束。对于由本体控制器状态接口提供的机体速度,该速度还可能已经利用足端运动学、接触状态、惯性测量或激光信息进行估计;若仍将其作为与惯性或激光完全独立的观测直接融合,可能造成共享信息的重复利用和协方差过度收缩。

6、因此,需要一种不依赖足端力或接触开关、能够从机身惯性测量中感知步态冲击,并根据激光几何退化程度和退化方向对激光约束及辅助机体速度观测进行方向性互补调节的融合定位方法。

技术实现思路

1、本发明的目的是克服现有技术中的不足,提供了一种四足机器人冲击感知与退化互补融合定位方法及系统。

2、第一方面,提供了一种四足机器人冲击感知与退化互补融合定位方法,包括:

3、s1、获取机身惯性测量数据、激光雷达扫描数据和辅助机体速度观测;

4、s2、执行时间同步和坐标变换,按时间戳顺序读取观测;

5、s3、根据惯性测量数据中的比力及当前姿态估计,计算步态冲击因子;

6、s4、根据所述步态冲击因子调节惯性预测过程噪声并执行状态预测,并根据所述步态冲击因子及比力模长条件,执行或限制重力方向观测更新;

7、s5、根据激光扫描与局部地图的几何约束构建信息矩阵,通过特征分解计算各特征方向的连续弱度,并构造世界坐标系弱度矩阵和退化子空间;

8、s6、根据各方向的连续弱度构造激光观测的各向异性测量协方差,执行激光观测更新;

9、s7、将世界坐标系弱度矩阵转换至机体坐标系,在创新满足一致性条件时,沿弱约束方向调节辅助机体速度观测的测量协方差,并基于步态冲击因子和观测源相关性进行整体膨胀,执行辅助机体速度观测更新;

10、s8、在同一误差状态卡尔曼滤波器中执行上述各更新,并输出定位结果。

11、作为优选,步骤s3中,所述步态冲击因子由比力在重力投影方向上的分量与慢变基线的残差计算,在滑动窗口内采用中位数和绝对中位差归一化,并经限幅映射和包络平滑得到;该计算不以足端力、关节力矩或接触开关为必要输入。

12、作为优选,步骤s4中,所述惯性预测过程噪声随步态冲击因子增大而增大;重力方向观测更新仅在步态冲击因子和比力模长偏差均满足预设门限时执行,否则增大其观测协方差或跳过更新。

13、作为优选,步骤s5中,所述信息矩阵由有效几何约束的法向量及权重归一化组合构造;对所述信息矩阵特征分解后,将各特征方向的绝对强度和相对强度分别映射为绝对弱度和比例弱度,取两者较大值作为该方向的连续弱度。

14、作为优选,步骤s6中,所述各向异性测量协方差在信息矩阵的特征向量基底下构造,各方向测量方差为基准方差关于对应连续弱度的非减函数;当连续弱度超过预设阈值的特征方向数量为一、二或三时,分别形成秩一、秩二或秩三弱子空间并相应增大对应方向的测量方差。

15、作为优选,步骤s7中,将世界坐标系弱度矩阵转换至机体坐标系后,对其特征分解并构造各方向方差衰减因子;辅助机体速度观测的测量协方差由基础协方差经所述衰减因子合同变换后,再乘以随步态冲击因子增大的冲击膨胀系数和反映观测源相关性的相关性膨胀系数得到。

16、作为优选,当辅助机体速度观测为独立解算且不共享惯性或激光信息时,所述相关性膨胀系数取第一预设值;当辅助机体速度观测由本体控制器接口提供且可能共享信息时,所述相关性膨胀系数取大于第一预设值的第二预设值;当速度创新的马氏距离超出门限时,取消弱方向增信、增大协方差或拒绝该次更新。

17、作为优选,步骤s5中,当有效几何约束不足、信息矩阵最大特征值不大于数值保护量或特征分解结果异常时,将激光观测标记为不可用或高退化状态,并暂停基于无效弱子空间的辅助速度方向性调节。

18、作为优选,步骤s8中,所述误差状态卡尔曼滤波器的状态至少包括世界坐标系下的位置、速度、姿态以及加速度计零偏和陀螺仪零偏;每次更新后,将误差状态注入名义状态并对姿态误差复位,采用对称化和正定性保持方式维护协方差。

19、第二方面,提供了一种四足机器人冲击感知与退化互补融合定位装置,用于执行如第一方面任一所述的方法,包括:

20、获取模块,用于获取机身惯性测量数据、激光雷达扫描数据和辅助机体速度观测;

21、执行模块,用于执行时间同步和坐标变换,按时间戳顺序读取观测;

22、计算模块,用于根据惯性测量数据中的比力及当前姿态估计,计算步态冲击因子;

23、调节模块,用于根据所述步态冲击因子调节惯性预测过程噪声并执行状态预测,并根据所述步态冲击因子及比力模长条件,执行或限制重力方向观测更新;

24、构建模块,用于根据激光扫描与局部地图的几何约束构建信息矩阵,通过特征分解计算各特征方向的连续弱度,并构造世界坐标系弱度矩阵和退化子空间;

25、构造模块,用于根据各方向的连续弱度构造激光观测的各向异性测量协方差,执行激光观测更新;

26、转换模块,用于将世界坐标系弱度矩阵转换至机体坐标系,在创新满足一致性条件时,沿弱约束方向调节辅助机体速度观测的测量协方差,并基于步态冲击因子和观测源相关性进行整体膨胀,执行辅助机体速度观测更新;

27、输出模块,用于在同一误差状态卡尔曼滤波器中执行上述各更新,并输出定位结果。

28、本发明的有益效果是:

29、1. 本发明由机身惯性测量直接计算步态冲击因子,无需足端力、关节力矩或接触开关等外部信号,便于在不同四足机器人平台上迁移实施;同时该因子同时作用于惯性预测过程噪声和重力方向观测更新,在冲击时段采用更保守的不确定度处理,有效抑制高频触地冲击对姿态和速度估计的过度修正。

30、2. 本发明通过激光法向信息矩阵的特征分解,同时获得几何退化的程度和具体方向,按各特征方向的连续弱度构造各向异性测量协方差,能够在削弱弱约束方向激光修正作用的同时,保留非退化方向上仍然有效的激光约束,避免统一降权带来的信息损失。

31、3. 本发明使辅助机体速度观测的增信方向与激光弱约束方向保持匹配,且增信程度受步态冲击因子、创新一致性和观测源相关性共同约束,避免无差别增信或共享信息的重复利用;各类观测在同一误差状态卡尔曼滤波器中完成预测、更新与状态复位,使冲击降信、激光方向性降权和辅助速度方向性约束在统一的不确定性表达下协同执行。