基于拓扑路网的多挡无人履带平台路径重规划

刘龙龙，陈慧岩，刘海鸥，关海杰，卢佳兴

(北京理工大学 机械与车辆学院，北京 100081)

0 引言

随着智能交通发展水平和平台无人化改造技术的快速提升，陆上无人装备在未来作战系统中有着巨大的应用前景和研究价值［1］。无人履带平台作为陆上无人装备的重要组成部分，正在成为实施军事打击的主战装备和重要支援力量。在越野环境下，根据任务需求，无人履带混合动力平台的行驶路径通过全局规划产生一条拓扑路径，引导平台沿着既定轨迹安全通行。但由于越野环境的复杂性或出现突发威胁，会出现全局规划路径上增添障碍物、局部规划局部最小或无解状况，无人平台无法继续按照原规划路径行驶，需要重新规划路径绕过该被阻断道路，继续完成未执行的任务点。因此，需要触发路径重规划，以规避不可通行区域或道路阻断。

关于无人履带混合动力平台路径重规划的技术已有许多相关研究及理论成果，目前常用的路径重规划方法有栅格法、采样搜索法、人工势场法、蚁群法和人工智能神经网络法等。Ganganath 等［2］和张晓晔等［3］提出了快速重规划A*(RRA*) 算法，用于部分未知环境中的路径规划和重规划。但未考虑路面通行时间，并未对平台做针对性研究。Chen 等［4］、朱杰等［5］和邹启杰等［6］研究了无人机航机重规划问题，在三维拓扑空间考虑了的无人机威胁源的影响，但未见在地上无人平台上使用。房立金等［7］、丛玉华等［8］和朱大奇等［9］提出一种改进RRT* FN 多场景运动规划算法，在障碍物环境变化或目标点改变时，环境适应性强、成功率高，但只适用于较低速度，不能满足大范围场景中速度幅值变化大的需求。孙鹏耀等［10］针对传统势场法存在的路径不被识别、局部极小陷阱等问题，提出适用于战场环境情况下机器人路径规划、结合多行为策略与可变影响范围的势场法，此方法在路径识别、多障碍区导致的振荡问题上具有较好的效果，但在具有已知拓扑路网的条件下效率较低，对障碍物或路面条件的影响效果也未进行量化区分。

针对上述问题，本文在基于拓扑路网的A*算法基础上，首次提出通过采用基于熵权法的多目标路径函数选择方法，对规划结果结合三次螺旋线的多阶段状态采样算法进行路径优化和换挡决策，解决平台在重规划过程中选择回退道路口的最优化问题，避免直接确定指标权重而造成主观随意性的问题，同时兼顾评价指标的全面性，最终达到缩短平台行驶消耗的时间和能量、提高路径重规划模块可靠性的目的。

1 双侧独立电驱动履带平台动力学建模

为满足平台的不同工况下速度和最大曲率限制的需要，并结合平台几何属性明确不同挡位下的平台特性，保证平台能够尽量精准跟踪预定轨迹，需要建立履带平台的动力学模型。由于换挡动作一般在直线行驶过程中进行，故本文只分析平台直线行驶时纵向运动的转矩变化。同时，在体现平台必要动力学特征的前提下，为避免考虑过多的约束条件，作如下假设:

1) 假设无人履带平台几何尺寸表现为左右对称，且质量中心与几何中心重合;

2) 假设平台两侧履带磨损和使用寿命相同;

3) 忽略两侧履带的沉陷效应;

4) 忽略平台悬架的作用，不考虑平台的俯仰、侧倾;

5) 忽略平台动力在传动系统中的能量损失。

平台纵向受力分析如图1 所示，其中:Oxy 为大地坐标系;Fl、Fr分别为左右两侧主动轮驱动力;Fi为两侧履带所受坡道阻力;Ff为两侧履带所受滚动阻力;Fw为平台表面所受空气阻力;v 为平台纵向运动速度。

图1 平台动力学模型Fig.1 Dynamic model of the platform

