[发明专利]一种自动驾驶多节拖车及其位姿确定方法

权利要求书

1.一种自动驾驶多节拖车的位姿确定方法，所述自动驾驶多节拖车包括牵引车和至少一节拖车，其特征在于，该方法包括以下步骤：

（1）利用牵引车上的车端传感器获取牵引车的航向角，获取牵引车中心轴上任一点的坐标；

（2）利用道路上的路端传感器获取自动驾驶多节拖车在转弯过程中每节拖车车身上的标记物位置坐标；

（3）结合所述牵引车的航向角、牵引车中心轴上任一点的坐标以及该点在牵引车中心轴上的位置、每节拖车车身上的标记物位置坐标、每节拖车车身上的标记物到该拖车前轴中心点的距离和到该拖车后轴中心点的距离、以及牵引车和各节拖车的尺寸信息，计算得到每节拖车的位姿信息；

所述拖车的位姿信息包括拖车的航向角、拖车的前轴中心点坐标和/或后轴中心点坐标；所述牵引车和各节拖车的尺寸信息包括：牵引车的拖钩长度、每节拖车的拖杆长度、除最后一节拖车之外的其余每节拖车的拖钩长度；

通过在牵引车车身的不同位置上安装标记物，利用路端传感器获取牵引车车身上标记物的位置坐标，根据牵引车车身上标记物的位置坐标得到所述牵引车中心轴上任一点的坐标；

当所述牵引车中心轴上任一点的坐标为牵引车的后轴中心点坐标、所述拖车的位姿信息为拖车的航向角和拖车的后轴中心点坐标时，第一节拖车位姿信息的计算过程包括：

结合牵引车的后轴中心点坐标、牵引车的航向角和牵引车的拖钩长度，计算得到牵引车拖钩O₁的位置坐标；

结合牵引车拖钩O₁的位置坐标和第一节拖车车身标记物F₁的位置坐标，计算得到O₁F₁与大地坐标系X轴方向的偏角ω₁；

结合第一节拖车的拖杆长度、第一节拖车车身标记物F₁到该拖车前轴中心点C₁的距离和牵引车拖钩O₁到第一节拖车车身标记物F₁的距离，利用余弦定理计算得到C₁F₁与O₁F₁的夹角；

结合ω₁和β₁计算得到第一节拖车的航向角θ₁=β₁+ω₁，并结合第一节拖车的航向角θ₁、第一节拖车车身标记物F₁的位置坐标和第一节拖车车身标记物F₁到该拖车后轴中心点D₁的距离，计算得到第一节拖车的后轴中心点坐标D₁。

2.根据权利要求1所述的自动驾驶多节拖车的位姿确定方法，其特征在于，通过牵引车上的车端传感器获取所述牵引车中心轴上任一点的坐标。