CN111026133A

CN111026133A - 路径规划方法及车辆、计算机可读介质

Info

Publication number: CN111026133A
Application number: CN201911421601.9A
Authority: CN
Inventors: 冯酉南; 张磊
Original assignee: Beijing Yikong Zhijia Technology Co Ltd
Current assignee: Beijing Yikong Zhijia Technology Co Ltd
Priority date: 2019-12-31
Filing date: 2019-12-31
Publication date: 2020-04-17
Anticipated expiration: 2039-12-31
Also published as: CN111026133B

Abstract

本发明公开了一种路径规划方法及车辆、计算机可读介质，应用于车辆的自动驾驶，该路径规划方法包括：获取预设请求数据对应的预处理结果；根据预处理结果获取预设规划数据的粗搜索数据；对粗搜索数据进行路径优化实现路径规划。该方法通过对传统混合A*算法中增加的转向切换惩罚项、转向惩罚项的改进，结合RS曲线启发算法，具备极强的求解能力，提升了计算效率；同时在结合对粗搜索结果的优化过程中保证了规划结果能能够满足车辆的运动学约束，充分考虑了满载与空载不同状态下车辆响应特性的差异，以及矿用自卸车辆的执行机构响应特性，更适应真实应用场景；对工作环境要求低，适应性更强。

Description

路径规划方法及车辆、计算机可读介质

技术领域

本发明涉及自动驾驶技术领域，尤其涉及一种路径规划方法及车辆、计算机可读介质。

背景技术

相较于人类驾驶员，自动驾驶在安全性与高效性方面具有巨大的潜力。尤其在高风险、高强度的矿山作业场景，其场景复杂度低于城市交通场景，无人驾驶技术(即自动驾驶技术)的应用有望减少安全事故同时提升生产效率。无人驾驶技术的主要包括：定位、感知、决策、规划、控制等模块。其中，规划模块直接向控制模块输出行驶路径轨迹，因此，矿用车辆运行状态的好坏与规划模块有着密不可分的联系。

自动驾驶技术已经发展了数十年，针对公路车辆的路径规划技术发展相对成熟，但是针对诸如矿用自卸车等专用矿用车辆的路径规划技术发展相对缓慢。目前针对专用矿用车辆路径规划方面的专利相对较少，现有技术方案都是较早提出的路径规划方法，已无法满足现有复杂路况矿用车辆作业场景和对应现有矿用车辆的路径规划要求，例如，矿区场景大多为采挖之后的裸露土石路面，行驶条件相较铺装路面更差，在路面上铺设预设路线来进行路径导向并不现实，适应性太差；另外，现有技术中通过自动驾驶车辆与装载设备的相对位置计算与预设路径的偏差再根据偏差对预设路线进行调整的方式，其路径规划的适应性非常低，无法满足复杂路况的路径规划要求，例如，复杂路况的车辆工作场景需要根据工作需求随时改变，固定路线或固定路线组合的方式很难覆盖车辆工作场景。最后，现有的路径规划方法很难满足自动驾驶车辆的运动学约束，无法满足自动驾驶的路径规划要求。

发明内容

(一)要解决的技术问题

为解决现有技术已无法满足复杂路况的矿用车辆作业场景和对应矿用车辆的路径规划要求，例如复杂路况的路径规划方法适应性太差、难以满足矿用车辆的运动学约束等技术问题，本发明公开了一种路径规划方法及车辆、计算机可读介质。

(二)技术方案

本发明的一个方面公开了一种路径规划方法，应用于车辆的自动驾驶，获取预设请求数据对应的预处理结果，预设请求数据至少包括：车辆的起始点、目标点、车辆约束以及地形信息；根据预处理结果获取预设规划数据的粗搜索数据，预设规划数据至少包括：车辆对应车辆当前状态和/或车辆目标状态的坐标、航向、档位、前轮转角、前驱状态以及已付代价；对粗搜索数据进行路径优化实现路径规划。

可选地，获取预设请求数据对应的预处理结果，包括：判断起始点和/或目标点是否发生边界干涉；预处理结果包括：起始点和/或目标点没有发生边界干涉。

可选地，获取预设请求数据对应的预处理结果，还包括：当预设请求数据中的起始点和/或目标点发生边界干涉时，对起始点和/或目标点进行位移变换；以及判断位移变换后的起始点和/或目标点是否发生边界干涉；预处理结果还包括：位移变换后的起始点和/或目标点没有发生边界干涉。

可选地，获取预设规划数据的粗搜索数据，包括：获取预设规划数据对应的欧式距离；根据欧式距离获取粗搜索数据对应的启发值。

