[发明专利]一种移动机器人导航方法与装置

摘要

本发明公开了一种移动机器人导航方法，包括：初始化、时间自增、读取传感器数据、位姿与滑动系数估计与计算左右轮期望旋转速度。同时公开了一种移动机器人导航装置，包括定位系统、电子罗盘、里程计与计算机，定位系统、电子罗盘、里程计电信连接计算机，计算机用于执行本发明公开的移动机器人导航方法。与已有的公开的技术方案相比，本发明考虑到了不同地面类型的打滑特性，能够同时估计出机器人的位姿与滑动系数，进而可以在导航算法种考虑打滑效应，在优化路径的过程中综合考虑运动时间与能耗，提升电池供电机器人的运行时间。

主权项

1.一种移动机器人导航方法，其特征在于，包括以下步骤：S101：初始化，令时间t＝1，设定采样时间间隔T、机器人宽度B，确定t时刻的状态最优估计值其中分别表示t时刻的机器人的东向坐标的最优估计值、北向坐标的最优估计值、方向的最优估计值、左滑动比的最优估计值、右滑动比的最优估计值和侧滑因子的最优估计值，设定过程噪声和观测噪声的方差Q和R，设定t时刻的状态最优估计误差协方差为6维方阵；令t时刻的最佳导航点为机器人初始坐标；S102：令t自增1；S103：从定位系统读取t时刻的机器人位置数据，从电子罗盘读取t时刻的机器人方向数据，得到t时刻的观测向量其中表示t时刻的机器人东向坐标检测值，表示t时刻的机器人北向坐标检测值，表示t时刻的机器人方向检测值；从里程计读取t时刻的机器人左右轮的旋转速度数据其中表示机器人左轮旋转速度检测值，表示机器人右轮旋转速度检测值；S104：利用y_t和w_t估计t时刻的机器人的位姿与滑动系数如下：S1041：状态预测估计，得到t时刻的状态预测估计值其中分别表示t时刻的机器人的东向坐标的预测估计值、北向坐标的预测估计值、方向的预测估计值、左滑动比的预测估计值、右滑动比的预测估计值、侧滑因子的预测估计值，可由状态转移方程推出，状态转移方程具体如下：并且计算状态预测估计误差协方差，如下：其中，F为相对于的雅可比矩阵，为t时刻的状态最优估计误差协方差，为6维方阵，Q表示过程噪声的方差，F′表示F的转置；S1042：计算观测新息与新息协方差其中计算新息协方差估计值其中，N_w为估计的滑窗宽度，计算衰减因子γ，如下：其中，α为一个大于1的实数，R表示观测噪声的方差，H′表示H的转置；S1043：调整令并重新计算新息协方差S1044：进行状态最优估计，得到t时刻的状态最优估计值如下：其中，并且计算状态最优估计误差协方差其中I₆表示6维单位阵；S105：根据步骤S104获取的t时刻的机器人的位姿与滑动系数，计算t+1时刻的左、右轮期望旋转速度与如下：首先，随机生成一组t+1时刻的左、右轮可能旋转速度的集合与每个集合有L个元素，其中，表示随机生成的t+1时刻的左轮可能旋转速度，表示随机生成的t+1时刻的右轮可能旋转速度，然后将与中的速度对带入到f为状态转移方程，得到对应的位置预测值记坐标点计算每个O_t+1,i对应的目标函数J_i，目标函数J_i＝J_i,1+J_i,2其中，k₁、k₂分别为旋转阻力能耗系数和前进阻力能耗系数，其中，O_T表示终点的坐标，与ε(O_t+1,i,O_T)分别表示O_t+1,i与的欧式距离、O_t+1,i与O_T的欧式距离；找出J_i取最小时的O_t+1,i，即为t+1时刻的最佳导航点