图2 展示了无人履带平台右侧驱动系统结构，其驱动动力由车载高压电池组和增程式发动机发电机组产生，提供给两侧独立分布式驱动电机，经制动器到两挡行星自动机械变速器，再到侧减速器传递至两侧主动轮。因此主动轮转矩大小受电机输出转矩和变速器挡位两变量控制，电机可实现低速大扭矩和高速恒功率两个特性之间的切换［11］。

图2 平台驱动系统单侧结构简图Fig.2 Single-sided structure of the platform-driven system

左、右两侧主动轮驱动力Fl、Fr采用如下公式计算:

式中:Tl为左侧电机输出转矩;Tr为右侧电机输出转矩;ig为所处挡位的传动比值;i0为两侧主减速器传动比;r 为主动轮转动半径。分布式电驱动无人履带平台的详细参数如表1 所示。

表1 无人履带平台系统参数Table 1 Parameters of the unmanned tracked platform system

履带平台在纵向上受到的阻力主要为坡道阻力Fi、空气阻力Fw、滚动阻力Ff和加速阻力Fj，平台处于纵向受力平衡状态下，可以得到以下平衡方程:

由于履带平台空气阻力接触面积较小，对总阻力影响效果较小，为简化后续建模研究过程，对空气阻力作忽略处理。将式(1) 代入式(2)，得到如下履带平台纵向动力学模型:

式中:β=sinα +fcosα，α 为坡道坡度，f 为滚动阻力系数;g 为重力加速度;δ 为旋转质量换算系数。式(3) 即为建立的无人履带混合动力平台纵向动力学模型［12］。

2 重规划引导策略

2.1 拓扑路网的图论方法表示

为建立无人平台在大范围越野环境中的精准导航地图，应结合平台的动力学模型确定合适的导航地图数学模型，针对非结构化道路，当前大多数平台采用均匀勾选路点的方式创建地图。拓扑路网模型就是把道路通过多个路点之间的连接关系以有向图的方式表达，拓扑路网模型是基于数学中的图论原理，图论是以拓扑图为研究对象，用于分析离散问题的数学方法，具有模型简单、概括力强的特点，因此利用拓扑路网模型对全局路径进行构建。

根据道路之间的邻接关系可以确定，拓扑路网中的双向路属性及其结构内部路点的递进关系使用双向图表达。双向路中每个路点都与相邻的两个点建立交互关系，如图3 所示的交叉路口处，L1、L2、L3均为双向边，在这种情况的路段上平台可以沿着道路前进或后退，全局规划时的道路选择性也更多。

图3 全局双向图示意图Fig.3 Global bidirectional diagram

2.2 重规划触发机制

在行驶过程中，平台前方道路长度小于Dh时，则认为可能发生了道路阻断，Dh后续根据平台动力学特性和传感器在环境感知上的局限性确定［16］。考虑到感知出现误检现象，平台需要倒驶一定距离后进行二次判断，当检测到道路阻断信息帧数达到设定阈值后，就确认为平台前方道路阻断，则需要重新规划一条绕过该被阻断区域的路径，以便能够继续通往后续任务目标点，本文研究中将此过程称为重规划［17］。

图4 展示了一个典型的平台前方道路被阻断的场景，平台在完成任务点Γi目标后，参考全局路径引导路段1 通往任务点Γi+1，但在驶入路段1 一段距离后检测到该路段不可通行，平台需要重规划从当前位置到终点之间的路径，无人控制系统更新引导策略，引导平台通过路段2 绕行至任务点Γi+1并继续行驶。根据平台的运动状态，本文将重规划分为重规划触发阶段和重规划执行阶段两个过程［18］。

图4 道路阻断时重规划场景Fig.4 Replanning scenario when roads are blocked

在整个控制系统中，重规划是全局规划模块的子模块，但又不完全受限于全局规划。平台与重规划相关的模块如图5 所示，通过融合定位系统和远程端下发路网文件、任务文件等信息，全局规划在初始阶段生成一条参考路径，并将其发送给局部规划。重规划收到局部规划和图像识别系统的检验结果进行实时判断是否触发和执行，并把结果反馈到全局规划中，最终形成路径规划的闭环控制。

