一种无人车的队形保持方法及终端与流程

阅读：评论：0

1.本发明涉及无人驾驶技术领域，特别涉及一种无人车的队形保持方法及终端。

背景技术：

2.无人寻迹车是无人驾驶汽车技术的一种应用，无人驾驶汽车是汽车领域今后发展的主要趋势。无人驾驶汽车通过车载传感系统感知道路环境、车辆位置、交通信号以及障碍物等信息，在此基础上自动规划行车路线并通过一定的控制逻辑实现车辆的纵、横向耦合控制，使车辆安全到达预定目的地，在此期间不需要人工额外干预。
3.但是，因道路环境复杂，在进行多车编组行进时，未必能实现多车路径完全平行的理想环境。

技术实现要素：

4.本发明所要解决的技术问题是：提供一种无人车的队形保持方法及终端，能够保证多车在非平行路线下保持横队队形。
5.为了解决上述技术问题，本发明采用的技术方案为：
6.一种无人车的队形保持方法，包括步骤：
7.接收主车的位置信息和航向角，以主车行驶方向为正方向，并建立坐标系；
8.根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程；
9.根据所述线性方程和所述主车位置信息，计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。
10.为了解决上述技术问题，本发明采用的另一种技术方案为：
11.一种无人车的队形保持终端，包括存储器、处理器以及存储在所述存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
12.接收主车的位置信息和航向角，以主车行驶方向为正方向，并建立坐标系；
13.根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程；
14.根据所述线性方程和所述主车位置信息，计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。
15.本发明的有益效果在于：在无人机编队中，从车为了跟随主车保持队形，接收主车的位置信息和航向角，以主车的行驶方向作为正方向并建立坐标系；根据主车航向角和从车航向角以及从车位置信息，建立从车的线性方程；根据线性方程和主车位置信息，能够计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。因此在进行多车编组行进时，能够保证多车在非平行路线下保持横队队形。
附图说明
16.图1为本发明实施例的一种无人车的队形保持方法的流程图；
17.图2为本发明实施例的一种无人车的队形保持终端的示意图；
18.图3为本发明实施例的一种无人车的队形保持方法的具体步骤流程图；
19.图4为本发明实施例的一种无人车的队形保持方法的主从车示意图；
20.图5为本发明实施例的一种无人车的队形保持方法的坐标系示意图；
21.标号说明：
22.1、一种无人车的队形保持终端；2、存储器；3、处理器。
具体实施方式
23.为详细说明本发明的技术内容、所实现目的及效果，以下结合实施方式并配合附图予以说明。
24.请参照图1，本发明实施例提供了一种无人车的队形保持方法，包括步骤：
25.接收主车的位置信息和航向角，以主车行驶方向为正方向，并建立坐标系；
26.根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程；
27.根据所述线性方程和所述主车位置信息，计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。
28.从上述描述可知，本发明的有益效果在于：在无人机编队中，从车为了跟随主车保持队形，接收主车的位置信息和航向角，以主车的行驶方向作为正方向并建立坐标系；根据主车航向角和从车航向角以及从车位置信息，建立从车的线性方程；根据线性方程和主车位置信息，能够计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。因此在进行多车编组行进时，能够保证多车在非平行路线下保持横队队形。
29.进一步地，所述根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程包括：
30.根据主车航向角和从车航向角计算所述线性方程的斜率；
31.将所述从车位置信息代入从车的线性方程，计算得到所述线性方程的截距；
32.基于所述斜率和截距得到从车的线性方程。
33.由上述描述可知，通过主车和从车的信息，能够计算线性方程的截距和斜率，便于后续判断从车是否位于编队的目标位置上。
34.进一步地，所述根据所述线性方程和所述主车位置信息，计算出从车的目标位置包括：
35.使用主车航向角对所述主车位置信息进行换算，将换算后的主车位置信息代入所述线性方程，计算得到从车的目标位置。
36.由上述描述可知，对主车位置信息进行换算并带入方程中计算，能够得到非平行的横队队形中从车的目标位置，便于后续进行从车速度的调整。
37.进一步地，根据从车位置信息和从车的目标位置调整从车的速度包括：
38.计算从车位置信息和从车的目标位置的当前距离；
39.预设从车位置信息和从车的目标位置的目标距离，根据所述目标距离和所述当前
距离计算从车的速度out：
40.out＝kp*d(t)+ki*σd(t)+kd*
△
d；
41.σd(t)＝d(t)+d(t-1)+d(t-2)+
…
+d(t-n)；
42.△
d＝d(t)-d(t-1)；
43.式中，kp表示比例调节系数，ki表示积分调节系数，kd表示微分调节系数，d(t)表示在t时间所述目标距离减去所述当前距离的差值；
44.若out》0，则以out作为从车速度的增加量，否则，以out的绝对值作为从车速度的减少量。
45.由上述描述可知，结合面向比例调节、积分调节和微分调节计算从车的调节速度，结合比例调节的初调和积分、微分调节的微调，能够快速且准确地确定出从车的调节速度。
46.进一步地，根据所述目标距离和所述当前距离计算从车的速度out还包括：
47.当所述目标距离和所述当前距离的差值不超过预设范围时，不调节从车速度。
48.由上述描述可知，设置调节从车速度的浮动空间，在目标距离和当前距离的差值不超过预设范围时，不进行从车速度的调节控制，使得队列的速度调节更灵活平滑。
49.请参照图2，本发明另一实施例提供了一种无人车的队形保持终端，包括存储器、处理器以及存储在所述存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现以下步骤：
50.接收主车的位置信息和航向角，以主车行驶方向为正方向，并建立坐标系；
51.根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程；
52.根据所述线性方程和所述主车位置信息，计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。
53.从上述描述可知，在无人机编队中，从车为了跟随主车保持队形，接收主车的位置信息和航向角，以主车的行驶方向作为正方向并建立坐标系；根据主车航向角和从车航向角以及从车位置信息，建立从车的线性方程；根据线性方程和主车位置信息，能够计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。因此在进行多车编组行进时，能够保证多车在非平行路线下保持横队队形。
54.进一步地，所述根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程包括：
55.根据主车航向角和从车航向角计算所述线性方程的斜率；
56.将所述从车位置信息代入从车的线性方程，计算得到所述线性方程的截距；
57.基于所述斜率和截距得到从车的线性方程。
58.由上述描述可知，通过主车和从车的信息，能够计算线性方程的截距和斜率，便于后续判断从车是否位于编队的目标位置上。
59.进一步地，所述根据所述线性方程和所述主车位置信息，计算出从车的目标位置包括：
60.使用主车航向角对所述主车位置信息进行换算，将换算后的主车位置信息代入所述线性方程，计算得到从车的目标位置。
61.由上述描述可知，对主车位置信息进行换算并带入方程中计算，能够得到非平行
的横队队形中从车的目标位置，便于后续进行从车速度的调整。
62.进一步地，根据从车位置信息和从车的目标位置调整从车的速度包括：
63.计算从车位置信息和从车的目标位置的当前距离；
64.预设从车位置信息和从车的目标位置的目标距离，根据所述目标距离和所述当前距离计算从车的速度out：
65.out＝kp*d(t)+ki*σd(t)+kd*
△
d；
66.σd(t)＝d(t)+d(t-1)+d(t-2)+
…
+d(t-n)；
67.△
d＝d(t)-d(t-1)；
68.式中，kp表示比例调节系数，ki表示积分调节系数，kd表示微分调节系数，d(t)表示在t时间所述目标距离减去所述当前距离的差值；
69.若out》0，则以out作为从车速度的增加量，否则，以out的绝对值作为从车速度的减少量。
70.由上述描述可知，结合面向比例调节、积分调节和微分调节计算从车的调节速度，结合比例调节的初调和积分、微分调节的微调，能够快速且准确地确定出从车的调节速度。
71.进一步地，根据所述目标距离和所述当前距离计算从车的速度out还包括：
72.当所述目标距离和所述当前距离的差值不超过预设范围时，不调节从车速度。
73.由上述描述可知，设置调节从车速度的浮动空间，在目标距离和当前距离的差值不超过预设范围时，不进行从车速度的调节控制，使得队列的速度调节更灵活平滑。
74.本发明上述的一种无人车的队形保持方法及终端，适用于多车编组行进时，因靶场环境复杂使得多车路径不平行时，保证在非平行路线下保持横队队形，以下通过具体的实施方式进行说明：
75.实施例一
76.请参照图1和图3，一种无人车的队形保持方法，包括步骤：
77.s1、接收主车的位置信息和航向角，以主车行驶方向为正方向，并建立坐标系。
78.具体的，请参照图4，在本实施例中，车辆正常的寻迹驾驶，主车a将自身经纬度(x1，y1)与航向角a，按照5hz的频率发送给从车b，以主车a行驶方向为正方向，建立坐标系。
79.s2、根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程。
80.具体的，从车b根据当前自身的经纬度和航向角以及a车实时传输过来的经纬度计算从车b的线性方程：y＝kx+z。
81.s21、根据主车航向角和从车航向角计算所述线性方程的斜率。
82.具体的，请参照图5，以主车行驶方向建立坐标系，得到从车b的航向角为z＝b-a，如果z小于0，则z＝z+360
°
，因此k＝tan[(90
°‑
z)/180*π]。
[0083]
s22、将所述从车位置信息代入从车的线性方程，计算得到所述线性方程的截距，基于所述斜率和截距得到从车的线性方程。
[0084]
具体的，将从车b的经纬度(x2，y2)带入y＝kx+z，可以计算得到截距z。
[0085]
s3、根据所述线性方程和所述主车位置信息，计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。
[0086]
s31、使用主车航向角对所述主车位置信息进行换算，将换算后的主车位置信息代
入所述线性方程，计算得到从车的目标位置。
[0087]
具体的，将主车a的纬度y1值进行换算，即y1’＝y1*cos(a)，将y1’代入线性方程，得到目标位置c(x，y)。
[0088]
s32、计算从车位置信息和从车的目标位置的当前距离；
[0089]
具体的，将c点与车辆当前位置b点，同时依次与整条轨迹从起点到终点的所有点进行遍历，与每个轨迹点计算距离，得到disb和disc。
[0090]
在本实施例中，令b点离第100个轨迹点最近，c点离第120个轨迹点最近，则可得出c点在b点前面，从车b需要加速。
[0091]
计算从车b当前位置与c点的距离：
[0092]
x_d＝x-x2；
[0093]
y_d＝y-y2；
[0094]
x_d＝x_d*cos(y/180*π)；
[0095]
其中，因地球近似为球体，在赤道上纬度最长，向两极延伸时，纬度逐渐缩小，到达南北极时，纬度近乎于0，因此在计算纬度的差值时，要考虑地球曲率的问题，要乘以当前纬度值的cos值得到实际的纬度长度。
[0096]
距离dist＝[sqrt(x_d*x_d+y_d*y_d)]*0.00054；
[0097]
其中，经纬度中一分大约等于0.00054米。
[0098]
s33、预设从车位置信息和从车的目标位置的目标距离，根据所述目标距离和所述当前距离计算从车的速度out；
[0099]
具体的，得到距离dist后，控制从车b的速度，以此让从车b到达目标位置c；由于主车每200ms向从车发送一次位置信息，因此从车b的车速也会每200ms调整一次。此时，设定目标距离为0，每200ms距离当前距离do(t)，目标距离减去当前距离的差值为d(t)，即面向比例项用的变动数据；因此，具体的out计算公式如下：
[0100]
out＝kp*d(t)+ki*σd(t)+kd*
△
d；
[0101]
σd(t)＝d(t)+d(t-1)+d(t-2)+
…
+d(t-n)；
[0102]
△
d＝d(t)-d(t-1)；
[0103]
式中，当σd(t)》800时，设置σd(t)＝800，kp表示比例调节系数，比例作用越大，可以加快调节，如果kp＝1时，将kp改为2，就相当于扩大2倍调节；ki表示积分调节系数，kd表示微分调节系数，也就是达到目标位置时，会有上下震荡，需要进行微调。当距离突然拉大时，希望通过补比例进行下一次调速，这就是kd微分控制。经过了长时间调整，一直离目标位置有差距，希望用多少比例去弥补这个长期空缺，这就是ki积分控制。
[0104]
本实施例中，用到的kp＝8，ki＝0.02，kd＝0.1，一般采取多次试验来确定三者的值。
[0105]
若out》0，则以out作为从车速度的增加量，否则，以out的绝对值作为从车速度的减少量。
[0106]
在一些实施例中，当所述目标距离和所述当前距离的差值不超过预设范围时，不调节从车速度。在本实施例中，从车b在距离c点
±
10米时，不进行速度调节控制。
[0107]
在一些实施例中，还预设了直线速度与弯道速度上限，避免无限制的加减速，从而使从车b安全到达目标位置。
[0108]
实施例二
[0109]
请参照图2，一种无人车的队形保持终端1，包括存储器2、处理器3以及存储在所述存储器2上并可在处理器3上运行的计算机程序，所述处理器3执行所述计算机程序时实现实施例一的一种无人车的队形保持方法的各个步骤。
[0110]
综上所述，本发明提供的一种无人车的队形保持方法及终端，在无人机编队中，从车为了跟随主车保持队形，接收主车的位置信息和航向角，以主车的行驶方向作为正方向并建立坐标系；根据主车航向角和从车航向角以及从车位置信息，建立从车的线性方程；根据线性方程和主车位置信息，能够计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。因此在进行多车编组行进时，能够保证多车在非平行路线下保持横队队形。
[0111]
以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等同变换，或直接或间接运用在相关的技术领域，均同理包括在本发明的专利保护范围内。

