一种移动机器人标准控制集局部路径规划方法及系统

1.本发明属于移动机器人路径规划领域，具体涉及一种移动机器人标准控制集局部路径规划方法及系统。


背景技术：

2.路径规划是实现移动机器人自主导航的关键技术之一。其目的是在给定的初始状态和最终状态之间，为移动机器人找到一条与环境中的障碍物不发生碰撞的路径。近年来，由于移动机器人应用场景的不断复杂，使得全局路径规划不能满足移动机器人自主运动的能力，所以局部路径规划受到越来越多学者的关注。
3.局部路径规划是利用传感器获得的局部环境信息在线实时地调整轨迹，现有的局部路径规划方法主要有动态窗口法(dwa)、时间弹性带算法(teb)、人工势场法和基于模型的最优控制方法。动态窗口法由于在速度空间直接进行采样，不能保证运动的平稳性；时间弹性带算法得到的结果往往速度波动较大且不能保证最优解；人工势场法存在局部最小和路径在障碍物附近震荡的问题，同时该方法未考虑移动机器人运动学模型，需对路径进一步优化；基于模型的最优控制将路径规划视为最优控制优化问题，由于模型复杂往往采用数值解法，因此，该方法保证最优解的同时导致求解速度较慢。
4.由此可见，虽然移动机器人局部路径规划的研究取得了长足的进展，但也存在运动不平稳、处理复杂环境问题能力弱、计算复杂等问题。


技术实现要素：

