王博洋 ,关海杰 ,龚建伟 ,陈慧岩 ,赵卉菁

(1.北京大学 信息科学技术学院,北京 100871;2.北京理工大学 机械与车辆学院,北京 100081)

0 引言

无人驾驶车辆的运动规划方法能够在可通行区域内,结合车辆自身的运动特性约束,生成从起始位姿到终止位姿的无碰撞轨迹,是无人驾驶系统的重要组成部分[1-2]。目前基于现役装备改造的无人驾驶履带车辆和新近研发的无人驾驶履带车辆,在转向机构的组成上存在显著差异,如何对转向机构不同所导致的运动特性差异进行建模,并将其融入到运动规划系统中,是建立异构履带车辆统一运动规划方法的关键。

在运动规划方法的研究中,将连续的轨迹分割成离散的基本单元,继而通过运动基元生成与选择的方式构建运动规划方法,能够显著提升算法的效率[3]。运动基元生成的核心是解决两点的边界值问题,即根据不同的目标位姿生成一组备选基元[4]。Hybrid A*以车辆运动学模型为依托,通过对车辆的控制空间进行采样,生成一组备选基元集[5];也可以通过对状态空间进行采样,完成备选基元集的设置,具体的采样方式包括:以优化后的参考线偏移点作为采样点[6],对超越障碍物的局部位置进行局部采样[7],以晶格形式完成空间采样[8],以及同时对时间和空间两个维度进行采样[9]。虽然上述基元的生成方法能够满足城市环境与越野环境下的基本规划需求,但生成的基元未能考虑异构履带车辆运动特性的差异。

在基元生成的基础上,如何从备选基元集合中扩展并选取适当的基元构成期望轨迹,是提升规划轨迹质量的另一项重要因素[10]。Klanˇcar 等着重讨论了在基元扩展过程中所要保证的曲率和速度的连续性[11]。但上述方法的平滑过渡为在线优化,降低了规划算法的效率。Dolgov 等在基元的选择过程中既考虑了车辆的非完整性约束,又结合了环境约束的启发项[12]。朱冰等通过基于安全距离模型的安全场,对基元的扩展与选择提供了指引[13]。Hu 等在基元的选择过程中综合考虑了动静态障碍物约束与轨迹的平滑度约束[14]。Zhang 等则通过时空地图构建基元选择的约束条件[15]。但上述基元的选择代价,主要是从环境约束的角度设定相应的指标,未能关注异构履带车辆的行为差异性。尤其是如何在基元的选择过程中权衡确定性较强的规定转向半径运动模式,与不确定性较强的非规定转向半径运动模式。

前期的研究已经针对有级转向履带车辆轨迹与操控层的行为模型[16-17],以及无级转向履带车辆的规划与控制方法开展了相应工作[18-19]。虽然研究人员通过建立不同类型车辆的运动微分约束,在基元生成过程中引入车辆的差异化特性[8],但对于异构履带车辆而言,很难从运动学模型角度区分车辆运动模式之间的差异。本文以运动基元的离线优化生成与在线扩展选择为依托,既将异构履带车辆的行为特性差异融入到基元的优化生成中,又在基元的选择中考虑了规定转向半径模式与非规定转向半径模式的差异性,最终提出了能够体现异构履带车辆特性差异的统一运动规划方法,生成与车辆运动模式匹配性高的轨迹。

1 统一运动规划方法整体流程

本文提出的统一运动规划方法主要包含两方面内容:运动基元的优化生成、运动基元的扩展选择。统一运动规划方法的整体流程如图1 所示。其中,运动基元的优化生成方法,能够在离线状态下生成每一类异构履带车辆独有的运动基元库;运动基元的在线扩展,以可通行区域与上一时刻的扩展基元簇为约束,扩展出备选的基元集合;运动基元的在线选择模块,则从集合中选择出最优的基元,构成运动基元序列,即具备时空信息的运动规划结果。

图1 统一运动规划方法的整体流程Fig.1 Overall process of an unified motion planning method

2 运动基元的优化生成

在异构履带车辆行为基元的优化生成问题中,优化求解的变量为车体的纵横向运动参量u,优化的目标函数g主要考虑了生成的运动基元在轨迹和速度层面的平滑度。优化问题的约束条件包括了履带车辆的运动微分约束f、异构履带车辆的差异化行为约束B、优化求解变量u与车辆状态参量s的不等式约束U,以及运动基元的平滑过渡约束T,并且基元生成时车辆的运动具有单调性(前进或后退)。优化问题的定义如(1)式所示:

