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

权利要求书

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时刻的最佳导航点

2.如权利要求1所述的移动机器人导航方法，其特征在于，所述步骤S105涉及的随机生成一组t+1时刻的左、右轮可能旋转速度的集合的方法如下所示：

S1051：在t＝1到t＝N_u时刻设定与其中v_m表示轮子旋转速度上限，表示0到v_m的均匀分布，N_u为一个大于1的正整数；在tN_u时刻计算其中与为序列的均值与方差，与为序列的均值与方差，表示t-N_u+1时刻到t时刻的所有的左轮期望旋转速度，表示t-N_u+1时刻到t时刻的所有的右轮期望旋转速度，设定与其中表示高斯分布；

S1052：按照步骤S1051设定好的分布，即与随机生成一组t+1时刻的左、右轮可能旋转速度的集合与其中每个集合有L个元素。