CN115837904A - 一种用于矿区四轮转向车辆的泊车规划方法 - Google Patents

Abstract

本发明属于矿区无人驾驶泊车规划技术领域，具体公开了一种用于矿区四轮转向车辆的泊车规划方法，根据驶入点的初始位置和初始朝向选取合适的停靠位姿，停靠位姿的参数包括车辆位置和车辆朝向，并选取具有相同车辆位置和不同车辆朝向的两种停靠位姿；根据不同驶入点以及不同曲率半径，按照前轮转向、后轮转向和四轮转向的三种倒车模式使用reeds‑sheep曲线生成多条路径作为备选集合；建立评价函数用于计算车辆采用每个泊车路径的代价值，选取备选集合中代价值最低的最优路径；以便车辆更够根据初始位置和最终位置进行选择，本发明的泊车规划方法避免“倒车切换路径”出现；充分发挥了四轮转向车辆的动力学性能。

Description

一种用于矿区四轮转向车辆的泊车规划方法

技术领域

本发明涉及矿区无人驾驶泊车规划技术领域，具体而言，涉及一种用于矿区四轮转向车辆的泊车规划方法。

背景技术

在矿区中，矿车一般以较低速度泊车至装卸载区域中的指定位置。矿区泊车场景往往较为狭窄且复杂，因此对矿车的行驶路径以及泊车位姿都有一定要求。

专利CN114812580A《矿区无人驾驶车辆的全局路径规划方法以及系统》提出了一种基于曲线拟合并筛选的全局路径规划方法，可以实现在装卸载区域的泊车规划。

专利CN112572416A《一种矿区无人驾驶车辆的泊车方法及系统》提出了一种针对于前轮转向矿卡的泊车路径规划方法。

目前现有的矿区泊车算法一般使用前轮转向矿卡的运动学模型进行曲线拟合来完成路径规划，未考虑到四轮转向车辆的转向特性，无法充分发挥其动力学性能，为此提出一种用于矿区四轮转向车辆的泊车规划方法，以解决上述提出的问题。

发明内容

本发明的目的是提出一种可以较好地发挥四轮转向车辆运动学特性的矿区无人驾驶车辆的泊车方法。该泊车方法可以在保证车辆行驶平顺性的前提下，避免与泊车区域中障碍物发生碰撞，发挥四轮转向车辆运动学特性，支持在狭窄区域内矿车的泊车停靠功能，提高矿区运输效率。

有鉴于此，本发明的第一方面在于提供一种用于矿区四轮转向车辆的泊车规划方法。

本发明的第一方面提供了一种用于矿区四轮转向车辆的泊车规划方法，包括如下步骤：S1，根据驶入点的初始位置和初始朝向选取两种停靠位姿，停靠位姿的参数包括车辆位置和车辆朝向，所述两种停靠位姿具有相同车辆位置和不同车辆朝向；S2，根据驶入点以及曲率半径，按照前轮转向、后轮转向和四轮转向的三种倒车模式使用reeds-sheep曲线生成多条泊车路径作为备选集合；S3，建立评价函数用于计算车辆采用备选集合中每条泊车路径的代价值，选取备选集合中代价值最低的为最优路径；S4，采用螺旋建模方法对S3中得出的最优路径进行可行驶区域搜索，并判断是否出现狭窄区域，若是则在备选集合中删除此次选取的最优路径并返回S3，若不是则下一步。S5，建立二次优化模型并使用序列二次优化算法(SQP)对无狭窄区域的最优路径进行平滑操作，并进行输出，得到最终泊车路径。

本发明提供的一种用于矿区四轮转向车辆的泊车规划方法，对四轮转向矿卡的运动学特性，选取两种停靠位姿进行泊车规划，提供能够选择的停靠位姿，以便车辆更够根据初始位置和最终位置进行选择，可以避免“倒车切换路径”出现，并且若仅使用车辆朝向进行路径规划，所获得的路径有可能存在“倒车切换路径”，并非四轮转向车辆的最优路径；

在前轮转向、后轮转向以及四轮转向的三种倒车模式下使用reeds-sheep曲线以及可变曲率半径进行路径拟合获得泊车路径集合，充分发挥四轮转向车辆的动力学性能，并获取到足够多的泊车路径，以使后续步骤中能够进行选择，获得最佳的路径；

