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

Abstract

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

Description

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

技术领域

本发明实施例涉及车辆运动参数测量领域，更具体地，涉及一种拖挂车辆的位置和姿态测量方法及系统。

背景技术

车辆的运动参数测量是车辆作为运动载体的核心参数，其包括车辆相对地面的位置、速度、姿态信息及加速度、角速率信息等。车辆运动参数的测量主要可以服务于车辆的研制分析、车辆运行状况的检测及安全监测。同时车辆运动参数信息是车辆自动控制的基础，车辆的自动控制包括农机具(拖拉机)的精准耕作控制、车辆的自动驾驶控制等。

车辆运动参数测量主要关注汽车作为一个刚体相对于物理空间的运动关系，目前的测量手段一般为GNSS高精度测量技术、INS/GNSS组合导航技术、高精度里程计及雷达等车身传感器。目前的技术手段及传感器系统可以实现汽车作为一个刚体的运动参数测量。但是作为拖挂车辆，其运动参数的测量不再为一个刚体，其是由多个刚体相关链接形成的一个整体，其测量问题就比单一车辆的测量更为复杂。传统的测量手段可以将每一个拖挂车辆作为独立的车辆进行独立的测量，这样既费时耗力由成本高昂。

将汽车作为刚体的测量系统以高精度GNSS与惯性导航设备形成的组合导航测量最为普遍。GNSS技术即通常所述的卫星导航技术，目前通用的系统包括美国GPS/中国BD/俄罗斯GLONASS卫星导航系统，GNSS技术可以通过RTK技术实现高精度的位置测量，高精度的GNSS测量必须采用载波相位观测值，RTK定位技术就是基于载波相位观测值的实时动态定位技术，它能够实时地提供测站点在指定坐标系中的三维定位结果，并达到厘米级精度。在RTK作业模式下，基准站通过数据链将其观测值和测站坐标信息一起传送给流动站。流动站不仅通过数据链接收来自基准站的数据，还要采集GPS观测数据，并在系统内组成差分观测值进行实时处理，同时给出厘米级定位结果，历时不到一秒钟。流动站可处于静止状态，也可处于运动状态；可在固定点上先进行初始化后再进入动态作业，也可在动态条件下直接开机，并在动态环境下完成周模糊度的搜索求解。在整周末知数解固定后，即可进行每个历元的实时处理，只要能保持四颗以上卫星相位观测值的跟踪和必要的几何图形，则流动站可随时给出厘米级定位结果。RTK技术的关键在于数据处理技术和数据传输技术，RTK定位时要求基准站接收机实时地把观测数据(伪距观测值，相位观测值)及已知数据传输给流动站接收机，数据量比较大，一般都要求9600的波特率，这在无线电上不难实现。

INS惯性导航技术是利用惯性传感器测量运动载体相对惯性空间的决定角速率及加速度信息，从而计算出来载体的姿态、速度及位置信息的一种测量技术。INS惯性导航系统多于GNSS卫星导航系统组合应用，完成载体的高精度位置/速度/姿态/角速率/加速度的高精度测量。

拖挂车辆一般由一个车头和多个拖挂体依次通过万向结构相连构成，可以将与车头相连的拖挂体叫做第一拖挂体，与第一拖挂体相连的拖挂体叫做第二拖挂体，依次类推；同时，可以将第一拖挂体与车头之间的连接点叫做第一连接点，第二拖挂体与第一拖挂体连之间的连接点叫做第二拖挂体，依次类推。常见的拖挂车辆有大型拖挂货车、火车、农业拖拉机及农具等。在拖挂车辆的自动控制及精密测量的时候(应用领域包括货车的驾驶员考试系统、农机的自动驾驶及自动控制、火车的测量控制等)，整个拖挂车辆不在可以利用一个刚体进行建模，即在其测量时需要测量每个拖挂结点的位置和姿态，然后将每个结点的测量结果统一叠加才可以表达整个拖挂车辆。

如图1所示，目前通用的车辆位置、速度、姿态的高精度测量设备一般为全球卫星导航系统(GNSS)/惯性导航系统(INS)组合导航系统，利用高精度GNSS测量板卡的RTK测量功能可以完成被测点厘米级精密位置测量，INS系统可以测量载体的高精度姿态信息，刚体的结构尺寸和GNSS测量点相对刚体的位置为已知，所以整个刚体的位置和姿态信息就成为一个确定的表达。

