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

说明书

本申请的目的是提供一种位姿优化方法及设备，本申请利用扩展卡尔曼融合算法和激光里程计方法分别进行机器人姿态估计、利用自适应加权和卡尔曼增益思想的修正算法，通过分析调整最优加权因子和局部动态加权因子，构建修正算法对机器人初步位姿进行修正，得到机器人准确位姿即最优位姿，减少了位姿累计误差，提高了姿态估计准确度和鲁棒性。

技术领域

本申请涉及计算机领域，尤其涉及一种位姿优化方法及设备。

背景技术

随着移动机器人被要求在复杂多变的环境下执行越来越严峻的任务，机器人实现对自身位姿的精确估计就显着尤为重要。目前对于在室内的未知条件下机器人位姿累计误差较大，单一传感器精度不够的问题，大多数采取的方法是加入视觉相机和激光雷达等传感器来进一步估计位姿。然而，视觉相机易受光照条件限制，不利于保证机器人位姿估计的精确度。虽然激光雷光的测量精度很高，并且可以在无光环境工作，但因激光数据是对外部环境的观测，所以激光雷达构建的激光里程计极其容易受环境影响，故需要结合其他传感器实现对机器人位姿精确估计。当使用不同传感器在室内未知环境下定位时，仅使用单一的位姿估计方法会造成鲁棒性低的问题。

因此，实现对机器人位姿进行修正，得到更精确的机器人位姿信息，减少位姿累计误差，提高姿态估计鲁棒性是本领域需要继续研究的方向。

发明内容

本申请的一个目的是提供一种位姿优化方法及设备，以解决现有技术中环卫车如何减少位姿累计误差，提高姿态估计精确度和鲁棒性的问题。

根据本申请的一个方面，提供了一种位姿优化方法，包括：

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

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

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

进一步地，所述位姿优化方法还包括：

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

进一步地，上述位姿优化方法中，所述基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿，包括：

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

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

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

进一步地，上述位姿优化方法中，所述获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿，包括：

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

进一步地，上述位姿优化方法中，所述基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿，包括：

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