5.为了解决现有技术中存在的问题，本发明提供一种与邻域搜索原则相结合的移动机器人标准控制集局部路径规划方法，该方法考虑机器人运动学模型以及其他约束，将路径规划问题转变为最优控制问题进而生成标准控制集；以邻域搜索原则生成全局路径点为引导，进行局部路径规划，从而得到最终路径。标准控制集保证了每段路径的最优性以及平稳性，直接查找控制集进行规划提升了计算效率。
6.为了实现上述目的，本发明采用的技术方案是：一种与邻域搜索原则相结合的移动机器人标准控制集局部路径规划方法，包括以下步骤：
7.确定移动机器人运动学模型；确定移动机器人的路径约束；通过邻域搜索原则确定标准控制集的初末状态；确定基于平滑和时间最优的目标函数后生成最优控制问题；对所述最优控制问题进行离散化，采用内点法求解该最优控制问题进而生成标准控制集；
8.根据邻域搜索原则生成全局路径点，根据相邻两个路径点查询标准控制集得到一组路径；
9.若该路径无新增障碍物，则移动机器人直接执行该路径；若该段路径新增障碍物，则以查询的该路径为初始解，加入无碰撞限制后重新求解最优控制问题得出新路径交给移动机器人执行；构建出移动机器人完整路径。
10.通过以下步骤建立标准控制集：
11.根据移动机器人运动学模型、路径约束、初末状态约束以及一个目标函数生成完整移动机器人路径规划最优控制问题；
12.移动机器人运动学模型采用微分方程表示，选择选择加速度a(t)和角加速度α(t)作为控制量，机器人坐标位置x(t)、y(t)、角向位置θ(t)、移动速度和移动角速度ω(t)作为状态变量；
13.路径约束中，针对移动机器人的性能以及物理特性，机器人移动速度机器人角速度ω(t)、加速度a(t)和角向加速度α(t)都有上下限，其中，加入和ω(t)连续并可导，使移动机器人的路径连续化；
14.通过邻域搜索原则确定标准控制集的每组初末状态，
15.选择一条兼顾平滑且时间最优的一条路径作为标准控制集在该状态下的路径，其中平滑度的定义为速度以及角速度的二范数，记为j1；关于时间的目标函数为t
f-t0，记为j2；提出基于平滑和时间最优的目标函数j：
[0016][0017]
移动机器人路径规划最优控制问题整体表述为：
[0018][0019][0020]
将所述最优化控制问题离散为非线性规划问题，进而采用内点法求得数值最优解；
[0021]
给定邻域搜索原则中搜索的方向个数n，则标准控制集中路径个数为n2，通过求解所述最优控制问题得出每组初末状态下的路径，组成一个整体的标准控制集。
[0022]
移动机器人模型的微分方程为：
[0023][0024]
其中，t为时间；整体的路径规划为t0到tf时刻运动过程；(x,y)为移动机器人当前时刻的位置；θ为方向角；为当前点的速度；a为相应的加速度；ω为当前点的角速度；α相应的角加速度，选择a(t)和α(t)作为控制量，x(t)、y(t)、θ(t)、和ω(t)作为状态变量。
[0025]
通过邻域搜索原则确定标准控制集的每组初末状态，具体为：通过邻域搜索原则生成的起点状态到终点状态之间的全局路径点中，相邻两点组成一个路径规划段，取每段中点状态作为标准控制集的每组初末状态。
[0026]
查询标准控制集的方法为，邻域搜索原则生成全局路径点，相邻两点组成一个路径规划段，查询相邻路径规划段的角度即可。
[0027]
加入无碰撞限制后重新求解最优控制问题的具体实施过程为：
[0028]
先将障碍物进行膨胀之后再选取膨胀后障碍物边界的n个点，计算所述n个点与移动机器人中心的距离，同时考虑速度对无碰撞限制的影响；
[0029]
惩罚函数通过障碍物点与目标点的距离偏差来调节函数值的大小，且距离越近，惩罚函数值越大，综合移动机器人速度与目标函数中惩罚函数的比重对避让障碍物的影
响，移动机器人无碰撞限制的避障功能函数为：
[0030][0031]
其中，s
obs
为权重系数，υi＝υ
x2
+υ
y2
，(xi,yi)是障碍物点在机器人坐标系下的位置坐标， (x0，y0)是机器人质心坐标，ζ为正数，用于防止出现分母为0。
[0032]
另外，本发明还提供一种移动机器人标准控制集局部路径规划系统，其特征在于，包括标准控制集生成模块、路径获取模块以及路径计算模块；
[0033]
标准控制集生成模块根据移动机器人运动学模型和移动机器人的路径约束；通过邻域搜索原则确定标准控制集的初末状态；确定基于平滑和时间最优的目标函数后生成最优控制问题；对所述最优控制问题进行离散化，采用内点法求解该最优控制问题进而生成标准控制集；
[0034]
路径获取模块用于根据邻域搜索原则生成全局路径点，根据相邻两个路径点查询标准控制集得到一组路径；
[0035]
路径计算模块用于根据所述路径构建移动机器人完整路径，具体为：若该段路径无新增障碍物，则移动机器人直接执行该路径；若该段路径新增障碍物，则以查询的该路径为初始解，加入无碰撞限制后重新求解最优控制问题得出新路径交给移动机器人执行；构建出移动机器人完整路径。
[0036]
在基于本发明方法构思的基础上还提供一种计算机设备，包括处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取部分或者全部所述计算机可执行程序并执行，处理器执行部分或全部计算可执行程序时能实现本发明所述的一种与邻域搜索原则相结合的移动机器人标准控制集路径规划方法。
[0037]
同时提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时，能实现本发明所述的一种与邻域搜索原则相结合的移动机器人标准控制集路径规划方法。
[0038]
与现有技术相比，本发明至少具有以下有益效果：
[0039]
基于本发明所述方法，并不需要实时重规划全局路径，在当前全局路径上出现突发障碍物，将该段对应控制集中的轨迹作为初始解，重新求解最优控制问题，大幅降低移动机器人应对突发障碍物的反映时间；标准控制集是通过最优控制问题进行生成，保证每段路径是基于最优目标函数的，减小了移动机器人在运动过程中线速度与角速度的剧烈变化，提升了运动的平稳性；本发明所述方法适应性强，能够运用到多种环境中，无突发障碍物时查找对应控制集，有突发障碍物快速求解新路径，使移动机器人能够更快更安全的从起始点到达目标点。
附图说明
[0040]
图1为本发明一种可实施方法的流程图。
[0041]
图2为本发明一种可实施方法的标准控制集。
[0042]
图3为本发明一种可实施方法的标准控制集的应用。
[0043]
图4为本发明一种可实施方法的过程。
具体实施方式
[0044]
下面对本发明实施例中的技术方案做进一步的详细说明：
[0045]
本发明提供一种与邻域搜索原则相结合的移动机器人标准控制集路径规划方法的流程如图1所示，具体如下：
[0046]
步骤1：确定移动机器人运动学模型；确定移动机器人的路径约束；通过邻域搜索原则确定标准控制集的初末状态；确定目标函数后生成最优控制问题；离散化后采用内点法求解该最优控制问题进而生成标准控制集；
[0047]
步骤2：邻域搜索原则生成全局路径点；根据相邻两个路径点查询标准控制集得到一组路径；
[0048]
步骤3：若该段路径无新增障碍物，则移动机器人直接执行该路径；若该段路径新增障碍物，则以查询的该路径为初始解，加入无碰撞限制后重新求解最优控制问题得出新路径交给移动机器人执行；构建出移动机器人完整路径。
[0049]
标准控制集的建立，包括以下步骤：1)最优控制问题的生成，2)离散求解该最优控制问题，3)组成标准控制集；
[0050]
1)最优控制问题的生成：
[0051]
完整移动机器人路径规划最优控制问题包括移动机器人运动学模型、路径约束、初末状态约束以及一个目标函数；
[0052]
移动机器人模型的微分方程为：
[0053][0054]
其中，t为时间；整体的路径规划为t0到tf时刻运动过程；(x,y)为移动机器人当前时刻的位置；θ为方向角；为当前点的速度；a为相应的加速度；ω为当前点的角速度；α相应的角加速度，选择a(t)和α(t)作为控制量，x(t)、y(t)、θ(t)、和ω(t)作为状态变量。
[0055]
作为示例，差动式轮式移动机器人模型的微分方程表示为：
[0056][0057]
其中，t为时间；整体的路径规划为t0到tf时刻运动过程；
[0058]
路径约束：
[0059][0060]
针对移动机器人的性能以及物理特性，必须考虑到ω(t)、a(t)和α(t)都有一定的上下限。其中，为了避免轮胎的不良磨损，使移动机器人的路径连续化，可加入和ω(t)连续并可导，α(t)和α(t)连续这样的约束条件。
[0061]
初末状态约束：
[0062]
通过邻域搜索原则确定标准控制集的每组初末状态：
[0063][0064]
目标函数：
[0065]
选择一条兼顾平滑且时间最优的一条路径作为标准控制集在该状态下的路径。其中平滑度的定义为速度以及角速度的二范数，记为j1；关于时间的目标函数为t
f-t0，记为j2；因此，提出基于平滑和时间最优的目标函数j：
[0066][0067]
整体表述：
[0068]
将移动机器人路径规划最优控制问题进行整体表述：
[0069][0070][0071]
2)离散求解该最优控制问题：
[0072]
该步骤包括两个阶段离散化阶段和内点法求解阶段。将上述最优化问题离散为非线性规划(nonlinear programming，nlp)问题，进而采用内点法求得数值最优解。
[0073]
3)组成标准控制集：
[0074]
采用启发式的邻域搜索原则生成起点状态到终点状态之间的全局路径点，采用8邻域搜索方案，则给定邻域搜索原则中搜索的方向个数8，则标准控制集中路径个数为64。
[0075]
作为示例，保证移动机器人只能前进以及原地转圈，不能后退，则标准控制集中路径个数更新为40个，通过求解上述最优控制问题得出每组初末状态下的路径，组成一个8邻域的标准控制集，如图2所示，图2中虚线表示初始角度为0的一组控制集，根据实际机器人运动状态，剪枝方向，根据将八邻域搜索原则得到终末角度的5个方向，其他角度分别进行求解，得到40组控制集，如实线所示。
[0076]
局部路径规划中，通过邻域搜索原则生成起点状态到终点状态之间的全局路径点，相邻两点组成一个路径规划段，取每段中点状态作为标准控制集的每组初末状态，查询相邻路径规划段的角度即可进行规划，如图3所示，图3中路径规划段用虚线表示，标准控制集用点线表示，在此段的局部路径规划中，取两段中点状态作为标准控制集的每组初末状态，查询相邻路径规划段的角度，得到一组控制集，如图实线表示，进行规划。
[0077]
参考图4，无突发障碍物则一步步进行规划至终点，出现突发障碍物，对突发障碍物处理的具体实施过程为：
[0078]
先将障碍物进行膨胀，之后选取膨胀后的障碍物的边界的n个点，计算其与移动机器人中心的距离，同时考虑速度对无碰撞限制的影响。
[0079]
惩罚函数的基本思路是通过障碍物点与目标点的距离偏差来调节函数值的大小，
且距离越近，函数值越大。综合移动机器人速度与目标函数中惩罚函数的比重对避让障碍物的影响，移动机器人无碰撞限制的避障功能函数为：
[0080][0081]
其中，s
obs
为权重系数，υi＝υ
x2
+υ
y2
，(xi，yi)是障碍物点在机器人坐标系下的位置坐标， (x0，y0)是机器人质心坐标，ζ为较小的正数，用于防止出现分母为0的现象。
[0082]
将该过程带入最优控制问题，并将该段路径对应的标准控制集中的路径作为初始解重新求解，进而得出这一段的路径，之后采用上述相同的方式规划至终点。
[0083]
参考图4，首先得到地图模型，之后进行全局路径点的生成，再采用本专利方法进行局部路径规划。在规划过程中，如无突发障碍物，则查询相邻路径规划段的角度得到标准控制集，用该段控制集中的路径进行规划，直至终点得到全局路径；如出现突发障碍物，则将该段路径对应的标准控制集中的路径作为初始解，重新求解最优控制问题，进而得出这一段的路径，之后采用上述相同的方式规划至终点。
[0084]
在基于本发明方法构思的基础上，本发明还提供一种移动机器人标准控制集局部路径规划系统，其特征在于，包括标准控制集生成模块、路径获取模块以及路径计算模块；
[0085]
标准控制集生成模块根据移动机器人运动学模型和移动机器人的路径约束；通过邻域搜索原则确定标准控制集的初末状态；确定基于平滑和时间最优的目标函数后生成最优控制问题；对所述最优控制问题进行离散化，采用内点法求解该最优控制问题进而生成标准控制集；
[0086]
路径获取模块用于根据邻域搜索原则生成全局路径点，根据相邻两个路径点查询标准控制集得到一组路径；
[0087]
路径计算模块用于根据所述路径构建移动机器人完整路径，具体为：若该段路径无新增障碍物，则移动机器人直接执行该路径；若该段路径新增障碍物，则以查询的该路径为初始解，加入无碰撞限制后重新求解最优控制问题得出新路径交给移动机器人执行；构建出移动机器人完整路径。
[0088]
还提供一种计算机设备，包括处理器以及存储器，存储器用于存储计算机可执行程序，处理器从存储器中读取所述计算机可执行程序并执行，处理器执行计算可执行程序时能实现本发明所述移动机器人标准控制集局部路径规划方法。
[0089]
也可以提供一种计算机可读存储介质，计算机可读存储介质中存储有计算机程序，所述计算机程序被处理器执行时，能实现本发明所述的移动机器人标准控制集局部路径规划方法。
[0090]
所述计算机设备可以采用笔记本电脑、桌面型计算机或工作站。
[0091]
处理器可以是中央处理器(cpu)、数字信号处理器(dsp)、专用集成电路(asic)或现成可编程门阵列(fpga)。
[0092]
对于本发明所述存储器，可以是笔记本电脑、桌面型计算机或工作站的内部存储单元，如内存、硬盘；也可以采用外部存储单元，如移动硬盘、闪存卡。
[0093]
计算机可读存储介质可以包括计算机存储介质和通信介质。计算机存储介质包括以用于存储诸如计算机可读指令、数据结构、程序模块或其他数据等信息的任何方法或技术实现的易失性和非易失性、可移动和不可移动介质。计算机可读存储介质可以包括：只读
存储器(rom， read only memory)、随机存取记忆体(ram，random access memory)、固态硬盘(ssd，solidstate drives)或光盘等。其中，随机存取记忆体可以包括电阻式随机存取记忆体 (reram，resistance random access memory)和动态随机存取存储器(dram，dynamic randomaccess memory)。
[0094]
以上内容是对本发明的详细说明，不能认定本发明的仅限于此，对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单的推演或替换，都应当视为属于本发明由所提交的权利要求书确定专利保护范围。