阅读说明：本技术 基于车辆运动模型的组合导航装置、算法及方法 (Combined navigation device, algorithm and method based on vehicle motion model ) 是由 曾斌 张奔 唐兴 苏岩 于 2019-10-22 设计创作，主要内容包括：本发明公开了一种基于车辆运动模型的组合导航装置、算法和方法,其装置包括传感器信息读取模块、传感器信息的预处理模块、车辆模型运动预测模块、惯性导航定位模块、GPS卫星导航定位模块、组合导航定位模块、陀螺仪、GPS和编码器；所述陀螺仪、GPS和编码器采集的车辆速度、位置、航向信息通过传感器信息读取模块输入传感器信息的预处理模块进行预处理,然后送入所述组合导航定位进行数据的整合和融合,再通过所述惯性导航定位模块和GPS卫星导航定位模块对车辆进行导航定位,最后通过车辆模型运动控制模块对车辆的导航定位进行反馈和修正,最终实现对车辆运动的控制进而实现对车辆的导航控制。(The invention discloses a combined navigation device, an algorithm and a method based on a vehicle motion model, wherein the device comprises a sensor information reading module, a sensor information preprocessing module, a vehicle model motion prediction module, an inertial navigation positioning module, a GPS satellite navigation positioning module, a combined navigation positioning module, a gyroscope, a GPS and an encoder; the vehicle speed, position and course information acquired by the gyroscope, the GPS and the encoder are input into a preprocessing module of sensor information through a sensor information reading module for preprocessing, then are sent into the integrated navigation positioning module for data integration and fusion, then are subjected to navigation positioning on the vehicle through the inertial navigation positioning module and the GPS satellite navigation positioning module, and finally are fed back and corrected through a vehicle model motion control module, so that the vehicle motion is controlled, and the navigation control on the vehicle is realized.)

基于车辆运动模型的组合导航装置、算法及方法

技术领域

本发明属于无人车导航技术，具体涉及一种基于车辆运动模型的组合导航装置、算法及方法。

背景技术

由于二战时期导弹制导的需要，德国研制出了基于惯性器件的V2火箭，惯性导航从此诞生，这也标志着现代导航技术的开始。导航技术是一门多学科综合交叉的领域，在军用领域(如：飞行器控制，精确制导，单兵定位等) 和民用领域(如：汽车导航，地图测绘，船舶导航，资源勘探等)方面都有着广泛的应用。

随着科技的进步和发展，现代导航系统种类也越来越多，如：多普勒导航系统，惯性导航系统，Omega导航系统，罗兰导航系统，卫星导航系统。其中现在应用最为广泛的当属惯性导航系统和卫星导航系统。

组合导航是指利用两种或者两种以上的导航技术进行组合，组合后的导航系统称为组合导航系统。根据不同的需要和不同的组合导航方式，对不同的导航系统进行组合。目前的组合导航方式主要有：惯性/卫星(INS/GNSS) 组合，惯性/天文(INS/CNS)，惯性/多普勒(INS/Doppler)，惯性/欧米茄 (INS/Omega)组合，惯性/侧向测距组合等方式。其中由于惯性导航系统(INS) 信号具有短时间定位精度高，但长时间误差累积造成定位发散，系统稳定性较差的特点；而卫星导航系统(GNSS)信号具有定位精度相对较差，系统稳定性好的特点。由于惯性/卫星导航系统的信号有着很好的互补优势，所以惯性/卫星导航定位系统是最为常用的组合导航方式。

近年来，卫星导航技术已广泛应用于民用领域，市面上的车载，船载，机载卫星导航定位系统已层出不穷。市面上的民用卫星导航定位已被GPS所垄断，为了打破GPS的垄断地位，我国也已于2000年开始构建自己的北斗卫星导航定位试验系统。经过十多年的研究和发展，我国已经建立起由14颗北斗2号卫星组成具有自主知识产权的北斗卫星定位系统。随着近年来我国对北斗定位系统的大力发展，围绕与北斗卫星导航定位系统相关的各种拓展应用也迅速蓬勃发展。而随着导航定位系统的广泛应用，人们对定位精度的要求越来越高，如何提高定位精度也是人们不断研究的问题。通过组合导航可以有效地提高导航的定位精度，所以GPS/INS组合导航算法也是一直以来人们研究的重点，相信组合导航算法的不断研究和成熟，会使导航定位有着更加广泛的应用前景。