如果按照传统方法测量，拖挂车辆有多少个拖挂体，就需要多少套GNSS/INS组合导航设备。对于拖挂体数量较多的拖挂车辆，采用现有的测量方法，存在需要GNSS/INS组合导航设备的数量多的问题，进而导致测量成本高，不利于应用于时间生产当中。

发明内容

本发明实施例提供了一种克服上述问题或者至少部分地解决上述问题的拖挂车辆的位置和姿态测量方法及系统。

一方面本发明实施例提供了一种拖挂车辆的位置和姿态测量方法，所述方法包括：

S1，利用安装在拖挂车辆的车头上的GNSS/INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；

S2，利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态；

S3，根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算所述第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态。

其中，在步骤S1中，所述根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸计算所述第一连接点的位置具体包括：

根据所述车头的姿态、所述车头的外形尺寸计算所述第一连接点与所述车头的几何中心的相对位置；

根据所述相对位置和所述车头的位置，计算所述第一连接点的位置。

其中，在步骤S2中，所述将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态具体包括：

将所述第一连接点的位置作为所述第一拖挂体的位置量测，根据第一连接点位置与所述第一拖挂体的预测位置之间的误差，利用信息融合滤波方法，组合得到所述第一拖挂体的位置和姿态。

其中，所述信息融合滤波方法为卡尔曼滤波方法。

其中，在步骤S3之后，所述方法还包括：

对于所述拖挂车辆所有拖挂体中任意一个拖挂体，利用计算获得的所述任意一个拖挂体的位置和姿态，对所述任意一个拖挂体上的INS导航系统中的INS传感器进行误差修正。

其中，所述方法还包括：

根据所述拖挂车辆的车头的位置和姿态，以及所述拖挂车辆所有拖挂体的位置和姿态，得到所述拖挂车辆的整体位置和姿态。

其中，所述拖挂车辆上的所有独立的INS导航系统通过CAN总线、串口、网口或者无线通信的方式相互通信。

另一方面本发明实施例提供了一种拖挂车辆的位置和姿态测量系统，所述系统包括：

第一连接点位置获取模块，用于利用安装在拖挂车辆的车头上的GNSS/INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；

第一拖挂体位置/姿态获取模块，用于利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态；

其他拖挂体位置/姿态获取模块，用于根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算所述第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态。

第三方面本发明实施例提供了一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使所述计算机执行所述拖挂车辆的位置和姿态测量方法。

第四方面本发明实施例提供了一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行所述拖挂车辆的位置和姿态测量方法。

本发明实施例提供的一种拖挂车辆的位置和姿态测量方法及系统，只需在车头安装一套GNSS/INS组合导航系统，而其他拖挂体上仅安装INS导航系统，通过利用拖挂体之间的连接关系和拖挂体已知的外形尺寸，对INS导航系统输出的预测位置和预测姿态进行修正，就可得到每个拖挂体的精确位置和姿态，大大减少了测量过程中所需安装GNSS设备的数量，降低了测量成本，有利于市场推广应用。

附图说明

图1为现有技术中拖挂车辆的位置和姿态测量方法示意图；

图2为本发明实施例提供的一种拖挂车辆的位置和姿态测量方法的流程图；

图3为本发明实施例提供的一种拖挂车辆的位置和姿态测量方法的示意图；

图4为本发明实施例提供的一种拖挂车辆的位置和姿态测量方法的计算过程示意图；

图5为本发明实施例提供的另一种拖挂车辆的位置和姿态测量方法的示意图；

图6为本发明实施例提供的一种拖挂车辆的位置和姿态测量系统的结构框图；

图7为利用本发明实施例提供的拖挂车辆的位置和姿态测量方法和系统进行测量的实例的示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图2为本发明实施例提供的一种拖挂车辆位置和姿态的测量方法的流程图，如图2所示，所述方法包括：

S1，利用安装在拖挂车辆的车头上的GNSS/INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；

S2，利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态；

S3，根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算所述第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态。

其中，如图3所示，所述拖挂车辆的车头安装有GNSS/INS导航系统，所述拖挂车辆的其他拖挂体上都安装有独立的INS导航系统，而无需安装GNSS导航系统。

具体地，由于拖挂车辆的车头与各拖挂体之间是通过万向节等装置依次活动连接，所以在拖挂车辆运动过程当中，每个拖挂体都有各自独立的位置和姿态。要表征拖挂车辆的位置和姿态就需要分别获取每个拖挂体的位置和姿态。为了简化测量装置，本发明实施例提供的方法只需要在车头上设置GNSS/INS导航装置，获取车头的精确位置和姿态，再通过车头和拖挂体的外形尺寸以及拖挂车辆的连接关系一次获取各连接点的位置，进而通过连接点位置与各拖挂体上INS导航系统输出的预测位置和预测姿态进行融合得到对应拖挂体高精度的位置和姿态。