可选地，获取预设规划数据的粗搜索数据，还包括：获取预设规划数据对应的路径惩罚项，路径惩罚项至少包括：档位切换惩罚项、转向惩罚项以及转向切换惩罚项；根据路径惩罚项获取粗搜索数据对应的已付代价。

可选地，获取预设规划数据的粗搜索数据，还包括：根据启发值和已付代价获取粗搜索数据对应的目标函数；根据目标函数和RS曲线启发算法确定粗搜索数据。

可选地，对粗搜索数据进行路径优化，包括：根据车辆的约束条件对应的优化函数对粗搜索数据进行路径优化；以及获取路径优化结果。

可选地，约束条件至少包括：前轮转角变化率约束、前轮转角约束、离散坐标点间隙约束以及边界约束。

可选地，预设规划数据中的航向包括正航向或负航向。

本发明的一个方面公开了一种车辆，应用上述的路径规划方法，实现车辆的自动驾驶。

本发明的一个方面公开了一种计算机可读介质，包括存储器与处理器，存储器存储有可执行指令，该指令被处理器执行时实现上述的路径规划方法。

(三)有益效果

本发明公开了一种路径规划方法及车辆、计算机可读介质，该路径规划方法包括：获取预设请求数据对应的预处理结果，根据预处理结果获取预设规划数据的粗搜索数据，对粗搜索数据进行路径优化实现路径规划。该方法适用于诸如矿用自卸车辆等矿用车辆的路径规划方法，通过对传统混合A*算法中增加的转向切换惩罚项、转向惩罚项的改进，结合RS曲线启发算法，具备极强的求解能力，对计算过程进行了合理的简化与等效处理，提升了计算效率；同时在结合对粗搜索结果的优化过程中保证了规划结果能能够满足车辆的运动学约束，充分考虑了满载与空载不同状态下车辆响应特性的差异，以及矿用自卸车辆的执行机构响应特性，更适应真实应用场景；与现有矿用车辆的路径规划方法相比运算更加灵活，求解能力更强，而且不需要如“预铺设地磁线”或“预设路径”等操作，对工作环境要求低，适应性更强。

附图说明

图1是本发明一实施例中路径规划方法的流程示意图；

图2是本发明一实施例中起始点和目标点发生边界干涉的示意图；

图3是本发明一实施例中起始点和目标点位移变换示意图；

图4是本发明一实施例中RS曲线启发过程中车辆最大前轮转角的示意图；

图5是本发明一实施例中路径规划方法的参考路径和优化路径对比示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

自动驾驶车辆(Automatic Driving Vehicle，简称ADV)是可以能够自行定位，通过传感器感知周围环境，依靠计算单元做出自主决策并行驶的车辆。在本发明中的自动驾驶车辆可以是矿用自卸车辆，矿用自卸车辆可以搭载矿货进行定点运输。例如，对于搭载自动驾驶系统的矿用自卸车辆，其工作模式可以是：空载状态前往装载工作区的装载点装载土石，满载状态下将土石运往卸载工作区，在卸载工作区的卸载点将土石倾倒，离开装载工作区前往装载工作区如此往复。上述过程都需要规划出合理的路径。

本发明的一个方面公开了一种路径规划方法，如图1所示，应用于车辆的自动驾驶，路径规划方法包括如下操作：

S110、获取预设请求数据对应的预处理结果，预设请求数据至少包括：车辆的起始点、目标点、车辆约束以及地形信息；

预处理过程是对后续是否能够成功获取粗搜索数据的一个预判，该预判过程可以根据预设请求数据获取预处理结果，该预处理结果可以是判断互为相反两种数据，例如输出预处理结果为正，或输出预处理结果为负，或者体现为不同的两种数值，例如输出预处理结果为1，或输出预处理结果为0。对应不同的预处理结果，可以决定是否进行后续的路径规划方法流程，例如输出预处理结果为1或正，则决定继续进行后续的路径规划方法流程，否则，则放弃后续规划，即输出路径规划失败。在本发明实施例中，为体现后续的路径规划流程，输出预处理结果可以是正值或1值，以确保后续的粗搜索路径计算的执行。

S120、根据预处理结果获取预设规划数据的粗搜索数据，预设规划数据至少包括：车辆对应车辆当前状态和/或车辆目标状态的坐标、航向、档位、前轮转角、前驱状态以及已付代价；

在本发明的实施例中，在输出预处理结果可以是正值或1值时，决定后续依据预设规划数据的在粗搜索计算，以获取粗搜索数据。具体的粗搜索计算，可以采用本发明的改进型的混合A*算法进行计算。以确保路径搜索的高效性和准确性。

S130、对粗搜索数据进行路径优化实现路径规划。

