一种机器人位姿估计方法、装置、可读存储介质及机器人制造方法及图纸

本申请属于机器人技术领域，尤其涉及一种机器人位姿估计方法、装置、计算机可读存储介质及机器人。所述方法获取机器人的惯性测量数据，并根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿；分别获取所述机器人的各个绝对测量数据，并根据各个绝对测量数据进行位姿估计，得到与各个绝对测量传感器分别对应的绝对估计位姿；分别获取所述机器人的各个相对测量数据，并根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿；根据所述绝对估计位姿和所述相对估计位姿对所述基准估计位姿进行融合处理，得到所述机器人的融合位姿。通过本申请，极大提高了机器人位姿估计的精度。

A robot pose estimation method, device, readable storage medium and robot

【技术实现步骤摘要】
一种机器人位姿估计方法、装置、可读存储介质及机器人
本申请属于机器人
，尤其涉及一种机器人位姿估计方法、装置、计算机可读存储介质及机器人。
技术介绍
在机器人的建图和导航中，需要对机器人的位姿进行准确估计。不同类型的传感器，如激光雷达、底盘编码器、IMU(惯性测量单元)、视觉传感器等，通过对环境的感知观测，均能完成对机器人位姿的估计。但由于不同传感器的测量都存在误差，其位姿估计结果的精度较低。
技术实现思路
有鉴于此，本申请实施例提供了一种机器人位姿估计方法、装置、计算机可读存储介质及机器人，以解决现有技术中的位姿估计结果精度较低的问题。本申请实施例的第一方面提供了一种机器人位姿估计方法，可以包括：获取机器人的惯性测量数据，并根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿；分别获取所述机器人的各个绝对测量数据，并根据各个绝对测量数据进行位姿估计，得到与各个绝对测量传感器分别对应的绝对估计位姿，所述绝对测量数据为由所述机器人的绝对测量传感器采集的测量数据；分别获取所述机器人的各个相对测量数据，并根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿，所述相对测量数据为由所述机器人的相对测量传感器采集的测量数据；根据所述绝对估计位姿和所述相对估计位姿对所述基准估计位姿进行融合处理，得到所述机器人的融合位姿。进一步地，所述惯性测量数据包括线加速度和角速度；所述根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿包括：根据所述线加速度对所述机器人在上一测量时刻的速度进行更新，得到所述机器人在当前测量时刻的速度；根据所述机器人在上一测量时刻的速度以及在当前测量时刻的速度，对所述机器人在上一测量时刻的位置进行更新，得到所述机器人在当前测量时刻的位置；根据所述角速度对所述机器人在上一测量时刻的姿态角进行更新，得到所述机器人在当前测量时刻的姿态角。进一步地，所述根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿包括：根据第n个相对测量数据计算所述机器人在当前测量时刻的位姿，其中，1≤n≤N，N为所述机器人的相对测量传感器的数目，第n个相对测量数据为由所述机器人的第n个相对测量传感器采集的测量数据；获取所述机器人在上一测量时刻的位姿，并计算所述机器人在当前测量时刻和上一测量时刻之间的位姿差；获取所述机器人在上一测量时刻的融合位姿，并根据所述位姿差和所述机器人在上一测量时刻的融合位姿计算与第n个相对测量传感器对应的相对估计位姿。进一步地，所述计算所述机器人在当前测量时刻和上一测量时刻之间的位姿差包括：根据下式计算所述位姿差：其中，T1p为在上一测量时刻的位置，T1q为在上一测量时刻的姿态角，T2p为在当前测量时刻的位置，T2q为在当前测量时刻的姿态角，dTp为当前测量时刻和上一测量时刻之间的位置差，dTq为当前测量时刻和上一测量时刻之间的姿态差。进一步地，所述根据所述位姿差和所述机器人在上一测量时刻的融合位姿计算与第n个相对测量传感器对应的相对估计位姿包括：根据下式计算与第n个相对测量传感器对应的相对估计位姿：T2′p＝T1′p+T1′q*dTpT2′q＝T1′q*dTq其中，T1′p为上一测量时刻的融合位置，T1′q为上一测量时刻的融合姿态角，T2′p为与第n个相对测量传感器对应的相对估计位置，T2′q为与第n个相对测量传感器对应的相对估计姿态角。进一步地，所述根据所述绝对估计位姿和所述相对估计位姿对所述基准估计位姿进行融合处理包括：计算待融合位姿与所述基准估计位姿之间的相对位姿，所述待融合位姿为任一所述绝对估计位姿或所述相对估计位姿；根据所述相对估计位姿计算所述待融合位姿的残差；根据所述待融合位姿的残差和预设的增益系数对所述基准估计位姿进行修正，得到所述融合位姿。进一步地，所述根据所述待融合位姿的残差和预设的增益系数对所述基准估计位姿进行修正，得到所述融合位姿包括：根据下式计算所述融合位姿：[p,q]update＝[p,q]+K[rp,rq]其中，[p,q]为所述基准估计位姿，[rp,rq]为所述待融合位姿的残差，K为所述增益系数，[p,q]update为所述融合位姿。本申请实施例的第二方面提供了一种机器人位姿估计装置，可以包括：第一位姿估计模块，用于获取机器人的惯性测量数据，并根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿；第二位姿估计模块，用于分别获取所述机器人的各个绝对测量数据，并根据各个绝对测量数据进行位姿估计，得到与各个绝对测量传感器分别对应的绝对估计位姿，所述绝对测量数据为由所述机器人的绝对测量传感器采集的测量数据；第三位姿估计模块，用于分别获取所述机器人的各个相对测量数据，并根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿，所述相对测量数据为由所述机器人的相对测量传感器采集的测量数据；位姿融合模块，用于根据所述绝对估计位姿和所述相对估计位姿对所述基准估计位姿进行融合处理，得到所述机器人的融合位姿。进一步地，所述惯性测量数据包括线加速度和角速度，所述第一位姿估计模块可以包括：速度更新单元，用于根据所述线加速度对所述机器人在上一测量时刻的速度进行更新，得到所述机器人在当前测量时刻的速度；位置更新单元，用于根据所述机器人在上一测量时刻的速度以及在当前测量时刻的速度，对所述机器人在上一测量时刻的位置进行更新，得到所述机器人在当前测量时刻的位置；姿态角更新单元，用于根据所述角速度对所述机器人在上一测量时刻的姿态角进行更新，得到所述机器人在当前测量时刻的姿态角。进一步地，所述第三位姿估计模块可以包括：位姿计算单元，用于根据第n个相对测量数据计算所述机器人在当前测量时刻的位姿，其中，1≤n≤N，N为所述机器人的相对测量传感器的数目，第n个相对测量数据为由所述机器人的第n个相对测量传感器采集的测量数据；位姿差计算单元，用于获取所述机器人在上一测量时刻的位姿，并计算所述机器人在当前测量时刻和上一测量时刻之间的位姿差；相对估计位姿计算单元，用于获取所述机器人在上一测量时刻的融合位姿，并根据所述位姿差和所述机器人在上一测量时刻的融合位姿计算与第n个相对测量传感器对应的相对估计位姿。进一步地，所述位姿差计算单元具体用于根据下式计算所述位姿差：其中，T1p为在上一测量时刻的位置，T1q为在上一测量时刻的姿态角，T2p为在当前测量时刻的位置，T2q为在当前测量时刻的姿态角，dTp为当前测量时刻和上一测量时刻之间的位置差，dTq为当前测量时刻和上一测量时刻之间的姿态差。进一步地，所述相对估计位姿计算单元具体用于根据下式本文档来自技高网...

