一种机器人位姿补偿控制方法及具身智能机器人与流程

摘要: 本发明涉及具身智能机器人,尤其涉及一种机器人位姿补偿控制方法及具身智能机器人。背景技术:1、目前,具身智能机器人在户外长距离运动场景中,普遍采用同步定位与地图构...
一种机器人位姿补偿控制方法及具身智能机器人与流程

本发明涉及具身智能机器人,尤其涉及一种机器人位姿补偿控制方法及具身智能机器人。

背景技术:

1、目前,具身智能机器人在户外长距离运动场景中,普遍采用同步定位与地图构建(simultaneous localization and mapping,slam)或全球导航卫星系统(globalnavigation satellite system,gnss)实现定位输出,以支撑机器人导航与步态控制。

2、gnss定位方案具备绝对地理定位精度高、无累积漂移的优势,但存在输出频率低、位姿平滑性差等缺陷,难以满足机器人实时性控制需求,极易造成机器人姿态抖动、行走失衡甚至摔倒风险。slam定位方案依赖里程计、激光雷达或视觉传感器获取定位数据,虽然实时性较好,但在马拉松赛道等长距离、大尺度、特征单一且场景高度重复的户外环境中,极易产生严重的累积定位误差,长期运行易出现定位漂移等问题,无法满足长距离连续稳定定位需求。

3、机器人长距离运动时,除定位漂移外还存在航向累积漂移问题。在运动过程中,slam局部坐标系相对全局坐标系的微小偏航偏差会随行进距离累积并形成横向位移分量,引发机器人横向偏移的“蟹行”现象,降低长距离轨迹跟踪精度与运动稳定性,难以满足机器人马拉松等赛事行进要求。

技术实现思路

1、鉴于上述的分析,本发明实施例旨在提供一种机器人位姿补偿控制方法及具身智能机器人,用以解决现有技术中机器人运动过程中定位漂移和航向偏差不断累积的问题。

2、一方面,本发明实施例提供了一种机器人位姿补偿控制方法,包括:在机器人进入运动状态前,获取slam局部坐标系对齐至全局坐标系所需的初始旋转补偿角;在机器人进入运动状态后,获取机器人在slam局部坐标系的当前slam位姿数据,根据初始旋转补偿角将当前slam位姿数据映射至全局坐标系,得到机器人的初始位置和初始航向角;获取机器人在全局坐标系的当前全局定位观测数据,基于当前全局定位观测数据与初始位置计算自适应融合权重,根据自适应融合权重计算位置漂移补偿量,并利用位置漂移补偿量修正初始位置,得到当前定位位置;基于当前全局定位观测数据计算全局航向角,根据全局航向角与初始航向角计算当前旋转补偿角,根据当前旋转补偿角重新将当前slam位姿数据映射至全局坐标系,得到当前航向角;基于当前定位位置和当前航向角得到机器人的位姿信息。

3、进一步地,获取slam局部坐标系对齐至全局坐标系所需的初始旋转补偿角,包括:获取机器人在全局坐标系下的全局原点以及在slam局部坐标系下的局部原点;控制机器人进行对齐测试,在对齐测试完成后,获取机器人在全局坐标系下的全局终点以及在slam局部坐标系下的局部终点;根据全局原点和全局终点构建全局运动矢量,根据局部原点和局部终点构建局部运动矢量;根据全局运动矢量和局部运动矢量计算初始旋转补偿角。

4、进一步地,根据初始旋转补偿角将当前slam位姿数据映射至全局坐标系,得到机器人的初始位置和初始航向角,包括:根据全局原点和局部原点计算全局坐标系与slam局部坐标系之间的基础平移矩阵;根据初始旋转补偿角和基础平移矩阵对当前slam位姿数据进行刚性变换,得到初始位置和初始航向角。

5、进一步地,基于当前全局定位观测数据与初始位置计算自适应融合权重,包括:计算当前全局定位观测数据与初始位置之间的位置偏差;根据位置偏差的范围对应设置自适应增益系数;根据位置偏差和自适应增益系数计算自适应融合权重。

6、进一步地,位置偏差至少包括当前全局定位观测数据与初始位置之间的欧式距离;根据位置偏差和自适应增益系数计算自适应融合权重,包括:获取自适应融合权重的基础权重和最大权重;通过如下公式计算自适应融合权重:;其中,为自适应融合权重,为基础权重,为位置偏差,为自适应增益系数,为最大权重。

