一种无人循迹驾驶穿越障碍物阵列的方法与存储介质与流程

1.本发明涉及无人驾驶技术领域，特别涉及一种无人循迹驾驶穿越障碍物阵 列的方法与存储介质。


背景技术：

2.无人循迹驾驶是无人车自动驾驶很常用的一种方式。循迹驾驶指的是汽车根据 预先设置好的路线进行自动循迹驾驶。执行无人循迹驾驶任务的环境是多种多 样，可能会存在各种各样的障碍物妨碍正常的无人循迹驾驶任务。如何在循迹 轨迹上存在影响车辆正常行驶的障碍物阵列，但相邻两个障碍物间的距离大于 车宽的环境下，能够快速规划出一条安全可通行的路径，是一个亟待解决的问 题。


技术实现要素：

3.本发明所要解决的技术问题是：提供一种无人循迹驾驶穿越障碍物阵列的 方法与存储介质，能够在需要穿越障碍物阵列时提供最佳路线规划。
4.为了解决上述技术问题，本发明采用的技术方案为：
5.一种无人循迹驾驶穿越障碍物阵列的方法，包括步骤：
6.s1、获取车辆当前位置的经纬度坐标以及车辆前方所有障碍物的经纬度坐 标；
7.s2、对于无人车前方障碍物阵列中的每排障碍物，选取循迹轨迹两侧最接 近的障碍物，计算两个障碍物连线的中点以及中垂线；
8.s3、以车辆的最小转弯半径为半径，计算经过车辆当前位置，以车辆当前 进行方向为切线且圆心最接近所述循迹轨迹的圆一，以及经过所述中点，以所 述中垂线为切线且圆心最接近所述循迹轨迹的圆二，并计算两圆的穿障公切线；
9.s4、根据所述圆一、圆二以及所述公切线得到穿障路线，并进行穿障；
10.s5、重复执行步骤s2至s4，直至穿过障碍物阵列。
11.为了解决上述技术问题，本发明采用的另一种技术方案为：
12.一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执 行时实现以下步骤：
13.s1、获取车辆当前位置的经纬度坐标以及车辆前方所有障碍物的经纬度坐 标；
14.s2、对于无人车前方障碍物阵列中的每排障碍物，选取循迹轨迹两侧最接 近的障碍物，计算两个障碍物连线的中点以及中垂线；
15.s3、以车辆的最小转弯半径为半径，计算经过车辆当前位置，以车辆当前 进行方向为切线且圆心最接近所述循迹轨迹的圆一，以及经过所述中点，以所 述中垂线为切线且圆心最接近所述循迹轨迹的圆二，并计算两圆的穿障公切线；
16.s4、根据所述圆一、圆二以及所述公切线得到穿障路线，并进行穿障；
17.s5、重复执行步骤s2至s4，直至穿过障碍物阵列。
18.本发明的有益效果在于：本发明的一种无人循迹驾驶穿越障碍物阵列的方 法与
存储介质，能够在原循迹驾驶路线上存在障碍物阵列，且障碍物阵列中的 障碍物两两间的距离均能够容纳车辆行驶的情况下，自主规划出穿越障碍物阵 列的最佳路线，以完成既定的无人循迹驾驶任务。
附图说明
19.图1为本发明实施例的一种无人循迹驾驶穿越障碍物阵列的方法的流程图；
20.图2为本发明实施例的一种无人循迹驾驶穿越障碍物阵列的方法的示意图。
具体实施方式
21.为详细说明本发明的技术内容、所实现目的及效果，以下结合实施方式并 配合附图予以说明。
22.请参照图1以及图2，一种无人循迹驾驶穿越障碍物阵列的方法，包括步骤：
23.s1、获取车辆当前位置的经纬度坐标以及车辆前方所有障碍物的经纬度坐 标；
24.s2、对于无人车前方障碍物阵列中的每排障碍物，选取循迹轨迹两侧最接 近的障碍物，计算两个障碍物连线的中点以及中垂线；
25.s3、以车辆的最小转弯半径为半径，计算经过车辆当前位置，以车辆当前 进行方向为切线且圆心最接近所述循迹轨迹的圆一，以及经过所述中点，以所 述中垂线为切线且圆心最接近所述循迹轨迹的圆二，并计算两圆的穿障公切线；
26.s4、根据所述圆一、圆二以及所述公切线得到穿障路线，并进行穿障；
27.s5、重复执行步骤s2至s4，直至穿过障碍物阵列。
28.从上述描述可知，本发明的有益效果在于：本发明的一种无人循迹驾驶穿 越障碍物阵列的方法与存储介质，能够在原循迹驾驶路线上存在障碍物阵列， 且障碍物阵列中的障碍物两两间的距离均能够容纳车辆行驶的情况下，自主规 划出穿越障碍物阵列的最佳路线，以完成既定的无人循迹驾驶任务。
29.进一步地，所述穿障公切线的计算具体为：
30.根据所述圆一和圆二计算得到四条公切线；
31.计算圆一和圆二的圆心连线，得到圆心连线的方程：
32.y＝kx b；
33.排除斜率与所述圆心连线的斜率k相等的两条外公切线，得到两圆的两条 内公切线；
34.分别计算两条所述内公切线与所述圆二的交点，并计算所述交点与两个障 碍物连线的所述中点的距离，选择与所述中点距离最近的所述交点所对应的内 切线为所述穿障公切线。
35.由上述描述可知，通过以上步骤，能够确定一条最接近障碍物和车辆当前 位置的公切线，从而使后续确定的路线最优。
36.进一步地，所述步骤s4具体为：
37.计算所述公切线与圆一的交点b，与圆二的交点c，由线段bc、圆弧ab 以及圆弧cd拼接为穿障路线ad，并控制车辆沿穿障路线ad行驶进行穿障；
38.其中，所述圆弧ab为车辆当前位置a与所述交点b在圆一上的劣弧，所 述圆弧cd为
所述交点c与两个障碍物连线的所述中点d在圆二上的劣弧。
39.由上述描述可知，圆一和圆二均为根据车辆最小转弯半径确定的圆，其上 的圆弧ab和圆弧cd符合车辆的行驶规则，通过圆弧ab、线段bc以及圆弧cd能够确定一条车辆能够循迹行驶的最优穿障路线。
40.进一步地，所述圆一的确定为：
41.建立经过车辆当前位置且垂直于车辆前进方向的第一直线，并在所述第一 直线上确定与车辆当前位置距离为所述最小转弯半径的两点，并计算两点与所 述循迹轨迹之间的距离，取距离最小的一点(x1，y1)为所述圆一的圆心，以车辆 最小转弯半径r为所述圆一的半径，根据公式：
42.(x-x1)2 (y-y1)2＝r2；
43.计算得到所述圆一；
44.所述圆二的确定为：
45.在两个障碍物连线及其延长线上确定与所述中点距离为所述最小转弯半径 的两点，并计算两点与所述循迹轨迹之间的距离，取距离最小的一点(x2，y2) 为所述圆二的圆心，以车辆最小转弯半径r为所述圆一的半径，根据公式：
46.(x-x2)2 (y-y2)2＝r2；
47.计算得到所述圆二。
48.由上述描述可知，我们可以通过以上方式确定用于车辆能够转向且更靠近 原来轨迹的最小转向路线形成的圆，用于确定车辆最终的转向路线，以形成最 优的穿障路线
49.进一步地，所述步骤s1具体包括：
50.s11、实时获取车辆当前的经纬度坐标，并实时采集激光雷达点云数据，通 过聚类算法计算识别出车辆正前方的所有障碍物，并记录记录各个障碍物在无 人车正前方的雷达坐标；
51.s12、根据车辆当前位置的经纬度坐标，利用旋转矩阵将所述雷达坐标转化 为经纬度坐标。
52.由上述描述可知，根据采集到的激光雷达点云数据，确定障碍物相对于车 辆的位置位置，并结合车辆当前的经纬度坐标确定障碍物的经纬度坐标，达到 统一度量的效果，便于后续的计算。
53.一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执 行时实现以下步骤：
54.s1、获取车辆当前位置的经纬度坐标以及车辆前方所有障碍物的经纬度坐 标；
55.s2、对于无人车前方障碍物阵列中的每排障碍物，选取循迹轨迹两侧最接 近的障碍物，计算两个障碍物连线的中点以及中垂线；
56.s3、以车辆的最小转弯半径为半径，计算经过车辆当前位置，以车辆当前 进行方向为切线且圆心最接近所述循迹轨迹的圆一，以及经过所述中点，以所 述中垂线为切线且圆心最接近所述循迹轨迹的圆二，并计算两圆的穿障公切线；
57.s4、根据所述圆一、圆二以及所述公切线得到穿障路线，并进行穿障；
58.s5、重复执行步骤s2至s4，直至穿过障碍物阵列。
59.从上述描述可知，本发明的有益效果在于：本发明的一种无人循迹驾驶穿 越障碍
物阵列的方法与存储介质，能够在原循迹驾驶路线上存在障碍物阵列， 且障碍物阵列中的障碍物两两间的距离均能够容纳车辆行驶的情况下，自主规 划出穿越障碍物阵列的最佳路线，以完成既定的无人循迹驾驶任务。
60.进一步地，所述穿障公切线的计算具体为：
61.根据所述圆一和圆二计算得到四条公切线；
62.计算圆一和圆二的圆心连线，得到圆心连线的方程：
63.y＝kx b；
64.排除斜率与所述圆心连线的斜率k相等的两条外公切线，得到两圆的两条 内公切线；
65.分别计算两条所述内公切线与所述圆二的交点，并计算所述交点与两个障 碍物连线的所述中点的距离，选择与所述中点距离最近的所述交点所对应的内 切线为所述穿障公切线。
66.由上述描述可知，通过以上步骤，能够确定一条最接近障碍物和车辆当前 位置的公切线，从而使后续确定的路线最优。
67.进一步地，所述步骤s4具体为：
68.计算所述公切线与圆一的交点b，与圆二的交点c，由线段bc、圆弧ab 以及圆弧cd拼接为穿障路线ad，并控制车辆沿穿障路线ad行驶进行穿障；
69.其中，所述圆弧ab为车辆当前位置a与所述交点b在圆一上的劣弧，所 述圆弧cd为所述交点c与两个障碍物连线的所述中点d在圆二上的劣弧。
70.由上述描述可知，圆一和圆二均为根据车辆最小转弯半径确定的圆，其上 的圆弧ab和圆弧cd符合车辆的行驶规则，通过圆弧ab、线段bc以及圆弧 cd能够确定一条车辆能够循迹行驶的最优穿障路线。
71.进一步地，所述圆一的确定为：
72.建立经过车辆当前位置且垂直于车辆前进方向的第一直线，并在所述第一 直线上确定与车辆当前位置距离为所述最小转弯半径的两点，并计算两点与所 述循迹轨迹之间的距离，取距离最小的一点(x1，y1)为所述圆一的圆心，以车辆 最小转弯半径r为所述圆一的半径，根据公式：
73.(x-x1)2 (y-y1)2＝r2；
74.计算得到所述圆一；
75.所述圆二的确定为：
76.在两个障碍物连线及其延长线上确定与所述中点距离为所述最小转弯半径 的两点，并计算两点与所述循迹轨迹之间的距离，取距离最小的一点(x2，y2) 为所述圆二的圆心，以车辆最小转弯半径r为所述圆一的半径，根据公式：
77.(x-x2)2 (y-y2)2＝r2；
78.计算得到所述圆二。
79.由上述描述可知，我们可以通过以上方式确定用于车辆能够转向且更靠近 原来轨迹的最小转向路线形成的圆，用于确定车辆最终的转向路线，以形成最 优的穿障路线。
80.进一步地，所述步骤s1具体包括：
81.s11、实时获取车辆当前的经纬度坐标，并实时采集激光雷达点云数据，通 过聚类
算法计算识别出车辆正前方的所有障碍物，并记录记录各个障碍物在无 人车正前方的雷达坐标；
82.s12、根据车辆当前位置的经纬度坐标，利用旋转矩阵将所述雷达坐标转化 为经纬度坐标。
83.由上述描述可知，根据采集到的激光雷达点云数据，确定障碍物相对于车 辆的位置位置，并结合车辆当前的经纬度坐标确定障碍物的经纬度坐标，达到 统一度量的效果，便于后续的计算。
84.本发明的一种无人循迹驾驶穿越障碍物阵列的方法与存储介质，适用于在 原循迹驾驶路线上存在障碍物阵列，需要穿越障碍物阵列，且障碍物阵列中的 障碍物两两间的距离均能够容纳车辆行驶的场景。
85.请参照图1和图2，本发明的实施例一为：
86.一种无人循迹驾驶穿越障碍物阵列的方法，包括步骤：
87.本实施例中，预先驾驶车辆在需要循迹的路线上行驶一遍，采集路线上gps 的精度和纬度数据(经度，纬度)并存储在文件中。
88.在循迹路线上摆放障碍物阵列作为示例，以垂直于循迹路线的方向为一排， 同排障碍物的间距至少大于车宽，确保无人车可以从相邻两个障碍物中间穿过， 前后排间距至少两倍车身长，确保无人车可以规划出一条穿过障碍物阵列的路 径。
89.而后无人车从预先采集的循迹路线中的起点位置出发，执行无人循迹驾驶 任务。无人循迹算法从实时的gps信息中获取无人车的当前位置(经度，纬度)， 从预先采集的循迹路线中的轨迹点中选择无人车前方距离无人车5米的一个轨 迹点作为循迹的目标点，利用三角函数计算出两点直线在gps坐标系中的方向， 与车当前的航向角做减法运算得出车头需要转动的角度，然后控制无人车转动 方向，从而进行无人循迹驾驶。
90.s1、获取车辆当前位置的经纬度坐标以及车辆前方所有障碍物的经纬度坐 标；
91.所述步骤s1具体包括：
92.s11、实时获取车辆当前的经纬度坐标，并实时采集激光雷达点云数据，通 过聚类算法计算识别出车辆正前方的所有障碍物，并记录记录各个障碍物在无 人车正前方的雷达坐标；
93.s12、根据车辆当前位置的经纬度坐标，利用旋转矩阵将所述雷达坐标转化 为经纬度坐标。
94.本实施例中，车辆在循迹驾驶过程中实时采集激光雷达点云数据，根据聚 类算法计算出无人车正前方的所有被识别到的障碍物阵列中的障碍物，并记录 各个障碍物在无人车正前方的雷达坐标(x,y)，并利用旋转矩阵将雷达坐标转换 为经纬度坐标。转换方法为：取同一时刻的gps数据，经度lat、纬度lon、航 向角θ，根据旋转矩阵x'＝xcosθ-ysinθ，y'＝xsinθ ycosθ，计算得出障碍物 的经纬度lon_x＝lon (x'*0.00000899281)，lat_y＝lat (y'*0.00000899281)。
95.本实施例中，激光雷达障碍物识别是利用欧几里得聚类算法，遍历激光雷 达点云中所有点的数据，计算点与点之间的距离，当距离小于第一距离就认为 这些点属于一个物体。聚类点云中所有点的最小x值和最大x值求得障碍物的 宽和离车距离，最小y值与最大y值求得障碍物宽度。从而得出障碍物的三维 信息，再将此数据传输给穿障模块，穿障模块
根据实时的障碍物阵列信息与车 前方待行驶路径轨迹进行比较计算，如果循迹轨迹在障碍物阵列内且离车距离 小于第二距离时，则运行穿障算法计算出穿障路径进行穿障。本实施例中，第 一距离为0.3m，第二距离为10m，在其他等同实施例中，第一距离和第二距离 可以根据实际需求或条件调整。
96.s2、对于无人车前方障碍物阵列中的每排障碍物，选取循迹轨迹两侧最接 近的障碍物，计算两个障碍物连线的中点以及中垂线。
97.本实施例中，根据无人车与障碍物间的距离，得到第一排的障碍物，并计 算第一排的各个障碍物与循迹轨迹的距离，得到循迹轨迹两侧最接近的障碍物， 并进一步计算两个障碍物连线的重点及中垂线。
98.s3、以车辆的最小转弯半径为半径，计算经过车辆当前位置，以车辆当前 进行方向为切线且圆心最接近所述循迹轨迹的圆一，以及经过所述中点，以所 述中垂线为切线且圆心最接近所述循迹轨迹的圆二，并计算两圆的穿障公切线；
99.所述圆一的确定为：
100.建立经过车辆当前位置且垂直于车辆前进方向的第一直线，并在所述第一 直线上确定与车辆当前位置距离为所述最小转弯半径的两点，并计算两点与所 述循迹轨迹之间的距离，取距离最小的一点(x1，y1)为所述圆一的圆心，以车辆 最小转弯半径r为所述圆一的半径，根据公式：
101.(x-x1)2 (y-y1)2＝r2；
102.计算得到所述圆一；
103.所述圆二的确定为：
104.在两个障碍物连线及其延长线上确定与所述中点距离为所述最小转弯半径 的两点，并计算两点与所述循迹轨迹之间的距离，取距离最小的一点(x2，y2) 为所述圆二的圆心，以车辆最小转弯半径r为所述圆一的半径，根据公式：
105.(x-x2)2 (y-y2)2＝r2；
106.计算得到所述圆二。
107.本实施例中，通过以上方式，能够计算得到以无人的最小转弯半径为半径r， 以无人车前进方向为切线，且圆心距离原轨迹最近得出经过无人车当前位置的 圆一，以及经过穿障目标连线中点以该连线的中垂线为切线，且圆心距离原轨 迹最近的圆二。
108.其中，若确定所述圆一时，所述无人车正位于所述循迹轨迹上，则判断所 述中点相对位于无人车的左前方还是右前方，若为左前方则由左侧选取距离车 辆当前位置为r的一点作为圆一的圆心，反之由右侧选取距离车辆当前位置为r 的一点作为圆一的圆心。
109.所述穿障公切线的计算具体为：
110.根据所述圆一和圆二计算得到四条公切线；
111.计算圆一和圆二的圆心连线，得到圆心连线的方程：
112.y＝kx b；
113.排除斜率与所述圆心连线的斜率k相等的两条外公切线，得到两圆的两条 内公切线；
114.分别计算两条所述内公切线与所述圆二的交点，并计算所述交点与两个障 碍物连线的所述中点的距离，选择与所述中点距离最近的所述交点所对应的内 切线为所述穿
障公切线。
115.本实施例中，我们需要求解两圆的公切线，假设两圆的公切线的方程为 yi＝kix bi，两圆的圆心到公切线的距离相等，则可以求解得到四条公切线。
116.求解两圆圆心构成的直线，由于两圆半径相等，因此外公切线与两圆心构 成的直线平行，直线方程的斜率相等。从而能够去除两条外公切线，确定两条 内公切线。
117.分别求解两条内公切线与圆二的两个角点，并计算交点与前文中两个障碍 物连线中点的距离，选择距离最近的一个交点所对应的内公切线为穿障公切线。
118.s4、根据所述圆一、圆二以及所述公切线得到穿障路线，并进行穿障；
119.所述步骤s4具体为：
120.计算所述公切线与圆一的交点b，与圆二的交点c，由线段bc、圆弧ab 以及圆弧cd拼接为穿障路线ad，并控制车辆沿穿障路线ad行驶进行穿障；
121.其中，所述圆弧ab为车辆当前位置a与所述交点b在圆一上的劣弧，所 述圆弧cd为所述交点c与两个障碍物连线的所述中点d在圆二上的劣弧。
122.本实施例中，计算穿障公切线与圆一的交点b，与圆二的交点c，根据车辆 当前位置a，两个障碍物连线的中点d，由圆弧ab、线段bc、圆弧cd构成 最短的穿障路线，按照间隔20cm生成穿障路线上的轨迹点，并沿所述轨迹点进 行穿障。
123.s5、重复执行步骤s2至s4，直至穿过障碍物阵列。
124.本实施例中，循环执行以上步骤，完成障碍物阵列的穿越。
125.本发明的实施例二为：
126.一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执 行时实现以上实施例一中的步骤。
127.综上所述，本发明提供的一种无人循迹驾驶穿越障碍物阵列的方法，能够 在原循迹驾驶路线上存在障碍物阵列，且障碍物阵列中的障碍物两两间的距离 均能够容纳车辆行驶的情况下，自主规划出穿越障碍物阵列的最佳路线，以完 成既定的无人循迹驾驶任务。
128.以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利 用本发明说明书及附图内容所作的等同变换，或直接或间接运用在相关的技术 领域，均同理包括在本发明的专利保护范围内。