式中:优化求解的变量u(t)=[η(t),α(t)]T,η(t)为车辆纵向加速度的导数,α(t)为横向速度差变化率的导数;s(t)为车辆的状态参量,s(t)=[x(t),y(t),θ(t),vx(t),ax(t),Δv(t),ω(t)]T,x(t)、y(t)和θ(t)为全局坐标系Oxy下的位置坐标值和航向角,vx(t)为车体平台坐标系Opxpyp下沿x轴的纵向速度,ax(t)为相对应的加速度,Δv(t)为两侧履带的速度差,Δv(t)=vrx-vlx,vrx与vlx分别为右侧和左侧的履带速度,ω(t)为两侧履带速度差的变化率,ts为基元生成的起始时刻,te为基元生成的终止时刻。履带车辆平台关键参量的定义如图2 所示。图2 中:B为履带车两侧履带之间的距离,L为履带车接地段的长度。

图2 履带车辆平台关键参量定义Fig.2 Definition of key parameters of tracked vehicle platform

2.1 履带车辆的运动微分约束

本文研究的有级转向履带车辆与无级转向履带车辆在运动特性上存在较大差异,但履带车辆的基本运动形式未发生本质上的变化,因此满足相同的运动微分约束关系。具体而言,虽然有级转向履带车辆仅有有限的规定转向半径,并且规定转向半径呈现离散而非连续的分布形式,但车辆的运动不会发生突变,因此有级转向履带车辆与无级转向履带车辆的运动微分约束如(2)式所示:

2.2 异构履带车辆的差异化行为约束

本文研究的异构履带车辆在所搭载的转向机构上存在显著差异,离合器转向机仅具备一个规定转向半径,二级行星转向机具备两个规定转向半径,双侧独立电机驱动则为无级转向。

由于本文仅面向异构履带车辆的运动规划问题,只对轨迹层面的运动模式进行差异化建模。将有级转向履带车的主要运动形式定义为规定转向半径运动与直线运动的模式组合。以Dubins 曲线为依托,基于起始点与终止点的位姿,生成以固定半径圆弧和直线构成的曲线,并从中提取若干采样点,构成基元优化生成的中间点约束。本文将倒车作为前进的相反方向进行考虑。

此外,还补充若干非规定转向半径模式到有级转向履带车辆的基元库中,该模式下的基元生成方式与无级转向履带车辆的基元生成方式完全一致。无级转向履带车辆不对基元生成过程中的中间点做约束,仅根据行为类别对起末点的位姿进行约束。

具体而言,异构履带车辆的运动基元库中包含了多种典型驾驶行为类别,包括直驶、调头、U 形弯、直角弯、障碍物规避、直驶方向校正等。每一种行为类别都对应于一组设定的起末点位姿。以障碍物规避行为为例,起末点的位姿约束be如下:

式中:d为横向移动的距离。对于无级转向履带车辆而言,起末点的位姿约束be被直接作为无级转向履带车辆的行为约束Bm=be。

有级转向履带车辆则是参照无级转向履带车辆生成的运动基元,选取近似的起始位姿{x(ts),y(ts),θ(ts)}与终止位姿{x(te),y(te),θ(te)}作为Dubins 曲线的起末状态约束,以规定转向半径作为Dubins 曲线的转向半径约束,并在生成的Dubins 曲线上选取Ns个采样点构成有级转向履带车辆的行为约束Bs={b1,…,bn,…,bNs}。其中,除去起始点b1与终止点bNs需要对位置和航向进行约束外,其余各采样点仅对位置进行约束。

Dubins 曲线的规划任务是以固定半径圆弧与直线相组合的形式找到连接起末位姿的最短曲线,并且曲线的构型需要包含在如下集合内:

式中:L 表示按规定转向半径左驶运动模式;R 表示按规定转向半径右驶运动模式;S 表示直驶运动模式。有级转向履带车辆行为约束中的Dubins 曲线生成示意如图3 所示。图3 中,r为规定转向半径,dl为左驶运动模式生成的路径,ds为直驶运动模式生成的路径,dr为右驶运动模式生成的路径,α、β为规定转向半径转过的角度。

图3 基于Dubins 曲线生成的起末点间的最短路径Fig.3 The shortest path between the start and end points generated based on the Dubins curve

2.3 车辆平台特性的不等式约束

受制于车辆平台执行器的特性约束,在运动基元的生成过程中需要满足如下不等式约束:

式中:vmin、vmax、amax、ηmax为车辆平台纵向参量所对应的极限值;Δvmax、ωmax、αmax为车辆平台横向参量所对应的最大值。

