[发明专利]一种机器人位姿估计方法、装置、可读存储介质及机器人

权利要求书

1.一种机器人位姿估计方法，其特征在于，包括：

获取机器人的惯性测量数据，并根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿；

分别获取所述机器人的各个绝对测量数据，并根据各个绝对测量数据进行位姿估计，得到与各个绝对测量传感器分别对应的绝对估计位姿，所述绝对测量数据为由所述机器人的绝对测量传感器采集的测量数据；

分别获取所述机器人的各个相对测量数据，并根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿，所述相对测量数据为由所述机器人的相对测量传感器采集的测量数据；

根据所述绝对估计位姿和所述相对估计位姿对所述基准估计位姿进行融合处理，得到所述机器人的融合位姿；

所述根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿包括：

根据第n个相对测量数据计算所述机器人在当前测量时刻的位姿，其中，1≤n≤N，N为所述机器人的相对测量传感器的数目，第n个相对测量数据为由所述机器人的第n个相对测量传感器采集的测量数据；

获取所述机器人在上一测量时刻的位姿，并根据下式计算所述机器人在当前测量时刻和上一测量时刻之间的位姿差：

其中，T1_p为在上一测量时刻的位置，T1_q为在上一测量时刻的姿态角，T2_p为在当前测量时刻的位置，T2_q为在当前测量时刻的姿态角，dT_p为当前测量时刻和上一测量时刻之间的位置差，dT_q为当前测量时刻和上一测量时刻之间的姿态差；

获取所述机器人在上一测量时刻的融合位姿，并根据所述位姿差和所述机器人在上一测量时刻的融合位姿计算与第n个相对测量传感器对应的相对估计位姿。

2.根据权利要求1所述的机器人位姿估计方法，其特征在于，所述惯性测量数据包括线加速度和角速度；

所述根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿包括：

根据所述线加速度对所述机器人在上一测量时刻的速度进行更新，得到所述机器人在当前测量时刻的速度；

根据所述机器人在上一测量时刻的速度以及在当前测量时刻的速度，对所述机器人在上一测量时刻的位置进行更新，得到所述机器人在当前测量时刻的位置；

根据所述角速度对所述机器人在上一测量时刻的姿态角进行更新，得到所述机器人在当前测量时刻的姿态角。

3.根据权利要求1所述的机器人位姿估计方法，其特征在于，所述根据所述位姿差和所述机器人在上一测量时刻的融合位姿计算与第n个相对测量传感器对应的相对估计位姿包括：

根据下式计算与第n个相对测量传感器对应的相对估计位姿：

T2′_p＝T1′_p+T1′_q*dT_p

T2′_q＝T1′_q*dT_q

其中，T1′_p为上一测量时刻的融合位置，T1′_q为上一测量时刻的融合姿态角，T2′_p为与第n个相对测量传感器对应的相对估计位置，T2′_q为与第n个相对测量传感器对应的相对估计姿态角。

4.根据权利要求1至3中任一项所述的机器人位姿估计方法，其特征在于，所述根据所述绝对估计位姿和所述相对估计位姿对所述基准估计位姿进行融合处理包括：

计算待融合位姿与所述基准估计位姿之间的相对位姿，所述待融合位姿为任一所述绝对估计位姿或所述相对估计位姿；

根据所述相对估计位姿计算所述待融合位姿的残差；

根据所述待融合位姿的残差和预设的增益系数对所述基准估计位姿进行修正，得到所述融合位姿。