由于矿用自卸车辆相对于传统车辆的一些特定的缺陷，在完成粗搜索计算之后，需要对获取的粗搜索数据作进一步的优化，以最终输出的优化路径数据作为本发明的路径规划结果。

基于上述方法，通过对传统混合A*算法中增加的转向切换惩罚项、转向惩罚项的改进，结合RS曲线启发算法，具备极强的求解能力，对计算过程进行了合理的简化与等效处理，提升了计算效率；同时在结合对粗搜索结果的优化过程中保证了规划结果能能够满足车辆的运动学约束，充分考虑了满载与空载不同状态下车辆响应特性的差异，以及矿用自卸车辆的执行机构响应特性，更适应真实应用场景；与现有矿用车辆的路径规划方法相比运算更加灵活，求解能力更强，而且不需要如“预铺设地磁线”或“预设路径”等操作，对工作环境要求低，适应性更强。

根据本发明的实施例，获取预设请求数据对应的预处理结果，包括：判断起始点和/或目标点是否发生边界干涉；预处理结果包括：起始点和/或目标点没有发生边界干涉。在本发明实施例中，该预设请求数据为后续预处理过程中采用的数据，可以包括车辆的起始点/目标点以及可行驶区域边界(通过车辆约束等其他数据限定)。其中，该判断起始点和/或目标点是否发生边界干涉，是判断该起始点和/或目标点所对应的车辆的车体包络框进行的，即该车辆的车体包络框是否发生边界干涉。

该预处理结果可以是判断互为相反两种数据，该预处理结果可以是根据判断起始点和/或目标点是否发生边界干涉来获得，例如起始点和/或目标点没有发生边界干涉，输出预处理结果为正，或起始点和/或目标点发生边界干涉，输出预处理结果为负，或者体现为不同的两种数值，例如起始点和/或目标点没有发生边界干涉，输出预处理结果为1，或起始点和/或目标点发生边界干涉，输出预处理结果为0。对应不同的预处理结果，可以决定是否进行后续的路径规划方法流程，例如输出预处理结果为1或正，则决定继续进行后续的路径规划方法流程，否则，则放弃后续规划，即输出路径规划失败。在本发明实施例中，为体现后续的路径规划流程，输出预处理结果可以是正值或1值，即预处理结果包括：起始点和/或目标点没有发生边界干涉。以确保后续的粗搜索路径计算的执行。

需要说明的是，由于工作环境的硬性限制，矿用车辆需要在工作区内进行工作，在本发明实施例中，工作区Work Area是由土石铺设的开放区域，例如，工作区可细分为装载土石的装载工作区以及倾倒土石的卸载工作区，工作区边缘有高度差时，边坡边缘可以具有高约80cm的土质挡墙，以上两种工作区在本文中统称为工作区。无论是在装载工作区还是在卸载工作区，均需要车辆的外框跨在边界上进行作业(如装载/卸载作业)，如图2所示，浅灰色的矩形框为车体，相当于该车体包络框，黑色实心圆点为车辆的后轴中心，车头方向如箭头所示，粗线即工作区的边界，此时可见车尾的部分是处于边界之外的。在本发明实施例中，车辆不得触碰边界，即车辆不得与工作区边界发生干涉，如图2所示，此时可见车尾的部分是处于边界之外，即车辆发生了边界干涉。但是，在路径规划过程中边界约束是硬约束，所以需要对已发生边界干涉的车辆预设请求数据进行调整，以避免该种情况发生。

根据本发明的实施例，获取预设请求数据对应的预处理结果，还包括：当预设请求数据中的起始点和/或目标点发生边界干涉时，对起始点和/或目标点进行位移变换；以及判断位移变换后的起始点和/或目标点是否发生边界干涉；预处理结果还包括：位移变换后的起始点和/或目标点没有发生边界干涉。

为了解决此矛盾，需要对起始点/目标点进行位移变换，以克服其边界干涉的缺陷。基于预设请求数据，例如起始点、目标点、边界约束以及地形信息对该发生边界干涉的车辆进行起始点/目标点进行位移变换。起始点表示待运行车辆当前的姿态，目标点表示车辆欲停止的姿态。边界约束是自动驾驶车辆在规划过程中不可触碰的区域，而且该区域往往为工作区的边界。地形信息以栅格的形式表征车辆通过某一位置时所应付出的代价值，地面越起伏，坡度越大，所付出的代价值越高，反之付出的代价值越低。

