一种自动驾驶特种作业车辆的决策规划方法与流程

本发明涉及车辆轨迹规划领域，尤其是涉及一种自动驾驶特种作业车辆的决策规划方法。

背景技术：

近年来，人工智能技术的飞跃发展、计算机硬件运算能力大幅提升、感知系统的不断完善以及车辆电动化、线控化的日趋成熟使自动驾驶技术的落地成为可能。无人驾驶乘用车是目前自动驾驶技术应用最为激烈的方向，包括像谷歌waymo、百度apollo等，然而，类似矿山车、清扫车、渣土车等的特种作业车辆有着比乘用车对自动驾驶技术更为急切的需求，这不仅仅是出于行车安全的考虑，同等重要的还有驾驶员劳动负荷的降低、用工需求增加与熟练工人不足矛盾的缓解。因而，特种作业车辆的自动驾驶技将会是市场竞争的另一个焦点。

现有自动驾驶特种作业车辆，轨迹规划往往是考虑一次规划一条曲线，为了能够规避障碍物，必须在距离障碍物较远的地方就开始规划绕开障碍物；轨迹决策要么采用保守的策略，遇到障碍物就停车，要么采取激进策略，遇到障碍物直接绕行。这都将极大的降低特种作业车辆的作业效率。此外，由于感知系统的fov、分辨率和测量精度导致的测量误差、栅格化后的系统误差以及障碍物的遮挡，传递给决策规划的环境地图存在严重的不确定性，出于对自动驾驶特种作业车辆的作业安全考虑，规划决策需要能兼容这些不确定性。

因此，如何提供一种自动驾驶特种作业车辆的决策规划方法解决上述问题的自动驾驶特种车辆决策规划策略是本领域技术人员亟待解决的问题。

技术实现要素：

本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种自动驾驶特种作业车辆的决策规划方法自动驾驶特种作业车辆的轨迹决策规划方法。

本发明的目的可以通过以下技术方案来实现：

一种自动驾驶特种作业车辆的决策规划方法，包括以下步骤：

1)特种作业车辆的自动驾驶作业模块通过gps/imu获取自车的当前定位位姿，包括经度、纬度、航向以及当前的定位状态；

2)以自车为中心将感知系统发送的环境信息投影到栅格地图，并在栅格地图中标注静态和动态障碍物生成环境地图；

3)根据自车的当前定位位姿，自动驾驶作业模块通过存储在本地或远程发送的任务文件，获取当前作业执行器的控制指令下发到运动控制系统；

4)自动驾驶作业模块获取存储在本地或远程发送的任务参考路径，并将任务参考路径投影到环境地图，采用路径-速度分解的轨迹规划方法结合车辆动力学约束进行轨迹簇规划，获取车辆可执行的基础轨迹簇，将基础轨迹簇和任务参考路径融合得到可执行轨迹簇；

轨迹融合的过程可以看作是轨迹片段的衔接与曲率平滑过程对于避障规划的轨迹，轨迹片段依次由任务参考轨迹、避障轨迹、任务参考路径平移轨迹衔接而成，在衔接点用三次b样条拟合实现曲率的连续，对于并道/返回规划的轨迹，轨迹片段依次由任务参考轨迹平移轨迹、并道/返回轨迹、任务参考轨迹迹衔接而成，在衔接点用三次b样条拟合实现曲率的连续，基础轨迹簇完成的是避障轨迹或者并道/返回轨迹的规划；

5)考虑环境感知的不确定性，对规划的可执行轨迹簇进行安全性和高效性的择优，最终生成一条车辆可执行的高效作业且低碰撞风险的高收益轨迹；

6)根据高收益轨迹与自车当前的定位位姿，获取航向角偏差和横向偏差，并下发给运动控制系统进行实时路径控制。

所述的自动驾驶作业模块包括三个功能区，分别为决策规划、作业控制和行车控制。

在路径-速度分解的轨迹规划方法中，将轨迹规划解耦为路径规划和速度规划，可先进行速度规划，再进行路径规划，也可先进行路径规划，再进行速度规划。

所述的步骤4)中，车辆动力学约束包括车辆最小转弯半径、最值车速、最值纵向加速度、最值侧向加速度和路面附着系数约束。

所述的步骤4)中，车辆可执行的基础轨迹簇的起点为车辆当前的位姿点，或车辆当前执行轨迹安全距离内的一点；