图5 触发重规划模块方框图Fig.5 Block diagram of the replanning triggering module

2.2.1 重规划触发阶段

重规划触发阶段的主要任务是判断前方路段是否可通行并根据判断结果规划新的全局路径。而判断前方路段是否可通行最直接、最精准的办法是局部路径规划结果。在本研究的算法中，正常行驶条件下，局部规划路径的欧氏距离长度为l。已知局部规划路径最小阈值长度是Dh，当局部规划连续N 帧的感知数据检测到平台前方路径长度均小于此阈值时，可初步假定前方路段不可通行。但是考虑到感知系统存在误检，导致局部规划路径暂时性的缩短，从而误判为前方无可通行区域，为排除此现象引发重规划动作，在检测到前方路段不可通行时，还需要倒车Dm等待Twait(Twait为重规划确认时的驻车等待时间) 进行再次确认，若倒车后仍然检测到前方路段不可通行，则确认触发重规划。根据平台几何尺寸和动力学特性，相关参数的具体数值定义如表2所示。

表2 重规划触发过程相关参数Table 2 Parameters of the replanning triggering process

另外，通过图像识别系统的探测，当其确认识别到对无人平台安全造成威胁的特定目标物体时，如水马、拒马或敌方作战装备等。规划系统收到信号后，不再进行二次判断而是直接进入重规划过程，使得无人平台及时撤出危险区域，保证平台的安全性。触发重规划流程如图6 所示。

图6 重规划触发流程图Fig.6 Flow chart of replanning triggering

2.2.2 重规划执行阶段

通过重规划触发阶段的判断确认需要重规划后，针对具有非完整性约束的地面无人平台，进入重规划执行阶段，平台通过A*算法首先重新规划一条全局路径。由于新的道路是从平台后方起始，平台需要改变行驶方向，将规划结果路径分为倒驶阶段与前进阶段两段执行，首先平台规划系统向执行系统发送倒驶命令，无人平台倒驶至当前位置的最近岔道口处调整航向，当倒车过程执行完毕后，控制系统向平台执行器发送前进命令，平台跟随全局路径继续行驶。

需要注意的是，前方路段发生阻断后，为防止路径搜索时会再次沿着原被阻断路段方向行进，规划前需要打断阻断位置处两路点间的拓扑关系，在算法中选择路段L1上平台投影点对应的第np个路点和第np+1 路点之间的拓扑关系打断，平台从当前位置为新的起点，根据剩余任务要求重新规划一条不包含道路阻断区域的全局路径(见图7) 。

图7 重规划拓扑关系打断策略Fig.7 Topological relationship interruption strategy in replanning inreplanning

2.3 基于熵权法的多目标路径重规划

在未知目标任务的情况下，平台未考虑实际通行条件，为能够满足平台对不同任务的不同需求，本文提出一种多目标路径选择的熵权法［19］。熵权法首先要依据各个指标的变异情况，运用模糊的综合评价矩阵和各个因素输出的信息熵计算出各个指标的熵值，其次采用熵权方法对指标进行修正，最后得到指标权重，此种方法能够为多指标综合评价提供依据。本文主要建立了基于时间最短、能量最低和换挡次数最少3 个子目标的多目标路径选择模型。与现有的迭代算法相比，基于熵权法的多目标路径重规划是考虑了通行时间、经济性和平台动力学特性而产生的重规划最优路径。

2.3.1 时间最短

在瞬息万变的作战环境中，正确快速的路径规划是尽量规避所有风险，能够自主地完成设定任务的基础。当无人履带混合动力平台执行的任务要求较为急迫时，需要把完成时间尽可能缩短，因此，提出基于时间最短的路径选择模型P1:

式中: V 为重规划次数;Δs 为平台正常通行距离;v1、v2分别为平台前进、后退状态纵向平均速度;为平台正常通行时间;为平台恒定速度v2倒驶至路口处所需时间;Tw为重规划第2 阶段平台开始正向行驶至下一目标点的行驶时间。