如图3所示，当起始点/目标点与边界发生干涉时，将起始点/目标点(黑色实心圆点)朝前进方向平移一个距离D，该距离根据实际情况而定，使车辆刚好与边界不发生干涉为最佳状态。规划结果在输出时自行将平移路段L0进行插补。该步骤对起始点/目标点进行位移，使起始点/目标点不与边界发生碰撞，即经过位移变换之后，预处理结果需要包括：位移变换后的起始点和/或目标点没有发生边界干涉。

根据本发明的实施例，当起始点/目标点并非工作目标位的时候不需要进行对其进行平移变换。起始点/目标点平移变换过程仅针对作业过程中不得不跨越边界的情况，平移距离D是一个定值，一般不超过1/2车身长度，若平移距离过长，将无法保证安全性。经过平移变换后，仍需要对起始点与目标点进行初步的判定，如果起始点/目标点任一点与边界发生干涉，预处理结果为位移变换后的起始点和/或目标点发生边界干涉，后续的路径规划过程也将不再进行，即路径规划失败。

根据本发明的实施例，在预处理结果为：起始点和/或目标点没有发生边界干涉或经过位移变换后的起始点和/或目标点没有发生边界干涉时，本发明依据改进型的混合A*算法进行路径规划的粗搜索计算，以获取一初步的规划路径，即粗搜索数据。混合A*算法(Hybrid A*)是一种考虑车辆运动方向的路径搜索算法。本领域技术人员应当理解的是，混合A*算法的具体过程本文不再赘述，本发明只对改进部分进行说明。

根据本发明的实施例，将预设规划数据(车辆对应车辆当前状态和/或车辆目标状态的坐标、航向、档位、前轮转角、前驱状态以及已付代价、车辆姿态等)传入规划器，规划器为配置于车辆的一数据处理设备。车辆姿态(Vehicle Status)指车辆后轴中心处的投影坐标，以及后轴中心到车头连线在投影坐标系下的朝向，车辆姿态包括：当前车辆姿态与目标车辆姿态。

根据本公开的实施例，在本发明的改进混合A*算法中，其目标函数不仅考虑了车辆的运行距离，在车辆的前进/后退档切换时增加了惩罚项，还考虑了转向惩罚和转向切换惩罚。

在改进的混合A*算法中，增加一改进目标函数，以实现对路径粗搜索过程中各个路径离散点的获取，具体可以定义一起始点或目标点，在起始点或目标点的基础上定义至少2个路径方向，本发明中可以定义三个路径方向，在每个路径方向上通过预设的离散点搜索关系，以找到每个适用于本发明路径规划的离散点。该目标函数以用于对获取的路径离散点进行是否可用于本发明待规划路径的判断，具体而言，当该离散点对应的各项信息与该目标函数的特征参数数值匹配，则该离散点可用于待规划路径。

在获取目标函数的过程中，首先需要对当前车辆状态的预设规划数据进行定义：

设当前车辆状态为X，其包含车辆当前的坐标、航向、档位、前轮转角、前驱状态，已付代价等信息。具体地，在本发明的实施例中，当前车辆状态的预设规划数据包括：

(x,y)为车辆后轴中心在投影坐标系下的坐标；θ为车辆后轴中心到车头连线在投影坐标系下的航向；R为车辆档位信息，表示车辆的前进与后退；φ为车辆的前轮与车辆中轴所呈夹角，即前轮转角；X_pre该状态的前驱状态；G为已付代价。

其中，投影坐标系(Projection Coordination)为取某固定点为坐标原点，东为x轴正方向，北为y轴正方向，向上为z轴正方向的坐标系。其与经纬度一一对应，且对应关系唯一。

现以上述预设规划数据的具体参量作为本发明粗搜索数据的获取依据，对本发明的目标函数的获取以及对应粗搜索路径的粗搜索数据的获取进行描述。本领域技术人员应当理解，上述预设规划数据的具体参量并非是对本发明保护范围的限定。

根据本发明的实施例，获取预设规划数据的粗搜索数据，包括：获取预设规划数据对应的欧式距离；根据欧式距离获取粗搜索数据对应的启发值。在获取到对应的预设规划数据之后，需要依据预设规划数据采用本发明的改进型混合A*算法实现路径搜索。

D(X,X₁)为当前车辆状态X到目标车辆状态X₁的欧式距离，可以直接依据当前车辆状态X到目标车辆状态X₁对应的预设规划数据中车辆的投影坐标系的坐标等数据来获取。

粗搜索数据对应的当前车辆状态X的启发值，如公式(1)：

H(X)＝D(X,X₁)

其中，启发值为当前车辆状态下的车辆的未付代价。

根据本发明的实施例，获取预设规划数据的粗搜索数据，还包括：获取预设规划数据对应的路径惩罚项，路径惩罚项至少包括：档位切换惩罚项、转向惩罚项以及转向切换惩罚项。