车辆可执行的基础轨迹簇的终点具有目标偏向性，即：

当车辆在任务参考路径上时，为规避障碍物，在任务参考路径所在的可行驶区域内，对任务参考路径进行横纵向粒度不一致的离散得到目标点集，当车辆在避障执行轨迹上时，为返回任务参考路径进行作业，在避障执行轨迹和任务参考路径所在的可行驶区域内，对避障执行轨迹进行横纵向粒度不一致的离散以及对任务参考路径进行纵向粒度一致的离散得到目标点集。

所述的步骤4)中，可执行轨迹簇由四种模式的轨迹片段组成，包括任务参考轨迹、避障轨迹、任务参考轨迹平移轨迹和并道/返回轨迹。

所述的步骤5)中，考虑环境感知的不确定性包括感知系统的fov、分辨率以及测量精度导致测量误差和栅格化后的系统误差。

所述的步骤5)中，轨迹择优根据车辆当前所处模式，划分为避障决策、并道/返回决策以及并道/返回循迹三种模式。

对任务参考路径进行横纵向粒度不一致的离散，具体为：

横向越偏离参考路径，离散的目标点越密集，纵向距规划起点越远，离散的目标点越稀疏；

对任务参考路径进行纵向粒度一致的离散得到目标点集，具体为：

沿参考路径的纵向，等间距离散出多个目标点，所有目标点作为自动驾驶特种作业车辆的并道点。

所述的避障决策模式根据自车相对任务参考路径上的障碍物的距离分为由有限状态机实现功能切换的6个子模式，包括自由循迹子模式、规划调整子模式、最优决策子模式、轨迹执行子模式、紧急制动子模式和紧急避障转向子模式；

所述的并道/返回决策模式根据自车相对最优并道/返回点的距离分为由有限状态机实现功能切换的3个子模式，包括避障循迹子模式、并道/返回决策子模式、紧急制动子模式；

所述的并道/返回循迹模式根据自车相对障碍物的距离和所处轨迹片段分为由有限状态机实现功能切换的2个模式，包括循迹子模式和紧急制动子模式。

所述的避障决策模式具体为：

当处在自由循迹子模式时，表明障碍物非常远或者没有障碍物，此时车辆继续执行当前作业轨迹；当处在规划调整模式时，表明当前作业轨迹上存在较近的障碍物，需要调用规划模块，规划避障的基础路径簇，在规划完成后，以路径簇终点偏离当前作业轨迹的横向距离为指标，进行由大到小排序，然后由小到大依次与障碍物进行碰撞检测，第一条不发生碰撞的路径作为候选路径，若都有碰撞，选择终点离作业轨迹横向最远那一条作为候选路径，将候选路径存入序列q1中；当处在最优决策子模式时，调取序列q1中所有的候选路径，若无候选路径，则执行轨迹不改变，若有候选路径，选择候选路径中终点离作业轨迹横向最远的那一条作为候选执行路径，并进行速度规划；当处在轨迹执行子模式时，将候选轨迹调取，并执行；当处在紧急制动子模式时，表明无有效避障的执行轨迹，此时进行紧急制动，以免发生碰撞；当处在紧急制动子模式时，表明紧急制动还是不能避免发生碰撞，只能通过将方向盘打死，尽可能避免碰撞带来的损失。

所述的并道/返回决策模式具体为：

当处在避障循迹子模式时，表明当前还不能返回作业轨迹，需要先平移部分作业轨迹到当前执行轨迹上，以便车辆还有后续参考轨迹可执行，并且不断规划返回作业轨迹的路径簇，按返回作业轨迹的切换点离车辆的距离由近及远排序，依次进行碰撞检测，第一条不碰撞的路径作为候选路径，存入序列q2中；当处在并道/返回决策子模式时，调取序列q2中所有的候选路径，选择候选路径中切换点最近的那一条路径作为候选路径，并进行速度规划，将该轨迹作为并道/返回决策轨迹；当处在紧急制动子模式时，表明无有效的执行轨迹，此时进行紧急制动，以免发生碰撞。

所述的并道/返回循迹模式具体为：

当处在循迹子模式时，执行并道/返回决策模式决策出来的并道/返回决策轨迹；当处在紧急制动子模式时，表明无有效的执行轨迹，此时进行紧急制动，以免发生碰撞。

与现有技术相比，本发明具有如下优点：

