半挂汽车列车、拖斗位姿确定方法、装置、设备和介质与流程

1.本技术涉及车辆控制技术领域，尤其涉及一种半挂汽车列车、拖斗位姿确定方法、装置、设备和介质。


背景技术：

2.随着车辆技术的发展，现在有越来越多的搭载智能驾驶系统(如无人驾驶系统、辅助驾驶系统等)的半挂汽车列车投入实际应用中。该半挂汽车列车是包含一个牵引车和至少一个以半挂方式连接的拖斗的汽车列车。为了确保半挂汽车列车的安全行驶，智能驾驶系统需要获得半挂汽车列车的每个车体/车厢的高精度位姿(如位置和航向角等)。
3.现有技术中，通常是在每个车体上安装高精度的传感器来获得相应车体的高精度位姿。例如，在每个车体上均安装全球卫星导航系统(global navigation satellite system,gnss)和惯性导航系统(inertial navigation system,ins)组合而成的导航系统，由导航系统测量获得每个车体的高精度位姿。但是，该实现方案会增加半挂汽车列车的应用成本。


技术实现要素：

4.为了解决上述半挂汽车列车中各车体的高精度位姿获取成本高的技术问题，本技术提供了一种半挂汽车列车、拖斗位姿确定方法、装置、设备和介质。
5.第一方面，本技术提供了一种半挂汽车列车的拖斗位姿确定方法，所述半挂汽车列车由多个车体构成，各所述车体中处于一侧边缘的一个所述车体为牵引车，各所述车体中除了所述牵引车之外的所述车体均为半挂拖斗，该方法包括：
6.获取各所述车体的初始位姿；
7.针对每个所述半挂拖斗：
8.基于第一相邻车体的目标位姿，确定所述第一相邻车体和所述半挂拖斗之间的第一联结点的第一坐标，并基于所述半挂拖斗的所述初始位姿，确定所述第一联结点的第二坐标；其中，所述第一相邻车体为靠近所述牵引车一端的、与所述半挂拖斗连接的所述车体；在所述第一相邻车体为所述牵引车时，所述目标位姿为所述初始位姿；在所述第一相邻车体也为半挂拖斗时，所述目标位姿为校正后的所述初始位姿；
9.基于所述第一坐标和所述第二坐标，确定所述半挂拖斗的所述目标位姿。
10.在一些实施例中，所述基于所述第一坐标和所述第二坐标，确定所述半挂拖斗的所述目标位姿包括：
11.基于所述第一坐标和所述第二坐标，确定所述第一联结点对应的第一位置偏差；
12.基于所述第一位置偏差与偏差阈值之间的比较结果、所述第一坐标和所述第二坐标，对所述半挂拖斗的所述初始位姿进行校正，确定所述半挂拖斗的所述目标位姿；其中，所述偏差阈值用于对相邻两个所述车体进行刚性连接约束。
13.在一些实施例中，所述偏差阈值至少基于所述第一联结点的铰接柱和铰接环之间
的缝隙距离确定。
14.在一些实施例中，所述基于所述第一位置偏差与偏差阈值之间的比较结果、所述第一坐标和所述第二坐标，对所述半挂拖斗的所述初始位姿进行校正，确定所述半挂拖斗的所述目标位姿包括：
15.若所述比较结果为所述第一位置偏差大于所述偏差阈值，则将所述半挂拖斗的所述初始位姿中的初始航向角确定为目标航向角；
16.利用所述第一坐标和所述第二坐标的坐标差值，对所述半挂拖斗的所述初始位姿中的初始坐标进行校正，确定所述半挂拖斗的目标坐标。
17.在一些实施例中，所述基于所述第一位置偏差与偏差阈值之间的比较结果、所述第一坐标和所述第二坐标，对所述半挂拖斗的所述初始位姿进行校正，确定所述半挂拖斗的所述目标位姿包括：
18.若所述比较结果为所述第一位置偏差小于或等于所述偏差阈值，则基于所述第一坐标和所述半挂拖斗的所述初始位姿中的初始坐标，对所述半挂拖斗的所述初始位姿中的初始航向角进行校正，确定所述半挂拖斗的目标航向角；
19.基于第一距离和第二距离的距离差值和所述目标航向角，对所述初始坐标进行校正，确定所述半挂拖斗的目标坐标；其中，所述第一距离为所述第一坐标和所述初始坐标之间的第二位置偏差，所述第二距离为所述第一联结点到所述半挂拖斗的后轴中心之间的距离。
20.在一些实施例中，所述获取各所述车体的初始位姿包括：
21.获取所述牵引车的所述初始位姿、所述牵引车的后轴中心处的第一速度和前轮偏角；
22.基于所述牵引车的所述初始位姿、所述第一速度和所述前轮偏角，以及各所述半挂拖斗的历史航向角，按照所述半挂汽车列车对应的运动学模型，确定各所述半挂拖斗的所述初始位姿；其中，所述历史航向角为当前位姿更新周期的前一相邻位姿更新周期中所得的航向角。
23.在一些实施例中，所述基于所述牵引车的所述初始位姿、所述第一速度和所述前轮偏角，以及各所述半挂拖斗的历史航向角，按照所述半挂汽车列车对应的运动学模型，确定各所述半挂拖斗的所述初始位姿包括：
24.针对每个所述半挂拖斗：
25.基于第二相邻车体的第二速度、所述第二相邻车体的航向角变化率和第三距离，以及所述第二相邻车体和所述半挂拖斗之间的航向夹角，按照所述运动学模型中的速度方程，确定所述半挂拖斗的后轴中心处的第三速度；其中，在所述第二相邻车体为所述牵引车时，所述第二速度为所述第一速度，所述第二相邻车体的航向角变化率基于所述第一速度、所述前轮偏角和第四距离，按照所述运动学模型中包含的所述牵引车的运动方程确定；所述第四距离为所述牵引车车头到所述牵引车的后轴中心之间的距离；在所述第二相邻车体也为半挂拖斗时，所述第二速度为所述第二相邻车体的后轴中心处的速度；所述第三距离为所述第二相邻车体的后轴中心到第二联结点之间的距离，所述第二联结点为所述第二相邻车体与所述半挂拖斗之间的联结点；所述航向夹角由所述第二相邻车体的所述历史航向角和所述半挂拖斗的所述历史航向角确定；
26.基于所述第三速度、所述半挂拖斗的所述历史航向角、第五距离、所述第二速度、所述航向夹角、所述第二相邻车体的航向角变化率和第六距离，按照所述运动学模型中包含的所述半挂拖斗的运动方程，确定所述半挂拖斗的后轴中心处的初始坐标以及所述半挂拖斗的航向角变化率；其中，所述初始坐标和所述半挂拖斗的所述历史航向角为所述半挂拖斗的所述初始位姿；所述第五距离为所述第二联结点到所述半挂拖斗的后轴中心之间的距离；所述第六距离为所述第二相邻车体的后轴中心到所述第二联结点之间的距离。
27.第二方面，本技术提供了一种半挂汽车列车的拖斗位姿确定装置，所述半挂汽车列车由多个车体构成，各所述车体中处于一侧边缘的一个所述车体为牵引车，各所述车体中除了所述牵引车之外的所述车体均为半挂拖斗，该装置包括：
28.初始位姿获取模块，用于获取各所述车体的初始位姿；
29.目标位姿确定模块，用于针对每个所述半挂拖斗执行：
30.基于第一相邻车体的目标位姿，确定所述第一相邻车体和所述半挂拖斗之间的第一联结点的第一坐标，并基于所述半挂拖斗的所述初始位姿，确定所述第一联结点的第二坐标；其中，所述第一相邻车体为靠近所述牵引车一端的、与所述半挂拖斗连接的所述车体；在所述第一相邻车体为所述牵引车时，所述目标位姿为所述初始位姿；在所述第一相邻车体也为半挂拖斗时，所述目标位姿为校正后的所述初始位姿；
31.基于所述第一坐标和所述第二坐标，确定所述半挂拖斗的所述目标位姿。
32.第三方面，本技术提供了一种电子设备，该电子设备包括：
33.处理器和存储器；
34.处理器通过调用存储器存储的程序或指令，用于执行本技术任意实施例中所说明的半挂汽车列车的拖斗位姿确定方法的步骤。
35.第四方面，本技术提供了一种计算机可读存储介质，该计算机可读存储介质存储程序或指令，该程序或所述指令使计算机执行本技术任意实施例中所说明的半挂汽车列车的拖斗位姿确定方法的步骤。
36.第五方面，本技术提供了一种半挂汽车列车，所述半挂汽车列车由多个车体构成，各所述车体中处于一侧边缘的一个所述车体为牵引车，各所述车体中除了所述牵引车之外的所述车体均为半挂拖斗，该半挂汽车列车还包括：控制器；
37.所述控制器，用于执行本技术任意实施例中所说明的半挂汽车列车的拖斗位姿确定方法的步骤。
38.本技术实施例提供的半挂汽车列车、拖斗位姿确定方法、装置、设备和介质，能够在获取半挂汽车列车包含的各车体的初始位姿的基础上，根据两个相邻车体的初始位姿分别推算得到该两个相邻车体之间的联结点的第一坐标和第二坐标，并根据第一坐标和第二坐标的坐标差异，对该两个相邻车体中背离牵引车的车体的初始位姿进行校正，得到校正后的目标位姿，且按照该过程迭代获得每个车体的目标位姿；无需每个车体均安装高精度传感器，便可获得各车体的高精度位姿，降低了半挂汽车列车中各车体的高精度位姿的获取成本，提高了各车体的高精度位姿的获取便捷性。
附图说明
39.为了更清楚地说明本技术实施例的技术方案，下面将对实施例描述中所需要使用
的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本技术应用于其它类似情景。除非从语言环境中显而易见或另做说明，图中相同标号代表相同结构和操作。
40.图1是本技术实施例提供的一种半挂汽车列车的拖斗位姿确定方法的流程图；
41.图2是本技术实施例提供的另一种半挂汽车列车的拖斗位姿确定方法的流程图；
42.图3是本技术实施例提供的一种初始位姿校正的结果示意图；
43.图4是本技术实施例提供的另一种初始位姿校正的结果示意图；
44.图5是本技术实施例提供的一种半挂汽车列车运动模型的示意图；
45.图6是本技术实施例提供的一种半挂汽车列车的拖斗位姿确定装置的结构示意图；
46.图7是本技术实施例提供的一种电子设备的结构示意图；
47.图8是本技术实施例提供的一种半挂汽车列车的结构示意图。
具体实施方式
48.在下面的详细描述中，通过示例阐述了本技术的许多具体细节，以便提供对相关披露的透彻理解。然而，对于本领域的普通技术人员来讲，本技术显而易见的可以在没有这些细节的情况下实施。应当理解的是，本技术中使用“系统”、“装置”、“单元”和/或“模块”术语，是用于区分在顺序排列中不同级别的不同部件、元件、部分或组件的一种方法。然而，如果其他表达式可以实现相同的目的，这些术语可以被其他表达式替换。
49.应当理解的是，当设备、单元或模块被称为“在
……
上”、“连接到”或“耦合到”另一设备、单元或模块时，其可以直接在另一设备、单元或模块上，连接或耦合到或与其他设备、单元或模块通信，或者可以存在中间设备、单元或模块，除非上下文明确提示例外情形。例如，本技术所使用的术语“和/或”包括一个或多个相关所列条目的任何一个和所有组合。
50.本技术所用术语仅为了描述特定实施例，而非限制本技术范围。如本技术说明书和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的特征、整体、步骤、操作、元素和/或组件，而该类表述并不构成一个排它性的罗列，其他特征、整体、步骤、操作、元素和/或组件也可以包含在内。
51.参看下面的说明以及附图，本技术的这些或其他特征和特点、操作方法、结构的相关元素的功能、部分的结合以及制造的经济性可以被更好地理解，其中说明和附图形成了说明书的一部分。然而，可以清楚地理解，附图仅用作说明和描述的目的，并不意在限定本技术的保护范围。可以理解的是，附图并非按比例绘制。
52.本技术中使用了多种结构图用来说明根据本技术的实施例的各种变形。应当理解的是，前面或下面的结构并不是用来限定本技术。本技术的保护范围以权利要求为准。
53.目前应用于半挂汽车列车的智能驾驶系统，需要获得半挂汽车列车中每个车体的高精度位姿，才能精准地进行车辆的运动规划与控制，以实现智能驾驶的半挂汽车列车的安全行驶。为了获得高精度位姿，相关技术中的一种实现方式是在每个车体上安装高精度传感器进行位姿测量，但是该实现方式会提高半挂汽车列车的智能驾驶的成本，尤其是半
挂拖斗较多的情况下，其实现成本更高；另一种实现方式是通过牵引车的测量位姿和半挂汽车列车的运动模型来推算半挂拖斗的位姿，但是该实现方式主要适用于挂载一个半挂拖斗的半挂汽车列车，无法适用于挂载多个半挂拖斗的半挂汽车列车的位姿推算，而且推算所得的位姿的精度较低，无法满足精准的车辆运动规划与控制的需求。
54.基于上述情况，本技术实施例提供一种半挂汽车列车、拖斗位姿确定方法、装置、设备和介质，以通过相邻两个车体的初始位姿分别来推算该两个车体中间的联结点的两个坐标，并根据该两个坐标之间的差异来推算背离牵引车的车体的高精度位姿，并迭代该过程，得到每个车体的高精度位姿，既避免了各车体均需安装高精度传感器而造成的高成本问题，又能适用于推算挂载多个半挂拖斗的半挂汽车列车的位姿推算，且能提高推算所得的各位姿的精度。
55.本技术实施例提供的半挂汽车列车的拖斗位姿确定方法，可应用于确定半挂汽车列车的位姿的场景中，尤其适用于挂载多个半挂拖斗的半挂汽车列车的各车体的位姿推算的场景中。在本技术实施例中，该半挂汽车列车的拖斗位姿确定方法可以由电子设备执行，该电子设备可以包括但不限于半挂汽车列车中的控制器、能够与半挂汽车列车进行实时通信的外部设备；该外部设备例如可以是调度车辆的调度系统或者智能驾驶系统的服务端所在的笔记本电脑、台式电脑或服务器等。
56.图1是本技术实施例提供的一种半挂汽车列车的拖斗位姿确定方法的流程图。如图1所示，该半挂汽车列车的拖斗位姿确定方法具体包括：
57.s110、获取半挂汽车列车中的各车体的初始位姿。
58.其中，半挂汽车列车是指由一个牵引车与一个或多个半挂拖斗(也称为半挂车)组成的铰接列车。本公开实施例中的半挂汽车列车由多个车体构成，且各车体中处于一侧边缘的一个车体为牵引车，除了牵引车之外的车体均为以半挂方式连接的半挂拖斗。初始位姿是初步获得的车体的位姿，其未经过校正，精度相对较低。这里的位姿可以包括车体的位置和航向角。
59.具体地，电子设备在进行位姿处理之前，先判断汽车列车是否为半挂汽车列车。在一示例中，电子设备从外部输入信息(如操作人员输入的信息)中获得标识车辆类型为半挂汽车列车的标识信息，并以此标识信息确定汽车列车为半挂汽车列车。在另一示例中，电子设备根据车辆的属性信息来判断。例如，属性信息表明除了牵引车之外的车体无前轮或者前轮为万向轮，那么可确定该汽车列车为半挂汽车列车。
60.然后，电子设备获取半挂汽车列车中每个车体的初始位姿，即获取牵引车的初始位姿和每个半挂拖斗的初始位姿。在一示例中，电子设备可通过每个车体上安装的传感器来获得初始位姿。例如，每个车体上安装有成本相对较低的、可测量位姿的传感器，由这些传感器测量获得初始位姿。在另一示例中，牵引车上安装有高精度的、可测量位姿的传感器，以测量获得牵引车的高精度位姿，作为牵引车的初始位姿；然后，根据半挂汽车列车的运动学模型，由牵引车的初始位姿推算获得每个半挂拖斗的初始位姿。
61.s120、针对每个半挂拖斗，基于第一相邻车体的目标位姿，确定第一相邻车体和半挂拖斗之间的第一联结点的第一坐标，并基于半挂拖斗的初始位姿，确定第一联结点的第二坐标。
62.其中，第一相邻车体为靠近牵引车一端的、与待推算目标位姿的半挂拖斗连接的
车体。目标位姿是对初始位姿进行校正后所得的高精度位姿。第一联结点是指第一相邻车体和半挂拖斗之间的联结点。在第一相邻车体为牵引车时，第一相邻车体的目标位姿为牵引车的初始位姿；在第一相邻车体也为半挂拖斗时，第一相邻车体的目标位姿为该半挂拖斗的校正后的初始位姿。
63.具体地，电子设备以迭代校正的方式，从牵引车一端开始，逐个校正各车体的初始位姿，以获得每个车体的高精度的目标位姿。并且，电子设备每次校正过程均对相邻两个车体的初始位姿进行处理，且校正背离牵引车的一个车体的初始位姿。
64.具体实施时，将第n-1(n＝1,2,3,
…
,n)个半挂拖斗作为第一相邻车体，而将与第n-1个半挂拖斗连接的第n个半挂拖斗作为待推算目标位姿的半挂拖斗。那么，按照如下公式(1)，电子设备可根据第n-1个半挂拖斗的目标位姿，即第n-1个半挂拖斗的后轴中心坐标(x
n-1
,y
n-1
)和航向角θ
n-1
，计算获得第n-1个半挂拖斗和第n个半挂拖斗之间的第一联结点的坐标，即第一坐标(xf,yf)。
[0065][0066]
其中，l
h,n-1
是第n-1个半挂拖斗的后轴中心到上述第一联结点之间的距离。
[0067]
并且，按照如下公式(2)，电子设备可根据第n个半挂拖斗的初始位姿，即第n个半挂拖斗的后轴中心坐标(xn,yn)和航向角θn，计算获得第n-1个半挂拖斗和第n个半挂拖斗之间的第一联结点的坐标，即第二坐标(xr,yr)。
[0068][0069]
其中，ln是上述第一联结点到第n个半挂拖斗的后轴中心之间的距离。
[0070]
按照上述过程，针对每个待推算目标位姿的半挂拖斗，电子设备可获得由第一相邻车体的目标位姿和待推算目标位姿的半挂拖斗的初始位姿，分别计算得到第一联结点的两个坐标。如此，半挂汽车列车中的每两个相邻车体之间的联结点均可获得第一坐标和第二坐标的两个坐标。
[0071]
s130、针对每个半挂拖斗，基于第一坐标和第二坐标，确定半挂拖斗的目标位姿。
[0072]
具体地，对于某个半挂拖斗，根据上述说明，该半挂拖斗与其第一相邻车体之间的第一联结点有第一坐标和第二坐标。这两个坐标之间的差异是由初始位姿的不准确而导致的。所以，电子设备可根据这两个坐标之间的差异来校正初始位姿。
[0073]
在一示例中，因为第一相邻车体已有目标位姿，可认为由目标位姿计算而得到第一坐标是精准的，故可根据两个坐标的差异来校正该半挂拖斗的初始位姿。例如，可利用两个坐标的差异以及第一联结点和半挂拖斗的中心轴之间的几何关系，反推算出半挂拖斗的后轴中心处的新的坐标和新的航向角，作为半挂拖斗的目标位姿。
[0074]
在另一示例中，考虑到每个半挂拖斗的目标位姿都是推算所得，该目标位姿也可能存在较小的误差，所以，为了降低目标位姿可能存在的误差的影响，可以根据某一权重，融合第一坐标和第二坐标，得到一个融合坐标，再根据该融合坐标以及第一联结点和半挂拖斗的中心轴之间的几何关系，反推算出半挂拖斗的后轴中心处的新的坐标和新的航向角，作为半挂拖斗的目标位姿。
[0075]
按照上述过程，针对每个半挂拖斗，可计算获得每个半挂拖斗的目标位姿。
[0076]
需要说明的是，任一半挂拖斗须先通过本公开实施例的位姿校正过程(即s120和s130)计算得到其目标位姿，才可作为与该半挂拖斗连接的另一半挂拖斗的第一相邻车体进行该另一半挂拖斗的位姿校正。如此，针对每个半挂拖斗，需循环执行s120和s130。
[0077]
为了更加便于理解上述过程，可将上述s120和s130的过程描述为图2所示的循环过程：
[0078]
s210、将牵引车作为第一相邻车体，且将牵引车连接的第一个半挂拖斗作为目标半挂拖斗。
[0079]
具体地，考虑到牵引车的初始位姿已经是高精度位姿，其不需要进行校正，故电子设备在第一次校正过程中，将牵引车作为第一相邻车体，而将连接牵引车的第一个半挂拖斗作为待推算目标位姿的目标半挂拖斗。
[0080]
s220、基于第一相邻车体的目标位姿，确定第一相邻车体和目标半挂拖斗之间的第一联结点的第一坐标，并基于目标半挂拖斗的初始位姿，确定第一联结点的第二坐标。
[0081]
具体地，当第一相邻车体为牵引车时，n＝1，那么根据公式(1)可获得如下公式(3)。这样，电子设备按照公式(3)，根据牵引车的目标位姿，即牵引车的后轴中心坐标(x0,y0)和航向角θ0，计算第一联结点的坐标，即第一坐标(xf,yf)：
[0082][0083]
其中，l
h,0
是牵引车的后轴中心到第一联结点之间的距离。
[0084]
然后，根据公式(2)可获得如下公式(4)，电子设备按照公式(4)，根据第一个半挂拖斗的初始位姿，即第一个半挂拖斗的后轴中心坐标(x1,y1)和航向角θ1，计算第一联结点的坐标，即第二坐标(xr,yr)：
[0085][0086]
其中，l1是第一联结点到第一个半挂拖斗的后轴中心之间的距离。
[0087]
当第一相邻车体为不为牵引车时，n＝2,3,
…
,n，那么按照s120中的公式(1)和公式(2)计算第一坐标(xf,yf)和第二坐标(xr,yr)。
[0088]
s230、基于第一坐标和第二坐标，确定目标半挂拖斗的目标位姿。
[0089]
具体地，当第一相邻车体为牵引车时，电子设备根据牵引车对应的第一坐标(xf,yf)和第一个半挂拖斗对应的第二坐标(xr,yr)，可计算获得第一个半挂拖斗的目标位姿。
[0090]
当第一相邻车体不为牵引车时，电子设备按照s130的描述过程计算目标半挂拖斗的目标位姿。
[0091]
s240、判断目标半挂拖斗是否为最后一个半挂拖斗。若是，则结束位姿校正流程；若否，则执行s250。
[0092]
s250、利用目标半挂拖斗更新第一相邻车体，且利用目标半挂拖斗连接的、背离牵引车的下一个半挂拖斗更新目标半挂拖斗，并返回执行s220～s240。
[0093]
本技术实施例提供的上述半挂汽车列车的拖斗位姿确定方法，能够在获取半挂汽车列车包含的各车体的初始位姿的基础上，根据两个相邻车体的初始位姿分别推算得到该两个相邻车体之间的联结点的第一坐标和第二坐标，并根据第一坐标和第二坐标的坐标差
异，对该两个相邻车体中背离牵引车的车体的初始位姿进行校正，得到校正后的目标位姿，且按照该过程迭代获得每个车体的目标位姿；无需每个车体均安装高精度传感器，便可获得各车体的高精度位姿，降低了半挂汽车列车中各车体的高精度位姿的获取成本，提高了各车体的高精度位姿的获取便捷性。
[0094]
在一些实施例中，s130包括如下步骤a和步骤b。
[0095]
步骤a、基于第一坐标和第二坐标，确定第一联结点对应的第一位置偏差。
[0096]
具体地，计算目标位姿的过程中，电子设备先按照如下公式(5)，根据第一坐标和第二坐标，计算第一联结点的位置偏差，即第一位置偏差dn：
[0097][0098]
步骤b、基于第一位置偏差与偏差阈值之间的比较结果、第一坐标和第二坐标，对半挂拖斗的初始位姿进行校正，确定半挂拖斗的目标位姿。
[0099]
其中，偏差阈值是预先设置的、位置偏差的临界值，其用于判断坐标偏差的大小，进而用于确定位姿校正的方式。因半挂汽车列车中的相邻两个车体之间是通过刚体连接的，所以相邻两个车体的目标位姿需确保相邻两个车体之间的第一联结点处满足刚性连接约束。基于此，本公开实施例中，偏差阈值用于对相邻两个车体进行刚性连接约束。
[0100]
示例性地，偏差阈值至少基于第一联结点的铰接柱和铰接环之间的缝隙距离确定。因为第一联结点处是通过铰接柱和铰接环进行刚性连接的，所以偏差阈值至少可以根据铰接环套住铰接柱后，两者之间的最大的缝隙距离来确定。此外，考虑到运动学模型的建模误差、车辆控制计算中的离散误差、道路情况引起的行驶误差等多种因素，本公开实施例中，还可在缝隙距离的基础上，叠加一个经验值，来确定最终的偏差阈值，以提高偏差阈值的准确性。
[0101]
具体地，电子设备可以根据第一位置偏差dn与偏差阈值ds的大小关系，判断第一位置偏差是否满足刚性连接约束，并根据判断结果来确定适合于该半挂拖斗的位姿校正方式，且对第一坐标和第二坐标进行校正处理，得到目标位姿。
[0102]
例如，第一位置偏差dn大于偏差阈值ds时，说明该第一位置偏差较大，其不满足刚性连接约束，此时可对其采用大尺度的位姿校正方式，以利用第一坐标和第二坐标对较大的第一位置偏差进行校正，得到目标位姿。
[0103]
又如，第一位置偏差dn小于或等于偏差阈值ds时，说明该第一位置偏差较小，其满足刚性连接约束，此时可对其采用小尺度的位姿校正方式，以利用第一坐标和第二坐标对较小的第一位置偏差进行校正，得到目标位姿。
[0104]
本技术上述实施例中，通过偏差阈值的设置，对第一坐标和第二坐标的位姿校正进行刚性连接约束，使得校正后的目标位姿满足各车体之间刚性连接的要求，进一步提高目标位姿的准确性。
[0105]
在一示例中，步骤b包括：若比较结果为第一位置偏差大于偏差阈值，则将半挂拖斗的初始位姿中的初始航向角确定为目标航向角；利用第一坐标和第二坐标的坐标差值，对半挂拖斗的初始位姿中的初始坐标进行校正，确定半挂拖斗的目标坐标。
[0106]
具体地，如果第一位置偏差大于偏差阈值，说明该第一位置偏差较大，第一相邻车体和待推算目标位姿的半挂拖斗之间，因距离偏差过大而不满足刚性连接，此时可对其采
用大尺度的位姿校正方式，即主要校正第一相邻车体和待推算目标位姿的半挂拖斗之间的距离偏差，航向角的偏差相对于距离偏差可以忽略。所以，电子设备保持半挂拖斗的航向角不变，即将待推算目标位姿的半挂拖斗的初始位姿中的航向角(即初始航向角)作为目标位姿中的航向角(即目标航向角)。
[0107]
然后，电子设备按照如下公式(6)，先利用第一坐标和第二坐标分别计算两个坐标轴上的坐标差值(x
f-xr)和(y
f-yr)，再利用坐标差值对初始位姿中的初始坐标(xn,yn)进行校正，得到目标位姿中的目标坐标(x
′n,y
′n)。
[0108][0109]
参见图3，根据第一相邻车体301的目标位姿推算得到第一联结点的第一坐标(xf,yf)，且根据半挂拖斗302的初始位姿推算得到第一联结点的第二坐标(xr,yr)。该第一坐标和第二坐标之间的第一位置偏差大于偏差阈值，便按照上述过程对半挂拖斗进行航向角不变的平移校正，确定校正后的半挂拖斗302
′
，且得到校正后的半挂拖斗302
′
的目标坐标(x
′n,y
′n)。
[0110]
在另一示例中，步骤b包括：若比较结果为第一位置偏差小于或等于偏差阈值，则基于第一坐标和半挂拖斗的初始位姿中的初始坐标，对半挂拖斗的初始位姿中的初始航向角进行校正，确定半挂拖斗的目标航向角；基于第一距离和第二距离的距离差值和目标航向角，对初始坐标进行校正，确定半挂拖斗的目标坐标。
[0111]
其中，第一距离为第一坐标和初始坐标之间的第二位置偏差，第二距离为第一联结点到半挂拖斗的后轴中心之间的距离。
[0112]
具体地，如果第一位置偏小于或等于偏差阈值，说明该第一位置偏差相对较小，第一相邻车体和待推算目标位姿的半挂拖斗之间满足刚性连接，此时可对其采用小尺度的位姿校正方式，即同时校正第一相邻车体和待推算目标位姿的半挂拖斗之间的距离偏差和航向角偏差。
[0113]
对于航向角校正，电子设备根据第一联结点的第一坐标(xf,yf)和半挂拖斗的初始位姿中的初始坐标(xn,yn)，按照如下公式(7)，计算得到半挂拖斗的目标航向角θ'n。
[0114][0115]
对于坐标校正，电子设备先利用第一联结点的第一坐标(xf,yf)和半挂拖斗的初始坐标(xn,yn)，按照公式计算得到第一坐标和初始坐标之间的第二位置偏差，即第一距离dg。然后，电子设备按照如下公式(8)，利用第一距离和第二距离之间的距离差值(d
g-ln)、目标航向角θ'n和初始坐标(xn,yn)，计算获得目标坐标(x
′n,y
′n)。
[0116][0117]
参见图4，根据第一相邻车体401的目标位姿推算得到第一联结点的第一坐标(xf,yf)，且根据半挂拖斗402的初始位姿推算得到第一联结点的第二坐标(xr,yr)。该第一坐标和第二坐标之间的第一位置偏差小于或等于偏差阈值，便按照上述过程对半挂拖斗的初始
1个车体之间的航向夹角；是第n-1个车体的航向角变化率；l
h,n-1
是第n-1个车体的后轴中心到第n-1个车体与第n个车体之间的联结点的距离。
[0131]
当第二相邻车体为第n-1个车体时，待推算初始位姿的半挂拖斗便为第n个车体，那么按照上述速度方程(9)，电子设备可根据第二相邻车体的第二速度v
n-1
、第二相邻车体的航向角变化率和第三距离l
h,n-1
，以及第二相邻车体和半挂拖斗之间的航向夹角βn，计算得到半挂拖斗的后轴中心处的第三速度vn。其中的航向夹角βn由第二相邻车体的历史航向角θ
n-1
和半挂拖斗的历史航向角θn确定，即βn＝θ
n-1-θn。
[0132]
上述过程中，当第二相邻车体也为半挂拖斗时，第二相邻车体的航向角通过本示例中的步骤d1～步骤d2迭代计算所得。当第二相邻车体为牵引车时，第二相邻车体的航向角变化率根据半挂汽车列车的运动学模型中包含的牵引车的运动方程(如公式(10)所示)，利用第一速度v0、牵引车的前轮偏角δf以及牵引车车头到牵引车的后轴中心之间的第四距离l0计算得到。
[0133][0134]
步骤d2、基于第三速度、半挂拖斗的历史航向角、第五距离、第二速度、航向夹角、第二相邻车体的航向角变化率和第六距离，按照运动学模型中包含的半挂拖斗的运动方程，确定半挂拖斗的后轴中心处的初始坐标以及半挂拖斗的航向角变化率。
[0135]
其中，第五距离为第二联结点到半挂拖斗的后轴中心之间的距离。第六距离为第二相邻车体的后轴中心到第二联结点之间的距离。
[0136]
具体地，电子设备在获得半挂拖斗的后轴中心处的第三速度后，便可利用如下公式(11)所示的运动学模型中包含的半挂拖斗的运动方程，分别计算半挂拖斗的后轴中心处的初始坐标以及半挂拖斗的航向角变化率。
[0137][0138]
其中，vn是第三速度，θn是半挂拖斗的历史航向角，ln是第二联结点到半挂拖斗的后轴中心之间的第五距离，v
n-1
是半挂拖斗的第二速度，βn是第二相邻车体和半挂拖斗之间的航向夹角，是第二相邻车体的航向角变化率，l
h,n-1
是第二相邻车体的后轴中心到第二联结点之间的第六距离。
[0139]
按照上述过程计算所得的半挂拖斗的初始坐标和上一周期所得的半挂拖斗的历史航向角为半挂拖斗的初始位姿。而计算所得的半挂拖斗的航向角变化率则作为该半挂拖斗的相邻的下一个半挂拖斗的计算基础数据。
[0140]
以下是本技术实施例提供的半挂汽车列车的拖斗位姿确定装置的实施例，该装置与上述各实施例的半挂汽车列车的拖斗位姿确定方法属于同一个发明构思，在半挂汽车列车的拖斗位姿确定装置的实施例中未详尽描述的细节内容，可以参考上述半挂汽车列车的拖斗位姿确定方法的实施例。
[0141]
图6示出了本技术实施例提供的一种半挂汽车列车的拖斗位姿确定装置的结构示
意图。该半挂汽车列车由多个车体构成，各车体中处于一侧边缘的一个车体为牵引车，各车体中除了牵引车之外的车体均为半挂拖斗。如图6所示，该半挂汽车列车的拖斗位姿确定装置600具体包括：
[0142]
初始位姿获取模块610，用于获取各车体的初始位姿；
[0143]
目标位姿确定模块620，用于针对每个半挂拖斗执行：
[0144]
基于第一相邻车体的目标位姿，确定第一相邻车体和半挂拖斗之间的第一联结点的第一坐标，并基于半挂拖斗的初始位姿，确定第一联结点的第二坐标；其中，第一相邻车体为靠近牵引车一端的、与半挂拖斗连接的车体；在第一相邻车体为牵引车时，目标位姿为初始位姿；在第一相邻车体也为半挂拖斗时，目标位姿为校正后的初始位姿；
[0145]
基于第一坐标和第二坐标，确定半挂拖斗的目标位姿。
[0146]
本技术实施例提供的上述半挂汽车列车的拖斗位姿确定装置，能够在获取半挂汽车列车包含的各车体的初始位姿的基础上，根据两个相邻车体的初始位姿分别推算得到该两个相邻车体之间的联结点的第一坐标和第二坐标，并根据第一坐标和第二坐标的坐标差异，对该两个相邻车体中背离牵引车的车体的初始位姿进行校正，得到校正后的目标位姿，且按照该过程迭代获得每个车体的目标位姿；无需每个车体均安装高精度传感器，便可获得各车体的高精度位姿，降低了半挂汽车列车中各车体的高精度位姿的获取成本，提高了各车体的高精度位姿的获取便捷性。
[0147]
在一些实施例中，目标位姿确定模块620包括：
[0148]
坐标确定子模块，用于基于第一相邻车体的目标位姿，确定第一相邻车体和半挂拖斗之间的第一联结点的第一坐标，并基于半挂拖斗的初始位姿，确定第一联结点的第二坐标；
[0149]
目标位姿确定子模块，用于：
[0150]
基于第一坐标和第二坐标，确定第一联结点对应的第一位置偏差；
[0151]
基于第一位置偏差与偏差阈值之间的比较结果、第一坐标和第二坐标，对半挂拖斗的初始位姿进行校正，确定半挂拖斗的目标位姿；其中，偏差阈值用于对相邻两个车体进行刚性连接约束。
[0152]
在一些实施例中，偏差阈值至少基于第一联结点的铰接柱和铰接环之间的缝隙距离确定。
[0153]
在一些实施例中，目标位姿确定子模块具体用于：
[0154]
若比较结果为第一位置偏差大于偏差阈值，则将半挂拖斗的初始位姿中的初始航向角确定为目标航向角；
[0155]
利用第一坐标和第二坐标的坐标差值，对半挂拖斗的初始位姿中的初始坐标进行校正，确定半挂拖斗的目标坐标。
[0156]
在另一些实施例中，目标位姿确定子模块具体用于：
[0157]
若比较结果为第一位置偏差小于或等于偏差阈值，则基于第一坐标和半挂拖斗的初始位姿中的初始坐标，对半挂拖斗的初始位姿中的初始航向角进行校正，确定半挂拖斗的目标航向角；
[0158]
基于第一距离和第二距离的距离差值和目标航向角，对初始坐标进行校正，确定半挂拖斗的目标坐标；其中，第一距离为第一坐标和初始坐标之间的第二位置偏差，第二距
离为第一联结点到半挂拖斗的后轴中心之间的距离。
[0159]
在一些实施例中，初始位姿获取模块610包括：
[0160]
前轮偏角获取子模块，用于获取牵引车的初始位姿、牵引车的后轴中心处的第一速度和前轮偏角；
[0161]
初始位姿确定子模块，用于基于牵引车的初始位姿、第一速度和前轮偏角，以及各半挂拖斗的历史航向角，按照半挂汽车列车对应的运动学模型，确定各半挂拖斗的初始位姿；其中，历史航向角为当前位姿更新周期的前一相邻位姿更新周期中所得的航向角。
[0162]
在一些实施例中，初始位姿确定子模块具体用于针对每个半挂拖斗：
[0163]
基于第二相邻车体的第二速度、第二相邻车体的航向角变化率和第三距离，以及第二相邻车体和半挂拖斗之间的航向夹角，按照运动学模型中的速度方程，确定半挂拖斗的后轴中心处的第三速度；其中，在第二相邻车体为牵引车时，第二速度为第一速度，第二相邻车体的航向角变化率基于第一速度、前轮偏角和第四距离，按照运动学模型中包含的牵引车的运动方程确定；第四距离为牵引车车头到牵引车的后轴中心之间的距离；在第二相邻车体也为半挂拖斗时，第二速度为第二相邻车体的后轴中心处的速度；第三距离为第二相邻车体的后轴中心到第二联结点之间的距离，第二联结点为第二相邻车体与半挂拖斗之间的联结点；航向夹角由第二相邻车体的历史航向角和半挂拖斗的历史航向角确定；
[0164]
基于第三速度、半挂拖斗的历史航向角、第五距离、第二速度、航向夹角、第二相邻车体的航向角变化率和第六距离，按照运动学模型中包含的半挂拖斗的运动方程，确定半挂拖斗的后轴中心处的初始坐标以及半挂拖斗的航向角变化率；其中，初始坐标和半挂拖斗的历史航向角为半挂拖斗的初始位姿；第五距离为第二联结点到半挂拖斗的后轴中心之间的距离；第六距离为第二相邻车体的后轴中心到第二联结点之间的距离。
[0165]
本技术实施例所提供的半挂汽车列车的拖斗位姿确定装置可执行本技术任意实施例所提供的半挂汽车列车的拖斗位姿确定方法，具备执行方法相应的功能模块和有益效果。
[0166]
值得注意的是，上述半挂汽车列车的拖斗位姿确定装置的实施例中，所包括的各个模块、子模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能模块/子模块的具体名称也只是为了便于相互区分，并不用于限制本技术的保护范围。
[0167]
图7是适于用来实现根据本技术实施方式的电子设备的结构示意图。该电子设备可以包括但不限于半挂汽车列车中的控制器、能够与半挂汽车列车进行实时通信的外部设备。该外部设备例如可以是调度车辆的调度系统或者智能驾驶系统的服务端所在的笔记本电脑、台式电脑或服务器等。
[0168]
如图7所示，电子设备700包括中央处理单元(cpu)701，其可以根据存储在只读存储器(rom)702中的程序或者从存储部分708加载到随机访问存储器(ram)703中的程序而执行上述任意实施例中的半挂汽车列车的拖斗位姿确定方法中的各种处理。在ram703中，还存储有电子设备700操作所需的各种程序和数据。cpu701、rom702以及ram703通过总线704彼此相连。输入/输出(i/o)接口705也连接至总线704。
[0169]
可选地，以下部件连接至i/o接口705：包括键盘、鼠标等的输入部分706；包括诸如阴极射线管(crt)、液晶显示器(lcd)等以及扬声器等的输出部分707；包括硬盘等的存储部
分708；以及包括诸如lan卡、调制解调器等的网络接口卡的通信部分709。通信部分709经由诸如因特网的网络执行通信处理。驱动器710也根据需要连接至i/o接口705。可拆卸介质711，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器710上，以便于从其上读出的计算机程序根据需要被安装入存储部分708。
[0170]
特别地，根据本技术的实施方式，上文任意实施例描述的方法可以被实现为计算机软件程序。例如，本技术的实施方式包括一种计算机程序产品，其包括有形地包含在及其可读介质上的计算机程序，计算机程序包含用于执行本技术任意实施例中的半挂汽车列车的拖斗位姿确定方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分709从网络上被下载和安装，和/或从可拆卸介质711被安装。
[0171]
本技术还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施方式中所说明的电子设备中所包含的计算机可读存储介质；也可以是单独存在，未装配入电子设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，程序被一个或者一个以上的处理器用来执行描述于本技术的半挂汽车列车的拖斗位姿确定方法。
[0172]
图8示出了本技术实施例提供的一种半挂汽车列车的结构示意图。该半挂汽车列车由多个车体构成(图8未示出)，各车体中处于一侧边缘的一个车体为牵引车，各车体中除了牵引车之外的车体均为半挂拖斗。如图8所示，该半挂汽车列车800包括：控制器810；
[0173]
控制器810，用于执行上述任意实施例所说明的半挂汽车列车的拖斗位姿方法的步骤。
[0174]
上述控制器810可以是软件系统、硬件系统或者软硬件结合的系统。例如，控制器810是运行在操作系统上的软件系统，车载硬件系统是支持操作系统运行的硬件系统。
[0175]
尽管图8中未示出，但是可理解的是，半挂汽车列车800至少还包括多个传感器和底层执行系统。
[0176]
多个传感器至少用于获取半挂汽车列车中牵引车的环境数据和行驶状态相关的数据，以及获取半挂拖斗的行驶状态相关的数据，并将环境数据和行驶状态相关的数据发送至控制器810，以为控制器810提供数据基础。
[0177]
在一些实施例中，多个传感器包括但不限于轮转速传感器、速度传感器、加速度传感器、方向盘转角传感器、前轮转角传感器等，用于获取车辆行驶数据；以及包括但不限于摄像头、激光雷达和毫米波雷达等，用于感知车辆周围环境。另外，用于测量位姿的传感器例如可以包括gnss传感器和ins传感器。
[0178]
底层执行系统至少用于接收来自控制器810的信息，并控制半挂汽车列车的行驶。底层执行系统包括但不限于底盘系统、驱动系统、转向系统和制动系统等等。
[0179]
需要说明的是，附图中的流程图和框图，图示了按照本技术各种实施方式的方法、装置、设备和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，路程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，上述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中
的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0180]
描述于本技术实施方式中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定。
[0181]
应当理解的是，本技术的上述具体实施方式仅仅用于示例性说明或解释本技术的原理，而不构成对本技术的限制。因此，在不偏离本技术的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本技术的保护范围之内。此外，本技术所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。