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

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.初始位姿获取模块，用于获取各所述车体的初始位姿；所述初始位姿包括初始前轴位姿和初始后轴位姿；
41.目标位姿确定模块，用于针对每个所述全挂拖斗执行：
42.基于第一相邻车体的目标后轴位姿，确定所述第一相邻车体和所述全挂拖斗之间的第一联结点的第一坐标，并基于所述全挂拖斗的所述初始前轴位姿，确定所述第一联结点的第二坐标；其中，所述第一相邻车体为靠近所述牵引车一端的、与所述全挂拖斗连接的所述车体；在所述第一相邻车体为所述牵引车时，所述目标后轴位姿为所述初始后轴位姿；在所述第一相邻车体也为全挂拖斗时，所述目标后轴位姿为校正后的所述初始后轴位姿；
43.基于所述第一坐标和所述第二坐标，确定所述全挂拖斗的目标前轴位姿；
44.基于所述全挂拖斗的所述初始后轴位姿，确定所述全挂拖斗的前轴中心的第三坐标；
45.基于所述目标前轴位姿中的目标前轴坐标和所述第三坐标，确定所述全挂拖斗的所述目标后轴位姿。
46.第三方面，本技术提供了一种电子设备，该电子设备包括：
47.处理器和存储器；
48.处理器通过调用存储器存储的程序或指令，用于执行本技术任意实施例中所说明的全挂汽车列车的拖斗位姿确定方法的步骤。
49.第四方面，本技术提供了一种计算机可读存储介质，该计算机可读存储介质存储程序或指令，该程序或所述指令使计算机执行本技术任意实施例中所说明的全挂汽车列车的拖斗位姿确定方法的步骤。
50.第五方面，本技术提供了一种车辆，该车辆包括：控制器；
51.所述控制器，用于执行本技术任意实施例中所说明的全挂汽车列车的拖斗位姿确定方法的步骤。
52.本技术实施例提供的全挂汽车列车、拖斗位姿确定方法、装置、设备和介质，能够在获取全挂汽车列车包含的各车体的初始位姿的基础上，根据两个相邻车体中的前一车体的目标后轴位姿和后一车体的初始前轴位姿，分别推算得到该两个相邻车体之间的联结点的第一坐标和第二坐标，并根据第一坐标和第二坐标的坐标差异，对后一车体的初始前轴位姿进行校正，得到校正后的目标前轴位姿，且根据后一车体的初始后轴位姿得到该后一车体的前轴中心的第三坐标后，利用后一车体的目标前轴位姿和第三坐标推算得到后一车体的目标后轴位姿；无需每个车体均安装高精度传感器，便可获得各车体的高精度位姿，降低了全挂汽车列车中各车体的高精度位姿的获取成本，提高了各车体的高精度位姿的获取便捷性。
附图说明
53.为了更清楚地说明本技术实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见地，下面描述中的附图仅仅是本技术的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图将本技术应用于其它类似情景。除非从语言环境中显而易见或另做说明，图中相同标号代表相同结构和操作。
54.图1是本技术实施例提供的一种全挂汽车列车的拖斗位姿确定方法的流程图；
55.图2是本技术实施例提供的一种初始位姿校正的结果示意图；
56.图3是本技术实施例提供的一种全挂汽车列车运动模型的示意图；
57.图4是本技术实施例提供的一种全挂汽车列车的拖斗位姿确定装置的结构示意图；
58.图5是本技术实施例提供的一种电子设备的结构示意图；
59.图6是本技术实施例提供的一种全挂汽车列车的结构示意图。
具体实施方式
60.在下面的详细描述中，通过示例阐述了本技术的许多具体细节，以便提供对相关披露的透彻理解。然而，对于本领域的普通技术人员来讲，本技术显而易见的可以在没有这些细节的情况下实施。应当理解的是，本技术中使用“系统”、“装置”、“单元”和/或“模块”术语，是用于区分在顺序排列中不同级别的不同部件、元件、部分或组件的一种方法。然而，如果其他表达式可以实现相同的目的，这些术语可以被其他表达式替换。
61.应当理解的是，当设备、单元或模块被称为“在
……
上”、“连接到”或“耦合到”另一设备、单元或模块时，其可以直接在另一设备、单元或模块上，连接或耦合到或与其他设备、单元或模块通信，或者可以存在中间设备、单元或模块，除非上下文明确提示例外情形。例如，本技术所使用的术语“和/或”包括一个或多个相关所列条目的任何一个和所有组合。
62.本技术所用术语仅为了描述特定实施例，而非限制本技术范围。如本技术说明书和权利要求书中所示，除非上下文明确提示例外情形，“一”、“一个”、“一种”和/或“该”等词并非特指单数，也可包括复数。一般说来，术语“包括”与“包含”仅提示包括已明确标识的特征、整体、步骤、操作、元素和/或组件，而该类表述并不构成一个排它性的罗列，其他特征、整体、步骤、操作、元素和/或组件也可以包含在内。
63.参看下面的说明以及附图，本技术的这些或其他特征和特点、操作方法、结构的相关元素的功能、部分的结合以及制造的经济性可以被更好地理解，其中说明和附图形成了说明书的一部分。然而，可以清楚地理解，附图仅用作说明和描述的目的，并不意在限定本技术的保护范围。可以理解的是，附图并非按比例绘制。
64.本技术中使用了多种结构图用来说明根据本技术的实施例的各种变形。应当理解的是，前面或下面的结构并不是用来限定本技术。本技术的保护范围以权利要求为准。
65.目前应用于全挂汽车列车的智能驾驶系统，需要获得全挂汽车列车中每个车体的高精度位姿，才能精准地进行车辆的运动规划与控制，以实现智能驾驶的全挂汽车列车的安全行驶。为了获得高精度位姿，相关技术中的一种实现方式是在每个车体上安装高精度传感器进行位姿测量，但是该实现方式会提高全挂汽车列车的智能驾驶的成本，尤其是全
挂拖斗较多的情况下，其实现成本更高；另一种实现方式是通过牵引车的测量位姿和全挂汽车列车的运动模型来推算全挂拖斗的位姿，但是该实现方式主要适用于挂载一个全挂拖斗的全挂汽车列车，无法适用于挂载多个全挂拖斗的全挂汽车列车的位姿推算，而且推算所得的位姿的精度较低，无法满足精准的车辆运动规划与控制的需求。
66.基于上述情况，本技术实施例提供一种全挂汽车列车、拖斗位姿确定方法、装置、设备和介质，以通过全挂汽车列车中每个车体的初始位姿来推算相应车体的高精度位姿，既避免了各车体均需安装高精度传感器而造成的高成本问题，又能适用于推算挂载多个全挂拖斗的全挂汽车列车的位姿推算，且能提高推算所得的各位姿的精度。
67.本技术实施例提供的全挂汽车列车的拖斗位姿确定方法，可应用于确定全挂汽车列车的位姿的场景中，尤其适用于挂载多个全挂拖斗的全挂汽车列车的各车体的位姿推算的场景中。在本技术实施例中，该全挂汽车列车的拖斗位姿确定方法可以由电子设备执行，该电子设备可以包括但不限于全挂汽车列车中的控制器、能够与全挂汽车列车进行实时通信的外部设备；该外部设备例如可以是调度车辆的调度系统或者智能驾驶系统的服务端所在的笔记本电脑、台式电脑或服务器等。
68.图1是本技术实施例提供的一种全挂汽车列车的拖斗位姿确定方法的流程图。如图1所示，该全挂汽车列车的拖斗位姿确定方法具体包括：
69.s110、获取各车体的初始位姿，该初始位姿包括初始前轴位姿和初始后轴位姿。
70.其中，全挂汽车列车是指由一个牵引车与一个或多各全挂拖斗(也称为全挂车)组成的铰接列车。本公开实施例中的全挂汽车列车由多个车体构成，且各车体中处于一侧边缘的一个车体为牵引车，除了牵引车之外的车体均为以全挂方式连接的全挂拖斗。初始位姿是初步获得的车体的位姿，其未经过校正，精度相对较低。因全挂汽车的车体结构，使得每个车体的前轴和后轴的位姿可能不同，故每个车体的初始位姿可以包含初始前轴位姿和初始后轴位姿。这里的位姿可以包括车体的位置和航向角。
71.具体地，电子设备在进行位姿处理之前，先判断汽车列车是否为全挂汽车列车。在一示例中，电子设备从外部输入信息(如操作人员输入的信息)中获得标识车辆类型为全挂汽车列车的标识信息，并以此标识信息确定汽车列车为全挂汽车列车。在另一示例中，电子设备根据车辆的属性信息来判断。例如，属性信息表明除了牵引车之外的车体为轴转向车体或轮转向车体，那么可确定该汽车列车为全挂汽车列车。这里的轴转向是指车体前轮和连接杆固结在一起，由连接杆的转向带动车体的前轮转向。轮转向是指车体和前轮固定，由连接杆带动转向机构旋转，控制车体前轮转向。
72.然后，电子设备可以按照全挂汽车列车对应的方式获取全挂汽车列车中每个车体的初始位姿，即获取牵引车的初始位姿和每个全挂拖斗的初始位姿。在一示例中，电子设备可通过每个车体上安装的传感器来获得初始位姿。例如，每个车体上安装有成本相对较低的、可测量位姿的传感器，由这些传感器测量获得初始位姿。在另一示例中，牵引车上安装有高精度的、可测量位姿的传感器，以测量获得牵引车的高精度位姿，作为牵引车的初始位姿；然后，根据全挂汽车列车的运动学模型，由牵引车的初始位姿推算获得每个全挂拖斗的初始位姿。
73.s120、针对每个全挂拖斗执行如下s121～s124。本公开实施例是通过迭代计算的方式逐个车体推算其前轴和后轴的高精度的位姿(即目标前轴位姿和目标后轴位姿)，所
以，对于每个全挂拖斗均执行相同的处理过程。
74.s121、基于第一相邻车体的目标后轴位姿，确定第一相邻车体和全挂拖斗之间的第一联结点的第一坐标，并基于全挂拖斗的初始前轴位姿，确定第一联结点的第二坐标。
75.其中，第一相邻车体为靠近牵引车一端的、与待推算目标前轴位姿和目标后轴位姿的全挂拖斗连接的车体。第一联结点是指第一相邻车体和全挂拖斗之间的联结点。在第一相邻车体为牵引车时，目标后轴位姿为牵引车的初始后轴位姿；在第一相邻车体也为全挂拖斗时，目标后轴位姿为该全挂拖斗按照本公开实施例的校正过程进行位姿校正后的初始后轴位姿。
76.具体地，电子设备将第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)。
[0077][0078]
其中，l
h,n-1
是第n-1个全挂拖斗的后轴中心到上述第一联结点之间的距离。
[0079]
并且，按照如下公式(2)，电子设备可根据第n个全挂拖斗的初始前轴位姿，即第n个全挂拖斗的前轴中心的初始前轴坐标(x
f,n
,y
f,n
)和前轴航向角θ
f,n
，计算获得上述第一联结点的坐标，即第二坐标(xr,yr)。
[0080][0081]
其中，l
b,n
是上述第一联结点到第n个全挂拖斗的前轴中心之间的距离。
[0082]
按照上述过程，针对每个全挂拖斗，电子设备可由第一相邻车体的目标后轴位姿和全挂拖斗的初始前轴位姿，分别计算得到第一联结点的两个坐标。如此，全挂汽车列车中的每两个相邻车体之间的联结点均可获得第一坐标和第二坐标的两个坐标。
[0083]
s122、基于第一坐标和第二坐标，确定全挂拖斗的目标前轴位姿。
[0084]
具体地，对于某个全挂拖斗，根据上述说明，该全挂拖斗与其第一相邻车体之间的第一联结点有第一坐标和第二坐标。这两个坐标之间的差异是由初始前轴位姿的不准确而导致的。所以，电子设备可根据这两个坐标之间的差异来校正初始前轴位姿。
[0085]
在一示例中，因为第一相邻车体已有目标后轴位姿，可认为由目标后轴位姿计算而得到第一坐标是精准的，故可根据两个坐标的差异来校正该全挂拖斗的初始前轴位姿。例如，可利用两个坐标的差异以及第一联结点和全挂拖斗的前轴中心轴之间的几何关系，反推算出全挂拖斗的前轴中心处的新的坐标和新的航向角，作为全挂拖斗的目标前轴位姿。
[0086]
在另一示例中，考虑到每个全挂拖斗的目标后轴位姿都是推算所得，该目标后轴位姿也可能存在较小的误差，所以，为了降低目标后轴位姿可能存在的误差的影响，可以根据某一权重，融合第一坐标和第二坐标，得到一个融合坐标，再根据该融合坐标以及第一联结点和全挂拖斗的前轴中心轴之间的几何关系，反推算出全挂拖斗的前轴中心处的新的坐
标和新的航向角，作为全挂拖斗的目标前轴位姿。
[0087]
按照上述过程，针对每个全挂拖斗，可计算获得每个全挂拖斗的目标前轴位姿。
[0088]
s123、基于全挂拖斗的初始后轴位姿，确定全挂拖斗的前轴中心处的第三坐标。
[0089]
具体地，上述过程可完成对全挂拖斗的初始前轴位姿的校正，然后，电子设备可按照上述过程进行该全挂拖斗的初始后轴位姿的校正，即利用得到的目标前轴位姿，进一步进行该全挂拖斗的初始后轴位姿的校正。
[0090]
因前述过程已获得由第n个全挂拖斗的前轴部分推算而得的第n个全挂拖斗的前轴中心的目标前轴位姿，此处仅需再利用第n个全挂拖斗的初始后轴位姿计算第n个全挂拖斗的前轴中心处的坐标(即第三坐标)。
[0091]
具体实施时，按照如下公式(3)，电子设备可根据第n个全挂拖斗的初始后轴位姿，即第n个全挂拖斗的后轴中心坐标(xn,yn)和后轴航向角θn，计算获得第n个全挂拖斗的前轴中心处的第三坐标(x
cr
,y
cr
)。
[0092][0093]
其中，ln是上述全挂拖斗的前轴中心与后轴中心之间的距离。
[0094]
如此，全挂汽车列车中的每个车体的前轴中心处均可获得目标前轴位姿中的目标前轴坐标和第三坐标的两个坐标。
[0095]
s124、基于目标前轴位姿中的目标前轴坐标和第三坐标，确定全挂拖斗的目标后轴位姿。
[0096]
具体地，对于某个全挂拖斗，根据上述说明，该全挂拖斗的前轴中心处有目标前轴坐标和第三坐标。这两个坐标之间的差异是由初始后轴位姿的不准确而导致的。所以，电子设备可根据这两个坐标之间的差异来校正初始后轴位姿。
[0097]
在一示例中，可认为由目标前轴位姿是精准的，故可根据两个坐标的差异来校正该全挂拖斗的初始后轴位姿。例如，可利用两个坐标的差异以及前轴中心和全挂拖斗的后轴中心轴之间的几何关系，反推算出全挂拖斗的后轴中心处的新的坐标和新的航向角，作为全挂拖斗的目标后轴位姿。
[0098]
在另一示例中，考虑到每个全挂拖斗的目标前轴位姿都是推算所得，该目标前轴位姿也可能存在较小的误差，所以，为了降低目标前轴位姿可能存在的误差的影响，可以根据某一权重，融合目标前轴位姿和第三坐标，得到一个融合坐标，再根据该融合坐标以及前轴中心和全挂拖斗的后轴中心轴之间的几何关系，反推算出全挂拖斗的后轴中心处的新的坐标和新的航向角，作为全挂拖斗的目标后轴位姿。
[0099]
按照上述过程，针对每个全挂拖斗，可计算获得每个全挂拖斗的目标后轴位姿。
[0100]
本技术实施例提供的上述全挂汽车列车的拖斗位姿确定方法，能够在获取全挂汽车列车包含的各车体的初始位姿的基础上，根据两个相邻车体中的前一车体的目标后轴位姿和后一车体的初始前轴位姿，分别推算得到该两个相邻车体之间的联结点的第一坐标和第二坐标，并根据第一坐标和第二坐标的坐标差异，对后一车体的初始前轴位姿进行校正，得到校正后的目标前轴位姿，且根据后一车体的初始后轴位姿得到该后一车体的前轴中心的第三坐标后，利用后一车体的目标前轴位姿和第三坐标推算得到后一车体的目标后轴位姿；无需每个车体均安装高精度传感器，便可获得各车体的高精度位姿，降低了全挂汽车列
车中各车体的高精度位姿的获取成本，提高了各车体的高精度位姿的获取便捷性。
[0101]
在一些实施例中，s122包括如下步骤a和步骤b。
[0102]
步骤a、基于第一坐标和第二坐标，确定第一联结点对应的第一位置偏差。
[0103]
具体地，计算目标前轴位姿的过程中，电子设备先按照如下公式(4)，根据第一坐标和第二坐标，计算第一联结点的位置偏差，即第一位置偏差dn：
[0104][0105]
步骤b、基于第一位置偏差与第一偏差阈值之间的比较结果、第一坐标和第二坐标，对全挂拖斗的初始前轴位姿进行校正，确定全挂拖斗的目标前轴位姿。
[0106]
其中，第一偏差阈值是预先设置的、位置偏差的一个临界值，其用于判断坐标偏差的大小，进而用于确定位姿校正的方式。因全挂汽车列车中的相邻两个车体之间是通过刚体连接的，所以相邻两个车体中的前一车体的目标后轴位姿和后一车体的目标前轴位姿需确保相邻两个车体之间的第一联结点处满足刚性连接约束。基于此，本公开实施例中，第一偏差阈值用于对相邻两个车体进行刚性连接约束。
[0107]
示例性地，第一偏差阈值至少基于第一联结点的铰接柱和铰接环之间的缝隙距离确定。因为第一联结点处是通过铰接柱和铰接环进行刚性连接的，所以第一偏差阈值至少可以根据铰接环套住铰接柱后，两者之间的最大的缝隙距离来确定。此外，考虑到运动学模型的建模误差、车辆控制计算中的离散误差、道路情况引起的行驶误差等多种因素，本公开实施例中，还可在缝隙距离的基础上，叠加一个经验值，来确定最终的第一偏差阈值，以提高第一偏差阈值的准确性。
[0108]
具体地，电子设备可以根据第一位置偏差dn与第一偏差阈值d
ff
的大小关系，判断第一位置偏差是否满足刚性连接约束，并根据判断结果来确定适合于该全挂拖斗的位姿校正方式，且对第一坐标和第二坐标进行校正处理，得到目标前轴位姿。
[0109]
例如，第一位置偏差dn大于第一偏差阈值d
ff
时，说明该第一位置偏差较大，其不满足刚性连接约束，此时可对其采用大尺度的位姿校正方式，以利用第一坐标和第二坐标对较大的第一位置偏差进行校正，得到目标前轴位姿。
[0110]
又如，第一位置偏差dn小于或等于第一偏差阈值d
ff
时，说明该第一位置偏差较小，其满足刚性连接约束，此时可对其采用小尺度的位姿校正方式，以利用第一坐标和第二坐标对较小的第一位置偏差进行校正，得到目标前轴位姿。
[0111]
本技术上述实施例中，通过第一偏差阈值的设置，对第一坐标和第二坐标的位姿校正进行刚性连接约束，使得校正后的目标前轴位姿满足各车体之间刚性连接的要求，进一步提高目标前轴位姿的准确性。
[0112]
在一示例中，步骤b包括：若比较结果为第一位置偏差大于第一偏差阈值，则将全挂拖斗的初始前轴位姿中的初始航向角确定为目标前轴航向角；利用第一坐标和第二坐标的坐标差值，对全挂拖斗的初始前轴位姿中的初始前轴坐标进行校正，确定全挂拖斗的目标前轴坐标。
[0113]
具体地，如果第一位置偏差大于第一偏差阈值，说明该第一位置偏差较大，第一相邻车体和待推算目标位姿的全挂拖斗之间，因距离偏差过大而不满足刚性连接，此时可对其采用大尺度的位姿校正方式，即主要校正第一相邻车体和待推算目标位姿的全挂拖斗的
前轴部分之间的距离偏差，航向角的偏差相对于距离偏差可以忽略。所以，电子设备保持全挂拖斗的前轴航向角不变，即将待推算目标位姿的全挂拖斗的初始前轴位姿中的航向角(即初始航向角)作为目标前轴位姿中的航向角(即目标前轴航向角)。
[0114]
然后，电子设备按照如下公式(5)，先利用第一坐标和第二坐标分别计算两个坐标轴上的坐标差值(x
f-xr)和(y
f-yr)，再利用坐标差值对初始前轴位姿中的初始前轴坐标(x
f,n
,y
f,n
)进行校正，得到目标前轴位姿中的目标前轴坐标(x
′
f,n
,y
′
f,n
)。
[0115][0116]
参见图2，根据第一相邻车体201的目标后轴位姿推算得到第一联结点的第一坐标(xf,yf)，且根据全挂拖斗202的初始前轴位姿推算得到第一联结点的第二坐标(xr,yr)。该第一坐标和第二坐标之间的第一位置偏差大于偏差阈值，便按照上述过程对全挂拖斗的前轴中心进行航向角不变的平移校正，确定校正后的全挂拖斗202
′
，且得到校正后的全挂拖斗202
′
的目标前轴坐标(x
′
f,n
,y
′
f,n
)。
[0117]
在另一示例中，步骤b包括：若比较结果为第一位置偏差小于或等于第一偏差阈值，则基于第一坐标和全挂拖斗的初始前轴位姿中的初始前轴坐标，对全挂拖斗的初始前轴位姿中的初始航向角进行校正，确定全挂拖斗的目标前轴航向角；基于第一距离和第二距离的距离差值和目标前轴航向角，对初始前轴坐标进行校正，确定全挂拖斗的目标前轴坐标。
[0118]
其中，第一距离为第一坐标和初始前轴坐标之间的第二位置偏差，第二距离为第一联结点到全挂拖斗的前轴中心之间的距离。
[0119]
具体地，如果第一位置偏小于或等于第一偏差阈值，说明该第一位置偏差相对较小，第一相邻车体和待推算目标位姿的全挂拖斗的前轴部分之间满足刚性连接，此时可对其采用小尺度的位姿校正方式，即同时校正第一相邻车体和待推算目标位姿的全挂拖斗的前轴部分之间的距离偏差和航向角偏差。
[0120]
对于航向角校正，电子设备根据第一联结点的第一坐标(xf,yf)和全挂拖斗的初始前轴位姿中的初始前轴坐标(x
f,n
,y
f,n
)，按照如下公式(6)，计算得到全挂拖斗的目标前轴航向角θ'
f,n
。
[0121][0122]
对于坐标校正，电子设备先利用第一联结点的第一坐标(xf,yf)和全挂拖斗的初始前轴坐标(x
f,n
,y
f,n
)，按照公式计算得到第一坐标和初始前轴坐标之间的第二位置偏差，即第一距离d
g1
。然后，电子设备按照如下公式(7)，利用第一距离和第二距离之间的距离差值(d
g1-l
b,n
)、目标前轴航向角θ'
f,n
和初始前轴坐标(x
f,n
,y
f,n
)，计算获得目标前轴坐标(x
′
f,n
,y
′
f,n
)。
[0123][0124]
在一些实施例中，s124包括：基于目标前轴坐标和第三坐标，确定全挂拖斗的前轴
中心对应的第三位置偏差，并基于第三位置偏差与第二偏差阈值之间的比较结果、目标前轴坐标和第三坐标，对全挂拖斗的初始后轴位姿进行校正，确定全挂拖斗的目标后轴位姿。
[0125]
其中，第二偏差阈值是预先设置的、位置偏差的另一临界值，其同样用于判断坐标偏差的大小，进而用于确定位姿校正的方式。因全挂汽车列车中的一个车体包含的前轴部分和后轴部分之间是通过刚体连接的，所以第二偏差阈值用于对全挂拖斗的前轴部分和后轴部分进行刚性连接约束。但是，因为前轴部分和后轴部分之间是固结的，其不存在上述连接环和连接柱的连接结构，故该第二偏差阈值可根据运动学模型的建模误差、车辆控制计算中的离散误差、道路情况引起的行驶误差等多种因素确定的经验值来确定。
[0126]
具体地，计算目标后轴位姿的过程中，电子设备先按照如下公式(8)，根据目标前轴坐标(x
′
f,n
,y
′
f,n
)和第三坐标(x
cr
,y
cr
)，计算全挂拖斗的前轴中心处的位置偏差，即第三位置偏差d
r,n
：
[0127][0128]
然后，电子设备可以根据第三位置偏差d
r,n
与第二偏差阈值d
fr
的大小关系，判断第三位置偏差是否满足刚性连接约束，并根据判断结果来确定适合于该全挂拖斗的后轴部分的位姿校正方式，且对目标前轴坐标和第三坐标进行校正处理，得到目标后轴位姿。
[0129]
例如，第三位置偏差d
r,n
大于第二偏差阈值d
fr
时，说明该第三位置偏差较大，其不满足刚性连接约束，此时可对其采用大尺度的位姿校正方式，以利用目标前轴坐标和第三坐标对较大的第三位置偏差进行校正，得到目标后轴位姿。
[0130]
又如，第三位置偏差d
r,n
小于或等于第二偏差阈值d
fr
时，说明该第三位置偏差较小，其满足刚性连接约束，此时可对其采用小尺度的位姿校正方式，以利用目标前轴坐标和第三坐标对较小的第三位置偏差进行校正，得到目标后轴位姿。
[0131]
本技术上述实施例中，通过第三偏差阈值的设置，对目标前轴坐标和第三坐标的位姿校正进行刚性连接约束，使得校正后的目标后轴位姿满足各车体之间刚性连接的要求，进一步提高目标后轴位姿的准确性。
[0132]
在一示例中，上述基于第三位置偏差与第二偏差阈值之间的比较结果、目标前轴坐标和第三坐标，对全挂拖斗的初始后轴位姿进行校正，确定全挂拖斗的目标后轴位姿包括：若比较结果为第三位置偏差大于第二偏差阈值，则将全挂拖斗的初始后轴位姿中的初始航向角确定为目标后轴航向角；利用目标前轴坐标和第三坐标的坐标差值，对全挂拖斗的初始后轴位姿中的初始后轴坐标进行校正，确定全挂拖斗的目标后轴坐标。
[0133]
具体地，如果第三位置偏差大于第二偏差阈值，说明该第三位置偏差较大，全挂拖斗的前轴部分和后轴部分之间，因距离偏差过大而不满足刚性连接，此时可对其采用大尺度的位姿校正方式，即主要校正全挂拖斗的前轴部分和后轴部分的距离偏差，航向角的偏差相对于距离偏差可以忽略。所以，电子设备保持后轴部分的航向角不变，即全挂拖斗的初始后轴位姿中的初始航向角作为目标后轴位姿中的航向角(即目标后轴航向角)。
[0134]
然后，电子设备按照如下公式(9)，先利用目标前轴坐标(x
′
f,n
,y
′
f,n
)和第三坐标(x
cr
,y
cr
)分别计算两个坐标轴上的坐标差值(x
′
f,n-x
cr
)和(y
′
f,n-y
cr
)，再利用坐标差值对初始后轴位姿中的初始后轴坐标(xn,yn)进行校正，得到目标后轴位姿中的目标后轴坐标(xn′
,yn′
)。
[0135][0136]
在另一示例中，上述基于第三位置偏差与第二偏差阈值之间的比较结果、目标前轴坐标和第三坐标，对全挂拖斗的初始后轴位姿进行校正，确定全挂拖斗的目标后轴位姿包括：若比较结果为第三位置偏差小于或等于第二偏差阈值，则基于目标前轴坐标和全挂拖斗的初始后轴位姿中的初始后轴坐标，对全挂拖斗的初始后轴位姿中的初始航向角进行校正，确定全挂拖斗的目标后轴航向角；基于第三距离和第四距离的距离差值和目标后轴航向角，对初始后轴坐标进行校正，确定全挂拖斗的目标后轴坐标。
[0137]
其中，第三距离为目标前轴坐标和初始后轴坐标之间的第四位置偏差，第四距离为全挂拖斗的前轴中心到相应车体的后轴中心之间的距离。
[0138]
具体地，如果第三位置偏小于或等于第二偏差阈值，说明该第三位置偏差相对较小，全挂拖斗的前轴部分和后轴部分之间满足刚性连接，此时可对其采用小尺度的位姿校正方式，即同时校正前轴部分和后轴部分之间的距离偏差和航向角偏差。
[0139]
对于航向角校正，电子设备根据前轴中心的目标前轴坐标(x
′
f,n
,y
′
f,n
)和全挂拖斗的初始后轴位姿中的初始后轴坐标(xn,yn)，按照如下公式(10)，计算得到全挂拖斗的目标后轴航向角θ'n。
[0140][0141]
对于坐标校正，电子设备先利用第一联结点的目标前轴坐标(x
′
f,n
,y
′
f,n
)和初始后轴坐标(xn,yn)，按照公式计算得到目标前轴坐标和初始后轴坐标之间的第四位置偏差，即第三距离d
g2
。然后，电子设备按照如下公式(11)，利用第三距离和第四距离之间的距离差值(d
g2-ln)、目标后轴航向角θ'n和初始后轴坐标(xn,yn)，计算获得目标后轴坐标(xn′
,yn′
)。
[0142][0143]
参见图2，根据全挂拖斗202的初始后轴位姿推算得到前轴中心处的第三坐标(x
cr
,y
cr
)。该第三坐标和目标前轴坐标(x
′
f,n
,y
′
f,n
)之间的第三位置偏差小于或等于第二偏差阈值，便按照上述过程对全挂拖斗的初始后轴位姿进行旋转式校正，即校正航向角和初始坐标，确定校正后的全挂拖斗202
′
，且得到校正后的全挂拖斗202
′
的目标后轴航向角θ'n和目标后轴坐标(xn′
,yn′
)。
[0144]
在一些实施例中，s110包括如下步骤c和步骤d。
[0145]
步骤c、获取牵引车的初始位姿、牵引车的后轴中心处的第一速度和前轮偏角。
[0146]
具体地，为了进一步降低全挂汽车列车的成本，本实施例中，可以只在牵引车上安装测量位姿的传感器，以测量获得牵引车的高精度位姿，即获得牵引车的初始后轴位姿，也是牵引车的目标后轴位姿。同时，通过牵引车上安装的速度传感器、角度传感器等，可测量获得牵引车的后轴中心处的行驶速度(即第一速度)和前轮偏角。
[0147]
步骤d、基于牵引车的初始位姿、第一速度和前轮偏角，以及各全挂拖斗的历史前
轴航向角和历史后轴航向角，按照全挂汽车列车对应的运动学模型，确定各全挂拖斗的初始位姿。
[0148]
其中，历史前轴航向角和历史后轴航向角分别为当前位姿更新周期的前一相邻位姿更新周期中所得的前轴航向角和后轴航向角。
[0149]
具体地，为了兼顾全挂汽车列车挂载多个全挂拖斗进行安全驾驶控制的情况，本公开实施例中构建了适用于挂载多个全挂拖斗的全挂汽车列车的运动学模型，其用于描述全挂汽车列车的各车体的运动过程。所以，电子设备在获得牵引车的初始位姿、第一速度和前轮偏角，以及各全挂拖斗的历史前轴航向角和历史后轴航向角后，便可根据运动学模型，通过迭代计算的方式，从第一个全挂拖斗开始，逐个推算出各全挂拖斗的初始位姿。
[0150]
本技术上述实施例中，仅在牵引车上安装测量位姿的传感器，而无需在各全挂拖斗上安装用于测量位姿的传感器的情况下，通过适用于挂载多个全挂拖斗的全挂汽车列车的运动学模型，由牵引车的相关信息推算获得各全挂拖斗的初始位姿，进一步降低了全挂汽车列车的成本。
[0151]
在一示例中，步骤d可实现为针对每个全挂拖斗执行如下步骤d1～步骤d4。
[0152]
步骤d1、基于第二相邻车体的第二速度、第二相邻车体的后轴航向角变化率和第五距离，以及第二相邻车体的后轴部分和全挂拖斗的前轴部分之间的第一航向夹角，按照运动学模型中的前轴速度方程，确定全挂拖斗的前轴中心处的第三速度。
[0153]
其中，第二相邻车体为靠近牵引车一端的、与待推算初始位姿的全挂拖斗连接的车体。第二速度是第二相邻车体的后轴中心处的速度。当第二相邻车体为牵引车时，第二速度为牵引车的第一速度；当第二相邻车体也为全挂拖斗，而不为牵引车时，第二速度为第二相邻车体的后轴中心处的速度。航向角变化率是航向角随着时间变化的速率。后轴航向角变化率是后轴中心处的航向角变化率。第五距离为第二相邻车体的后轴中心到第二联结点之间的距离。第二联结点为第二相邻车体与全挂拖斗之间的联结点。航向夹角是两个航向方向之间的夹角。第一航向夹角是第二相邻车体的后轴部分的航向方向和全挂拖斗的前轴部分的航向方向之间的夹角。本实施例中，第一航向夹角由第二相邻车体的历史后轴航向角和全挂拖斗的历史前轴航向角确定。
[0154]
具体地，为了兼顾全挂汽车列车挂载多个全挂拖斗进行安全驾驶控制的情况，本公开实施例根据图3所示的全挂汽车列车的运动关系，构建了适用于挂载多个全挂拖斗的全挂汽车列车的运动学模型，其中包含了推算第n个车体的前轴中心处的第三速度v
f,n
的前轴速度方程，如公式(12)所示：
[0155][0156]
其中，如图3所示，v
n-1
是第n-1个车体的后轴中心处的速度；β
f,n
是第n个车体的后轴航向角与第n-1个车体的前轴航向角之间的航向夹角；是第n-1个车体的后轴航向角变化率；l
h,n-1
是第n-1个车体的后轴中心到第n-1个车体与第n个车体之间的联结点的距离。
[0157]
当第二相邻车体为第n-1个车体时，待推算初始位姿的全挂拖斗便为第n个车体，那么按照上述速度方程(12)，电子设备可根据第二相邻车体的第二速度v
n-1
、第二相邻车体的后轴航向角变化率和第五距离l
h,n-1
，以及第二相邻车体的后轴部分和全挂拖斗的前轴部分之间的第一航向夹角β
f,n
，计算得到全挂拖斗的前轴中心处的第三速度vn。其中的第
一航向夹角β
f,n
由第二相邻车体的历史后轴航向角θ
n-1
和全挂拖斗的历史前轴航向角θ
fn
确定，即β
f,n
＝θ
n-1-θ
fn
。
[0158]
上述过程中，当第二相邻车体也为全挂拖斗时，第二相邻车体的后轴航向角通过本示例中的步骤d1～步骤d4迭代计算所得。当第二相邻车体为牵引车时，第二相邻车体的后轴航向角变化率根据全挂汽车列车的运动学模型中包含的牵引车的运动方程(如公式(13)所示)，利用第一速度v0、牵引车的前轮偏角δf以及牵引车车头到牵引车的后轴中心之间的第六距离l0计算得到。
[0159][0160]
步骤d2、基于第三速度、全挂拖斗的历史前轴航向角、第七距离、第二速度、第一航向夹角、第二相邻车体的航向角变化率和第八距离，按照运动学模型中包含的全挂拖斗的运动方程，确定全挂拖斗的前轴中心处的初始前轴坐标。
[0161]
其中，第七距离为第二联结点到全挂拖斗的后轴中心之间的距离。第八距离为第二相邻车体的后轴中心到第二联结点之间的距离。
[0162]
具体地，电子设备在获得第n个全挂拖斗的前轴中心处的第三速度v
f,n
后，便可利用如下公式(14)所示的运动学模型中包含的全挂拖斗的运动方程，分别计算全挂拖斗的前轴中心处的初始前轴坐标(x
f,n
,y
f,n
)以及全挂拖斗的前轴航向角变化率
[0163][0164]
其中，v
f,n
是第n个全挂拖斗的第三速度，θ
f,n
是第n个全挂拖斗的历史前轴航向角，l
b,n
是第二联结点到第n个全挂拖斗的后轴中心之间的第五距离，v
n-1
是第二相邻车体的第二速度，β
f,n
是第一航向夹角，是第二相邻车体的后轴航向角变化率，l
h,n-1
是第二相邻车体的后轴中心到第二联结点之间的第八距离。
[0165]
按照上述过程计算所得的初始前轴坐标和全挂拖斗的历史前轴航向角为全挂拖斗的初始前轴位姿。而计算所得的全挂拖斗的前轴航向角变化率则作为该全挂拖斗的后轴部分的计算基础数据。
[0166]
步骤d3、基于第三速度、以及全挂拖斗的前轴部分和相应车体的后轴部分之间的第二航向夹角，按照运动学模型中的后轴速度方程，确定全挂拖斗的后轴中心处的第四速度。
[0167]
具体地，全挂拖斗后轴的运动由其前轴驱动，所以，可以根据如下公(15)所示的运动学模型中的后轴速度方程，由第n个全挂拖斗的前轴中心处的第三速度v
f,n
和第n个全挂拖斗的前轴部分和后轴部分之间的第二航向夹角βn，计算第n个全挂拖斗后轴中心处的第四速度vn。其中的第二航向夹角βn由第n个全挂拖斗的历史前轴航向角θ
fn
及其历史后轴航向角θn确定，即βn＝θ
fn-θn。
[0168]
vn＝v
f,n
cos(βn)(15)
[0169]
步骤d4、基于第四速度、全挂拖斗的历史后轴航向角、第九距离、第三速度和第二航向夹角，按照运动学模型中包含的全挂拖斗的运动方程，确定全挂拖斗的后轴中心处的
初始后轴坐标以及全挂拖斗的后轴航向角变化率。
[0170]
其中，第九距离为全挂拖斗的前轴中心到相应车体的后轴中心之间的距离。
[0171]
具体地，电子设备在获得第n个全挂拖斗的后轴中心处的第四速度vn后，便可利用如下公式(16)所示的运动学模型中包含的全挂拖斗的运动方程，分别计算第n个全挂拖斗的后轴中心处的初始后轴坐标(xn,yn)以及第n个全挂拖斗的后轴航向角变化率
[0172][0173]
其中，vn是第n个全挂拖斗的第四速度，θn是第n个全挂拖斗的历史后轴航向角，ln是第n个全挂拖斗的第九距离，v
f,n
是第n个全挂拖斗的前轴中心处的第三速度，βn是第二航向夹角。
[0174]
按照上述过程计算所得的第n个全挂拖斗的初始后轴坐标和历史后轴航向角为第n个全挂拖斗的初始后轴位姿。而计算所得的第n个全挂拖斗的后轴航向角变化率则作为该全挂拖斗的下一个相邻的全挂拖斗的前轴部分的计算基础数据。
[0175]
以下是本技术实施例提供的全挂汽车列车的拖斗位姿确定装置的实施例，该装置与上述各实施例的全挂汽车列车的拖斗位姿确定方法属于同一个发明构思，在全挂汽车列车的拖斗位姿确定装置的实施例中未详尽描述的细节内容，可以参考上述全挂汽车列车的拖斗位姿确定方法的实施例。
[0176]
图4示出了本技术实施例提供的一种全挂汽车列车的拖斗位姿确定装置的结构示意图。该全挂汽车列车由多个车体构成，各车体中处于一侧边缘的一个车体为牵引车，各车体中除了牵引车之外的车体均为全挂拖斗。如图4所示，该全挂汽车列车的拖斗位姿确定装置400具体包括：
[0177]
初始位姿获取模块410，用于获取各车体的初始位姿；初始位姿包括初始前轴位姿和初始后轴位姿；
[0178]
目标位姿确定模块420，用于针对每个全挂拖斗执行：
[0179]
基于第一相邻车体的目标后轴位姿，确定第一相邻车体和全挂拖斗之间的第一联结点的第一坐标，并基于全挂拖斗的初始前轴位姿，确定第一联结点的第二坐标；其中，第一相邻车体为靠近牵引车一端的、与全挂拖斗连接的车体；在第一相邻车体为牵引车时，目标后轴位姿为初始后轴位姿；在第一相邻车体也为全挂拖斗时，目标后轴位姿为校正后的初始后轴位姿；
[0180]
基于第一坐标和第二坐标，确定全挂拖斗的目标前轴位姿；
[0181]
基于全挂拖斗的初始后轴位姿，确定全挂拖斗的前轴中心的第三坐标；
[0182]
基于目标前轴位姿中的目标前轴坐标和第三坐标，确定全挂拖斗的目标后轴位姿。
[0183]
本技术实施例提供的上述全挂汽车列车的拖斗位姿确定装置，能够在获取全挂汽车列车包含的各车体的初始位姿的基础上，根据两个相邻车体中的前一车体的目标后轴位姿和后一车体的初始前轴位姿，分别推算得到该两个相邻车体之间的联结点的第一坐标和第二坐标，并根据第一坐标和第二坐标的坐标差异，对后一车体的初始前轴位姿进行校正，
得到校正后的目标前轴位姿，且根据后一车体的初始后轴位姿得到该后一车体的前轴中心的第三坐标后，利用后一车体的目标前轴位姿和第三坐标推算得到后一车体的目标后轴位姿；无需每个车体均安装高精度传感器，便可获得各车体的高精度位姿，降低了全挂汽车列车中各车体的高精度位姿的获取成本，提高了各车体的高精度位姿的获取便捷性。
[0184]
在一些实施例中，目标位姿确定模块420包括：
[0185]
第一位置偏差确定子模块，用于基于第一坐标和第二坐标，确定第一联结点对应的第一位置偏差；
[0186]
目标前轴位姿确定子模块，用于基于第一位置偏差与第一偏差阈值之间的比较结果、第一坐标和第二坐标，对全挂拖斗的初始前轴位姿进行校正，确定全挂拖斗的目标前轴位姿；其中，第一偏差阈值用于对相邻两个车体进行刚性连接约束。
[0187]
在一些实施例中，第一偏差阈值至少基于第一联结点的铰接柱和铰接环之间的缝隙距离确定。
[0188]
在一些实施例中，目标前轴位姿确定子模块具体用于：
[0189]
若比较结果为第一位置偏差大于第一偏差阈值，则将全挂拖斗的初始前轴位姿中的初始航向角确定为目标前轴航向角；
[0190]
利用第一坐标和第二坐标的坐标差值，对全挂拖斗的初始前轴位姿中的初始前轴坐标进行校正，确定全挂拖斗的目标前轴坐标。
[0191]
在另一些实施例中，目标前轴位姿确定子模块具体用于：
[0192]
若比较结果为第一位置偏差小于或等于第一偏差阈值，则基于第一坐标和全挂拖斗的初始前轴位姿中的初始前轴坐标，对全挂拖斗的初始前轴位姿中的初始航向角进行校正，确定全挂拖斗的目标前轴航向角；
[0193]
基于第一距离和第二距离的距离差值和目标前轴航向角，对初始前轴坐标进行校正，确定全挂拖斗的目标前轴坐标；其中，第一距离为第一坐标和初始前轴坐标之间的第二位置偏差，第二距离为第一联结点到全挂拖斗的前轴中心之间的距离。
[0194]
在一些实施例中，目标位姿确定模块420包括：
[0195]
目标后轴位姿确定子模块，用于基于目标前轴坐标和第三坐标，确定全挂拖斗的前轴中心对应的第三位置偏差，并基于第三位置偏差与第二偏差阈值之间的比较结果、目标前轴坐标和第三坐标，对全挂拖斗的初始后轴位姿进行校正，确定全挂拖斗的目标后轴位姿；其中，第二偏差阈值用于对全挂拖斗的前轴部分和后轴部分进行刚性连接约束。
[0196]
在一些实施例中，目标后轴位姿确定子模块具体用于：
[0197]
若比较结果为第三位置偏差大于第二偏差阈值，则将全挂拖斗的初始后轴位姿中的初始航向角确定为目标后轴航向角；
[0198]
利用目标前轴坐标和第三坐标的坐标差值，对全挂拖斗的初始后轴位姿中的初始后轴坐标进行校正，确定全挂拖斗的目标后轴坐标。
[0199]
在另一些实施例中，目标后轴位姿确定子模块具体用于：
[0200]
若比较结果为第三位置偏差小于或等于第二偏差阈值，则基于目标前轴坐标和全挂拖斗的初始后轴位姿中的初始后轴坐标，对全挂拖斗的初始后轴位姿中的初始航向角进行校正，确定全挂拖斗的目标后轴航向角；
[0201]
基于第三距离和第四距离的距离差值和目标后轴航向角，对初始后轴坐标进行校
正，确定全挂拖斗的目标后轴坐标；其中，第三距离为目标前轴坐标和初始后轴坐标之间的第四位置偏差，第四距离为全挂拖斗的前轴中心到相应车体的后轴中心之间的距离。
[0202]
在一些实施例中，初始位姿获取模块410包括：
[0203]
前轮偏角获取子模块，用于获取牵引车的初始位姿、牵引车的后轴中心处的第一速度和前轮偏角；
[0204]
初始位姿确定子模块，用于基于牵引车的初始位姿、第一速度和前轮偏角，以及各全挂拖斗的历史前轴航向角和历史后轴航向角，按照全挂汽车列车对应的运动学模型，确定各全挂拖斗的初始位姿；其中，历史前轴航向角和历史后轴航向角分别为当前位姿更新周期的前一相邻位姿更新周期中所得的航向角。
[0205]
在一些实施例中，初始位姿确定子模块具体用于针对每个全挂拖斗执行：
[0206]
基于第二相邻车体的第二速度、第二相邻车体的后轴航向角变化率和第五距离，以及第二相邻车体的后轴部分和全挂拖斗的前轴部分之间的第一航向夹角，按照运动学模型中的前轴速度方程，确定全挂拖斗的前轴中心处的第三速度；其中，在第二相邻车体为牵引车时，第二速度为第一速度，第二相邻车体的后轴航向角变化率基于第一速度、前轮偏角和第六距离，按照运动学模型中包含的牵引车的运动方程确定；第六距离为牵引车车头到牵引车的后轴中心之间的距离；在第二相邻车体也为全挂拖斗时，第二速度为第二相邻车体的后轴中心处的速度；第五距离为第二相邻车体的后轴中心到第二联结点之间的距离，第二联结点为第二相邻车体与全挂拖斗之间的联结点；第一航向夹角由第二相邻车体的历史后轴航向角和全挂拖斗的历史前轴航向角确定；
[0207]
基于第三速度、全挂拖斗的历史前轴航向角、第七距离、第二速度、第一航向夹角、第二相邻车体的航向角变化率和第八距离，按照运动学模型中包含的全挂拖斗的运动方程，确定全挂拖斗的前轴中心处的初始前轴坐标；其中，初始前轴坐标和全挂拖斗的历史前轴航向角为全挂拖斗的初始前轴位姿；第七距离为第二联结点到全挂拖斗的后轴中心之间的距离；第八距离为第二相邻车体的后轴中心到第二联结点之间的距离；
[0208]
基于第三速度、以及全挂拖斗的前轴部分和相应车体的后轴部分之间的第二航向夹角，按照运动学模型中的后轴速度方程，确定全挂拖斗的后轴中心处的第四速度；
[0209]
基于第四速度、全挂拖斗的历史后轴航向角、第九距离、第三速度和第二航向夹角，按照运动学模型中包含的全挂拖斗的运动方程，确定全挂拖斗的后轴中心处的初始后轴坐标以及全挂拖斗的后轴航向角变化率；其中，初始后轴坐标和全挂拖斗的历史后轴航向角为全挂拖斗的初始后轴位姿；第九距离为全挂拖斗的前轴中心到相应车体的后轴中心之间的距离。
[0210]
本技术实施例所提供的全挂汽车列车的拖斗位姿确定装置可执行本技术任意实施例所提供的全挂汽车列车的拖斗位姿确定方法，具备执行方法相应的功能模块和有益效果。
[0211]
值得注意的是，上述全挂汽车列车的拖斗位姿确定装置的实施例中，所包括的各个模块、子模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能模块/子模块的具体名称也只是为了便于相互区分，并不用于限制本技术的保护范围。
[0212]
图5是适于用来实现根据本技术实施方式的电子设备的结构示意图。该电子设备
可以包括但不限于全挂汽车列车中的控制器、能够与全挂汽车列车进行实时通信的外部设备。该外部设备例如可以是调度车辆的调度系统或者智能驾驶系统的服务端所在的笔记本电脑、台式电脑或服务器等。
[0213]
如图5所示，电子设备500包括中央处理单元(cpu)501，其可以根据存储在只读存储器(rom)502中的程序或者从存储部分508加载到随机访问存储器(ram)503中的程序而执行上述任意实施例中的全挂汽车列车的拖斗位姿确定方法中的各种处理。在ram 503中，还存储有电子设备500操作所需的各种程序和数据。cpu 501、rom 502以及ram 503通过总线504彼此相连。输入/输出(i/o)接口505也连接至总线504。
[0214]
可选地，以下部件连接至i/o接口505：包括键盘、鼠标等的输入部分506；包括诸如阴极射线管(crt)、液晶显示器(lcd)等以及扬声器等的输出部分507；包括硬盘等的存储部分508；以及包括诸如lan卡、调制解调器等的网络接口卡的通信部分509。通信部分509经由诸如因特网的网络执行通信处理。驱动器510也根据需要连接至i/o接口505。可拆卸介质511，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器510上，以便于从其上读出的计算机程序根据需要被安装入存储部分508。
[0215]
特别地，根据本技术的实施方式，上文任意实施例描述的方法可以被实现为计算机软件程序。例如，本技术的实施方式包括一种计算机程序产品，其包括有形地包含在及其可读介质上的计算机程序，计算机程序包含用于执行本技术任意实施例中的全挂汽车列车的拖斗位姿确定方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分509从网络上被下载和安装，和/或从可拆卸介质511被安装。
[0216]
本技术还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施方式中所说明的电子设备中所包含的计算机可读存储介质；也可以是单独存在，未装配入电子设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，程序被一个或者一个以上的处理器用来执行描述于本技术的全挂汽车列车的拖斗位姿确定方法。
[0217]
图6示出了本技术实施例提供的一种全挂汽车列车的结构示意图。该全挂汽车列车由多个车体构成(图6未示出)，各车体中处于一侧边缘的一个车体为牵引车，各车体中除了牵引车之外的车体均为全挂拖斗。如图6所示，该全挂汽车列车600包括：控制器610；
[0218]
控制器610，用于执行上述任意实施例所说明的全挂汽车列车的拖斗位姿方法的步骤。
[0219]
上述控制器610可以是软件系统、硬件系统或者软硬件结合的系统。例如，控制器610是运行在操作系统上的软件系统，车载硬件系统是支持操作系统运行的硬件系统。
[0220]
尽管图6中未示出，但是可理解的是，全挂汽车列车600至少还包括多个传感器和底层执行系统。
[0221]
多个传感器至少用于获取全挂汽车列车中牵引车的环境数据和行驶状态相关的数据，以及获取全挂拖斗的行驶状态相关的数据，并将环境数据和行驶状态相关的数据发送至控制器610，以为控制器610提供数据基础。
[0222]
在一些实施例中，多个传感器包括但不限于轮转速传感器、速度传感器、加速度传感器、方向盘转角传感器、前轮转角传感器等，用于获取车辆行驶数据；以及包括但不限于摄像头、激光雷达和毫米波雷达等，用于感知车辆周围环境。另外，用于测量位姿的传感器
例如可以包括gnss传感器和ins传感器。
[0223]
底层执行系统至少用于接收来自控制器610的信息，并控制全挂汽车列车的行驶。底层执行系统包括但不限于底盘系统、驱动系统、转向系统和制动系统等等。
[0224]
需要说明的是，附图中的流程图和框图，图示了按照本技术各种实施方式的方法、装置、设备和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，路程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，上述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0225]
描述于本技术实施方式中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定。
[0226]
应当理解的是，本技术的上述具体实施方式仅仅用于示例性说明或解释本技术的原理，而不构成对本技术的限制。因此，在不偏离本技术的精神和范围的情况下所做的任何修改、等同替换、改进等，均应包含在本技术的保护范围之内。此外，本技术所附权利要求旨在涵盖落入所附权利要求范围和边界、或者这种范围和边界的等同形式内的全部变化和修改例。