一、本发明中作业人员仅需要在作业区域附近打开自动驾驶作业模式开关，系统能够自动调取作业任务与作业路线等信息，规划此后动作，无需人类参与判断；

二、本发明中的决策规划模块在获取定位信息、环境信息、任务模式和任务参考路径后能够完全自动地进行特种作业，将作业过程和行车过程程序化和自动化，提高特种作业效率，降低作业的劳动负荷。

三、本发明提供了一种目标偏向性规划方法，主要包括横纵向粒度不一致和纵向粒度一致的基础轨迹簇规划方法，以及融合任务参考路径和基础轨迹簇的规划策略，提高了避障的成功率，同时增加了快速返回任务参考路径的可能，提升作业效率。

四、本发明使用多模式的轨迹决策策略，通过对最优轨迹的不断检测，以及紧急避障与紧急转向的模式接入，可以实现轨迹对感知环境的不确定性进行兼容，实现自动驾驶的安全性。

附图说明

图1为本发明的方法流程图。

图2为本发明的避障规划的起点和横纵向粒度不一致的目标点离散示意图。

图3为本发明的并道/返回规划的起点和横纵向粒度不一致与纵向粒度一致的目标点离散示意图。

图4为本发明的轨迹组成片段示意图。

图5为本发明的轨迹决策的有限状态机切换示意图。

图6为本发明的避障决策细分的六个子模式示意图。

图7为本发明的并道/返回决策细分的三个子模式示意图。

图8为本发明的并道/返回循迹细分的两个子模式示意图。

具体实施方式

下面结合附图和具体实施例对本发明进行详细说明。注意，以下的实施方式的说明只是实质上的例示，本发明并不意在对其适用物或其用途进行限定，且本发明并不限定于以下的实施方式。

实施例

如图1所示，本发明提供一种自动驾驶特种作业车辆的决策规划方法，具体步骤包括：

步骤1：由驾驶员控制至自动驾驶作业区附近，开启自动驾驶作业模式，自动驾驶作业模块接管整车控制，包括作业控制和行车控制；

步骤2：自动驾驶作业模块通过gps/imu确定自车的当前位姿，主要包括经度、纬度、航向以及当前的定位状态；

步骤3：以自车为中心，将感知系统发送的环境信息投影到一张栅格地图当中，无障碍物的栅格统一标注为0；对于有障碍物的栅格，可以根据速度信息标注为静态或者动态；对于障碍物的类型，同样可以根据感知的识别结果，标注为水泥路沿、灌木等等。

步骤4：根据自车的当前定位位姿，自动驾驶作业模块调用存储在本地或者远程发送的任务文件，决策出当前作业执行器的控制指令，并下发；

步骤5：根据自车的当前定位位姿，自动驾驶作业模块调用存储在本地或者远程发送的任务参考路径；

步骤6：将步骤5获得的任务参考路径投影到步骤3中获得的环境地图，并进行轨迹簇规划，采用路径-速度分解的策略，结合车辆动力学约束，规划出车辆可执行的基础轨迹簇，然后将基础轨迹簇和任务参考路径融合得到可执行轨迹簇，具体实施步骤：

步骤61：采用路径-速度分解的轨迹规划策略，将轨迹规划解耦为路径规划和速度规划两部分。

步骤62：如图2和图3所示，以车辆当前点为规划起点；

步骤63：如图2所示，当车辆在任务参考路径上时，在任务参考路径所在的可行驶区域内，对任务参考路径进行横纵向粒度不一致的离散得到终点目标点集；如图3所示，当车辆在避障执行轨迹上时，在避障执行轨迹和任务参考路径所在的可行驶区域内，对避障执行轨迹进行横纵向粒度不一致的离散和对任务参考路径进行纵向粒度一致的离散得到终点目标点集；

步骤64：结合车辆动力学约束，包括车辆最小转弯半径、最值车速、最值纵向加速度、最值侧向加速度、路面附着系数等等与车辆动力学相关的约束，计算出路径的曲率约束，通过最优化方法，规划出n个离散路径点，并通过三次b样条平滑；

步骤65：将任务参考路径进行裁剪，剩余部分横向偏移到规划的基础路径目标点，并结合车辆动力学约束，包括车辆最小转弯半径、最值车速、最值纵向加速度、最值侧向加速度、路面附着系数等等与车辆动力学相关的约束，计算出路径的曲率约束，通过最优化方法，规划出对应数量的离散速度点，并通过三次b样条平滑；