在本发明的实施例中，前进/后退档位切换惩罚项P_c(X)用于在当前车辆状态下前进/后退方向发生变化的情况下，增加档位切换惩罚项值，档位切换惩罚值的大小取决于系数ω₁，即前进/后退档位切换惩罚项P_c(X)，如公式(2)：

其中：R(X),R(X_pre)分别表示当前车辆状态X以及车辆前驱状态X_pre的前进/后退档位。

在本发明的实施例中，转向惩罚项P_s(X)用于尽可能减少车辆转向的次数，避免车辆进行大幅度的转向，即转向惩罚项P_s(X)，如公式(3)：

P_s(S)＝Φ(X)²

其中：Φ(X)状态X的前轮转角。采用二次函数计算可以使得车辆在小幅度转向的情况下惩罚值尽可能的小，随着转向角度的增加，惩罚值随之上升，最终的转向惩罚项P_s(X)惩罚值大小受系数ω₂的影响。

在本发明的实施例中，左/右转向切换惩罚项P_t(X)用于减少左右转向的切换，避免出现“波浪”形的路径，即左/右转向切换惩罚项P_t(X)，如公式(4)：

其中：Φ(X),Φ(X_pre)为分别表示状态X以及其前驱状态X_pre的前轮转角，左/右转向切换惩罚项P_t(X)惩罚值的大小取决于系数ω₃。

基于上述档位切换惩罚项、转向惩罚项以及转向切换惩罚项，本发明考虑了矿用自卸车辆的执行机构响应特性，更适应真实应用场景，较早期提出的矿用自卸车辆的路径规划方法更加灵活，求解能力更强。

根据本发明的实施例，根据路径惩罚项获取粗搜索数据对应的已付代价。

对于状态X的已付代价G(X)，如公式(5)：

G(X)＝G(X_pre)+ω₁P_c(X)+ω₂P_s(X)+ω₃P_t(X)

其中：ω₁,ω₂,ω₃为常量系数；X_pre为车辆前驱状态；X₁为目标车辆状态；P_c(X)为前进/后退档位切换惩罚项；P_s(X)为转向惩罚项；P_t(X)为左/右转向切换惩罚项。

根据本发明的实施例，获取预设规划数据的粗搜索数据，还包括：根据启发值和已付代价获取粗搜索数据对应的目标函数；根据目标函数和RS曲线启发算法确定粗搜索数据。

根据本发明的实施例，在获取了已付代价G(X)和启发值H(X)(未付代价)，便可以基于该两值对粗搜索过程的目标函数进行定义。

设起始状态为X₀，目标状态为X₁。则起始状态X₀的已付代价G(X₀)＝0，G(X₀)是在粗搜索过程中的已付代价。

在车辆前驱状态为空的情况下，当前车辆状态X对应的的目标函数F(X)，如公式(6)：

F(X)＝G(X)+H(X)

其中：G(X)为状态X的已付代价；F(X)为状态X的启发值。

即对应粗搜索路径的搜索过程中每个路径离散点需要满足上述公式(6)的目标函数，该粗搜索数据对应的目标函数＝已付代价+未付代价。因此，目标函数实际上是当前车辆状态到目标车辆状态路径上的总代价，或可以用于预估该过程中的总代价。在粗搜索过程中，需要在每次粗搜索计算中，选择预估总代价最小的离散点继续搜索，以提升搜索效率。

由于上述的粗搜索过程需要逐个离散点进行粗搜索匹配，因此粗搜索精度较高，但是粗搜索效率比较低。为了进一步提高粗搜索的效率，对对应的每一个车辆状态都进行RS曲线的启发。RS曲线(Reeds-Shepp CurveReeds-Shepp)是一种考虑车辆前进后退的，由若干半径固定的圆弧和直线段拼接组成，而且圆弧的半径就是车辆的最小转向半径。RS曲线启发算法是针对该曲线的计算，该曲线的计算过程仅考虑粗搜索的起始点，无需考虑粗搜索过程中的搜索障碍，因此，计算效率非常高，但在具有搜索障碍的路径规划过程中，计算精度有限。

根据本发明的实施例，RS曲线启发计算涉及对车辆最大转向曲率的计算。如图4所示，灰色矩形框为自动驾驶车辆，在车辆进行转弯动作时，设车辆最大前轮转角为φ_max，车辆轴距L。理论车辆最大转向曲率为κ_max，如公式(7)：

车辆最大转向曲率为κ_max为理论上的RS曲线表征参量，为更加符合车辆的实际应用场景(实际车辆运行过程中车辆一般不会达到最大转向曲率)，引入RS曲线松弛因子

