一种自动驾驶多节拖车及其位姿确定方法与流程

1.本发明涉及一种自动驾驶多节拖车及其位姿确定方法，属于多节拖车自动驾驶技术领域。


背景技术：

2.区别于普通自动驾驶车辆，自动驾驶多节拖车本身的系统模型复杂，如何在动态变化的环境中，规划和控制多节拖车的运动，是一个巨大的挑战。而为了实现更好的运动规划和控制，需要确定每节拖车的实时位姿。由于牵引车头以及各节拖车之间属于半刚性连接，始终做直线行驶时，每节拖车的实时位姿容易确定，即各节拖车成一条直线；但是在转弯过程中，各节拖车的运动轨迹均不相同，此时需要有一种方法来确定各节拖车的位姿。
3.目前的拖车位姿确定方法有以下几种：
4.(1)通过在牵引车上安装激光雷达，利用激光雷达直接扫描拖车标记物，基于扫描得到的拖车标记物信息得到拖车的位姿信息，或者通过在牵引车上安装特定的图像传感器，利用图像传感器获取拖车的图像信息，基于获取的拖车图像信息得到拖车的位姿信息。这两种方式将激光雷达或图像传感器安装在牵引车上，很容易获取第一节拖车的标记物信息或图像信息确定第一节拖车的位姿，但是在存在多节拖车的情况下，由于多节拖车间前后遮挡严重，后节拖车会被前节拖车遮挡，难以获取后节拖车的标记物信息或图像信息，进而难以确定后节拖车的位姿。因此，这两种方式只适用于只有单节拖车时的位姿确定，不适用于有多节拖车时的位姿确定。
5.(2)通过在牵引车和拖车上分别安装差分gps(global positioning system，全球定位系统)设备，基于获取的牵引车和拖车的gps差分数据计算牵引车和拖车的位姿信息。但是，这种方式存在两个弊端：1)每节拖车均需配置高精度差分gps设备，成本较高；2)拖车车体不便于安装电气设备，例如机场和仓库的拖车车体，出于可靠性和拓展性的设计，拖车车体只有机械连接没有电气连接，增加差分gps设备会破坏拖车车体结构并降低拖车的可靠性和拓展性。
6.综上所述，现有的拖车位姿确定方法，要么只适用于单节拖车时的位姿确定，不适用于有多节拖车时的位姿确定；要么需要每节拖车均需配置高精度差分gps设备才能获取多节拖车的位姿，成本较高。


技术实现要素：