拖挂车辆只有车头安装有GNSS/INS组合导航系统，其它拖挂车辆均只按照INS系统，其中利用拖挂车辆车头的位置可以推算出车头与第一节过挂体的连接点的位置，有这个位置与拖挂车辆1的INS系统进行组合，依次求取后面拖挂车辆的位置/速度及姿态信息，顺序完成整个拖挂车辆的高精度位置/速度/姿态的测量，计算流程如图4所示。

进一步地，如图5所示，拖挂车辆在整体测量过程中，GNSS定位设备只测量车头和车尾两个独立拖挂体的高精度位置，INS设备安装与每个独立的拖挂车辆上测量车辆的惯性IMU信息，由于车辆的结构尺寸和物理连接关系为确定已知，算法可以利用车头/车尾两个高精度的定位数据以及INS的惯性测量数据计算得出整个拖挂车辆每个节点的高精度位置及姿态信息，从而整体建模拖挂车辆。

本发明实施例提供的一种拖挂车辆的位置和姿态测量方法，只需在车头安装一套GNSS/INS组合导航系统，而其他拖挂体上仅安装INS导航系统，通过利用拖挂体之间的连接关系和拖挂体已知的外形尺寸，对INS导航系统输出的预测位置和预测姿态进行修正，就可得到每个拖挂体的精确位置和姿态，大大减少了测量过程中所需安装GNSS设备的数量，降低了测量成本，有利于市场推广应用。

基于上述实施例，在步骤S1中，所述根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸计算所述第一连接点的位置具体包括：

根据所述车头的姿态、所述车头的外形尺寸计算所述第一连接点与所述车头的几何中心的相对位置；

根据所述相对位置和所述车头的位置，计算所述第一连接点的位置。

基于上述实施例，在步骤S2中，所述将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态具体包括：

将所述第一连接点的位置作为所述第一拖挂体的位置量测，根据第一连接点位置与所述第一拖挂体的预测位置之间的误差，利用信息融合滤波方法，组合得到所述第一拖挂体的位置和姿态。

进一步地，所述信息融合滤波方法为卡尔曼滤波方法。

基于上述实施例，在步骤S3之后，所述方法还包括：

对于所述拖挂车辆所有拖挂体中任意一个拖挂体，利用计算获得的所述任意一个拖挂体的位置和姿态，对所述任意一个拖挂体上的INS导航系统中的INS传感器进行误差修正。

具体地，对每个拖挂体上的INS导航系统中的惯性导航传感器进行误差修正可以使的INS导航系统在修正后输出的位置和姿态数据更加准确，有利于提高测量的精度。

基于上述实施例，根据所述拖挂车辆的车头的位置和姿态，以及所述拖挂车辆所有拖挂体的位置和姿态，得到所述拖挂车辆的整体位置和姿态。

进一步地，所述拖挂车辆上的所有独立的INS导航系统通过CAN总线、串口、网口或者无线通信的方式相互通信。

图6为本发明实施例提供的一种拖挂车辆的位置和姿态测量系统，所述系统包括：第一连接点位置获取模块1、第一拖挂体位置/姿态获取模块2以及其他拖挂体位置/姿态获取模块3。其中：

第一连接点位置获取模块1用于利用安装在拖挂车辆的车头上的GNSS/INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置。

第一拖挂体位置/姿态获取模块2用于利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态。

其他拖挂体位置/姿态获取模块3用于根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算所述第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态。

本发明实施例提供的一种拖挂车辆的位置和姿态测量系统，只需在车头安装一套GNSS/INS组合导航系统，而其他拖挂体上仅安装INS导航系统，通过利用拖挂体之间的连接关系和拖挂体已知的外形尺寸，对INS导航系统输出的预测位置和预测姿态进行修正，就可得到每个拖挂体的精确位置和姿态，大大减少了测量过程中所需安装GNSS设备的数量，降低了测量成本，有利于市场推广应用。

图7为利用本发明实施例提供的拖挂车辆的位置和姿态测量方法和系统进行测量的实例的示意图，如图7所示，拖挂车辆在进行驾校考试的过程中，需要判别包括车头和车厢两个部分的物理位置与交通路线的关系，本发明中在车头的A位置安装有高精度GNSS设备，可以完成A位置的厘米级高精度位置车辆，同时车辆上的INS系统(也可以通过双天线GNSS/INS系统测量)可以完成车辆的姿态信息的测量，车辆的物理轮廓为已知量，这样整个车头就可以在坐标系下清晰的表达。由A位置测量数据可以推算B连接点位置，B连接点位置结合车厢的组合导航设备可以得到b车的位置。相比与现有技术，不再需要在b车上按照GNSS设备。