2.4 基元之间的平滑过渡约束

由于最终生成的规划轨迹是为各独立基元组成的基元序列,为了保证基元之间速度以及曲率的平滑过渡,需要添加首末时刻的平滑过渡约束,约束的具体形式如下:

该平滑过渡约束为过渡基元生成时所使用的Hermite插值创造了二重零点条件。

2.5 优化生成的目标函数

运动基元优化生成的目标函数主要考虑了轨迹时序点的平滑度,具体的形式如(7)式所示:

式中:c1、c2、c3、c4、c5为各项评价指标的权重系数。

2.6 异构履带车辆基元库的优化求解

通过(1)式～(7)式完成基元优化生成问题的定义后,在离线状态下完成优化问题的求解。最终在统一算法框架下,依据异构履带车辆的差异化行为约束,生成突显各异构履带车辆独特运动特性的运动基元库,为后续运动基元的扩展与选择提供备选集合。

3 运动基元的扩展选择

运动基元的在线扩展选择是从离线生成的运动基元库中优化选择出符合环境约束的基元序列,得到包含轨迹与速度信息的运动规划结果。运动基元优化选择问题的定义如(8)式所示,其中优化问题的目标函数为基元序列中各基元代价的加和,优化问题的约束包含起末位姿的约束和在可通行区域内的基元扩展约束。

式中:pk为选择得到的基元序列中第k个基元;M为基元序列中所包含基元的总数目;J为基元选择的代价函数;sb为基元初始状态,sbs为设定的基元初始状态,se为基元终止状态,ses为设定的基元终止状态;Ep为环境中的可通行区域;P为离线生成的行为基元库。

3.1 基元的扩展

基元的扩展实质是在可通行区域内,实现基元序列中相邻基元的关联,其中的关键是保证基元序列中相邻基元在曲率与速度上的平滑过渡。由于(6)式所示的基元平滑过渡约束,使得各独立基元在起末点都处于匀速直线运动状态,保证了基元过渡处曲率的连续性。但由于离线生成的基元,在速度空间上呈现离散分布状态,相邻基元之间有速度跳变的可能,因此需要在基元扩展过程中,在线构建平滑过渡基元,实现速度的连续平滑变化。

基元之间的过渡形式为变速直线运动,并采用三次Hermite 插值完成过渡段中速度的生成,具体形式如(9)式～(11)式所示:

式中:vn为插值点的速度;l0=(t-t1)/(t0-t1),l1=(t-t0)/(t1-t0)。由于过渡基元所要关联的起末点加速度都为0 即v·n=0,通过对v(t)求导,能够得到过渡基元加速度的函数a(t),该函数在ta=(t1-t0)/2 处取得最大值,在最大值处的取值如(12)式所示:

式中:v1-v0为基元要过渡的速度偏差。由(12)式可知,在已知最大加速度的条件下,能够得到基元的时间尺度,也就能求解得到基元的长度,完成基元扩展中过渡基元的生成。

3.2 基元的选择代价

基元的选择代价包含3 方面内容,分别是与期望轨迹与速度的偏差代价、轨迹的平滑代价以及碰撞风险代价。

3.2.1 与参考轨迹和期望速度的偏差代价

参考轨迹与期望速度作为运动规划系统的输入,能够为规划提供最基本的参照与指引。在偏差代价的计算中,通过引入高斯函数作为各点偏差的权重系数,使得近处的参考轨迹点发挥更主要的引导作用。此外,由于基元内部的速度变化趋势是基元本身的固有性质,在与期望速度匹配的过程中,仅对基元终点速度进行考量。基元与期望轨迹的偏差代价Jp如(13)式所示,与期望速度的偏差代价Jv如(16)式所示。

式中:Np为生成轨迹上等间隔采样点的总数目;高斯函数g(·)为标准正态分布;jn为生成轨迹采样点与参考线匹配点之间的偏差代价。

式中:μ为均值;σ为标准差;ωd与ωh分别为距离偏差与航向偏差的权重系数;dn为与参考线的距离偏差;Δθn为与参考线的航向偏差。

式中:vr为参考线匹配点上的期望速度;ve为基元终点处的速度。

3.2.2 轨迹的平滑代价

轨迹的平滑度直接反映了基元上各点曲率的平均变化情况,该数值越小、曲率越平滑,轨迹平滑度的定义如(17)式所示:

式中:ωs为轨迹平滑度的权重系数;κn为基元采样点上的曲率值;Δsn为相邻采样点之间的间距。设和为依据基元类型与车辆平台类型设定的权重系数,无级转向履带车辆权重系数的定义如(18)式所示,有级转向履带车辆权重系数的定义如(19)式所示。

式中:ωM和ωR分别为无级转向履带车辆方向维持和反向基元的权重系数;pk为基元序列中的第k个基元;和分别为直驶过规定转向半径方向维持和反向基元的权重系数;和分别为非规定转向半径方向维持和反向基元的权重系数。

3.2.3 碰撞风险代价

为了避免与环境中的障碍物发生碰撞,需要定义碰撞风险代价,并使得该数值尽可能低。以六覆盖圆近似车身,能够兼顾碰撞检测的效率与对车身近似的精度。本文选用六覆盖圆近似地表示车身,通过计算障碍物到圆心的距离作为碰撞风险代价的指标,具体计算公式如(20)式所示:

3.3 基元的在线优化选择

基元的在线选择需要对定义的选择代价进行综合评估,基元选择的总代价如(21)式所示:

式中:ωp、ωv、ωs、ωc为相应代价指标的权重系数。

在计算得到备选基元的选择代价后,在线选择模块基于计算得到的代价值从基元簇中选择出代价最低的基元,逐步扩展生成从起始点到终止点的基元序列,构成最终的期望轨迹时序点。

4 实验结果与讨论

本文选用3 种不同类型的异构履带车辆平台完成对算法的验证,这3 种类型的履带车辆分别是基于离合器转向机的有级转向履带车辆(Ⅰ型平台)、基于二级行星转向机的有级转向履带车辆(Ⅱ型平台)、基于双侧独立电机的无级转向履带车辆(Ⅲ型平台)。

履带车辆从转向机理上能够分为无级转向履带车辆与有级转向履带车辆两大类。有级转向履带车辆又根据可选择规定转向半径数目的不同,有了更细致的区分。本文选用的3 种不同类型履带车辆平台既能覆盖有级和无级两大类别,又能体现不同类型有级转向履带车辆的特点,因此具有一定的代表性。

所选用的车辆平台如图4 所示,车辆平台的结构以及执行器参数如表1 所示。

图4 所选用的异构履带车辆平台Fig.4 Seclected heterogeneous tracked vehicle platforms

表1 车辆平台结构及执行器参数Tab.1 Vehicle platform structure and actuator parameters

本节首先展示各履带车辆平台基元离线生成的总体情况,并从高低速两个状态中分别选择若干典型的运动基元进行展示,突显各异构履带车辆平台之间以及高低速之间的基元特性差异;其次展示两个典型越野场景下的运动规划结果,从差异化的运动规划结果、算法的时间消耗等层面对结果进行讨论。

4.1 异构履带车辆的运动基元库

异构履带车辆的运动基元库离线生成采用了统一的优化算法框架,只是在行为约束与车辆模型两个方面,对不同类别的履带车辆平台进行了区隔。表2 反映了异构履带车辆运动基元库的总体情况,图5 展示了不同车辆平台、不同速度区间下的典型运动基元簇。在基元的离线优化生成中,(7)式中的各权重系数完全一样,对速度变化和曲率变化引起的平滑度波动同等考虑。

表2 运动基元库总体情况Tab.2 Overall situation of motion primitive library

从表2 中可以观察到:随着车辆速度的提升,基元的数目逐渐下降,这主要是因为车辆在低速区间具备较强的转向校正能力,能够组合出更多样的运动基元,以应对狭小空间内的复杂运动规划任务;在高速区间内转向校正能力变弱,能够组合出的运动基元种类变少;基元的长度基本上随速度单调递增,但Ⅰ型平台和Ⅱ型平台在中高速时长度有所下降,这主要是因为该平 台在中高速时仅存在非规定转向半径,非规定转向半径下转向机构中的部件处于滑磨状态,难以持续较长的时间并且转向不确定性较强,因此转向校正的时长被严格限制,基元的平均长度也显著下降。

从图5(a)、图5(c)、图5(d)中可以观察到,有级转向履带车辆规定转向半径模式下的运动基元,由于受到了Dubins 曲线生成的行为约束引导,在基元内部的运动规律为转向-大段直驶-转向,完全符合有级转向履带车辆的运动特性,并且与图5(e)、图5(f)中无级转向履带车辆的运动基元存在显著差异。

图5 各异构履带车辆平台典型运动基元Fig.5 Typical motion primitives of various heterogeneous tracked vehicle platforms

4.2 异构履带车辆的运动规划结果