通过路径评价函数，对所获取的泊车路径进行择优选取，可获得四轮转向矿卡的最低代价路径，并采用螺旋建模方法，可以有效地搜索矿卡在矿区场景下的可行驶区域并实现避障效果，并为后续的路径平滑问题提供了搜索区域，并在其中判断是否具有狭窄区域，以避免最低代价路径难以被车辆在实际的泊车中难以实施或者产生碰撞；

建立二次优化问题对路径进行平滑处理，可以获得便于矿卡预瞄跟踪控制的最优路径，使用序列二次优化(SQP)方法求解二次优化问题，降低优化问题求解难度并提高求解质量。

另外，根据本发明的实施例提供的技术方案还可以具有如下附加技术特征：

上述任一技术方案中，所述S1中的两种停靠位姿的车辆朝向相反。

在该技术方案中，根据车辆的实际泊车的方向选择，添加了车头的正向进入以及车尾的反向进入，以便对更多的初始位置进行适应，并进一步获得能够选择的泊车路径。

上述任一技术方案中，所述S2的步骤，具体包括：S201，分别获取四轮车辆在前轮转向、后轮转向和四轮转向对应的转向几何；S202，在三种转向模式下分别按照两种停靠位姿，根据不同的所述驶入点和不同的所述转向半径获取相应的reeds-sheep泊车路径，并添加至备选集合中；其中，所述转向几何包括转向半径及转向中心点。

在该技术方案中，由于车辆可以进行四轮转向，因此可有三种不同的转向模式，具体为：前轮转向、后轮转向和四轮转向，并且分别具有不同的转向半径以及转向几何，在针对于两种不同的停靠位姿，可以获得多种不同的泊车方案路径，将所有获得的路径都添加至备选集合中，以便进行后续的选择，并挑选出能够行驶的最佳的路径。

上述任一技术方案中，在S3中所述代价值的参数，包括：距离代价、方向代价和倒车切换代价。

在该技术方案中，采用三种代价聚合的代价值判定方式，能够提供准确的判定结果，使得被挑选出的路径适应实际的车辆泊车需求。

上述任一技术方案中，在S3中所述评价函数，具体为下述公式：f:Min(cost_distance+cost_heading+cost_isHeadBack)；其中，cost_distance为距离代价，由泊车路径的总长度决定、cost_heading为方向代价，由泊车路径中的圆弧路径部分决定、cost_isHeadBack为倒车切换代价，由泊车路径中的倒车行驶路径部分决定。

在该技术方案中，将cost_distance距离代价、cost_heading方向代价和cost_isHeadBack倒车切换代价，以加法的方式汇集成一个完成的判定函数，能够从多方面从何考量，cost_distance为距离代价表示泊车路径的总长度，该项表示选择出泊车路径尽可能短，cost_heading为方向代价表示泊车路径中的圆弧路径部分，该项表示选择出泊车路径尽可能平滑并减少转向过程，cost_isHeadBack为倒车切换代价表示泊车过程中的倒车行驶路径部分，该项表示选择出泊车过程中尽量避免倒车行为。

上述任一技术方案中，所述S4中可行驶区域搜索的步骤，具体包括：S401，对泊车路径中的每一点，向四周进行螺旋式拓展；S402，每次拓展后对拓展点与障碍物进行距离判断，若满足要求则继续拓展，若拓展点不满足要求，将该点的坐标记录为在该拓展方向的最大点；S403，当四个拓展方向的最大点均被寻找到时停止拓展，四个方向上最大点围成的区域即为该路径点的可行驶区域。

在该技术方案中，在获取到代价值最低的最优路径后，对其泊车路径中的某一点的可行驶区域进行检测，并将所有点的可行驶区域记性汇总得到该路径的全部可行驶区域。

上述任一技术方案中，所述S402中所述距离判断为：判断所述拓展点与所述障碍物的距离是否大于所述车辆的宽度的一半。

在该技术方案中，对拓展点与所述障碍物的距离进行判定，并对判定要求进行限定，以避免在车辆实际行驶中车体不会与外部障碍物发生碰撞，保证了车辆的正常泊车。

上述任一技术方案中，所述S4中狭窄区域的判定步骤，具体为：在搜索过程中，若前4次拓展均失败或拓展后可行驶区域的面积小于0.5m²，则判定为狭窄区域。

在该技术方案中，若出现狭窄区域则表明车辆在改路径行驶容易发生碰撞。

本发明与现有技术相比所具有的有益效果：

本专利针对四轮转向矿卡的运动学特性，选取两种停靠位姿进行泊车规划，可以避免“倒车切换路径”出现；