对应实际的RS曲线，车辆实际转向曲率κ′_max，如公式(8)：

其中：

为区间(0,1)间的常数。对于本发明的车辆而言，例如矿用自卸车辆，由于空载与满载状态下车辆的动力学特性不同，一般情况下满载状态下的松弛因子小于空载状态下的松弛系数。具体松弛因子

的取值需要根据实际车辆参数进行调整。RS曲线启发算法不仅提高了粗搜索算法的搜索效率，还充分考虑了满载与空载不同状态下车辆响应特性的差异，考虑了矿用自卸卡车的执行机构响应特性，更适应真实应用场景。

根据本发明的实施例，依据前述的预设规划数据，设当前车辆状态为X，车辆边界约束为O。基于上述RS曲线启发，当前车辆状态X到目标车辆状态X₁的RS曲线路径为T_rs(X,X₁)。如果RS曲线路径满足车辆在RS曲线路径计算过程中不会发生边界干涉，即

可以获取将起始车辆状态X₀到当前车辆状态X的路径T(X₀,X)与T_rs(X,X₁)进行拼接，得到粗搜索结果T(X₀,X₁)，如公式(9)：

此时，对应的粗搜索结果T(X₀,X₁)即对应的改进型混合A*算法经过粗搜索计算得到的粗搜索数据，该粗搜索数据考虑了矿用自卸卡车的执行机构响应特性，更适应真实应用场景，实现了混合A*算法与RS曲线启发的结合，对计算过程进行了合理的简化与等效处理，提升了计算效率，较早期提出的矿用自卸卡车的路径规划方法更加灵活，求解能力更强，可以实现对路径的直接搜索，对工作环境要求低，不需要如“铺设地磁线”或“预设路径”等操作。

需要说明的是，若经过改进型的混合A*算法和RS曲线启发算法的结合，仍然无法获得有效的粗搜索数据，则对应于粗搜索路径规划失败。

考虑到本发明车辆所适用的为矿用自卸车辆及其相应的实际工作环境，通过上述粗搜索等到的路径T(X₀,X₁)并不适宜直接用作为最终结果进行输出。例如，实际车辆对实际最大转角曲率的适应要求：上述改进混合A*算法中用到的RS曲线是由直线和曲率相同的圆弧构成的，且圆弧的曲率都为最大曲率，即所得结果的曲率是突变的。对于装配了自动驾驶系统的矿用自卸车辆而言，其控制执行机构并不能适应突变的曲率。另外，实际车辆对于最小转弯半径的要求，例如，在本发明中，当车辆为矿用自卸车辆，则该车辆的轮胎侧偏刚度较大，即使满打转向盘，车辆的转弯半径也会比较大。加之工作区路面为土石铺设路面，所能提供的摩擦力有限，车辆很难达到最小转弯半径。最后，实际车辆会出现空载与满载两种状态的不同动力学特性，车辆控制器的响应速度也不同。相较于空载状态满载状态下车辆的响应速度更慢，所能达到的控制极限更小。因此，需要对上述的粗搜索数据作进一步的优化，以符合车辆在实际工作场景中的应用。

根据本发明的实施例，对粗搜索数据进行路径优化，包括：根据车辆的约束条件对应的优化函数对粗搜索数据进行路径优化；以及获取路径优化结果。通过对粗搜索数据传输给车辆的优化器进行优化，以使得最终路径规划结果更适应本发明车辆的实际约束。具体地，设由对路径粗搜索得到的粗搜索数据为一系列离散坐标点的有序集合，即上述公式(9)的T(X₀,X₁)可以以如下公式(10)中T进行表达：

T＝{∑T_i|i＝1,2,3,...,k,k≥1}