7.本发明的目的在于提供一种自动驾驶多节拖车及其位姿确定方法，适用于多节拖车的位姿确定，且成本较低。
8.为了实现上述目的，本发明提供了一种自动驾驶多节拖车的位姿确定方法，所述自动驾驶多节拖车包括牵引车和至少一节拖车，该方法包括以下步骤：
9.(1)利用牵引车上的车端传感器获取牵引车的航向角，获取牵引车中心轴上任一点的坐标；
10.(2)利用道路上的路端传感器获取自动驾驶多节拖车在转弯过程中每节拖车车身上的标记物位置坐标；
11.(3)结合所述牵引车的航向角、牵引车中心轴上任一点的坐标以及该点在牵引车中心轴上的位置、每节拖车车身上的标记物位置坐标、每节拖车车身上的标记物到该拖车前轴中心点的距离和到该拖车后轴中心点的距离、以及牵引车和各节拖车的尺寸信息，计算得到每节拖车的位姿信息；
12.所述拖车的位姿信息包括拖车的航向角、拖车的前轴中心点坐标和/或后轴中心点坐标；所述牵引车和各节拖车的尺寸信息包括：牵引车的拖钩长度、每节拖车的拖杆长度、除最后一节拖车之外的其余每节拖车的拖钩长度。
13.本发明还提供了一种自动驾驶多节拖车，包括牵引车和至少一节拖车，各节拖车的车身上均安装有标记物；牵引车上安装有控制器和车端传感器，所述控制器与所述车端传感器通讯连接，所述车端传感器用于获取牵引车的航向角；
14.所述控制器还与路端传感器通讯连接，所述路端传感器用于获取自动驾驶多节拖车在转弯过程中每节拖车车身上的标记物位置坐标；
15.所述控制器用于接收牵引车的航向角和每节拖车车身上的标记物位置坐标，还用于获取牵引车中心轴上任一点的坐标，并对牵引车的航向角、牵引车中心轴上任一点的坐标以及该点在牵引车中心轴上的位置、每节拖车车身上的标记物位置坐标、每节拖车车身上的标记物到该拖车前轴中心点的距离和到该拖车后轴中心点的距离、以及牵引车和各节拖车的尺寸信息进行处理，计算得到每节拖车的位姿信息；
16.所述拖车的位姿信息包括拖车的航向角、拖车的前轴中心点坐标和/或后轴中心点坐标；所述牵引车和各节拖车的尺寸信息包括：牵引车的拖钩长度、每节拖车的拖杆长度、除最后一节拖车之外的其余每节拖车的拖钩长度。
17.本发明的有益效果是：本发明综合利用车端传感器获取的信息和路端传感器获取的信息实现自动驾驶多节拖车的位姿确定，是一种基于车路协同的自动驾驶多节拖车的位姿确定方法，本方法只需在各节拖车车身上安装标记物就能实现，成本低，并且，本方法对拖车节数没有要求，适用于多节拖车的位姿确定；同时，拖车车体不需安装电气设备，保证了拖车系统的可靠性和拓展性。
18.进一步地，在上述自动驾驶多节拖车及其位姿确定方法中，通过牵引车上的车端传感器获取所述牵引车中心轴上任一点的坐标。
19.进一步地，在上述自动驾驶多节拖车及其位姿确定方法中，通过在牵引车车身的不同位置上安装标记物，利用路端传感器获取牵引车车身上标记物的位置坐标，根据牵引车车身上标记物的位置坐标得到所述牵引车中心轴上任一点的坐标。
20.进一步地，在上述自动驾驶多节拖车及其位姿确定方法中，当所述牵引车中心轴上任一点的坐标为牵引车的后轴中心点坐标、所述拖车的位姿信息为拖车的航向角和拖车的后轴中心点坐标时，第一节拖车位姿信息的计算过程包括：
21.结合牵引车的后轴中心点坐标、牵引车的航向角和牵引车的拖钩长度，计算得到牵引车拖钩o1的位置坐标；
22.结合牵引车拖钩o1的位置坐标和第一节拖车车身标记物f1的位置坐标，计算得到o1f1与大地坐标系x轴方向的偏角ω1；
23.结合第一节拖车的拖杆长度、第一节拖车车身标记物f1到该拖车前轴中心点c1的距离和牵引车拖钩o1到第一节拖车车身标记物f1的距离，利用余弦定理计算得到c1f1与o1f1的夹角；
24.结合ω1和β1计算得到第一节拖车的航向角θ1＝β1+ω1，并结合第一节拖车的航向角θ1、第一节拖车车身标记物f1的位置坐标和第一节拖车车身标记物f1到该拖车后轴中心点d1的距离，计算得到第一节拖车的后轴中心点坐标d1。
25.进一步地，在上述自动驾驶多节拖车及其位姿确定方法中，第二节拖车位姿信息的计算过程包括：
26.利用第一节拖车的后轴中心点坐标d1、第一节拖车的航向角θ1和第一节拖车的拖钩长度，计算得到第一节拖车拖钩o2的位置坐标；
27.结合第一节拖车拖钩o2的位置坐标和第二节拖车车身标记物f2的位置坐标，计算得到o2f2与大地坐标系x轴方向的偏角ω2；
28.利用第二节拖车的拖杆长度、第二节拖车车身标记物f2到该拖车前轴中心点c2的距离、第一节拖车的拖钩o2到第二节拖车车身标记物f2的距离，计算得到c2f2与o2f2的夹角β2；
29.结合ω2和β2计算得到第二节拖车航向角θ2＝β2+ω2，并利用第二节拖车车身标记物f2的位置坐标、第二节拖车的航向角θ2和第二节拖车车身标记物f2到该拖车后轴中心点d2的距离，计算得到第二节拖车的后轴中心点坐标d2。
附图说明
30.图1是本发明车辆实施例中利用路端传感器识别各节拖车车身上标记物位置坐标的示意图；
31.图2是本发明车辆实施例中自动驾驶多节拖车的位姿确定方法流程图；
32.图3是本发明车辆实施例中的位姿计算原理图。
具体实施方式
33.由于自动驾驶多节拖车在直线行驶过程中的位姿容易确定，本发明针对自动驾驶多节拖车(包括牵引车和至少一节拖车)的转弯行驶工况(尤其是路口转弯行驶工况)，提出了一种自动驾驶多节拖车的位姿确定方法，能够准确地确定自动驾驶多节拖车在转弯过程中每节拖车的位姿。
34.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。
35.车辆实施例：
36.如图1所示，本实施例的自动驾驶多节拖车包括牵引车和3节拖车，其中，各节拖车的车身上均安装有标记物，只有牵引车上安装有控制器和自动驾驶所需的车端传感器，各节拖车均不设置传感器，在自动驾驶多节拖车行驶路线上安装有路端传感器，控制器分别与车端传感器和路端传感器通讯连接。
37.以自动驾驶多节拖车在路口转弯为例，自动驾驶多节拖车转弯过程中，车端传感器用于获取牵引车的位姿信息，牵引车的位姿信息包括牵引车的航向角和牵引车的后轴中
心点坐标(为了便于获取牵引车的后轴中心点坐标，一般将传感器设置在牵引车的后轴中心)；路端传感器用于获取自动驾驶多节拖车在转弯过程中每节拖车车身上的标记物位置坐标；控制器用于对接收到的牵引车位姿信息、每节拖车车身上的标记物位置坐标、每节拖车车身上的标记物到该拖车前轴中心点的距离和到该拖车后轴中心点的距离、以及牵引车和各节拖车的尺寸信息进行处理，实现如图2所示的自动驾驶多节拖车的位姿确定方法(以下简称位姿确定方法)。
38.其中，拖车车身上的标记物可以是反光标志条或具有特殊形状/颜色的物体，当采用反光标志条作为标记物时，由于激光雷达检测到反光标志条的反射强度高，可以利用激光雷达作为路端传感器以检测反光标志条的位置坐标；当采用具有特殊形状/颜色的物体作为标记物时，由于相机对颜色和纹理信息敏感，可以利用相机作为路端传感器以检测标记物的位置坐标；标记物可以置于任何便于路端传感器检测的位置，只需知道下述3个距离中的任意两个即可实现位姿确定方法，这3个距离为：每节拖车前轴到后轴的距离、每节拖车车身上的标记物到该拖车前轴中心点的距离和每节拖车车身上的标记物到该拖车后轴中心点的距离；只要知道这3个距离中的任意两个就能确定每节拖车车身上的标记物到该拖车前轴中心点的距离和每节拖车车身上的标记物到该拖车后轴中心点的距离。
39.本实施例中以标记物为反光标志条、路端传感器为激光雷达、标记物分别安装在每节拖车的车身中点进行说明，图1展示了利用路端传感器识别各节拖车车身上标记物位置坐标的场景。
40.下面对本实施例的位姿确定方法进行详细介绍：
41.(1)利用牵引车上的车端传感器获取牵引车的位姿信息(即牵引车的航向角和牵引车的后轴中心点坐标)；
42.(2)利用路端传感器获取自动驾驶多节拖车在转弯过程中每节拖车车身上的标记物位置坐标；
43.(3)结合牵引车位姿信息、每节拖车车身上的标记物位置坐标、每节拖车车身上的标记物到该拖车前轴中心点的距离和到该拖车后轴中心点的距离、以及牵引车和各节拖车的尺寸信息，确定每节拖车的位姿信息，即确定每节拖车的航向角和后轴中心点坐标。
44.其中，牵引车和各节拖车的尺寸信息包括：牵引车的拖钩长度、每节拖车的拖杆长度、除最后一节拖车之外的其余每节拖车的拖钩长度。
45.下面结合图3详细介绍每节拖车位姿的计算过程，如图3所示，本实施例的位姿确定方法假设标记物(图中黑色圆点)位于拖车的中心轴上，在大地坐标系下计算每节拖车的位姿，并且本实施例的位姿确定方法中涉及到的长度及角度信息均为俯视图下的二维平面投影信息，不涉及高度。
46.计算第一节拖车位姿所需的各变量如下：牵引车的后轴中心点b在大地坐标系下的坐标b(x0,y0)、牵引车的航向角α(即牵引车中心轴与大地坐标系x轴方向的夹角)、第一节拖车车身上的标记物f1在大地坐标系下的位置坐标f1(x1,y1)、牵引车拖钩o1b的长度b、第一节拖车拖杆o1c1的长度c、第一节拖车车身上的标记物f1到该拖车前轴中心点c1的距离c1f1以及第一节拖车车身上的标记物f1到该拖车后轴中心点d1的距离d1f1；其中，b(x0,y0)、α、f1(x1,y1)通过传感器获取，b、c、c1f1和d1f1通过测量得到。
47.如图3所示，第一节拖车位姿信息的计算过程如下：
48.首先，结合牵引车的后轴中心点坐标b(x0,y0)、牵引车的航向角α和牵引车拖钩o1b的长度b，计算得到牵引车拖钩o1的位置坐标o1(x2,y2)：
49.接着，结合牵引车拖钩o1和第一节拖车车身标记物f1的位置坐标，计算得到o1f1与大地坐标系x轴方向的偏角
50.然后，结合o1c1的长度、c1f1的长度和o1f1的长度，利用余弦定理计算得到c1f1与o1f1的夹角β1：其中，d为第一节拖车前轴到后轴的距离(测量得到)，由于本实施例中将标记物设置在每节拖车的车身中点，因此c1f1的长度为e为o1f1的长度，c为第一节拖车拖杆o1c1的长度；
51.最后，得到第一节拖车的航向角θ1＝β1+ω1，并结合第一节拖车的航向角θ1、第一节拖车车身标记物f1的位置坐标和d1f1的长度计算得到第一节拖车的后轴中心点坐标d1(x3,y3)：至此，确定了第一节拖车的位姿信息。
52.根据以上方法进行递推，即可得到后面各节拖车的位姿信息，例如计算第二节拖车的位姿信息时，先利用第一节拖车的后轴中心点坐标d1、第一节拖车的航向角θ1和第一节拖车的拖钩长度d1o2，计算得到第一节拖车拖钩o2的位置坐标；然后，结合第一节拖车拖钩o2的位置坐标和第二节拖车车身标记物f2的位置坐标，计算得到o2f2与大地坐标系x轴方向的偏角ω2；接着，利用第二节拖车的拖杆长度o2c2、第二节拖车车身标记物f2到该拖车前轴中心点c2的距离c2f2、第一节拖车的拖钩o2到第二节拖车车身标记物f2的距离o2f2，计算得到c2f2与o2f2的夹角β2，进而得到第二节拖车航向角θ2＝β2+ω2；最后，利用第二节拖车车身标记物f2的位置坐标、第二节拖车的航向角θ2和第二节拖车车身标记物f2到该拖车后轴中心点d2的距离d2f2，计算得到第二节拖车的后轴中心点坐标d2。
53.本实施例中，牵引车的位姿信息为牵引车的航向角和牵引车的后轴中心点坐标，利用牵引车的航向角、牵引车的后轴中心点坐标和牵引车的拖钩长度计算出牵引车拖钩的位置坐标；结合图3容易看出：如果牵引车的前轴中心点坐标a和牵引车前轴到后轴的距离a已知，那么结合牵引车的航向角、牵引车的前轴中心点坐标a、牵引车前轴到后轴的距离a和牵引车的拖钩长度，同样也可以计算出牵引车拖钩的位置坐标；特殊地，只要知道牵引车中心轴上任一点r的坐标和该点在牵引车中心轴上的位置，就能够结合牵引车的航向角、牵引车中心轴上r点的坐标、r点在牵引车中心轴上的位置到牵引车后轴中心点的距离和牵引车的拖钩长度，计算出牵引车拖钩的位置坐标，其中，牵引车中心轴上任一点r的坐标可通过车端传感器获取，r点在牵引车中心轴上的位置到牵引车后轴中心点的距离可通过测量得到；当然，也可以在牵引车车身不同位置上安装标记物，利用路端传感器获取牵引车车身上标记物的位置坐标，根据牵引车车身上标记物的位置坐标得到牵引车中心轴上任一点r的坐标，例如在牵引车车身中点安装标记物e，利用路端传感器获取牵引车车身标记物e的位置坐标，结合e的位置坐标、be的长度、牵引车的航向角和牵引车的拖钩长度计算出牵引车
拖钩的位置坐标。
54.本实施例中，拖车的位姿信息为拖车的航向角和拖车的后轴中心点坐标，由图3可以看出，当计算出第一节拖车的航向角θ1后，还可以结合第一节拖车的航向角θ1、第一节拖车车身标记物f1的位置坐标和第一节拖车车身标记物到该拖车前轴中心点c1f1的长度，计算出第一节拖车的前轴中心点坐标，因此，拖车的位姿信息也可以为拖车的航向角和拖车的前轴中心点坐标，或者为拖车的航向角、拖车的前轴中心点坐标和拖车的后轴中心点坐标。
55.综上所述，本实施例的位姿确定方法综合利用牵引车上车端传感器获取的信息和路端传感器获取的信息实现自动驾驶多节拖车的位姿确定，是一种基于车路协同的自动驾驶多节拖车的位姿确定方法，本方法只需在各节拖车车身上安装标记物就能实现，成本低，并且，本方法对拖车节数没有要求，适用于多节拖车的位姿确定；同时，拖车车体不需安装电气设备，保证了拖车系统的可靠性和拓展性。
56.方法实施例：
57.本实施例的自动驾驶多节拖车的位姿确定方法如图2所示，该方法已在车辆实施例中进行了详细介绍，此处不再赘述。