步骤6：如图4所示，融合任务参考路径和基础轨迹的轨迹簇进行片段划分，来自于任务参考路径的轨迹片段划分为任务参考轨迹(id1)，来自于避障规划的轨迹片段划分为避障轨迹(id2)，来自于任务参考路径平移的轨迹划分为任务参考轨迹平移轨迹(id3)，来自于并道/返回轨迹规划的轨迹片段划分为并道/返回轨迹(id4)；

步骤7：从步骤6规划出的轨迹簇中，考虑环境感知的不确定性，不断地对规划的可执行轨迹簇进行安全性和高效性的择优，最终决策输出一条车辆可执行的高效作业且低碰撞风险的高收益轨迹。如图5所示，轨迹决策分位避障决策、并道/返回决策、并道/返回循迹三大块，其中，如图6所示，避障决策依次是自由循迹模式(s1)、规划调整模式(s2)、最优决策模式(s3)、轨迹执行模式(s4)、紧急制动模式(s5)和紧急避障转向模式(s6)；如图7所示，并道/返回决策依次是避障循迹模式(b1)、并道/返回决策模式(b2)、紧急制动模式(b3)；如图8所示，并道/返回循迹依次是循迹(c1)、紧急制动(c2)，具体实施步骤：

步骤71：沿任务参考路径或当前执行轨迹，以车宽加上一定的横向安全裕度，往前搜索障碍物，若障碍物不存在，执行任务参考路径或当前执行轨迹的循迹，若存在障碍物，按照当前所处模式，分别跳转到步骤72、步骤73或者步骤74。

步骤72：如图5的避障决策状态机所示，若是避障决策，具体步骤为：

步骤721：如图5所示，默认从自由循迹模式(s1)区开始，若无障碍物或障碍物距离落在s1区间，则执行任务参考路径或当前执行轨迹的循迹；若有障碍物，距离落在s2区间(跳转条件e1)，则跳转到规划调整模式(s2)区；若有障碍物，距离落在s5区(跳转条件e5)，则跳转到紧急制动模式(s5)区；若有障碍物，距离落在s6区(跳转条件e6)，则跳转到紧急避障转向模式(s6)区。

步骤722：如图5所示，若车辆与障碍物的距离落在规划调整模式(s2)区，则执行步骤6所述的轨迹簇规划，并继续执行任务参考路径或当前执行轨迹的循迹，在每次规划完成后，按照目标点的横向距离由远及近逐条进行碰撞检测，若无碰轨迹，则最优点best_point_queue队列记为无效，若存在无碰轨迹，则更新best_point_queue队列为最靠近障碍物的那条轨迹序号；若障碍物消失，或障碍物的距离落在s1区(跳转条件e2)，则执行自由循迹模式(s1)；若障碍物的距离落在s3区(跳转条件e3)，则跳转到最优决策模式(s3)区；若有障碍物，距离落在s5区(跳转条件e5)，则跳转到紧急制动模式(s5)区；若有障碍物，距离落在s6区(跳转条件e6)，则跳转到紧急避障转向模式(s6)区。

步骤723：如图5所示，若车辆与障碍物的距离落在最优决策模式(s3)区，则将步骤722队列best_point_queue所有有效点取出，选择偏离横向障碍物最远的那个点作为最优点best_point；若障碍物的距离落在s4区(跳转条件e4)，则跳转到轨迹执行模式(s4)区；若有障碍物，距离落在s5区(跳转条件e5)，则跳转到紧急制动模式(s5)区；若有障碍物，距离落在s6区(跳转条件e6)，则跳转到紧急避障转向模式(s6)区。

步骤724：如图5所示，若车辆与障碍物的距离落在轨迹执行模式(s4)区，以步骤723的best_point为终点，规划出一条完整的轨迹，并以此作为执行轨迹(跳转条件h1)，跳转到并道/返回决策；若有障碍物，距离落在s5区(跳转条件e5)，则跳转到紧急制动模式(s5)区；若有障碍物，距离落在s6区(跳转条件e6)，则跳转到紧急避障转向模式(s6)区。

步骤725：如图5所示，若车辆与障碍物的距离落在紧急制动模式(s5)区，落在该区表明障碍物是突然出现的(动态障碍物)或者障碍物无法规避(障碍物过大，覆盖了作业车可绕行的区间)，为避免碰撞，必须采取紧急制动；若有障碍物，距离落在s6区(跳转条件e6)，则跳转到紧急避障转向模式(s6)区；若无障碍物，或障碍物的距离落在s1～s4区间(跳转条件e7)，则跳转到规划调整模式(s2)区。