本专利创新性地在前轮转向、后轮转向以及四轮转向的三种倒车模式下使用reeds-sheep曲线以及可变曲率半径进行路径拟合获得泊车路径集合，充分发挥四轮转向车辆的动力学性能；

本专利提出一种针对于四轮转向矿卡的路径评价函数，对所获取的泊车路径进行择优选取，可获得四轮转向矿卡的最低代价路径；

本专利创新性提出了一种螺旋建模方法，可以有效地搜索矿卡在矿区场景下的可行驶区域并实现避障效果，并为后续的路径平滑问题提供了搜索区域；

本专利建立二次优化问题对路径进行平滑处理，可以获得便于矿卡预瞄跟踪控制的最优路径。使用序列二次优化(SQP)方法求解二次优化问题，降低优化问题求解难度并提高求解质量。

根据本发明的实施例的附加方面和优点将在下面的描述部分中变得明显，或通过根据本发明的实施例的实践了解到。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制。

图1为本发明的一种矿区四轮转向车辆的泊车方法流程图；

图2为本发明的本发明中前轮转向、后轮转向、四轮转向三种转向运动学模型；

图3为本发明的一种搜索矿卡可行驶区域的螺旋建模方法过程图。

具体实施方式

为了可以更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。

请参阅图1-3，本发明的第一方面提供了一种用于矿区四轮转向车辆的泊车规划方法，包括如下步骤：

步骤1，根据驶入点的初始位置和初始朝向选取合适的停靠位姿，停靠位姿的参数包括车辆位置p和车辆朝向φ，并选取(p,φ)和(p,φ+180°)两种停靠位姿。

若仅使用停靠航向φ进行路径规划，所获得的路径有可能存在“倒车切换路径”，并非四轮转向车辆的最优路径。

步骤2，根据不同驶入点以及不同曲率半径，按照前轮转向、后轮转向和四轮转向的三种倒车模式使用reeds-sheep曲线生成多条路径作为备选集合。

其中，曲率半径为车辆本身运动学特性具有的最小转向半径，设为r，使用r，2r及3r分别作为曲率半径。

具体地，如图2所示，四轮转向车辆前轮转向、后轮转向以及四轮转向三种转向模式对应不同的转向半径以及转向几何，在三种转向模式下分别按照两种位姿，根据不同驶入点以及不同曲率半径获取相应的reeds-sheep泊车路径，并添加至同一备选集合进行比较。

其中，转向几何不同时，(x，y)坐标代指车辆不同的几何位置，当前轮转向时，(x，y)坐标为车辆的后轴中心；后轮转向时，为车辆前轴中心；当四轮转向时，(x，y)为车辆的几何中心点。

步骤3，建立评价函数用于计算车辆采用每个泊车路径的代价值，选取备选集合中代价值最低的最优路径。其中评价函数包括距离代价、方向代价和倒车切换代价的三种参数，分别为：

f:Mincost_distance+cost_heading+cost_isHeadBack

其中，cost_distance为距离代价表示泊车路径的总长度，该项表示希望泊车路径尽可能短；cost_heading为方向代价表示泊车路径中的圆弧路径部分，该项表示希望泊车路径尽可能平滑并减少转向过程；cost_isHeadBack为倒车切换代价表示泊车过程中的倒车行驶路径部分，该项表示希望泊车过程中尽量避免倒车行为。根据上述计算方法计算出每条备选路径所对应的代价值，选择其中的最小值所对应的路径。

步骤4，采用螺旋建模方法对步骤3中得出的最优路径进行可行驶区域搜索，若搜索过程中出现过于狭窄区域，判定该路径存在碰撞风险。

其中，图3所示的螺旋建模方法是针对于泊车路径上的离散点所进行的。如图3所示，在泊车路径中的某一点处，向四周进行螺旋式拓展，每次拓展后首先检查当前拓展点是否与障碍物过近，若当前拓展点到最近障碍物点距离小于车宽一半则认为过近，若满足要求则继续检查下次拓展点是否满足要求，若下一拓展点也满足要求则认为本次拓展成功并保留本次拓展点。若本次拓展不满足以上要求，将该点的坐标记录为在该拓展方向的最大点。当四个拓展方向的最大点均被寻找到时停止搜索，此时获得的四个方向上的最大点即为该路径点的可行驶区域。

步骤5，对步骤4中获得的可行驶区域是否具有碰撞风险进行判断，若是则在备选集合中删除此次选取的路径并返回步骤3，若不是则进行步骤6。