技术特征：

1.一种无人车的队形保持方法，其特征在于，包括步骤：接收主车的位置信息和航向角，以主车行驶方向为正方向，并建立坐标系；根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程；根据所述线性方程和所述主车位置信息，计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。2.根据权利要求1所述的一种无人车的队形保持方法，其特征在于，所述根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程包括：根据主车航向角和从车航向角计算所述线性方程的斜率；将所述从车位置信息代入从车的线性方程，计算得到所述线性方程的截距；基于所述斜率和截距得到从车的线性方程。3.根据权利要求1所述的一种无人车的队形保持方法，其特征在于，所述根据所述线性方程和所述主车位置信息，计算出从车的目标位置包括：使用主车航向角对所述主车位置信息进行换算，将换算后的主车位置信息代入所述线性方程，计算得到从车的目标位置。4.根据权利要求1所述的一种无人车的队形保持方法，其特征在于，根据从车位置信息和从车的目标位置调整从车的速度包括：计算从车位置信息和从车的目标位置的当前距离；预设从车位置信息和从车的目标位置的目标距离，根据所述目标距离和所述当前距离计算从车的速度out：out＝kp*d(t)+ki*σd(t)+kd*
△
d；σd(t)＝d(t)+d(t-1)+d(t-2)+
…
+d(t-n)；
△
d＝d(t)-d(t-1)；式中，kp表示比例调节系数，ki表示积分调节系数，kd表示微分调节系数，d(t)表示在t时间所述目标距离减去所述当前距离的差值；若out>0，则以out作为从车速度的增加量，否则，以out的绝对值作为从车速度的减少量。5.根据权利要求4所述的一种无人车的队形保持方法，其特征在于，根据所述目标距离和所述当前距离计算从车的速度out还包括：当所述目标距离和所述当前距离的差值不超过预设范围时，不调节从车速度。6.一种无人车的队形保持终端，包括存储器、处理器以及存储在所述存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现以下步骤：接收主车的位置信息和航向角，以主车行驶方向为正方向，并建立坐标系；根据主车航向角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程；根据所述线性方程和所述主车位置信息，计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。7.根据权利要求6所述的一种无人车的队形保持终端，其特征在于，所述根据主车航向
角和从车航向角以及从车位置信息，基于所述坐标系建立从车的线性方程包括：根据主车航向角和从车航向角计算所述线性方程的斜率；将所述从车位置信息代入从车的线性方程，计算得到所述线性方程的截距；基于所述斜率和截距得到从车的线性方程。8.根据权利要求6所述的一种无人车的队形保持终端，其特征在于，所述根据所述线性方程和所述主车位置信息，计算出从车的目标位置包括：使用主车航向角对所述主车位置信息进行换算，将换算后的主车位置信息代入所述线性方程，计算得到从车的目标位置。9.根据权利要求6所述的一种无人车的队形保持终端，其特征在于，根据从车位置信息和从车的目标位置调整从车的速度包括：计算从车位置信息和从车的目标位置的当前距离；预设从车位置信息和从车的目标位置的目标距离，根据所述目标距离和所述当前距离计算从车的速度out：out＝kp*d(t)+ki*σd(t)+kd*
△
d；σd(t)＝d(t)+d(t-1)+d(t-2)+
…
+d(t-n)；
△
d＝d(t)-d(t-1)；式中，kp表示比例调节系数，ki表示积分调节系数，kd表示微分调节系数，d(t)表示在t时间所述目标距离减去所述当前距离的差值；若out>0，则以out作为从车速度的增加量，否则，以out的绝对值作为从车速度的减少量。10.根据权利要求9所述的一种无人车的队形保持终端，其特征在于，根据所述目标距离和所述当前距离计算从车的速度out还包括：当所述目标距离和所述当前距离的差值不超过预设范围时，不调节从车速度。

技术总结

本发明公开了一种无人车的队形保持方法及终端，在无人机编队中，从车为了跟随主车保持队形，接收主车的位置信息和航向角，以主车的行驶方向作为正方向并建立坐标系；根据主车航向角和从车航向角以及从车位置信息，建立从车的线性方程；根据线性方程和主车位置信息，能够计算出从车的目标位置，根据从车位置信息和从车的目标位置调整从车的速度。因此在进行多车编组行进时，能够保证多车在非平行路线下保持横队队形。保持横队队形。保持横队队形。