[发明专利]一种拖挂车辆的位置和姿态测量方法及系统

摘要

本发明实施例提供了一种拖挂车辆的位置和姿态测量方法及系统，所述方法包括：根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；S2，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态；S3，再按照步骤S2的方法依次得到所述拖挂车辆其他拖挂体的位置和姿态，直至得到所述拖挂车辆所有拖挂体的位置和姿态。通过利用拖挂体之间的连接关系和拖挂体已知的外形尺寸，对INS导航系统输出的预测位置和预测姿态进行修正，就可得到每个拖挂体的精确位置和姿态，大大减少了测量过程中所需安装GNSS设备的数量，降低了测量成本，有利于市场推广应用。

主权项

1.一种拖挂车辆的位置和姿态测量方法，其特征在于，所述方法包括：S1，利用安装在拖挂车辆的车头上的全球卫星导航GNSS/惯性导航INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；S2，利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合，得到所述第一拖挂体的位置和姿态；S3，根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态；其中，将第一拖挂体与车头之间的连接点叫做第一连接点，第二拖挂体与第一拖挂体之间的连接点叫做第二连接点，依次类推。