本发明公开了一种移动机器人导航方法，包括：初始化、时间自增、读取传感器数据、位姿与滑动系数估计与计算左右轮期望旋转速度。同时公开了一种移动机器人导航装置，包括定位系统、电子罗盘、里程计与计算机，定位系统、电子罗盘、里程计电信连接计算机，计算机用于执行本发明公开的移动机器人导航方法。与已有的公开的技术方案相比，本发明考虑到了不同地面类型的打滑特性，能够同时估计出机器人的位姿与滑动系数，进而可以在导航算法种考虑打滑效应，在优化路径的过程中综合考虑运动时间与能耗，提升电池供电机器人的运行时间。
1.一种移动机器人导航方法，其特征在于，包括以下步骤：S101：初始化，令时间t＝1，设定采样时间间隔T、机器人宽度B，确定t时刻的状态最优估计值 其中 分别表示t时刻的机器人的东向坐标的最优估计值、北向坐标的最优估计值、方向的最优估计值、左滑动比的最优估计值、右滑动比的最优估计值和侧滑因子的最优估计值，设定过程噪声和观测噪声的方差Q和R，设定t时刻的状态最优估计误差协方差 为6维方阵；令t时刻的最佳导航点 为机器人初始坐标；
S103：从定位系统读取t时刻的机器人位置数据，从电子罗盘读取t时刻的机器人方向数据，得到t时刻的观测向量 其中 表示t时刻的机器人东向坐标检测值，表示t时刻的机器人北向坐标检测值， 表示t时刻的机器人方向检测值；从里程计读取t时刻的机器人左右轮的旋转速度数据 其中 表示机器人左轮旋转速度检测值， 表示机器人右轮旋转速度检测值；
S104：利用yt和wt估计t时刻的机器人的位姿 与滑动系数 如下：S1041：状态预测估计，得到t时刻的状态预测估计值 其中分别表示t时刻的机器人的东向坐标的预测估计值、北向坐标的预测估计值、方向的预测估计值、左滑动比的预测估计值、右滑动比的预测估计值、侧滑因子的预测估计值，可由状态转移方程 推出，状态转移方程具体如下：并且计算状态预测估计误差协方差，如下： 其中，F为 相对于的雅可比矩阵， 为t时刻的状态最优估计误差协方差， 为6维方阵，Q表示过程噪声的方差，F′表示F的转置；
S1042：计算观测新息 与新息协方差 其中计算新息协方差估计值
其中，Nw为 估计的滑窗宽度，计算衰减因子γ，如下：其中，α为一个大于1的实数，R表示观测噪声的方差，H′表示H的转置；
S1043：调整 令 并重新计算新息协方差S1044：进行状态最优估计，得到t时刻的状态最优估计值 如下： 其中， 并且计算状态最优估计误差协方差 其中I6表示6维单位阵；
S105：根据步骤S104获取的t时刻的机器人的位姿与滑动系数，计算t+1时刻的左、右轮期望旋转速度 与 如下：首先 ，随机生成一组t+1时刻的左、右轮可能旋转速度的集合与 每个集合有L个元素，其中，
表示随机生成的t+1时刻的左轮可能旋转速度， 表示随机生成的t+1时刻的右轮可能旋转速度，然后将 与 中的速度对带入到 f为状态转移方程，得到对应的位置预测值 记坐标点 计算每个Ot+1,i对应的目标函数Ji，目标函数Ji＝Ji,1+Ji,2其中，k1、k2分别为旋转阻力能耗系数和前进阻力能耗系数，其中，OT表示终点的坐标， 与ε(Ot+1,i,OT)分别表示Ot+1,i与 的欧式距离、Ot+1,i与OT的欧式距离；找出Ji取最小时的Ot+1,i，即为t+1时刻的最佳导航点
2.如权利要求1所述的移动机器人导航方法，其特征在于，所述步骤S105涉及的随机生成一组t+1时刻的左、右轮可能旋转速度的集合的方法如下所示：S1051：在t＝1到t＝Nu时刻设定 与 其中vm表示轮子旋转速度上限， 表示0到vm的均匀分布，Nu为一个大于1的正整数；在t>Nu时刻计算其中 与 为序列 的均值与方差， 与 为序列 的均值与方差， 表
示t-Nu+1时刻到t时刻的所有的左轮期望旋转速度， 表示t-Nu+1时刻到t时刻的所有的右轮期望旋转速度，设定 与 其中 表示高斯分布；
S1052：按照步骤S1051设定好的分布，即 与随机生成一组t+1时刻的左、右轮可能旋转速度的集合 与其中每个集合有L个元素。
3.一种移动机器人导航装置，其特征在于，包括定位系统、电子罗盘、里程计与计算机，定位系统、电子罗盘、里程计电信连接计算机；
里程计用于读取t时刻的机器人左右轮的旋转速度数据；
计算机用于利用所述权利要求1-2所述的方法对所述机器人位置数据、机器人方向数据以及机器人左右轮的旋转速度数据进行处理，实现移动机器人导航。
4.一种计算机可读储存介质，其特征在于，所述计算机可读存储介质上存储有若干导航程序，所述若干导航程序用于被处理器调用并执行所述权利要求1-2任一项的移动机器人导航方法中的步骤。
5.一种差分转向轮式移动机器人，包括导航装置，其特征在于，所述导航装置为权利要求3所述的导航装置。
6.一种履带式移动机器人，包括导航装置，其特征在于，所述导航装置为权利要求3所述的导航装置。
一种移动机器人导航方法与装置
技术领域
[0001] 本发明涉及机器人技术领域，特别是涉及一种移动机器人导航方法与装置。
背景技术
[0002] 近年来，自主机器人在空间探索、军事任务、农业等领域发挥了重要作用。在未来，人们期望这些机器人能够在非结构化和动态的户外环境中执行各种任务，并增加自主性。然而，机器人所能携带的电池和/或燃料中的能量是有限的，这限制了它的使用寿命。为了使机器人能够在不充电或加油的情况下执行更广泛的任务，节能是非常重要的。因此，通过良好的导航机制可以最大程度上降低能源的消耗。
[0003] 文献“孙世颖,赵晓光,边疆,谭民.基于行人间交互意图检测的服务机器人导航[J].华中科技大学学报(自然科学版),2017,45(10):80-84.”针对人与机器人共存环境中的机器人导航问题，提出一种基于行人间交互意图检测的服务机器人导航方法。专利CN201610203026.5提供了一种机器人导航的方法及导航机器人，该方法包括：当检测到机器人移动时，实时采集设置于预置位置的图像，将采集到的该图像进行处理，获取该图像中的路径图，将处理后得到的该图像中的路径图与预置的路径地图做比较，获取该机器人的瞬时位置，根据该瞬时位置、该机器人的目的地以及该路径地图，生成该机器人移动到该目的地的移动路径，控制该机器人按生成的该移动路径移动。本发明通过图像处理技术，控制机器人按预设的移动路径移动，使机器人的导航变得更加精准。专利CN201510891364.8提供一种机器人导航方法及系统，其中方法包括：通过根据导航指令控制机器人上设置的摄像头获取机器人周围的标识点；根据周围的标识点查询预置地图，确定机器人的当前位置，其中，预置地图包括：各个导航轨迹以及各个导航轨迹上的标识点；根据机器人的当前位置和目标位置查询预置地图，获取与当前位置和目标位置匹配的导航轨迹；根据当前位置和导航轨迹确定机器人的行进方向和行进路线，从而使得机器人可以根据行进方向和行进路线移动至目标位置，实现导航，避免了成本较高的接入点设备的设置，以及对场所的施工改造，节省了成本，降低了对其所应用的场所的施工要求。
[0004] 传统的导航手段很少考虑了不同地面类型对机器人能耗的影响，为了实现路径最短有时会规划一条阻力较大的道路，提升了机器人的能量消耗。
发明内容
[0005] 为解决上述问题，本发明公开了一种移动机器人导航方法，具体包括以下步骤：
[0006] S101：初始化，令时间t＝1，设定采样时间间隔T、机器人宽度B，确定t时刻的状态最优估计值 其中 分别表示t时刻的机器人的东向坐标的最优估计值、北向坐标的最优估计值、方向的最优估计值、左滑动比的最优估计值、右滑动比的最优估计值和侧滑因子的最优估计值，设定过程噪声和观测噪声的方差Q和R，设定t时刻的状态最优估计误差协方差 为6维方阵；令t时刻的最佳导航点 为机器人初始坐标；
[0007] S102：令t自增1；
[0008] S103：从定位系统读取t时刻的机器人位置数据，从电子罗盘读取t时刻的机器人方向数据，得到t时刻的观测向量 其中 表示t时刻的机器人东向坐标检测值， 表示t时刻的机器人北向坐标检测值， 表示t时刻的机器人方向检测值；从里程计读取t时刻的机器人左右轮的旋转速度数据 其中 表示机器人左轮旋转速度检测值， 表示机器人右轮旋转速度检测值；
[0009] S104：利用yt和wt估计t时刻的机器人的位姿 与滑动系数 如下：
[0010] S1041：状态预测估计，得到t时刻的状态预测估计值 其中 分别表示t时刻的机器人的东向坐标的预测估计值、北向坐标的预测
估计值、方向的预测估计值、左滑动比的预测估计值、右滑动比的预测估计值、侧滑因子的预测估计值，可由状态转移方程 推出，状态转移方程具体如下：
[0011]
[0012]
[0013]
[0014]
[0015] 并且计算状态预测估计误差协方差，如下： 其中，F为相对于 的雅可比矩阵， 为t时刻的状态最优估计误差协方差， 为6维方阵，Q表示过程噪声的方差，F′表示F的转置；
[0016] S1042：计算观测新息 与新息协方差 其中
[0017]
[0018] 计算新息协方差估计值
[0019]
[0020] 其中，Nw为 估计的滑窗宽度，计算衰减因子γ，如下：
[0021]
[0022] 其中，α为一个大于1的实数，R表示观测噪声的方差，H′表示H的转置；
[0023] S1043：调整 令 并重新计算新息协方差
[0024] S1044：进行状态最优估计，得到t时刻的状态最优估计值 如下：其中， 并且计算状态最优估计误差协方差 其中I6表
示6维单位阵；
[0025] S105：根据步骤S104获取的t时刻的机器人的位姿与滑动系数，计算t+1时刻的左、右轮期望旋转速度 与 如下：
[0026] 首先，随机生成一组t+1时刻的左、右轮可能旋转速度的集合与 每个集合有L个元素，其中，
表示随机生成的t+1时刻的左轮可能旋转速度， 表示
随机生成的t+1时刻的右轮可能旋转速度，然后将 与 中的速度对
带入到 f为状态转移方程，得到对应的位置
预测值 记坐标点 计算每个Ot+1,i对应的目
标函数Ji，目标函数Ji＝Ji,1+Ji,2
[0027]
[0028] 其中，k1、k2分别为旋转阻力能耗系数和前进阻力能耗系数，
[0029]
[0030] 其中，OT表示终点的坐标， 与ε(Ot+1,i,OT)分别表示Ot+1,i与 的欧式距离、Ot+1,i与OT的欧式距离；找出Ji取最小时的Ot+1,i，即为t+1时刻的最佳导航点[0031] 其中，所述步骤S105涉及的随机生成一组t+1时刻的左、右轮可能旋转速度的集合的方法如下所示：
[0032] S1051：在t＝1到t＝Nu时刻设定 与 其中vm表示轮子旋转速度上限， 表示0到vm的均匀分布，Nu为一个大于1的正整数；在t>Nu时刻计算 其中 与 为序列 的均
值与方差， 与 为序列 的均值与方差，
表示t-Nu+1时刻到t时刻的所有的左轮期望旋转速度，
表示t-Nu+1时刻到t时刻的所有的右轮期望旋转速度，设定
与 其中 表示高斯分布；
[0033] S1052：按照步骤S1051设定好的分布，即 与随机生成一组t+1时刻的左、右轮可能旋转速度的集合
与 其中每个集合有L个元素。
[0034] 本发明还公开了一种移动机器人导航装置，其特征在于，包括定位系统、电子罗盘、里程计与计算机，定位系统、电子罗盘、里程计电信连接计算机；
[0035] 定位系统用于读取t时刻的机器人位置数据；
[0036] 电子罗盘用于读取t时刻的机器人方向数据；
[0037] 里程计用于读取t时刻的机器人左右轮的旋转速度数据；
[0038] 计算机用于利用所述权利要求1-2所述的方法对所述机器人位置数据、机器人方向数据以及机器人左右轮的旋转速度数据进行处理，实现移动机器人导航。
[0039] 本发明还公开了一种计算机可读储存介质，其特征在于，所述计算机可读存储介质上存储有若干导航程序，所述若干导航程序用于被处理器调用并执行所述权利要求1-2任一项的移动机器人导航方法中的步骤。
[0040] 本发明还公开了一种差分转向轮式移动机器人，包括导航装置，其特征在于，所述导航装置为权利要求3所述的导航装置。
[0041] 本发明还公开了一种履带式移动机器人，包括导航装置，其特征在于，所述导航装置为权利要求3所述的导航装置。
[0042] 与已有的技术相比，本发明考虑到了不同地面类型的打滑特性，能够同时估计出机器人的位姿与滑动系数，进而可以在导航算法种考虑打滑效应，在优化路径的过程中综合考虑运动时间与能耗，提升电池供电机器人的运行时间。
附图说明
[0043] 下面结合附图，对本发明的具体实施方式进行详细描述：
[0044] 图1是一种移动机器人导航装置结构框图。
具体实施方式
[0045] 为了更进一步说明本发明的特征，请参阅以下有关本发明的详细说明与附图。所附图仅供参考与说明之用，并非用来对本发明的保护范围加以限制。
[0046] 本发明公开了一种移动机器人导航方法，具体包括以下步骤：
[0047] S101：初始化，令时间t＝1，设定采样时间间隔T、机器人宽度B，确定t时刻的状态最优估计值 其中 分别表示t时刻的机器人的东向坐标的最优估计值、北向坐标的最优估计值、方向的最优估计值、左滑动比的最优估计值、右滑动比的最优估计值和侧滑因子的最优估计值，设定过程噪声和观测噪声的方差Q和R，设定t时刻的状态最优估计误差协方差 为6维方阵；令t时刻的最佳导航点 为机器人初始坐标；
[0048] 确定 的可采用如下方法：人工检测机器人的东向坐标、北向坐标与方向，并将结果分别赋予 对于 可设定为0；根据传感器本身的参数或者通过对其输出噪声的统计可以确定Q与R，状态最优估计误差协方差 可以设定为一个对角阵，其对角元素皆为0.01；
[0049] S102：令t自增1；
[0050] S103：从定位系统读取t时刻的机器人位置数据，从电子罗盘读取t时刻的机器人方向数据，得到t时刻的观测向量 其中 表示t时刻的机器人东向坐标检测值， 表示t时刻的机器人北向坐标检测值， 表示t时刻的机器人方向检测值；从里程计读取t时刻的机器人左右轮的旋转速度数据 其中 表示机器人左轮旋转速度检测值， 表示机器人右轮旋转速度检测值；
[0051] S104：利用yt和wt估计t时刻的机器人的位姿 与滑动系数 如下：
[0052] S1041：状态预测估计，得到t时刻的状态预测估计值 其中 分别表示t时刻的机器人的东向坐标的预测估计值、北向坐标的预测
估计值、方向的预测估计值、左滑动比的预测估计值、右滑动比的预测估计值、侧滑因子的预测估计值，可由状态转移方程 推出，状态转移方程具体如下：
[0053]
[0054]
[0055]
[0056]
[0057] 并且计算状态预测估计误差协方差，如下： 其中，F为相对于 的雅可比矩阵， 为t时刻的状态最优估计误差协方差， 为6维方阵，Q表示过程噪声的方差，F′表示F的转置；
[0058] S1042：计算观测新息 与新息协方差 其中
[0059]
[0060] 计算新息协方差估计值
[0061]
[0062] 其中，Nw为 估计的滑窗宽度，计算衰减因子γ，如下：
[0063]
[0064] 其中，α为一个大于1的实数，R表示观测噪声的方差，H′表示H的转置；
[0065] S1043：调整 令 并重新计算新息协方差
[0066] S1044：进行状态最优估计，得到t时刻的状态最优估计值 如下：其中， 并且计算状态最优估计误差协方差 其中I6表示6维
单位阵；
[0067] S105：根据步骤S104获取的t时刻的机器人的位姿与滑动系数，计算t+1时刻的左、右轮期望旋转速度 与 如下：
[0068] 首先，随机生成一组t+1时刻的左、右轮可能旋转速度的集合与 每个集合有L个元素，其中，
表示随机生成的t+1时刻的左轮可能旋转速度， 表示
随机生成的t+1时刻的右轮可能旋转速度，然后将 与 中的速度对
带入 到 得 到对 应的 位置 预测 值
其中f为状态转移方程，用 替换 中 即
可 的具体形式；记坐标点 计算每个Ot+1,i对应的目标函数
Ji，目标函数Ji＝Ji,1+Ji,2
[0069]
[0070] 其中，k1、k2分别为旋转阻力能耗系数和前进阻力能耗系数，
[0071]
[0072] 其中，OT表示终点的坐标， 与ε(Ot+1,i,OT)分别表示Ot+1,i与 的欧式距离、Ot+1,i与OT的欧式距离；找出Ji取最小时的Ot+1,i，即为t+1时刻的最佳导航点[0073] 优选地，所述步骤S105涉及的随机生成一组t+1时刻的左、右轮可能旋转速度的集合的方法如下所示：
[0074] S1051：在t＝1到t＝Nu时刻设定 与 其中vm表示轮子旋转速度上限， 表示0到vm的均匀分布，Nu为一个大于1的正整数；在t>Nu时刻计算 其中 与 为序列 的均
值与方差， 与 为序列 的均值与方差，
表示t-Nu+1时刻到t时刻的所有的左轮期望旋转速度，
表示t-Nu+1时刻到t时刻的所有的右轮期望旋转速度，设定
与 其中 表示高斯分布；
[0075] S1052：按照步骤S1051设定好的分布，即 与随机生成一组t+1时刻的左、右轮可能旋转速度的集合
与 其中每个集合有L个元素。
[0076] 本发明还公开了一种移动机器人导航装置，如附图1所示，其特征在于，包括定位系统、电子罗盘、里程计与计算机，定位系统、电子罗盘、里程计电信连接计算机；
[0077] 定位系统用于读取t时刻的机器人位置数据；
[0078] 电子罗盘用于读取t时刻的机器人方向数据；
[0079] 里程计用于读取t时刻的机器人左右轮的旋转速度数据；
[0080] 计算机用于利用所述权利要求1-2所述的方法对所述机器人位置数据、机器人方向数据以及机器人左右轮的旋转速度数据进行处理，实现移动机器人导航。
[0081] 本发明还公开了一种计算机可读储存介质，其特征在于，所述计算机可读存储介质上存储有若干导航程序，所述若干导航程序用于被处理器调用并执行所述权利要求1-2任一项的移动机器人导航方法中的步骤。
[0082] 本发明还公开了一种差分转向轮式移动机器人，包括导航装置，其特征在于，所述导航装置为权利要求3所述的导航装置。
[0083] 本发明还公开了一种履带式移动机器人，包括导航装置，其特征在于，所述导航装置为权利要求3所述的导航装置。
[0084] 本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。
[0085] 以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