为验证运动基元的在线扩展与选择算法,本节选取两种典型的非结构化场景对算法进行验证。在低速测试场景中,期望触发U 形弯、直角弯、调头等低速状态下的典型驾驶行为;在中速测试场景中,期望触发换道、直线升降速等中速状态下的典型驾驶行为。图6 和图7 分别展示了两个场景的基本设置情况,不同车辆平台的规划结果,以及基元扩展与选择的细节展示。图8 和图9 分别展示了规划得到的速度v与轨迹曲率κ之间的对应关系。表3 展示了规划算法的部分关键性能指标。图6(a)和图7(a)的场景设置图中,标注了参考线与期望速度,vr为参考速度,vs为速度切换点。

表3 规划算法性能评估Tab.3 Performance evaluation of motion planning algorithm

图6 面向非结构化场景1 的轨迹规划结果Fig.6 Trajectory planning results for unstructured scene 1

图7 面向非结构化场景2 的轨迹规划结果Fig.7 Trajectory planning results for unstructured scene 2

图8 面向非结构化场景1 的速度与曲率对应结果Fig.8 Corresponding results of velocity and curvature for unstructured scene 1

图9 面向非结构化场景2 的速度与曲率对应结果Fig.9 Corresponding results of velocity and curvature for unstructured scene 2

在基元的在线选择中,(21)式中权重系数ωp、ωv、ωc的数值都为1。对于无级转向履带车辆只对行驶的方向做区分,期望车辆尽量维持同样的行驶方向,完成整个规划任务,ωM=1,ωR=10。对于有级转向履带车辆,不仅对行驶方向进行区分,还对是否采用规定转向半径的基元模式、完成规划任务进行了限定,期望尽量通过确定性的规定转向半径运动完成规划任务,辅助以不确定性较强的非规定转向半径运动,因此权重系数为10,=10=100。

从运动规划得到的结果来看,有级转向履带车辆的规划轨迹主要由直线行驶模式与规定转向半径模式组成。这主要是因为在(21)式的选择代价中,针对规定转向半径与非规定转向半径分别设定了不等的权重系数,使得运动规划系统偏向于选择规定转向半径模式与直驶模式组合的方式,完成运动规划任务。无级转向履带车辆则没有进行上述区分,并且基元自身也以连续转向运动为主,因此无级转向履带车辆的轨迹更平滑,与有级转向履带车辆的规划结果存在显著差异。特殊地,如图7(b)所示,由于Ⅰ型平台在中高速情况下无法触发规定转向半径,基元簇的构成也以短距离的非规定转向半径为主,因此在该场景下的规划轨迹由直线段和多次小幅的转向修正构成,与同为有级转向履带的Ⅱ型平台存在显著差异。此外,从图6(c)、图6(e)、图6(g)的细节图上也能直观地体现出3 种不同的车辆平台在完成相同规划任务时的差异性(图中的虚框表示上一个基元结束时车辆所处的位置)。

从算法的求解效率上来分析,由于本文所提出的运动规划方法采用基元离线生成、在线扩展选择的方式,在基元扩展过程中,仅在线完成基元簇的旋转与过渡基元的生成,但由于运动基元库中所包含的基元种类较丰富,在选择过程的耗时平均比基元扩展的耗时高出37 倍。在限制场景中基元的扩展步数不超过8 步的前提下,能够将整体的时间消耗控制在120 ms 以内,整体而言满足异构履带车辆实时运动规划的需求。相反地,所选用的对比算法Hybrid A*,既不能体现车辆之间的特性差异,平均耗时也增加了3.2 倍。

5 结论

本文提出了一种异构履带车辆的统一运动规划方法,在基元生成与扩展选择的框架下,生成了能够体现异构平台差异性的规划结果。本文的主要贡献及所得主要结论如下。

1)利用Dubins 曲线构建了有级转向履带车辆的差异化行为约束,并借助基于车辆模型的优化生成方法,构建了能够体现异构履带车辆平台特性差异的运动基元库。

2)在基元的在线选择过程中,通过设定不等权重系数,提升了有级转向履带车辆规划结果中,直线运动模式与规定转向半径运动模式的占比,在一般场景下有降低控制难度的可能。

3)所提出的运动基元离线生成与在线扩展选择框架结构,兼顾了规划方法的通用性与异构平台的差异性,并且满足实时运动规划系统的效率需求。

在后续的研究工作中将开展运动规划与控制的集成研究,通过基元的类别信息构建规划与控制之间的关联,建立通用化的异构履带车辆运动规划与控制系统。