本发明涉及一种室内移动机器人的路径规划算法，属于移动机器人导航领域。
背景技术：
在当下机器人大力发展的时代，移动机器人现已可以在许多场合下代替人力完成各项任务，是一种高度自规划、自组织、自适应能力的机械，可在各种复杂、危险环境中工作，目前被广泛应用于物流、探测、服务等领域。路径规划综合各种运动约束条件，进行全局和局部的规划，使得机器人可以安全高效的移动行走，是移动机器人的核心技术，也是机器人领域内的热点研究问题。
技术实现要素：
本发明要解决的技术问题是：对室内机器人移动路径进行规划，获取机器人速度连续、加速度连续的轨迹。
为了解决上述问题，本发明的技术方案是提供了一种室内移动机器人的路径规划算法，其特征在于，包括如下步骤：
步骤一、根据已知地图中障碍物和非障碍物的位置信息以及机器人的长宽尺寸建立障碍物膨胀尺寸较大的全局规划地图；
步骤二、利用经典a*算法进行全局路径规划，根据当前位置和目标位置，规划出全局路径；
步骤三、提取出机器人当前位置附近区域中全局路径序号最接近目标点序号的全局路径点，将此点作为全局路径的动态关键点local_goal；
步骤四、根据已知地图中障碍物和非障碍物的位置信息、机器人的长宽尺寸和全局规划地图的膨胀参数，建立障碍物膨胀尺寸较小的局部规划地图；
步骤五、建立室内移动机器人运动模型；
步骤六、根据室内移动机器人运动模型和运动参数约束，计算出可行的加速度矢量空间ar；
步骤七、根据步骤五中得到的加速度矢量空间ar和运动模型及运动参数约束，计算出可行的速度采样空间vr；
步骤八、将速度采样空间vr按采样时间间隔dt进行速度采样，进而将采样速度代入移动机器人运动模型中，进行时间t内的仿真模拟，然后将模拟结果带入到改进评价函数fn进行计算，选出到动态关键点local_goal的最优的速度轨迹。
优选地，所述步骤六中的运动参数包括室内移动机器人的加加速度矢量和加速度矢量，所述步骤七中的运动参数包括室内移动机器人的加速度矢量和速度矢量。
优选地，所述步骤五中移动机器人运动模型：
x(t 1)＝xt v*dt*cosθt
y(t 1)＝yt v*dt*sinθt
θ(t 1)＝θt dt*ω
其中，[xt,yt,θt]为当前时刻的机器人位姿信息，[xt 1,yt 1,θt 1]为下一时刻的机器人位姿，dt为采样时间，v和ω分别为机器人的线速度值和角速度值。优选地，所述步骤六中可行的加速度矢量空间ar表示搜索的加速度矢量空间，
ar＝as∩ad，
其中as表示参数设置中可达到的加速度矢量集合；ad表示符合加加速度要求的全部加速度矢量集合；
as＝{(av,aw)|av∈[0,avmax],aw∈[-awmax,awmax]}；
ad＝{(av,aw)|av∈[av0-aαv·dt,av0 aαv·dt],aw∈[aw0-aαv·dt,aw0 aαv·dt]}；
其中，avmax为机器人最大可达到的平移加速度；awmax为最大旋转加速度；aαv和aαw分别为最大平移加加速度和最大旋转加加速度；av0和aw0分别为当前时刻的加平移速度和旋转加速度；dt为采样周期间隔。
优选地，所述步骤七中可行的速度采样空间vr表示搜索的速度矢量空间，
vr＝vs∩va∩vd；
其中，vs表示可达到的速度矢量集合；va表示可安全避开障碍物的速度矢量集合；vd表示符合加速度要求的全部速度矢量集合；
vs＝{(v,w)|v∈[0,vmax],w∈[-wmax,wmax]}；
vd＝{(v,w)|v∈[v0-αv·dt,v0 αv·dt],w∈[w0-αv·dt,w0 αv·dt]}；
其中，vmax为机器人最大可达到的平移速度；wmax为最大旋转速度；dist(v,w)表示机器人运动模型以运动学参数v和w模拟出的轨迹位置与障碍物之间的最小距离；αv和αw分别为步骤六中计算出的ar里的最大平移加速度和最大旋转加速度；v0和w0分别为当前时刻的平移速度和旋转速度；dt为采样周期间隔。
优选地，所述步骤七中所述评价函数fn：
fn＝a*nor(heading) b*nor(obstacledist) c*nor(vel) d*nor(oscillation)
其中，a、b、c、d为权重参数，nor代表正则化；heading代表当前位置动态关键点local_goal连线角度与当前位姿角度的差值角δθ的指标，obstacledist代表当前最近障碍物点与local_goal之间的距离δl的指标，ta、tb为可变参数，c为机器人半径，vel代表速度v的指标，maxv是机器人最大线速度。
优选地，所述步骤一中膨胀尺寸为机器人半径值，所述步骤四中膨胀尺寸为小于机器人半径的常数值的值。
优选地，所述步骤三将以移动机器人当前位置为中心，移动机器人半径加上1m为半径的圆作为当前位置附近区域。
与现有技术相比，本发明的有益效果是：
本发明通过a*算法获得全局路径，在此基础上利用改进的dwa算法，根据室内移动机器人的运动约束规划出可行的加速度采样空间，选出使评价函数总分最高的加速度并作用于速度采样空间，根据采样速度模拟轨迹并根据计算各条轨迹的改进评价函数，选出到上述全局路径动态关键点的最优轨迹。获得加速度连续的轨迹，规划出了速度连续、加速度连续并且不易陷入局部最优问题的轨迹。
附图说明
图1为本发明一种室内移动机器人的路径规划算法流程图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1所示，本发明的具体实施包括以下步骤：
步骤一、根据已知地图信息中障碍物和非障碍物的位置信息、机器人尺寸大小(长：0.8m；宽：0.6m)，建立分辨率为0.02，障碍物膨胀比例为42：1的全局规划地图。
步骤二、利用a*算法进行全局路径规划，根据当前位置和目标位置，规划出全局路径。
步骤三、将以当前位置为中心,机器人半径加上1m为半径的圆作为当前位置附近区域，提取出当前位置附近区域中全局路径序号最接近目标点序号的全局路径点，将此点作为全局路径的动态关键点local_goal。
步骤四、根据已知地图中障碍物和非障碍物的位置信息、机器人长宽尺寸大小和全局规划地图的膨胀参数，建立膨胀比例为40：1的局部规划地图。
步骤五、建立室内移动机器人运动模型：
x(t 1)＝xt v*dt*cosθt
y(t 1)＝yt v*dt*sinθt
θ(t 1)＝θt dt*ω
其中，[xt,yt,θt]为当前时刻的机器人位姿信息，[xt 1,yt 1,θt 1]为下一时刻的机器人位姿，dt为采样时间，v和ω分别为机器人的线速度值和角速度值。
步骤六、根据室内移动机器人运动模型和运动参数(包括加加速度矢量、加速度矢量)约束，算出可行的加速度矢量空间ar。
ar表示搜索的加速度矢量空间，as表示参数设置中可达到的加速度矢量集合；ad表示符合加加速度要求的全部加速度矢量集合。则ar表示为：
ar＝as∩ad
as＝{(av,aw)|av∈[0,avmax],aw∈[-awmax,awmax]}
ad＝{(av,aw)|av∈[av0-aαv·dt,av0 aαv·dt],aw∈[aw0-aαv·dt,aw0 aαv·dt]}
其中，avmax为机器人最大可达到的平移加速度1.5m/s；awmax为最大旋转速度280°/s；aαv和aαw分别为最大平移加加速度0.4m/s2和最大旋转加加速度45°/s2；av0和aw0分别为当前时刻的加平移速度和旋转加速度；dt为采样周期间隔0.3s。
步骤七，根据步骤六中得到的加速度矢量空间ar和运动模型及运动参数(包括加速度矢量、速度矢量)约束，计算出可行的速度采样空间vr。
vr表示搜索的速度矢量空间，vs表示可达到的速度矢量集合；va表示可安全避开障碍物的速度矢量集合；vd表示符合加速度要求的全部速度矢量集合。则vr表示为：
vr＝vs∩va∩vd
vs＝{(v,w)|v∈[0,vmax],w∈[-wmax,wmax]}
vd＝{(v,w)|v∈[v0-αv·dt,v0 αv·dt],w∈[w0-αv·dt,w0 αv·dt]}
其中，vmax为机器人最大可达到的平移速度1.5m/s；wmax为最大旋转速度280°/s；dist(v,w)表示机器人运动模型以运动学参数v和w模拟出的轨迹位置与障碍物之间的最小距离；αv和αw分别为步骤六中计算出的ar里的最大平移加速度和最大旋转加速度；v0和w0分别为当前时刻的平移速度和旋转速度；dt为采样周期间隔0.3s。
步骤八、将速度采样空间vr按采样时间间隔dt进行速度采样，进而将采样速度代入移动机器人运动模型中，进行时间t内的仿真模拟，然后将模拟结果带入到改进评价函数fn进行计算，选出到动态关键点local_goal的最优的速度轨迹。
评价函数fn：
fn＝a*nor(heading) b*nor(dist) c*nor(vel) d*nor(oscillation)
fn＝a*nor(heading) b*nor(obstacledist) c*nor(vel) d*nor(oscillation)
其中，a、b、c、d为权重参数，nor代表正则化；heading代表当前位置动态关键点local_goal连线角度与当前位姿角度的差值角δθ的指标，obstacledist代表当前最近障碍物点与local_goal之间的距离δl的指标，ta、tb为可变参数，在本例中分别取为1.43、3；c为机器人半径，本例取0.5m；vel代表速度v的指标，maxv是机器人最大线速度。
oscillation是本发明评价函数里的改进指标项，即横向摆动代价，若当前点到local_goal的变化趋势与下一时刻点到local_goal的变化趋势一致，则此参数为0，若不一致则此参数为负数。此项评分函数的加入使得改进dwa算法可以很大程度的减少轨迹形成环路的概率，进而降低了陷入最小局部问题的概率。
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
本说明书中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。
1.一种室内移动机器人的路径规划算法，其特征在于，包括如下步骤：
步骤一、根据已知地图中障碍物和非障碍物的位置信息以及机器人的长宽尺寸建立障碍物膨胀尺寸较大的全局规划地图；
步骤二、利用经典a*算法进行全局路径规划，根据当前位置和目标位置，规划出全局路径；
步骤三、提取出机器人当前位置附近区域中全局路径序号最接近目标点序号的全局路径点，将此点作为全局路径的动态关键点local_goal；
步骤四、根据已知地图中障碍物和非障碍物的位置信息、机器人的长宽尺寸和全局规划地图的膨胀参数，建立障碍物膨胀尺寸较小的局部规划地图；
步骤五、建立室内移动机器人运动模型；
步骤六、根据室内移动机器人运动模型和运动参数约束，计算出可行的加速度矢量空间ar；
步骤七、根据步骤五中得到的加速度矢量空间ar和运动模型及运动参数约束，计算出可行的速度采样空间vr；
步骤八、将速度采样空间vr按采样时间间隔dt进行速度采样，进而将采样速度代入移动机器人运动模型中，进行时间t内的仿真模拟，然后将模拟结果带入到改进评价函数fn进行计算，选出到动态关键点local_goal的最优的速度轨迹。
2.如权利要求1所述的一种室内移动机器人的路径规划算法，其特征在于：所述步骤六中的运动参数包括室内移动机器人的加加速度矢量和加速度矢量，所述步骤七中的运动参数包括室内移动机器人的加速度矢量和速度矢量。
3.如权利要求1所述的一种室内移动机器人的路径规划算法，其特征在于：所述步骤五中移动机器人运动模型：
x(t 1)＝xt v*dt*cosθt
y(t 1)＝yt v*dt*sinθt
θ(t 1)＝θt dt*ω
其中，[xt,yt,θt]为当前时刻的机器人位姿信息，[xt 1,yt 1,θt 1]为下一时刻的机器人位姿，dt为采样时间，v和ω分别为机器人的线速度值和角速度值。
4.如权利要求1所述的一种室内移动机器人的路径规划算法，其特征在于：所述步骤六中可行的加速度矢量空间ar表示搜索的加速度矢量空间，
ar＝as∩ad，
其中as表示参数设置中可达到的加速度矢量集合；ad表示符合加加速度要求的全部加速度矢量集合；
as＝{(av,aw)|av∈[0,avmax],aw∈[-awmax,awmax]}；
ad＝{(av,aw)|av∈[av0-aαv·dt,av0 aαv·dt],aw∈[aw0-aαv·dt,aw0 aαv·dt]}；
其中，avmax为机器人最大可达到的平移加速度；awmax为最大旋转加速度；aαv和aαw分别为最大平移加加速度和最大旋转加加速度；av0和aw0分别为当前时刻的加平移速度和旋转加速度；dt为采样周期间隔。
5.如权利要求1所述的一种室内移动机器人的路径规划算法，其特征在于：所述步骤七中可行的速度采样空间vr表示搜索的速度矢量空间，
vr＝vs∩va∩vd；
其中，vs表示可达到的速度矢量集合；va表示可安全避开障碍物的速度矢量集合；vd表示符合加速度要求的全部速度矢量集合；
vs＝{(v,w)|v∈[0,vmax],w∈[-wmax,wmax]}；
vd＝{(v,w)|v∈[v0-αv·dt,v0 αv·dt],w∈[w0-αv·dt,w0 αv·dt]}；
其中，vmax为机器人最大可达到的平移速度；wmax为最大旋转速度；dist(v,w)表示机器人运动模型以运动学参数v和w模拟出的轨迹位置与障碍物之间的最小距离；αv和αw分别为步骤六中计算出的ar里的最大平移加速度和最大旋转加速度；v0和w0分别为当前时刻的平移速度和旋转速度；dt为采样周期间隔。
6.如权利要求1所述的一种室内移动机器人的路径规划算法，其特征在于：所述步骤七中所述评价函数fn：
fn＝a*nor(heading) b*nor(obstacledist) c*nor(vel) d*nor(oscillation)
其中，a、b、c、d为权重参数，nor代表正则化；heading代表当前位置动态关键点local_goal连线角度与当前位姿角度的差值角δθ的指标，obstacledist代表当前最近障碍物点与local_goal之间的距离δl的指标，ta、tb为可变参数，c为机器人半径，vel代表速度v的指标，maxv是机器人最大线速度。
7.如权利要求1所述的一种室内移动机器人的路径规划算法，其特征在于：所述步骤一中膨胀尺寸为机器人半径值，所述步骤四中膨胀尺寸为小于机器人半径的常数值的值。
8.如权利要求1所述的一种室内移动机器人的路径规划算法，其特征在于：所述步骤三将以移动机器人当前位置为中心，移动机器人半径加上1m为半径的圆作为当前位置附近区域。
技术总结