[发明专利]一种在高动态环境下的工业移动机器人定位方法

权利要求书

1.一种在高动态环境下的工业移动机器人定位方法，所述工业移动机器人上安装有轮式编码器、惯性测量单元和激光扫描单元，其特征在于，包括以下步骤：

分别采集轮式编码器、惯性测量单元和激光扫描单元的感知数据，并进行速度估计，求得速度估计对应的协方差矩阵；

采用扩展卡尔曼滤波器对轮式编码器、惯性测量单元和激光扫描单元的速度估计和协方差矩阵进行融合，得到精准的融合后速度估计；

对工业移动机器人进行航迹推演，并将融合后速度估计转换成第一位姿估计；所述将融合后速度估计转换成第一位姿估计，其表达式如下：

时间增量：Δt＝current_t-last_t

X方向位移增量：Δx＝(v_x.cosθ-v_y.sinθ).Δt

Y方向位移增量：Δy＝(v_x.sinθ-v_y.cosθ).Δt

姿态角增量：Δθ＝v_θ.Δt

X坐标：x+＝Δt

Y坐标：y+＝Δy

姿态角：θ+＝Δθ

其中，v_x表示X方向线速度，v_y表示Y方向线速度，v_θ表示角速度，θ表示机器人的姿态角；

采集光束似然场算法将激光扫描单元的感知数据与地图匹配定位；求得任一时刻激光扫描单元采集的激光地图的追踪激光观测质量，并进行环境变化检验，得到第二位姿估计；所述环境变化检验，包括以下步骤：

求得追踪激光观测质量在时间上的变化率；

若变化率的数值小于预设的阈值，则放弃计算第二位姿估计；

若变化率的数值大于或等于预设的阈值，则求得第二位姿估计；

采用扩展卡尔曼滤波器对第一位姿估计和第二位姿估计进行融合，得到精准的第三位姿估计。

2.根据权利要求1所述的一种在高动态环境下的工业移动机器人定位方法，其特征在于，所述轮式编码器的感知数据的处理过程如下：

利用轮式编码器采集获得线速度和角速度，并根据工业移动机器人预设的运动模型，求得线速度估计和角速度估计；根据线速度估计、角速度估计与实际数据的统计方式对比求得协方差矩阵。