[发明专利]基于车辆运动模型的组合导航装置、算法及方法

权利要求书

1.一种基于车辆运动模型的组合导航装置，其特征在于，包括传感器信息读取模块、传感器信息的预处理模块、车辆模型运动预测模块、惯性导航定位模块、GPS卫星导航定位模块、组合导航定位模块、陀螺仪、GPS和编码器；所述陀螺仪、GPS和编码器采集的车辆速度、位置、航向信息通过传感器信息读取模块输入传感器信息的预处理模块进行预处理，然后送入所述组合导航定位进行数据的整合和融合，再通过所述惯性导航定位模块和GPS卫星导航定位模块对车辆进行导航定位，最后通过车辆模型运动控制模块对车辆的导航定位进行反馈和修正，最终实现对车辆运动的控制进而实现对车辆的导航控制。

2.一种基于车辆运动模型的组合导航算法，其特征在于，包括如下步骤：

步骤1、基于车辆运动模型的组合导航方法启动后，程序会读取车辆的陀螺仪、GPS和编码器配置并开始读取陀螺仪、GPS和编码器数据并进行预处理，并转入步骤2；

步骤2、程序会建立相应的坐标系，根据轮数信息，MEMS传感器进行导航定位中的航位推算，建立车辆运动模型，并转入步骤3；

步骤3、判断陀螺仪、GPS和编码器数据读取处理是否正常，如果有GPS位置速度信息，就进行GPS信息和INS信息进行卡尔曼滤波并融合数据进行车辆的速度位姿信息的计算，并转入步骤5，如果没有GPS信息则转入步骤4；

步骤4、当GPS无数据或GPS信息暂时失效时，则利用INS信息并利用车辆运动模型对车辆的位姿进行定位和预测，并进入步骤5；

步骤5、采用组合滤波器对当前的信息再做一次滤波，并进行反馈修正再次进入步骤2，同时输出的速度位置信息进入步骤6；

步骤6、对当前的速度位置信息经过车辆运动模型进行组合导航算法输出。

3.如权利要求2所述的一种基于车辆运动模型的组合导航算法，其特征在于，步骤2中，坐标系包括导航坐标系和载体坐标系，所述导航坐标系选择东北天坐标系，所述载体坐标系选为右前上坐标系。

4.如权利要求2所述的一种基于车辆运动模型的组合导航算法，其特征在于，步骤5中，所述组合滤波器采用组合GPS/INS的导航滤波器；所述组合GPS/INS的导航滤波器采用卡尔曼滤波。

5.如权利要求2所述的一种基于车辆运动模型的组合导航算法，其特征在于，步骤6中，所在没有干扰或机动不大的环境中,GPS的载波环对码环进行速度辅助；在较高动态或机动性较大的环境中，利用惯性补偿方法,将使用INS惯性导航。

6.一种基于车辆运动模型的组合导航方法，其特征在于，包括如下步骤：

步骤一、基于车辆控制模型的组合导航方法启动后，读取车辆的陀螺仪、GPS和编码器配置参数并启动陀螺仪、GPS和编码器，转入步骤二；

步骤二、传感器信息读取模块会检测当前的陀螺仪、GPS和编码器连接状态，并将陀螺仪、GPS和编码器的数据进行读取，进入步骤三；

步骤三、传感器信息的预处理模块会对陀螺仪、GPS和编码器的信息进行滤波、分析，并对陀螺仪、GPS和编码器的数据是否有效进行判断，判断GPS的信息是否有效，若GPS信息有效，转入步骤四，若GPS信息无效，则转入步骤五；

步骤四、组合导航定位模块依据组合导航算法会根据已有的状态信息进行融合滤波对车辆进行导航定位，并根据车辆模型运动预测模块车辆模型推算出车辆的位置和航向并计算出修正量，转入步骤六；

步骤五、将INS信息通过车辆模型运动预测模块对车辆的位置进行计算和预测，并根据车辆运动模型并计算出修正值，转入步骤六；

步骤六、车辆运动模型预测模块接受到车辆的修正量信息时，通过对车辆的位姿进行预测并输出车辆的控制信息并判断车辆导航是否完成，若没有完成直接转入步骤三，若完成则转入步骤七；

步骤七、对车辆的信息进行保存并推出组合导航。