基于时空图决策的非结构化环境下机器人运动规划方法

1.本发明属于移动机器人运动规划领域，具体涉及一种基于时空图决策的非结构化环境下机器人运动规划方法。


背景技术：

2.运动规划技术在自主导航技术中具有承上启下的作用，上接环境地图信息，下接跟踪控制的实现，是自主导航功能实现的核心技术之一。然而，目前运动规划技术常采用图搜索方法、采样方法等，通过将移动机器人简化为一个质点，然后规划出一条从起点到终点的路径。现有方法虽然可以寻找到一条几何路径，但对动态环境下交通参与者的运动影响考虑静态障碍物。除此以外，现有方法并没有充分考虑移动机器人所处的地形环境对运动平稳性的影响，使得机器人在非结构化环境的自主导航过程中安全性和平稳性都难以保证。因此，设计一种在动态环境中安全高效的机器人运动规划方法与流程对于移动机器人在野外的自主导航实现至关重要。


技术实现要素：

3.有鉴于此，本发明提出了一种基于时空图决策的非结构化环境下机器人运动规划方法，该方法包括：
4.基于静态地图信息、机器人定位信息进行轨迹规划，获得基于初始路径轨迹的初始化占据栅格图；
5.根据动态环境信息与所述初始路径轨迹构建时空图；所述时空图为以初始路径轨迹、时间为参数构建的以其他交通参与者为占据点的占据栅格图；
6.基于所述时空图，根据其他交通参与者对移动机器人的初始路径轨迹在时空上的影响，对移动机器人的自身行为进行决策；根据决策结果执行相应的轨迹重规划与优化并输出轨迹。
7.特别地，初始路径规划时，根据环境感知信息，机器人定位信息，构建所述初始化占据栅格地图；其中根据感知的不同地形结果和障碍物结果，设置所述初始化占据栅格图中不同节点的转移代价值，引导机器人在平稳地形下运动，基于图搜索算法，进行路径搜索，获得安全的初始路径。
8.特别地，所述占据点的计算方式如下：感知获得其他交通参与者的速度信息(v
x
,vy),其中，v
x
,vy分别为所述其他交通参与者在两个坐标轴上的速度；按直线匀速运动模型处理其他交通参与者，得到预测轨迹直线方程ax+by+c＝0；搜索移动机器人初始轨迹中距离预测轨迹直线方程小于安全阈值th
safe
的位姿点簇{(x1,y2),(x2,y2),
…
,(xm,ym)}，其中m为正整数；计算初始路径起点到以上各位姿点的距离簇{s1,s2,
…
,sm},计算其他交通参与者运动状态影响初始路径位姿点的时间簇{t1,t2,
…
,tm},至此得到占据点簇 occupyj＝{(s1,t1),(s2,t2),
…
,(sm,tm),}，其中下标j表示影响初始路径参考点的交通参与者下标。
9.特别地，所述决策结果适用一个交通参与者或者多个交通参与者的复杂动态环
境，包括巡航、超车、避让或快速通过，具体包括如下步骤：
10.取时空图下移动机器人的时空运动边界s
end
和t
end
，空间运动边界s
end
与初始路径的长度数值一致，时间运动边界t
end
是该运动过程的时间上界，首先在时空占据栅格图下速度分配，然后进行以下判断：
11.若时空图在width＝[0:s
end
],height＝[0,t
end
]的占据栅格内都没有受交通参与者运动影响的占据occupyj，此时，所述决策结果为巡航，移动机器人则开始巡航，巡航保持初始路径下的位姿；
[0012]
若时空图在占据栅格的时空边界内存在占据点，且对应的占据点 (si,ti)∈occupyj出现在时空占据栅格曲线左侧；并且满足超车条件:max(s
i in occupyj)＝s
end
和t
end-max(t
i in occupyj)《t
th
，则进行对 occupyj所表示的交通参与者的超车，其中，t
th
表示抵达局部目标点的目标时间， max (s
i in occupyj)、max(t
i in occupyj)分别表示交通参与者occupyj最大的空间占据和最大的时间占据；
[0013]
若占据点仅出现在时空占据栅格曲线左侧，且不满足所述超车条件，则所述决策结果为避让；
[0014]
若时空图在占据栅格的时空边界内存在占据点，且对应的占据点 (si,ti)∈occupyj出现在时空占据栅格曲线在右侧，则所述决策结果为快速通过该交通参与者。
[0015]
特别地，若所述决策结果是巡航、避让或快速通过，则启动速度分配模块，在时空图下优化速度和加速度；
[0016]
若所述决策结果是超车，则启动轨迹再规划模块和速度分配模块，所述启动轨迹再规划模块将规划两条超车轨迹，所述速度分配模块将在初始路径轨迹的基础上分配速度；得到的三条轨迹将在轨迹选择与输出模块进行综合评判打分，选择最优的轨迹作为机器人局部轨迹。
[0017]
特别地，若所述决策结果是超车，所述速度分配模块根据不同地形地面对速度的影响增加二次规划线性约束到所述分配速度。
[0018]
特别地，若所述决策结果是超车，该单元轨迹重规划模块依次执行生成参考序列的步骤及超图模型构建和求解的步骤；参考序列的确定按以下方式：生成超车决策并将相应的交通参与者作为超车目标，获得超车目标时刻t＝k下在水平面投影中心点与轮廓点，在每个时刻下取所有轮廓点与中心点o
center
距离的最大值为超车目标最大半径r
obj
，在此基础上乘以安全系数ec，获得安全半径rc＝ec*r
obj
,在以超车目标中心点o
center
为圆心，以rc为半径的圆周上计算经过机器人坐标的两条切线，将切点和作为参考点；按该方式获得多个时刻下的参考点，将位于超车目标同一侧的切点集合作为参考序列，共得到两条参考序列；进行两次相互独立的超图模型优化，将独立的优化轨迹与初始路径轨迹一同评判，评价最高的作为最优轨迹输出。
[0019]
特别地，所述超图模型优化包括超图优化模型的目标函数增加参考点代价函数，位置变量的初值在参考序列、起点、终点之间通过线性采样确定，选择初值位于参考点邻域1m内的位置变量xi＝{(xi,yi)|(xi,yi)∈neibor(tp)}，对于每一个满足条件的位置变量，计算位置变量与参考序列的最小值d
min
以及最小安全时间t
tp,min
，参考序列约束以惩罚函数的方式加入目标函数中：
[0020][0021]
其中r
pmax
是参考序列目标的安全半径，∈与n、s影响近似精度；
[0022]
障碍物约束通过以下函数加入目标函数：
[0023][0024]
特别地，将独立的优化轨迹与初始路径轨迹一同评判，评价最高的作为最优轨迹输出包括：对轨迹再规划模块输出轨迹进行安全碰撞检测，若两条超车轨迹均不满足安全碰撞检测，则选择速度分配模块生成的耗时较长的初始轨迹；若至少有一条超车轨迹满足安全碰撞检测，则计算满足要求的超车轨迹和初始轨迹的代价函数，代价函数由加速度代价、加加速度代价、曲率代价、时间代价组成；所述安全检测包括：对于新的再规划超车轨迹，构建该轨迹下包含其他交通参与者的时空占据栅格图，将该轨迹下时刻与路程对应的坐标点在图上描绘，若存在坐标点落在不可通行的占据栅格上，那么不满足要求，反之该轨迹是安全的。
[0025]
有益效果：
[0026]
(1)本发明提供了基于时空图决策的非结构化环境下机器人运动规划方法与流程，基于已有环境地图信息、机器人定位信息进行轨迹规划，一方面，该方法提供了动态非结构化环境中机器人运动规划的整体框架流程，部分子模块实现方式灵活多变，可兼容众多算法，可适应不同平台，方便调试与改进。
[0027]
(2)初始路径规划考虑不同地形对规划路径的影响，通过设置不同的节点转移代价，引导机器人在平稳地形下运动，大大提高了机器人运动的舒适度和安全性。
[0028]
(3)本发明中利用匀速运动模型充分考虑其他交通参与者的运动状态，并在机器人本体的轨迹规划结果中得以体现，大大降低动态非结构化环境中机器人与其他动态物体的碰撞风险。
[0029]
(4)本发明中基于时空图得到的多种决策结果适用于多个交通参与者的复杂动态环境，决策结果包括巡航、超车、避让或快速通过等多种形态，提高了机器人的智能规划能力。
[0030]
(5)本发明中决策为超车后，会规划三条轨迹将在轨迹选择与输出模块进行综合评判打分，兼顾了超车策略的路径和原有的路径作为备选；将独立的优化轨迹与初始路径轨迹一同评判，评价最高的作为最优轨迹输出；并且进行两次相互独立的超图模型优化，提高了超车策略的安全可靠性。
[0031]
(6)本发明中决策后速度分配模块还会考虑颠簸地形和突变地形下对机器人运动平稳性的影响，通过特殊处理目标函数和速度约束实现该类地形下轨迹点速度与加速度的优化，提高了运动的舒适度和减少了机器人磨损。
[0032]
(7)本发明中轨迹再规划模块通过设置超车对象时空参考序列，并通过碰撞检测保证轨迹的安全性，通过速度分配模块保证规划系统的冗余度，实现动态非结构化环境中机器人安全、自主的“超车”行为。
附图说明
[0033]
图1为本发明中轨迹再规划参考序列示意图；
[0034]
图2为本发明中基于时空图决策的机器人运动规划流程示意图。
具体实施方式
[0035]
以下结合附图对本发明的具体实施方式作出详细说明。
[0036]
下面结合附图并举实施例，对本发明进行详细描述。
[0037]
本发明提出了一种基于时空图决策的机器人运动规划方法，包括：
[0038]
步骤1：基于静态地图信息、机器人定位信息进行轨迹规划，获得基于初始路径轨迹的初始化占据栅格图；
[0039]
初始路径规划过程中，根据环境感知信息，机器人定位信息，构建占据栅格地图，根据感知的不同地形结果和障碍物结果，设置占据栅格图中不同节点的转移代价值，引导机器人在平稳地形下运动，基于图搜索的方法，如a*、hybrida*等，进行路径搜索，获得安全无碰撞、符合车辆运动学的初始路径。
[0040]
步骤2：机器人运动决策。在生成初始路径和获得动态交通参与者运动状态的基础上，构建时空图并进行机器人运动决策，完成机器人下一时刻运动的行为决策。
[0041]
步骤2.1、构建时空图。
[0042]
所述时空图中占据点(si,ti)物理含义是对于移动机器人本体，在ti时刻路径长度为si的位置是不安全的，该时空坐标下本体将与其他交通参与者发生碰撞，占据点(si,ti)的计算方式如下：
[0043]
感知获得其他交通参与者的速度信息(v
x
,vy),其中，v
x
,vy分别为所述其他交通参与者在两个坐标轴上的速度；按直线匀速运动模型处理其他交通参与者，得到预测轨迹直线方程ax+by+c＝0；搜索移动机器人初始轨迹中距离预测轨迹直线方程小于安全阈值th
safe
的位姿点簇{(x1,y2),(x2,y2),
…
,(xm,ym)}，计算初始路径起点到以上各位姿点的距离簇{s1,s2,
…
,sm},计算其他交通参与者运动状态影响初始路径位姿点的时间簇{t1,t2,
…
,tm},至此得到占据点簇occupyj＝ {(s1,t1),(s2,t2),
…
,(sm,tm),}，其中下标j表示影响初始路径参考点的交通参与者下标。
[0044]
步骤2.2、时空图决策。
[0045]
时空图包含移动机器人规划的速度与加速度信息，时空图决策是根据其他交通参与者对移动机器人初始轨迹在时空上的影响，对移动机器人自身行为进行决策，决策结果包括巡航，超车，避让，快速通过。其中，巡航指移动机器人在初始路径在时空上不受其他交通参与者影响时跟踪初始轨迹的位姿 (xk,yk,θk)运动；超车指移动机器人初始路径被其他交通参与者影响程度较为严重，无法在规定时间内抵达局部目标点时重新规划安全轨迹实现对交通参与者的超车行为；避让指移动机器人以较低速度的运动等待交通参与者对初始轨迹在时空上的影响消失，在初始轨迹受影响的时刻过去后再到达受影响的空间位置；快速通过指移动机器人以较大速度运动，在交通参与者对初始轨迹的影响时刻到来前离开受影响的空间位置。
[0046]
决策按以下方式：取时空图下移动机器人的时空运动边界s
end
和t
end
，前者与初始路径的长度数值一致，后者是该运动过程的时间上界，首先在时空占据栅格图下进行图搜
索，其中图搜索过程具有其特殊性，体现在两个方面：
①
节点拓展方向，
②
时空图下末端时刻可变。前者要求节点只能向右侧和右上方拓展，后者处于时间最优性考虑，启发函数设置为终止条件设置为nodes＝s
end
；然后进行以下判断：
[0047]
(1)若时空图在width＝[0:s
end
],height＝[0,t
end
]的占据栅格内都没有受交通参与者运动影响的占据occupyj，移动机器人则开始巡航，巡航保持初始路径下的位姿；
[0048]
(2)若时空图在占据栅格的时空边界内存在占据点，且对应的占据点 (si,ti)∈occupyj出现在时空占据栅格曲线左侧并且满足max(s
i in occupyj)＝ s
end
和t
end-max(t
i in occupyj)《t
th
，则进行对occupyj所表示的交通参与者的超车；
[0049]
(3)若仅仅是左侧出现占据点而不满足(2)的判断条件则进行避让；
[0050]
(4)若时空图在占据栅格的时空边界内存在占据点，且对应的占据点 (si,ti)∈occupyj出现在时空占据栅格曲线在右侧，则快速通过该交通参与者，以上判断方式对多个交通参与者的复杂动态环境仍适用；
[0051]
步骤3、轨迹重规划与优化，若决策结果是巡航、避让或快速通过，执行 3.1步骤；若决策结果是超车，由于初始轨迹被其他交通参与者影响程度大，继续使用初始轨迹导致安全性差和耗时长，因此该决策启动速度分配模块和轨迹再规划模块，前者将在初始路径的基础上分配较低的速度以实现安全驾驶，后者将规划两条超车轨迹，超车决策执行3.1和3.2步骤。
[0052]
步骤3.1、原始路径速度分配
[0053]
该步骤在图2中速度分配模块进行，旨在原始轨迹上分配速度，以实现在其他交通参与者影响初始轨迹的时刻到来前离开受影响区域或在时刻经过后到达受影响区域。初始速度分配通过图搜索方法，如步骤2.2所示，搜索出初始状态确定，末端位置确定，末端时间松弛的s-t轨迹，该轨迹可体现机器人在s 方向的速度、加速度、加加速度信息。对该s-t轨迹进行二次规划，使机器人速度、加速度平滑且满足约束。
[0054]
二次规划目标函数增加考虑地形突变对舒适度影响的代价，假设二次规划命题中优化变量为根据结合规划路径和地形信息，一般在地形变化处如水平地面与坡面衔接处速度和加速度若保持较大值，则影响平稳性和舒适度，在地形变化处相邻的变量在目标函数中增加以下约束:
[0055][0056]
速度分配的二次优化线性约束增加考虑颠簸地面对速度的约束，假设位于颠簸地面的待优化点集为添加以下约束：
[0057][0058]
步骤3.2、超车轨迹重规划
[0059]
超车轨迹重规划环节在图2中轨迹重规划模块进行。该环节内容包括参考序列的确定和超图模型构建。前者用于提供超图模型中参考序列代价函数，以实现对动态目标的
安全“超车”，参考序列示意图如图1所示，按以下方式生成：生成超车决策并将相应的交通参与者作为超车目标，获得超车目标时刻t＝k下在水平面投影中心点与轮廓点，在每个时刻下取所有轮廓点与中心点o
center
距离的最大值为超车目标最大半径r
obj
，在此基础上乘以安全系数ec，获得安全半径 rc＝ec*r
obj
,在以超车目标中心点o
center
为圆心，以rc为半径的圆周上计算经过机器人坐标的两条切线，将切点和作为参考点。按该方式获得多个时刻下的参考点，将位于超车目标同一侧的切点集合作为参考序列，共得到两条参考序列，进行两次相互独立的超图模型优化，将独立的优化轨迹与初始轨迹一同评判，评价最高的作为最优轨迹输出。
[0060]
超图优化模型的目标函数增加参考点代价函数，位置变量的初值在参考序列，起点、终点之间通过线性采样确定，选择初值位于参考点邻域1m内的位置变量 xi＝{(xi,yi)|(xi,yi)∈neibor(tp)}，对于每一个满足条件的位置变量，计算位置变量与参考序列的最小值d
min
以及最小安全时间t
tp,min
，参考序列约束以惩罚函数的方式加入目标函数中：
[0061][0062]
其中r
pmax
是参考序列目标的安全半径，∈与n、s影响近似精度。
[0063]
障碍物约束通过以下函数加入目标函数：
[0064][0065]
在优化的过程中d
min
将逼近r
pmax
，当r
pmax
足够大时，可实现轨迹对障碍物的“远离”。
[0066]
速度与加速度的惩罚函数为：
[0067][0068]
关于时间最优性的目标函数为
[0069][0070]
步骤4、轨迹输出。若决策结果是巡航、避让或快速通过，则直接回溯轨迹，将包含位姿、速度、时刻信息的轨迹点输出；若决策结果是超车，则需对生成候选轨迹进行打分和选择，如下所述:
[0071]
对轨迹效果进行打分评判，首先对轨迹再规划模块输出轨迹进行碰撞检测，若两条超车轨迹均不满足安全检测，则选择速度分配模块生成的耗时较长的初始轨迹；若至少有一条超车轨迹满足安全检测，则计算满足要求的超车轨迹和初始轨迹的代价函数，代价函数由加速度代价、加加速度代价、曲率代价、时间代价组成。
[0072]
安全检测按以下方式，对于新的再规划超车轨迹，构建该轨迹下包含其他交通参与者的时空占据栅格图，将该轨迹下时刻与路程对应的坐标点在图上描绘，若存在坐标点落在不可通行的占据栅格上，那么不满足要求，反之该轨迹是安全的。
[0073]
代价函数按以下方式计算：
[0074][0075]
对于两个模块输出的轨迹，速度、加速度、加加速度的计算方式为st曲线的一阶、二阶、三阶导数。
[0076][0077][0078][0079][0080]
对于本领域技术人员而言，显然本发明实施例不限于上述示范性实施例的细节，而且在不背离本发明实施例的精神或基本特征的情况下，能够以其他的具体形式实现本发明实施例。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明实施例的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明实施例内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。系统、装置或终端权利要求中陈述的多个单元、模块或装置也可以由同一个单元、模块或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。
[0081]
最后应说明的是，以上实施方式仅用以说明本发明实施例的技术方案而非限制，尽管参照以上较佳实施方式对本发明实施例进行了详细说明，本领域的普通技术人员应当理解，可以对本发明实施例的技术方案进行修改或等同替换都不应脱离本发明实施例的技术方案的精神和范围。