【技术保护点】
1.一种机器人位姿估计方法，其特征在于，包括：/n获取机器人的惯性测量数据，并根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿；/n分别获取所述机器人的各个绝对测量数据，并根据各个绝对测量数据进行位姿估计，得到与各个绝对测量传感器分别对应的绝对估计位姿，所述绝对测量数据为由所述机器人的绝对测量传感器采集的测量数据；/n分别获取所述机器人的各个相对测量数据，并根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿，所述相对测量数据为由所述机器人的相对测量传感器采集的测量数据；/n根据所述绝对估计位姿和所述相对估计位姿对所述基准估计位姿进行融合处理，得到所述机器人的融合位姿。/n

【技术特征摘要】
1.一种机器人位姿估计方法，其特征在于，包括：
获取机器人的惯性测量数据，并根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿；
分别获取所述机器人的各个绝对测量数据，并根据各个绝对测量数据进行位姿估计，得到与各个绝对测量传感器分别对应的绝对估计位姿，所述绝对测量数据为由所述机器人的绝对测量传感器采集的测量数据；
分别获取所述机器人的各个相对测量数据，并根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿，所述相对测量数据为由所述机器人的相对测量传感器采集的测量数据；
根据所述绝对估计位姿和所述相对估计位姿对所述基准估计位姿进行融合处理，得到所述机器人的融合位姿。


2.根据权利要求1所述的机器人位姿估计方法，其特征在于，所述惯性测量数据包括线加速度和角速度；
所述根据所述惯性测量数据进行位姿估计，得到所述机器人的基准估计位姿包括：
根据所述线加速度对所述机器人在上一测量时刻的速度进行更新，得到所述机器人在当前测量时刻的速度；
根据所述机器人在上一测量时刻的速度以及在当前测量时刻的速度，对所述机器人在上一测量时刻的位置进行更新，得到所述机器人在当前测量时刻的位置；
根据所述角速度对所述机器人在上一测量时刻的姿态角进行更新，得到所述机器人在当前测量时刻的姿态角。


3.根据权利要求1所述的机器人位姿估计方法，其特征在于，所述根据各个相对测量数据进行位姿估计，得到与各个相对测量传感器分别对应的相对估计位姿包括：
根据第n个相对测量数据计算所述机器人在当前测量时刻的位姿，其中，1≤n≤N，N为所述机器人的相对测量传感器的数目，第n个相对测量数据为由所述机器人的第n个相对测量传感器采集的测量数据；
获取所述机器人在上一测量时刻的位姿，并计算所述机器人在当前测量时刻和上一测量时刻之间的位姿差；
获取所述机器人在上一测量时刻的融合位姿，并根据所述位姿差和所述机器人在上一测量时刻的融合位姿计算与第n个相对测量传感器对应的相对估计位姿。


4.根据权利要求3所述的机器人位姿估计方法，其特征在于，所述计算所述机器人在当前测量时刻和上一测量时刻之间的位姿差包括：
根据下式计算所述位姿差：






其中，T1p为在上一测量时刻的位置，T1q为在上一测量时刻的姿态角，T2p为在当前测量时刻的位置，T2q为在当前测量时刻的姿态角，dTp为当前测量时刻和上一测量时刻之间的位置差，dTq为当前测量时刻和上一测量时刻之间的姿态差。


5.根据权利要求4所述的机器人位姿估计方法，其特征在于，所述根据所述位姿差和所述机器人在上一测量时刻的...

【专利技术属性】
技术研发人员：何婉君，刘志超，庞建新，熊友军，
申请(专利权)人：深圳市优必选科技股份有限公司，
类型：发明
国别省市：广东;44