步骤726：如图5所示，若车辆与障碍物的距离落在紧急避障转向模式(s6)区，落在该区表明障碍物是突然出现的(动态障碍物，极有可能是人)，紧急制动已难以避免碰撞，为尽可能降低伤害，将方向盘按避障方向打死，紧急避开障碍物；若无障碍物，或障碍物的距离落在s1～s5区间(跳转条件e8)，则跳转到紧急制动模式(s5)区。

步骤73：如图5所示，若是并道/返回决策，具体步骤为：

步骤731：如图5所示，默认进入避障循迹模式(b1)区，执行避障轨迹的循迹，并且不断规划并道/返回的轨迹，更新best_point_queue队列为最先并道/返回的轨迹序号；在若车辆与障碍物的距离落在并道/返回决策模式(b2)区或者无障碍物且车辆到达id3轨迹片段(跳转条件f1)，则跳转到并道/返回决策模式(b2)区；若车辆与障碍物的距离落在b3区(跳转条件f3)，则跳转到紧急制动模式(b3)区。

步骤732：如图5所示，若车辆与障碍物的距离落在并道/返回决策模式(b2)区或者无障碍物且车辆到达id3轨迹片段，将步骤731队列best_point_queue所有有效点取出，选择序号最大的那个点作为最优点best_point，规划出一条完整的轨迹，并以此作为执行轨迹(跳转条件h2)，跳转到并道/返回循迹；若无障碍物且车辆到达id3轨迹片段(跳转条件f2)，则跳转到避障循迹模式(b1)区；若车辆与障碍物的距离落在b3区(跳转条件f3)，则跳转到紧急制动模式(b3)区。

步骤733：如图7所示，若车辆与障碍物的距离落在紧急制动模式(b3)区，落在该区表明障碍物是突然出现的(动态障碍物)或者障碍物无法规避(障碍物过大，覆盖了作业车可绕行的区间)，为避免碰撞，必须采取紧急制动；若无障碍物或者障碍物距离落在b1～b2区(跳转条件f4)，则跳转到避障循迹模式(b1)区。

步骤74：如图5所示，若是并道/返回循迹，具体步骤为：

步骤731：如图5所示默认进入循迹(c1)区，执行并道/返回轨迹的循迹，如车辆当前位置在id1轨迹片段(跳转条件h3)，则跳转到避障决策；若有障碍物且距离落在c2区(跳转条件g1)，则跳转到紧急制动(c2)区。

步骤732：如图5所示，若车辆与障碍物的距离落在紧急制动(c2)区，落在该区表明障碍物是突然出现的(动态障碍物)或者障碍物无法规避(障碍物过大，覆盖了作业车可绕行的区间)，为避免碰撞，必须采取紧急制动；若无障碍物或障碍物距离落在c1区(跳转条件g2)，则跳转到循迹(c1)区。

步骤8：根据执行轨迹与自车当前的位姿，计算出航向角偏差和横向偏差，并下发给运动控制系统。

本发明依次执行获取自车的定位信息；获取自车周围的环境信息；根据自车的定位信息确定作业任务模式；根据自车的定位信息获取任务参考路径；根据作业任务模式确定作业执行器控制指令；结合任务参考路径和自车周围的环境信息，进行轨迹规划，得到可执行轨迹簇；轨迹决策从可执行轨迹簇当中决策出一条满足安全性与高效性的执行轨迹；根据执行轨迹与自车当前的位姿，计算出航向角和横向偏差，并下发给运动控制系统。其中，轨迹规划模块采用路径-速度分解的策略，结合车辆动力学约束，规划出车辆可执行的基础轨迹簇，然后将基础轨迹簇和任务参考路径融合得到可执行轨迹簇；轨迹决策模块考虑规划对环境不确定性的兼容，通过不断地对规划的可执行轨迹簇进行安全性和高效性的择优，最终决策输出一条车辆可执行的高效作业且低碰撞风险的高收益轨迹。

上述实施方式仅为例举，不表示对本发明范围的限定。这些实施方式还能以其它各种方式来实施，且能在不脱离本发明技术思想的范围内作各种省略、置换、变更。