2.3.2 能量最优

平台在满足动力性的前提下，降低能量消耗，节约能量成本是执行无人行驶任务、实现平台性能的关键。因此需要合理的安排行驶路线，避免在行驶过程中出现能量“供不应求”现象，由于横向运动能量消耗较小，本文只考虑纵向运动能量消耗，提出了基于能量最优的路径选择模型P2:

式中:t0～tf1为平台从前一任务点至重规划前行驶时间;tf1～tb为进入重规划至倒驶结束平台行驶时间;tb～tf2为重规划第2 阶段平台开始正向行驶至下一目标点的行驶时间。

2.3.3 换挡次数最少

由于此平台搭载有多挡自动变速机构，平台会根据规划的速度、前方路径长度和路径曲率大小匹配不同的挡位，结合平台的动力学特性，给平台匹配不同的挡位，为提高平台行驶过程中的平稳性，减少变速器的损耗，保证平台的行驶安全，提出了基于换挡次数最少的路径选择模型P3:

式中: Nf为平台从前一任务点到重规划前行驶过程的换挡次数;Nc为进入重规划至倒驶结束平台行驶过程的换挡次数;Ns为重规划第2 阶段平台正向行驶至下一目标点过程的换挡次数。

2.3.4 基于熵权法的多目标路径选择模型

为通过多个子目标类函数表示多目标类函数，需要对子目标函数进行赋权处理，采取基于熵权值的G1 组合赋权法，避免直接确定指标权重而造成主观随意性，也避免熵权法不适用多样性问题。

首先对时间P1、能量P2和换挡次数P3三个子目标参数进行重要性排序:能量P2＞时间P1＞换挡次数P3。根据每个模型的第i 次试验中对第j 个子目标进行标准化处理:

式中:xij为第i 次试验中对第j 个子目标评分的特征比重，i=1，2，…，nt，j=1，2，…，ns。

设ej为第j 个子目标的熵权，计算出每个子目标数据的熵权:

利用熵权法确定相邻子目标的重要性之比rj:

通过比较熵权大小确定指标之间的rj，可以反映出经验和数据二者的信息，避免主客观权重分配不合理的问题。计算第j 个子目标对其上级目标基于熵权修正的G1 组合权重vj:

最后，根据不同目标任务点要求，结合试验数据计算得到熵权后，可得到表3 所示的结果。

表3 熵权法赋值结果Table 3 Entropy method assignment results

由表3 可知，时间权重占比0.32，能量权重占比0.55，换挡次数占比0.13，从而得到多目标路径选择模型公式为

3 曲线模型

为保证重规划轨迹曲率连续性，在给定平台的当前状态和参考路径后，平台规划一条从重规划位置的前一任务点到下一任务点之间的路径，并确保此路径尽量与参考路径一致、平滑、无碰撞，同时满足平台动力学模型。本文方法将状态采样空间等间距分割，在每个采样阶段，先采用自适应采样策略生成多个可行终点状态，利用基于三次螺旋线的非线性参数优化求解到达终点状态的多个两点边界值问题，生成与参考路径有一定恒定偏差的候选路径基元集。在每个采样阶段，其终点又作为下一采样阶段的起点，依次进行采样，直至参考曲线终点结束。

为能够产生较多的路点和更高阶的多项式曲线，使其接近参考路径的确切形状，同时又不导致系统计算量过于庞大，本文选择将复杂的参考曲线分割成多个采样阶段，每一段都用相对简单的运动单元近似保证连接处高阶连续。对于每一阶段的路径，采用三次螺旋线表示以减少参数空间的大小，同时保证曲线模型对路径的表达准确。参考路径选择基于弧长大小的分割方式。通过这种复杂的方式，运动曲线被分段平滑的三次螺旋线近似替代［20］。