透过组合导航技术发展的历程可以看到，早期的组合导航系统是以惯性导航系统为基础的工作方式，其工作方式从最初的重调方式，到后来运用经典的控制理论。直到20世纪60年代以后现代控制理论的出现，运用最优控制理论和卡尔曼滤波算法的设计，成为组合导航系统的核心工作方式。

基于卡尔曼滤波算法的组合导航系统是将各个传感器提供的导航信息输入卡尔曼滤波器中，并应用卡尔曼滤波算法对信息进行融合处理，从而估计出惯性导航系统的误差，再利用反馈输出对惯性导航系统进行修正，使得惯性导航系统的误差最小，通常将基于卡尔曼滤波算法的组合导航系统称为最优组合导航系统。

国外主要采用的GPS/INS的组合导航方式主要有以下几种：(1)松组合方法；(2)紧组合方法；(3)超紧密组合方法；(4)标量GPS修正方法；(5) 双跟踪闭环方法。

对于松组合和紧组合方法，主要通过GPS输出速度/位置信息，或伪距/ 伪距率信息和INS的陀螺仪和加速度计输出信息进行融合处理。这两种组合导航方式是利用接收机的输出结果进行的融合，没有涉及到接收机内部的信息，该方法不仅适用于GPS，也同样适用于其他卫星导航定位系统，我国的北斗导航定位系统也可以通过这两种方式进行组合导航定位。对于后面三种组合导航方式，需要通过惯性导航系统对GPS内部跟踪环进行辅助，同时也需要对跟踪环内部算法进行统一建模。该方法不仅算法复杂不容易实现，在应用实现上，也需要与接收机制造商进行密切的合作才能进行开发。

发明内容

本发明的目的之一在于提供一种基于车辆运动模型的组合导航装置，实现了无人车的导航定位功能。

本发明的目的之二在于提供一种基于车辆运动模型的组合导航算法，实现了无人车的导航定位功能。

本发明的目的之三提供一种基于车辆运动模型的组合导航方法,实现了无人车的导航定位功能.

实现本发明目的的技术解决方案为：

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

作为本发明第二方面的一种基于车辆运动模型的组合导航算法，包括如下步骤：

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

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

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

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

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

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

在本发明的一个优选实施例中，步骤2中，坐标系包括导航坐标系和载体坐标系，所述导航坐标系选择东北天坐标系，所述载体坐标系选为右前上坐标系。

在本发明的一个优选实施例中，步骤5中，所述组合滤波器采用组合GPS/INS的导航滤波器；所述组合GPS/INS的导航滤波器采用卡尔曼滤波。

在本发明的一个优选实施例中，步骤6中，所在没有干扰或机动不大的环境中,GPS的载波环对码环进行速度辅助；在较高动态或机动性较大的环境中，利用惯性补偿方法,将使用INS惯性导航。

作为本发明第三方面的一种基于车辆运动模型的组合导航方法，包括如下步骤：

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

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

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

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

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

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

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

本发明与现有技术相比，其显著优点：

(1)可以根据用户的要求进行陀螺仪、GPS和编码器方案的选择和配置。

(2)基于车辆运动模型的组合导航算法可根据车辆运动模型根据当前陀螺仪、GPS和编码器的信息自动选择合适的算法进行定位和导航。

附图说明

图1为本发明的基于车辆运动模型的组合导航装置的功能框图。

图2为本发明的基于车辆运动模型的组合导航方法的程序流程图。

具体实施方式

下面结合附图对本发明作进一步详细描述。

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

结合图1，种基于车辆运动模型的组合导航算法，包括如下步骤：

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

步骤2、程序会建立相应的坐标系，根据轮数信息，MEMS传感器进行导航定位中的航位推算，建立车辆运动模型，并转入步骤3；该步骤中，坐标系包括导航坐标系和载体坐标系，所述导航坐标系选择东北天坐标系，所述载体坐标系选为右前上坐标系。

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

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

步骤5、采用组合滤波器对当前的信息再做一次滤波，并进行反馈修正再次进入步骤2，同时输出的速度位置信息进入步骤6；该步骤中，组合滤波器采用组合GPS/INS的导航滤波器；所述组合GPS/INS的导航滤波器采用卡尔曼滤波。

步骤6、对当前的速度位置信息经过车辆运动模型进行组合导航算法输出。该步骤中，所在没有干扰或机动不大的环境中,GPS的载波环对码环进行速度辅助；在较高动态或机动性较大的环境中，利用惯性补偿方法,将使用 INS惯性导航。

结合图2，一种基于车辆运动模型的组合导航方法，包括如下步骤：

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

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

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

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

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

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