一种拖挂车辆的位置和姿态测量方法及系统

摘要

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