在进行单阶段状态空间采样之前，首先从参考路径分割模块中获取用于等间距横偏的参数目标状态，基于该目标状态以不同的横偏距离沿该点路径方向的法向两侧生成横向目标状态集，采用自适应横向采样策略用于生成动力学可行路径的横向目标状态。另外，本文采用平台沿垂向的二维外形轮廓投影对采样点状态进行相对精确的碰撞检测，以过滤目标状态集中的不可行采样点。只有无碰撞的目标状态才会被保留下来，传递给候选路径集。通过这种方式，当一个目标状态不能到达时，避免生成不必要的、计算代价较高的候选路径，最终使得采样效率得到进一步提升。

由于相应曲线定义在笛卡尔坐标系下对x 轴和y 轴分别采用参考化表示等问题，回旋曲线是1 阶多项式螺旋线，并广泛应用于道路设计与运动规划中。然而，螺旋线只有3 个参数，不能同时满足边界条件中的航向和曲率约束，而采用三次多项式螺旋线可以同时满足上述约束条件，并保持着较小的参数空间维度，在自由度和对运动的表达性方面满足要求。因此，通过将控制输入表示为3 阶螺旋线，笛卡尔坐标系下路径的弧长参数化表示形式为

式中:a、b、c、sf为需要优化的变量，sf也是到目标状态的弧长;d 为平台当前状态所对应的曲率。由此参数定义的工作空间路径r(s) 可表示为

式中:θ(s) 为平台的航向。

将式(13)～式(16) 代入式(12)，可知r(s) 至少2 阶连续，至多5 阶连续。

4 实车验证

本文在无人履带平台上进行试验验证，该平台为混合动力双侧独立电驱动结构，在无人控制系统中，平台共配置有3 个激光雷达，其中平台前侧中心一个64 线、两侧各一个16 线激光雷达;图像识别模块配置有3 个单目相机，正前方1 个主相机，左右各1 个侧相机，以进行障碍物和特定场景探测与识别;实时差分、全球定位系统和惯性导航共同组成的融合定位系统用以确定平台的位置和航向信号(见图8) 。两台用于数据计算处理的工控机，一台用于感知子系统，完成激光雷达点云处理和相机图片处理，另一台用于完成控制子系统和规划子系统的算法运行。两台工控机均使用Linux 操作系统架构，用于测试的算法通过ROS 软件平台与其他子系统之间进行通信。

图8 试验平台系统组合Fig.8 Test platform with subsystems

由于驱动电机的功率密度有限，为能够适应较重的履带平台，需配置变速器提高性能。因驱动电机具有恒功率输出特性，电动平台选择使用少挡位、高阶比的有级变速器，并配合伺服电机和VCU 组成完整的AMT 系统。

在越野环境的六岔道口场景中，根据每条道路分支的平坦程度、弯曲程度和坡度大小定义参数，并将其赋值到路网属性中，通过实车试验对无人履带平台的重规划算法逻辑及路径优化方法进行验证，以图9 中蓝色线条显示的一种工况为例。平台从任务点①开始至⑤处结束，行驶至②处遇到如图10(a) 所示道路阻断情形，平台进入重规划逻辑，并如图10(b) 所示打断路网间拓扑关系。在重规划执行阶段平台从②处倒驶至③处停车，从③处向前行驶并左转进入新的路段经过④处直至到达下一任务点⑤处结束。由于④至⑤之间平台运动状态基本一致，④、⑤之间距离较大，为提高试验效率、简化试验数据，在试验中未考虑这段过程的试验数据，这样也可以保证每次试验具有相同的起点和终点及相同位置的障碍物。试验分别以时间最短、能量最低和换挡次数最少为单参数最优试验作为参照，体现多目标函数算法的适应性最佳、综合代价最低，试验结果如图11(a)、图11(b) 和图11(c) 所示，其中路段1、路段2、路段3 是单目标最优路径规划结果，路段4是基于熵权法的多目标路径规划结果。在只考虑时间最短的单参数目标实车平台试验中，平台规划选择了如图12(a) 和图12(b) 所示路段。为了能够快速完成重规划过程，缩短行驶时间，无论是前进还是倒驶，直线轨迹上平台挡位设置为2 挡。转向过程中2 挡时两侧转矩较小、达不到精确速差，为保证高速时具有较好的转向特性，在转向时挡位减至1 挡，这种情况下平台的转向轨迹曲率较小。