7、进一步地,根据自适应融合权重计算位置漂移补偿量,包括:获取前一时刻定位位置,计算初始位置相对于前一时刻定位位置的位置增量;根据自适应融合权重和位置增量计算偏移补偿增量;获取前一时刻位置漂移补偿量,将前一时刻位置漂移补偿量和偏移补偿增量相加,得到位置漂移补偿量。

8、进一步地,基于当前全局定位观测数据计算全局航向角,包括:判断机器人是否满足预设航向计算触发条件;当满足预设航向计算触发条件时,采用预设时长的滑动窗口采集全局定位观测数据的历史位置数据;基于历史位置数据计算全局航向角。

9、进一步地,根据全局航向角与初始航向角计算当前旋转补偿角,包括:计算全局航向角与初始航向角的角度差值;获取角度差值的增益系数,根据增益系数和角度差值计算角度补偿量;获取前一时刻旋转补偿角,根据前一时刻旋转补偿角和角度补偿量计算得到当前旋转补偿角。

10、进一步地,当前旋转补偿角通过如下公式计算得到:;其中,为当前旋转补偿角,为前一时刻旋转补偿角,为增益系数,为全局航向角,为初始航向角。

11、另一方面,本发明实施例提供了一种具身智能机器人,采用如上述的机器人位姿补偿控制方法进行位姿补偿。

12、在本发明中,在机器人进入运动状态前预先求解slam局部坐标系与全局坐标系的初始旋转补偿角,进入运动状态后,利用该初始旋转补偿角对slam位姿数据做刚性变换,得到全局坐标系下的初始位置与初始航向。再结合全局定位观测数据计算自适应融合权重,得到位置漂移补偿量,修正长距离运动带来的累积位置漂移,提升定位位置精度。同时,基于全局定位观测数据实时计算全局航向角,得到旋转补偿角,持续修正随运动产生的航向漂移,克服固定初始旋转补偿角无法适配运动全程的缺陷,抑制机器人长距离行进时因航向偏差累积引发的横向“蟹行”现象。本方案兼顾定位精度与航向稳定性,能够输出平滑、精确的全局位姿,满足人形机器人户外长距离行走的轨迹跟踪与步态控制需求。

13、本发明中,上述各技术方案之间还可以相互组合,以实现更多的优选组合方案。本发明的其他特征和优点将在随后的说明书中阐述,并且,部分优点可从说明书中变得显而易见,或者通过实施本发明而了解。本发明的目的和其他优点可通过说明书以及附图中所特别指出的内容中来实现和获得。

1.一种机器人位姿补偿控制方法,其特征在于,包括:

2.根据权利要求1所述的机器人位姿补偿控制方法,其特征在于,获取slam局部坐标系对齐至全局坐标系所需的初始旋转补偿角,包括:

3.根据权利要求2所述的机器人位姿补偿控制方法,其特征在于,根据所述初始旋转补偿角将所述当前slam位姿数据映射至全局坐标系,得到所述机器人的初始位置和初始航向角,包括:

4.根据权利要求1所述的机器人位姿补偿控制方法,其特征在于,基于所述当前全局定位观测数据与所述初始位置计算自适应融合权重,包括:

5.根据权利要求4所述的机器人位姿补偿控制方法,其特征在于,所述位置偏差至少包括所述当前全局定位观测数据与所述初始位置之间的欧式距离;根据所述位置偏差和所述自适应增益系数计算所述自适应融合权重,包括:

6.根据权利要求1所述的机器人位姿补偿控制方法,其特征在于,根据所述自适应融合权重计算位置漂移补偿量,包括:

7.根据权利要求1所述的机器人位姿补偿控制方法,其特征在于,基于所述当前全局定位观测数据计算全局航向角,包括:

8.根据权利要求1所述的机器人位姿补偿控制方法,其特征在于,根据所述全局航向角与所述初始航向角计算当前旋转补偿角,包括:

9.根据权利要求8所述的机器人位姿补偿控制方法,其特征在于,所述当前旋转补偿角通过如下公式计算得到:

10.一种具身智能机器人,其特征在于,采用如权利要求1-9中任一项所述的机器人位姿补偿控制方法进行位姿补偿。