步骤6，建立二次优化模型并使用序列二次优化(SQP)算法对无碰撞的最优路径进行平滑操作，建立二次优化问题，二次优化模型的优化量为离散点(x_i，y_i)，且具体为下述公式：

f:Mincost₁+cost₂+cost₃

其中，cost₁、cost₂、cost₃分别采用下式求解：

其中，(x_i-ref，y_i-ref)表示路径点(x_i，y_i)所对应的原有路径点。cost₁，cost₂，cost₃分别表示平滑度代价，长度代价以及与原有路径之间的偏差量代价，n为路径点总数量，i表示第i个路径点，x为x轴坐标，y为y轴坐标。

二次优化模型的约束条件为：

其中，公式①和②表示优化变量的约束；③中的是为了便于二次优化问题求解所引入的松弛变量；④中的表示离散点之间的平均距离长度，表示最大曲率约束。

此时④约束仍为非线性约束，无法构成二次优化问题。

因此下述函数：

F＝(x_i-1+x_i+1-2*x_i)²+(y_i-1+y_i+1-2*y_i)²-stack_i

对该函数在处进行泰勒展开并仅保留一阶项可以得到：

F＝F(X_ref)+F^′(X_ref)*(X-X_ref)

故第四项非线性约束可以转化为线性约束：

F(X_ref)+F^′(X_ref)*(X-X_ref)≤(Δs²*cur_cstr)²

此时上述优化问题的约束均已转化为线性约束，使用SQP方法进行问题求解，以提高求解质量以及求解速度。

在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

以上所述的实施例仅是对本发明的优选方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (8)

1.一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，包括如下步骤：

S1，根据驶入点的初始位置和初始朝向选取两种停靠位姿，停靠位姿的参数包括车辆位置和车辆朝向，所述两种停靠位姿具有相同车辆位置和不同车辆朝向；

S2，根据驶入点以及曲率半径，按照前轮转向、后轮转向和四轮转向的三种倒车模式使用reeds-sheep曲线生成多条泊车路径作为备选集合；

S3，建立评价函数用于计算车辆采用备选集合中每条泊车路径的代价值，选取备选集合中代价值最低的为最优路径；

S4，采用螺旋建模方法对S3中得出的最优路径进行可行驶区域搜索，并判断是否出现狭窄区域，若是则在备选集合中删除此次选取的最优路径并返回S3，若不是则下一步。

S5，建立二次优化模型并使用序列二次优化算法(SQP)对无狭窄区域的最优路径进行平滑操作，并进行输出，得到最终泊车路径。

2.根据权利要求1所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S1中的两种停靠位姿的车辆朝向相反。

3.根据权利要求1所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S2的步骤，具体包括：

S201，分别获取四轮车辆在前轮转向、后轮转向和四轮转向对应的转向几何；

S202，在三种转向模式下分别按照两种停靠位姿，根据不同的所述驶入点和不同的所述转向半径获取相应的reeds-sheep泊车路径，并添加至备选集合中；

其中，所述转向几何包括转向半径及转向中心点。

4.根据权利要求1所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，在S3中所述代价值的参数，包括：距离代价、方向代价和倒车切换代价。

5.根据权利要求4所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，在S3中所述评价函数，具体为下述公式：

f:Min(cost_distance+cost_heading+cost_isHeadBack)；

其中，cost_distance为距离代价，由泊车路径的总长度决定、cost_heading为方向代价，由泊车路径中的圆弧路径部分决定、cost_isHeadBack为倒车切换代价，由泊车路径中的倒车行驶路径部分决定。

6.根据权利要求1所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S4中可行驶区域搜索的步骤，具体包括：

S401，对泊车路径中的每一点，向四周进行螺旋式拓展；

S402，每次拓展后对拓展点与障碍物进行距离判断，若满足要求则继续拓展，若拓展点不满足要求，将该点的坐标记录为在该拓展方向的最大点；

S403，当四个拓展方向的最大点均被寻找到时停止拓展，四个方向上最大点围成的区域即为该路径点的可行驶区域。

7.根据权利要求6所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S402中所述距离判断为：判断所述拓展点与所述障碍物的距离是否大于所述车辆的宽度的一半。

8.根据权利要求6所述的一种用于矿区四轮转向车辆的泊车规划方法，其特征在于，所述S4中狭窄区域的判定步骤，具体为：在搜索过程中，若前4次拓展均失败或拓展后可行驶区域的面积小于0.5m²，则判定为狭窄区域。

Images