图9 试验场地卫星图Fig.9 Satellite image of the test site

图10 重规划触发并执行Fig.10 Replanning triggered and executed

在以能量最低为指标的单目标代价函数实车试验，平台规划选择如图12(c) 和图12(d) 所示路段。为使平台的驱动电机运行在较经济的负载区间，在低速下平台的模型控制效果较好，合理地调整不同速度下的减速比，此过程中变速箱换挡次数增多。在不考虑时间的条件下，道路口处平台以较低速度、大曲率轨迹行驶，在A*算法规划的全局路径中只考虑距离代价，保证了行驶路径最短。

本文试验无人履带混合动力平台换挡规律采用速度单参数控制方式。在以换挡次数最少为指标的单目标代价函数实车试验，平台规划选择如图12(e) 和图12(f) 所示路段。此路段在转弯处曲率较小，可全程2 挡条件下完成重规划过程，较少的换挡次数可有效避免换挡时两侧轮速不一致、换挡失败等故障的发生。

图12 单参数目标下重规划路径结果Fig.12 Results of path replanning using single parameter

最后，通过采用多目标路径选择模型函数，综合考虑时间、能量和换挡次数3 个参数，利用熵权法进行权重赋值，最后选出一条综合考虑换挡平顺性、安全性和动力学特性的最优轨迹，如图13 所示。利用熵权法分析单、多参数最优的实验结果如表4 所示，由表4 可知，通过多目标路径选择模型的规划路径结果，与时间成本最优的试验结果相比，虽然时间成本较高，但能量消耗减少了4.74%，节省的能量可使得平台续航增加200 m，换挡次数较少，减少了换挡机构的磨损;与能量成本最优的试验结果相比，虽然增加了能耗，但通行时间缩短了10.35%，减少了换挡次数，提高了平台行驶的平顺性;与换挡次数最优的试验结果相比，由于路段较短，换挡次数上虽未有显示出差异，但时间成本和能量成本都有缩减。因此，采用多目标模型函数选择的路径更合理，更能适应越野环境下复杂多变的道路环境。平台在此参考轨迹的引导下，兼备考虑了时间、能量以及不同转弯半径和速度时应该选择的挡位，确保了纵向运动的动力性的同时，也充分考虑了时效性和经济性。

表4 不同道路熵权法试验结果Table 4 Results of the entropy method for different roads

图13 多目标下的重规划路径Fig.13 Replanning path using multiple parameters

5 结论

本文针对越野环境中无人履带混合动力平台重规划鲁棒性差、场景适应性弱的问题分析，重点研究了重规划过程中回退路段的选择方法，通过建立基于熵权法的多目标路径重规划算法，成功求解出复杂战场环境下的最优路径。在环境适应性和稳定性方面取得了较好的效果。得到如下主要结论:

1) 本文重点研究了重规划执行过程中回退路段的决策方法，提出了基于熵权法的多目标路径重规划算法，根据重要性对时间、能量和换挡次数三参数划分不同的权重系数，建立了多目标路径选择模型。与现有的迭代算法相比，解决了平台在道路被阻断或改变行驶方向时，未考虑平台特性、时间成本和续航能力的问题。

2) 本文试验用无人履带混合动力平台搭载了两挡行星自动机械变速器，建立了考虑电机输出转矩和传动比两个因素的动力学约束条件，使无人平台在行进过程中可根据行驶速度切换到不同的挡位，克服了采用固定速比的变速装置导致驱动电机长时间工作在低效区的弊病，应对不同路面条件下的速度需求，保证平台高速下具有较强的通过性。

3) 通过实车平台验证了本文方法的有效性，并在越野环境进行了单、多目标条件下的实车数据对比，通过试验结果可知，本算法在时间成本上缩减了4.74%，在能量成本上缩减了10.35%，综合熵值降低了9.21%，兼备考虑了时间、能量以及不同转弯半径和速度时应该选择的挡位，确保了纵向运动的动力性的同时，也充分考虑了时效性和经济性。