[发明专利]半挂汽车列车、拖斗位姿确定方法、装置、设备和介质

权利要求书

1.一种半挂汽车列车的拖斗位姿确定方法，所述半挂汽车列车由多个车体构成，各所述车体中处于一侧边缘的一个所述车体为牵引车，各所述车体中除了所述牵引车之外的所述车体均为半挂拖斗，其特征在于，包括：

获取各所述车体的初始位姿；

针对每个所述半挂拖斗：

基于第一相邻车体的目标位姿，确定所述第一相邻车体和所述半挂拖斗之间的第一联结点的第一坐标，并基于所述半挂拖斗的所述初始位姿，确定所述第一联结点的第二坐标；其中，所述第一相邻车体为靠近所述牵引车一端的、与所述半挂拖斗连接的所述车体；在所述第一相邻车体为所述牵引车时，所述目标位姿为所述初始位姿；在所述第一相邻车体也为半挂拖斗时，所述目标位姿为校正后的所述初始位姿；

基于所述第一坐标和所述第二坐标，确定所述半挂拖斗的所述目标位姿。

2.根据权利要求1所述的方法，其特征在于，所述基于所述第一坐标和所述第二坐标，确定所述半挂拖斗的所述目标位姿包括：

基于所述第一坐标和所述第二坐标，确定所述第一联结点对应的第一位置偏差；

基于所述第一位置偏差与偏差阈值之间的比较结果、所述第一坐标和所述第二坐标，对所述半挂拖斗的所述初始位姿进行校正，确定所述半挂拖斗的所述目标位姿；其中，所述偏差阈值用于对相邻两个所述车体进行刚性连接约束。

3.根据权利要求2所述的方法，其特征在于，所述基于所述第一位置偏差与偏差阈值之间的比较结果、所述第一坐标和所述第二坐标，对所述半挂拖斗的所述初始位姿进行校正，确定所述半挂拖斗的所述目标位姿包括：

若所述比较结果为所述第一位置偏差大于所述偏差阈值，则将所述半挂拖斗的所述初始位姿中的初始航向角确定为目标航向角；

利用所述第一坐标和所述第二坐标的坐标差值，对所述半挂拖斗的所述初始位姿中的初始坐标进行校正，确定所述半挂拖斗的目标坐标。

4.根据权利要求2所述的方法，其特征在于，所述基于所述第一位置偏差与偏差阈值之间的比较结果、所述第一坐标和所述第二坐标，对所述半挂拖斗的所述初始位姿进行校正，确定所述半挂拖斗的所述目标位姿包括：

若所述比较结果为所述第一位置偏差小于或等于所述偏差阈值，则基于所述第一坐标和所述半挂拖斗的所述初始位姿中的初始坐标，对所述半挂拖斗的所述初始位姿中的初始航向角进行校正，确定所述半挂拖斗的目标航向角；

基于第一距离和第二距离的距离差值和所述目标航向角，对所述初始坐标进行校正，确定所述半挂拖斗的目标坐标；其中，所述第一距离为所述第一坐标和所述初始坐标之间的第二位置偏差，所述第二距离为所述第一联结点到所述半挂拖斗的后轴中心之间的距离。

5.根据权利要求1所述的方法，其特征在于，所述获取各所述车体的初始位姿包括：

获取所述牵引车的所述初始位姿、所述牵引车的后轴中心处的第一速度和前轮偏角；

基于所述牵引车的所述初始位姿、所述第一速度和所述前轮偏角，以及各所述半挂拖斗的历史航向角，按照所述半挂汽车列车对应的运动学模型，确定各所述半挂拖斗的所述初始位姿；其中，所述历史航向角为当前位姿更新周期的前一相邻位姿更新周期中所得的航向角。