{T_i＝{∑x_j|j＝1,2,3,...,m,m≥2}

R(T_i)≠R(T_i+1)

其中，x_j为投影坐标系下的坐标点；T_i为粗搜索数据中对应有序离散坐标点组成的路径段；R(T_i)为路径段T_i的运动方向。

上述公式(10)，即表征了路径由多个路径段组成，在路径段中，车辆的运动方向一致，即前进或后退，不能同时出现两种前进方向，例如侧方停车的“之”字型路径，先前进，再倒退，之后再前进，则该路径分为三个路径段，每个路径段之间存在一定顺序，相邻路径段之间的前进方向不一致。

根据本发明的实施例，粗搜索的结果为一段或多段路径段的集合，每个路径段都由一系列离散坐标点有序排列组成，每相邻两路径段的方向是相反的。在本发明的实施例中，针对单一路径段进行设计，设参考路径段(即粗搜索数据或起始路径段)为T_k，粗搜索数据对应的路径优化结果为

即优化函数，则有如下公式(11)：

T_k＝{∑x_ki|i＝1,2,3...,m,m≥2}

其中：X_k为起始路径段T_k时车辆的初始状态；Opt(T_k,X_k)为以T_k为参考路径段、X_k为车辆初始状态的优化函数。

则对应于T_k和X_k的粗搜索路径优化函数Opt(T_k,X_k)，如公式(12)：

其中：

为参考坐标x_kj所应的优化坐标

所应满足的约束函数，即在粗搜索数据中对应的路径段进行优化时，每个路径段需要在满足该约束函数时，才可以被优化函数所优化，以尽可能地将粗搜索数据的最终优化结果更加精准，使得最终的优化结果符合车辆的实际执行需要，同时提高了优化计算效率。另外，为了上述公式(11)或公式(12)在数学上的美观，每个优化坐标

只取一个与之最靠近的参考坐标x_kj进行计算，而非针对所有参考坐标点(对应于粗搜索数据中的离散坐标点)进行计算。

根据本发明的实施例，约束条件至少包括：前轮转角变化率约束、前轮转角约束、离散坐标点间隙约束以及边界约束。约束条件用以作为上述约束函数的预设条件，即约束函数需要在满足上述约束条件的情况下，对粗搜索数据中的每个路径段提供更加精准的优化筛选，用以保证粗搜索路径的优化过程更加精细，优化筛选的路径段更加符合实际需要。

在本发明的实施例中，相应地，约束函数满足如下公式(13)：

相应地，约束条件中前轮转角变化率约束、前轮转角约束、离散坐标点间隙约束以及边界约束需要分别满足如下公式：

前轮转角变化率约束，如公式(14)：

前轮转角约束，如公式(15)：

离散坐标点间隙约束，如公式(16)：

边界约束，如公式(17)：

其中，在上述约束条件中，

为优化坐标与参考坐标状态下的航向差，满足如下公式(18)：

为优化坐标

与参考坐标x_kj的欧式距离；Δφ_max为车辆最大允许车辆前轮转角变化率；φ_max为最大允许车辆前轮转角；Δd为优化结果坐标点间隙，为一常数；

为

处车辆标记距离，满足如下公式(19)：

为

处车辆前轮转角；

为

处车辆航向；

为

处车辆包络框；O为障碍物，即边界约束。

上述约束条件的相关公式，在粗搜索数据的优化过程中，取起始车辆状态为参考路径段(即粗搜索数据)的初始状态，通过初始状态递推，将优化状态

向前递推一个微小距离Δd得到

则

与

必须满足三个条件：第一，从

到

的过程，车辆的前轮转角

的变化率必须小于最大允许前轮转角变化率Δφ_max。第二，在递推过程中，每一个

其车辆前轮转角

必须小于最大允许车辆前轮转角φ_max。第三，在递推过程中，对于每一个

车辆与障碍物(或边界约束)O不能发生边界干涉。因此，本发明的规划方法，基于上述约束函数及相关约束条件，考虑了矿用自卸卡车的执行机构响应特性，更适应真实应用场景

本发明将优化结果的离散点的间距进行了固定，且假设车辆是均速前进。因此，最大允许车辆前轮转角变化率Δφ_max便排除了时间因素，即可将其设为一个常数值。可大幅提降低求解难度，并提高计算效率。

在此基础上，计算以车辆优化状态与车辆参考状态的角度差，距离差以及到障碍物的标记距离的和作为约束函数。通过不等式求解器即可求解优化函数，得到最终的粗搜索数据的路径优化结果

最后，对上述路径优化结果进行

求和，可以得到最终的粗搜索路径，如公式(20)：

通过上述公式(20)，本发明的路径规划得以实现，车辆可以采用上述公式(20)所定义的路径进行实际执行。另外，由于空载与满载状态下，车辆的动态响应特性不同，所以针对空载与满载状态，最大允许前轮转角变化率Δφ_max以及最大允许前轮转角φ_max取不同的值。一般情况下，满载状态下的最大允许前轮转角变化率以及最大允许前轮转角均小于空载状态。如图5所示，可见，经过优化后的路径优化结果(即优化路径)与未经优化的粗搜索数据(即参考路径)相比，路径曲线更加平缓，转弯幅度更小，符合车辆的实际转向等行驶需要。因此，本发明的路径规划方法充分考虑了满载与空载不同状态下车辆响应特性的差异，考虑了矿用自卸卡车的执行机构响应特性，更适应真实应用场景。

根据本发明的实施例，预设规划数据中的航向包括正航向或负航向。在本发明实施例中，上述所记载的路径规划过程，主要是针对的车辆前进方向的计算。对于车辆需要进行倒车行驶的情况，将车辆航向取为上述定义的真实值的反向，同时将车辆前轮转角变换符号即可转化为车辆前进过程，如公式(21)：

其中，θ′为变换后的车辆航向；φ′为变换后的车辆前轮转角。采用上述公式(21)可以实现对倒车行驶的路径规划，在此不再赘述。因此，本发明是实现了对计算过程进行了合理的简化与等效处理，提升了计算效率。

本发明的一个方面公开了一种车辆，应用上述的路径规划方法，实现车辆的自动驾驶。基于上述路径规划方法，该车辆将具有更强的路径规划计算效率，路径规划更加准确；另外该车辆可以适应满载或空载等不同状态下车辆相应特性的差异，更加适应矿区的真实应用场景，同时具备更强的道路适应性，可以很好的适应矿区、山路等路况的自动驾驶。

本发明的另一个方面公开了一种计算机可读介质，包括存储器与处理器，存储器存储有可执行指令，该指令被处理器执行时实现上述的路径规划方法。该计算机可读存储介质可以是上述实施例中描述的设备/装置/系统中所包含的；也可以是单独存在，而未装配入该设备/装置/系统中。上述计算机可读存储介质承载有一个或者多个程序，当上述一个或者多个程序被执行时，实现根据本公开实施例的路径规划方法。

根据本公开的实施例，计算机可读存储介质可以是非易失性的计算机可读存储介质，例如可以包括但不限于：便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。例如，根据本公开的实施例，计算机可读存储介质可以包括上文描述的ROM和/或RAM和/或ROM和RAM以外的一个或多个存储器。

附图中的流程图和框图，图示了按照本公开各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims

1.一种路径规划方法，应用于车辆的自动驾驶，其特征在于，

获取预设请求数据对应的预处理结果，预设请求数据至少包括：所述车辆的起始点、目标点、车辆约束以及地形信息；

根据所述预处理结果获取预设规划数据的粗搜索数据，所述预设规划数据至少包括：所述车辆对应车辆当前状态和/或车辆目标状态的坐标、航向、档位、前轮转角、前驱状态以及已付代价；

对所述粗搜索数据进行路径优化实现所述路径规划。

2.根据权利要求1所述的路径规划方法，其特征在于，所述获取预设请求数据对应的预处理结果，包括：

判断所述起始点和/或目标点是否发生边界干涉；

所述预处理结果包括：所述起始点和/或目标点没有发生边界干涉。

3.根据权利要求2所述的路径规划方法，其特征在于，所述获取预设请求数据对应的预处理结果，还包括：

当所述预设请求数据中的起始点和/或目标点发生边界干涉时，对所述起始点和/或目标点进行位移变换；以及

判断所述位移变换后的起始点和/或目标点是否发生边界干涉；

所述预处理结果还包括：所述位移变换后的所述起始点和/或目标点没有发生边界干涉。

4.根据权利要求1所述的路径规划方法，其特征在于，所述获取预设规划数据的粗搜索数据，包括：

获取所述预设规划数据对应的欧式距离；

根据所述欧式距离获取所述粗搜索数据对应的启发值。

5.根据权利要求4所述的路径规划方法，其特征在于，所述获取预设规划数据的粗搜索数据，还包括：

获取所述预设规划数据对应的路径惩罚项，所述路径惩罚项至少包括：档位切换惩罚项、转向惩罚项以及转向切换惩罚项；

根据所述路径惩罚项获取所述粗搜索数据对应的已付代价。

6.根据权利要求5所述的路径规划方法，其特征在于，所述获取预设规划数据的粗搜索数据，还包括：

根据所述启发值和所述已付代价获取所述粗搜索数据对应的目标函数；

根据所述目标函数和RS曲线启发算法确定所述粗搜索数据。

7.根据权利要求1所述的路径规划方法，其特征在于，所述对所述粗搜索数据进行路径优化，包括：

根据车辆的约束条件对应的优化函数对所述粗搜索数据进行路径优化；以及

获取所述路径优化结果。

8.根据权利要求7所述的路径规划方法，其特征在于，所述约束条件至少包括：前轮转角变化率约束、前轮转角约束、离散坐标点间隙约束以及边界约束。

9.根据权利要求1所述的路径规划方法，其特征在于，所述预设规划数据中的航向包括正航向或负航向。

10.一种车辆，应用权利要求1-9中任一项所述的路径规划方法，实现所述车辆的自动驾驶。

11.一种计算机可读介质，包括存储器与处理器，所述存储器存储有可执行指令，该指令被所述处理器执行时实现根据权利要求1～9中任意一项所述的路径规划方法。