本发明实施例公开一种计算机程序产品，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，计算机能够执行上述各方法实施例所提供的方法，例如包括：利用安装在拖挂车辆的车头上的GNSS/INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态；根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算所述第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态。

本发明实施例提供一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行上述各方法实施例所提供的方法，例如包括：利用安装在拖挂车辆的车头上的GNSS/INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态；根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算所述第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态。

本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于一计算机可读取存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：ROM、RAM、磁碟或者光盘等各种可以存储程序代码的介质。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行各个实施例或者实施例的某些部分所述的方法。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种拖挂车辆的位置和姿态测量方法，其特征在于，所述方法包括：

S1，利用安装在拖挂车辆的车头上的全球卫星导航GNSS/惯性导航INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；

S2，利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合，得到所述第一拖挂体的位置和姿态；

S3，根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态；

其中，将第一拖挂体与车头之间的连接点叫做第一连接点，第二拖挂体与第一拖挂体之间的连接点叫做第二连接点，依次类推。

2.根据权利要求1所述方法，其特征在于，在步骤S1中，所述根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸计算所述第一连接点的位置具体包括：

根据所述车头的姿态、所述车头的外形尺寸计算所述第一连接点与所述车头的几何中心的相对位置；

根据所述相对位置和所述车头的位置，计算所述第一连接点的位置。

3.根据权利要求1所述方法，其特征在于，在步骤S2中，所述将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合，得到所述第一拖挂体的位置和姿态具体包括：

将所述第一连接点的位置作为所述第一拖挂体的位置量测，根据第一连接点位置与所述第一拖挂体的预测位置之间的误差，利用信息融合滤波方法，组合得到所述第一拖挂体的位置和姿态。

4.根据权利要求3所述方法，其特征在于，所述信息融合滤波方法为卡尔曼滤波方法。

5.根据权利要求1所述方法，其特征在于，在步骤S3之后，所述方法还包括：

对于所述拖挂车辆所有拖挂体中任意一个拖挂体，利用计算获得的所述任意一个拖挂体的位置和姿态，对所述任意一个拖挂体上的INS导航系统中的INS传感器进行误差修正。

6.根据权利要求1所述方法，其特征在于，所述方法还包括：

根据所述拖挂车辆的车头的位置和姿态，以及所述拖挂车辆所有拖挂体的位置和姿态，得到所述拖挂车辆的整体位置和姿态。

7.根据权利要求1所述方法，其特征在于，所述拖挂车辆上的所有独立的INS导航系统通过CAN总线、串口、网口或者无线通信的方式相互通信。

8.一种拖挂车辆的位置和姿态测量系统，其特征在于，所述系统包括：

第一连接点位置获取模块，用于利用安装在拖挂车辆的车头上的GNSS/INS组合导航系统获取所述车头的位置和姿态，并根据所述车头的位置、所述车头的姿态以及所述车头的外形尺寸，计算第一连接点的位置；

第一拖挂体位置/姿态获取模块，用于利用安装在第一拖挂体上的INS导航系统获取所述第一拖挂体的预测位置和预测姿态，将所述第一连接点的位置与所述第一拖挂体的预测位置和预测姿态进行融合得到所述第一拖挂体的位置和姿态；

其他拖挂体位置/姿态获取模块，用于根据所述第一拖挂体的位置、所述第一拖挂体的姿态以及所述第一拖挂体的外形尺寸计算第二连接点的位置，利用安装在第二拖挂体上的INS导航系统获取所述第二拖挂体的预测位置和预测姿态，将所述第二连接点的位置与所述第二拖挂体的预测位置和预测姿态进行融合，得到所述第二拖挂体的位置和姿态，重复上述计算过程，直至得到所述拖挂车辆所有拖挂体的位置和姿态；

其中，将第一拖挂体与车头之间的连接点叫做第一连接点，第二拖挂体与第一拖挂体之间的连接点叫做第二连接点，依次类推。

9.一种计算机程序产品，其特征在于，所述计算机程序产品包括存储在非暂态计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被计算机执行时，使所述计算机执行如权利要求1至7任一项所述的方法。

10.一种非暂态计算机可读存储介质，其特征在于，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如权利要求1至7任一项所述的方法。