[发明专利]一种位姿优化方法及设备

权利要求书

1.一种位姿优化方法，其特征在于，所述方法包括：

获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿；

基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿；

计算所述激光里程计位姿和所述初步位姿的位姿偏差值，若所述位姿偏差值大于位姿偏差阈值，则基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿。

2.根据权利要求1所述的方法，其特征在于，所述位姿优化方法还包括：

若所述偏差值小于等于所述位姿偏差阈值，则计算所述激光里程计位姿和所述初步位姿的位姿平均值作为所述最优位姿。

3.根据权利要求2所述的方法，其特征在于，所述基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿，包括：

分别计算所述激光里程计位姿和所述初步位姿的方差，并结合多元函数极值方法得到激光里程计位姿整体最优加权因子和初步位姿整体最优加权因子；

分别计算所述激光里程计位姿和所述初步位姿与上一时刻修正后位姿的偏差信息，基于所述偏差信息通过平方差计算方法得到激光里程计位姿局部动态权重因子和初步位姿局部动态权重因子；

将所述激光里程计位姿整体最优加权因子、所述初步位姿整体最优加权因子、所述激光里程计位姿局部动态权重因子、所述初步位姿局部动态权重因子进行融合得到修正权重因子，通过所述修正权重因子进行当前时刻的位姿矫正得到最优位姿。

4.根据权利要求1所述的方法，其特征在于，所述获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿，包括：

选择PL-ICP的点云匹配的方法构建激光里程计，并计算得到所述激光里程计位姿。

5.根据权利要求1所述的方法，其特征在于，所述基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿，包括：

惯性传感器测量得到机器人角速度和加速度后，通过Mahony姿态结算算法计算得到姿态结算位姿数据；

通过轮式里程计方法得到轮式里程计位姿数据；

利用所述扩展卡尔曼滤波将所述姿态结算位姿数据和所述轮式里程计位姿数据进行融合得到所述初步位姿。

6.一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行时，使所述处理器实现如权利要求1至5中任一项所述的方法。

7.一种位姿优化设备，其特征在于，该设备包括：

一个或多个处理器；

计算机可读介质，用于存储一个或多个计算机可读指令，

当所述一个或多个计算机可读指令被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1至5中任一项所述的方法。