CN110864688A - 一种用于车载方位开环水平姿态角闭环的航姿方法 - Google Patents

一种用于车载方位开环水平姿态角闭环的航姿方法 Download PDF

Info

Publication number
CN110864688A
CN110864688A CN201911193389.5A CN201911193389A CN110864688A CN 110864688 A CN110864688 A CN 110864688A CN 201911193389 A CN201911193389 A CN 201911193389A CN 110864688 A CN110864688 A CN 110864688A
Authority
CN
China
Prior art keywords
navigation
loop
attitude
error
equation
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201911193389.5A
Other languages
English (en)
Inventor
陈冬芳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan Rate Control Technology Co Ltd
Original Assignee
Hunan Rate Control Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan Rate Control Technology Co Ltd filed Critical Hunan Rate Control Technology Co Ltd
Priority to CN201911193389.5A priority Critical patent/CN110864688A/zh
Publication of CN110864688A publication Critical patent/CN110864688A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明涉及一种用于车载方位开环水平姿态角闭环的航姿方法，该方法通过GPS/INS组合导航***获得车辆的导航参数和导航参数误差，所述导航参数包括车辆进行姿态、速度和位置；根据预设的阈值速度，选择开环卡尔曼滤波和闭环卡尔曼滤波中的一种卡尔曼滤波方式；将GPS/INS组合导航***获取的导航参数和导航参数误差代入选择的卡尔曼滤波方式进行导航计算，输出导航结果通过该方法该方法利用卡尔曼滤波对GPS/INS组合导航***进行导航解算，消除了无人驾驶***在静态或准静态或低速行进时方位角小幅度振荡，提高无人驾驶导航***的导航精度。

Description

一种用于车载方位开环水平姿态角闭环的航姿方法
技术领域
本发明涉及无人驾驶***研究技术领域，更具体的说是涉及一种用于车载方位开环水平姿态角闭环的航姿方法。
背景技术
近年来无人驾驶车受到广泛关注，为交通的便利提供了一种全新的解决方案。无人驾驶车是一种以计算机控制技术为核心，集自动控制、体系结构、人工智能、视觉计算等众多技术于一体的高新科技载体。研究表明，基于车辆***特征的认识与精确建模，设计出的无人驾驶***可提高人-车-路闭环中的车辆安全性、燃油经济性、行驶平顺性、出行效率等。国内外对于无人驾驶***的研究多衍生于微型轮式移动车辆控制领域，集中于对车辆局部周边环境感知、同时定位与地图构建、运动规划与控制的研究。这种完全自主感知、规划与控制结构的无人驾驶***需要在没有任何前期环境信息的情况下，实时完成对环境的建模，且由于运动规划与控制的需要，对环境的建模必须精细，这就对无人驾驶车辆的感知***与处理器提出更高的要求。
随着导航技术的发展，利用全自主式导航***与卫星导航***结合而成的组合导航***越来越受到无人驾驶领域研究者的重视；其中，最常用的全自助式导航***的是光纤惯性导航***；光纤惯性导航***由三轴光纤陀螺和三轴加速度计构成，其中光纤陀螺具有精度高、体积轻便、结构紧凑、设计灵活、抗冲击和振动以及不易受磁性环境干扰等优势，是当前适合无人驾驶导航***的理想惯性传感器；
GPS是美国军方启动研发的项目，作为国际领先的定位***，具有精度高的全天候导航性能。GPS的定位精度为米级，可完全满足日常的汽车导航用，但是在无人驾驶车辆的城市道路行走时，精度远远不够。因此在工程中，由无人车的GPS与惯性导航***的组合导航***完成定位定姿功能，其定位精度高达厘米级，在无人车辆导航中获得广泛应用。由于在车辆实际行进中，会因环境的变化，其行进速度会有所改变。当在城市道路中行驶时，由于受到周围建筑或是人群的障碍，行进速度会变得缓慢甚至是准静态模式；此时，由GPS/INS组合导航解算出来姿态信息中的方位角会有小幅度振荡的现象，振荡幅度为0.5°左右。该振动的出现会给组合导航***带来一定的姿态误差，最终给导航执行机构带来错误判断。
因此，如何消除无人驾驶***在静态或准静态或低速行进时方位角小幅度振荡是本领域技术人员亟需解决的问题。
发明内容
本发明的目的在于，提出的一种用于车载方位开环水平姿态角闭环的航姿方法，消除无人驾驶***在静态或准静态或低速行进时方位角小幅度振荡，提高无人驾驶导航***的导航精度。
为了解决上述技术问题，本发明实施例提供一种基于改进神经网络的风功率中短期预测方法，包括：
S1、通过GPS/INS组合导航***获得车辆的导航参数和导航参数误差，所述导航参数包括车辆进行姿态、速度和位置；
S2、根据预设的阈值速度，选择开环卡尔曼滤波和闭环卡尔曼滤波中的一种卡尔曼滤波方式；
S3、将步骤S1中获取的导航参数和导航参数误差代入步骤S2中的选择的卡尔曼滤波方式进行导航计算，输出导航结果。
在一个实施例中，所述步骤S2中的卡尔曼滤波的设计为：
取***的状态方程和量测方程分别为：
Figure BDA0002294138030000021
Z＝HX+V
其中，X为***状态量；F为***矩阵；G为***噪声分布矩阵；W为***噪声矩阵；Z为观测量；H为观测矩阵；V为量测噪声阵；
根据以上的状态方程、量测方程以及噪声满足的条件，则离散卡尔曼滤波器的状态估计过程按以下五个基本方程求解：
状态一步预测方程：
Figure BDA0002294138030000022
状态估计方程：
Figure BDA0002294138030000023
滤波增益方程：
Figure BDA0002294138030000024
一步预测均方差方程：
Figure BDA0002294138030000025
估计均方差方程：
Figure BDA0002294138030000031
式中:
Figure BDA0002294138030000032
是滤波器利用上一时刻估计值
Figure BDA0002294138030000033
对tk时刻的状态量Xk的一步预测值；
Figure BDA0002294138030000034
是卡尔曼滤波器利用状态量和量测量对此刻状态量Xk的最终估计值；P阵对应各步预测的均方误差；Kk为增益矩阵，是使状态量估计值的均方误差达到最小的最佳增益矩阵。
在一个实施例中，所述步骤S3包括：
当速度低于预设的速度阈值时，采用开环卡尔曼滤波进行导航解算；当速度高于预设的速度阈值时，采用闭环卡尔曼滤波进行导航解算。
在一个实施例中，将GPS/INS组合导航***组合导航***的姿态误差、速度误差和位置误差，用于在开环卡尔曼滤波的每次迭代中校正惯性导航***的导航参数，但是并不反馈给INS；而最终输出的导航结果是由INS捷联惯性导航算法计算得到的导航结果，即校正后的惯性导航参数
Figure BDA0002294138030000035
和
Figure BDA0002294138030000036
构成组合导航输出；
校正后的参数是由原始惯性导航参数
Figure BDA0002294138030000037
和
Figure BDA0002294138030000038
获得：
Figure BDA0002294138030000039
Figure BDA00022941380300000310
Figure BDA00022941380300000311
其中，
Figure BDA00022941380300000312
和
Figure BDA00022941380300000313
分别是校正后的从载体b系到导航n系的惯性导航中姿态参数、速度参数和位置参数，该导航参数由卡尔曼滤波估计得到，卡尔曼滤波器中的P阵对应各步预测的均方误差设置为0，即P阵不反馈给INS；
Figure BDA00022941380300000314
和
Figure BDA00022941380300000315
分别是校正后的从载体b系到导航n系的原始惯性导航中姿态参数、速度参数和位置参数；
Figure BDA00022941380300000316
和
Figure BDA00022941380300000317
分别是从载体b系到导航n系的姿态误差、速度误差和位置误差。
在一个实施例中，所述闭环卡尔曼滤波为：
将GPS/INS组合导航***组合导航***的姿态误差、速度误差和位置误差，反馈到惯性导航处理器，用来校正惯性导航参数本身，反馈可以出现在每次的卡尔曼滤波器迭代；在每次反馈校正后，卡尔曼滤波器的姿态误差、速度误差和位置误差估计设置成初始值，在闭环组合中，只有经过校正的惯性导航参数，其校正方程为：
Figure BDA0002294138030000041
Figure BDA0002294138030000042
Figure BDA0002294138030000043
其中，(+)和(-)分别表示校正前和校正后的惯性导航参数；
在闭环结构中，IMU测量作为导航方程的输入，卡尔曼滤波器估计的陀螺误差和加速度计误差，均被用于反馈校正IMU测量；与姿态、速度和位置校正不同，在导航方程的每个迭代周期中，都必须进行加速度计和陀螺的校正，且卡尔曼滤波必须周期性地更新陀螺误差和加速度误差。
在一个实施例中，所述卡尔曼滤波器中的P阵对应方位角的均方误差设置为300"。
在一个实施例中，所述GPS/INS组合导航***中的惯性导航***包括：相互正交的三轴光纤陀螺(FOG)和三轴加速度计，其中FOG的精度为0.1°/h，加速度计的精度为300μg。
本发明的优点在于，本发明提供的一种用于车载方位开环水平姿态角闭环的航姿方法，该方法利用卡尔曼滤波对GPS/INS组合导航***进行导航解算。当无人车辆速度处于静态或准静态或低速行进时，利用所设置的速度阈值判断其行驶状态。当速度低于所设置的速度阈值时，采用开环卡尔曼滤波进行导航解算。当速度高于所设置的速度阈值时，采用闭环卡尔曼滤波进行导航解算。由此消除由于姿态信息的方位角振荡所带来的误差，提高无人驾驶导航***的导航精度。
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。
下面通过附图和实施例，对本发明的技术方案做进一步的详细描述。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
图1为为本发明实施例提供用于车载方位开环水平姿态角闭环的航姿方法的流程图；
图2为本发明实施例提供用于车载方位开环水平姿态角闭环的航姿方法的方位角修正方案的示意图；
图3为本发明实施例提供的开环卡尔曼滤波器的结构示意图；
图4为本发明实施例提供的闭环卡尔曼滤波器的结构示意图。
具体实施方式
下面将参照附图更详细地描述本公开的示例性实施例。虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员。
如图1-2所示，本发明实施例提供了一种用于车载方位开环水平姿态角闭环的航姿方法，包括：
S1、通过GPS/INS组合导航***获得车辆的导航参数和导航参数误差，所述导航参数包括车辆进行姿态、速度和位置；
S2、根据预设的阈值速度，选择开环卡尔曼滤波和闭环卡尔曼滤波中的一种卡尔曼滤波方式；
S3、将步骤S1中获取的导航参数和导航参数误差代入步骤S2中的选择的卡尔曼滤波方式进行导航计算，输出导航结果。
在本实施例中，步骤S1中采用的无人驾驶导航***为GPS/INS组合导航***，其中，惯性导航***包括：相互正交的三轴光纤陀螺(FOG)和三轴加速度计，其中FOG的精度为0.1°/h，加速度计的精度为300μg，卫星定位导航***为GPS。
GPS是三维全球定位***，可用于海陆空领域，完成距离观测和位置解算，是现代导航手段的主流技术，其主要特点是导航精度高，导航误差不随时间积累，但自主性能差，容易受到干扰，数据采样率低，难以满足实时导航要求；而惯性导航***是一种利用惯性测量元件、基准方向及最初位置信息来确定运载体姿态、速度和位置的自主式推算导航***；其特点导航误差随时间积累；这与GPS形成互补性；故GPS/INS组合导航***集成两个***的优点，克服其不足，其中GPS/INS组合导航***通常由惯性导航***、GPS接收模块、导航计算机及相关配套电路构成。
步骤S2-S3中，根据车辆实时速度与速度阈值之间大小关系，选择合适的滤波方式进行导航计算，消除导航误差，输出导航结果。
在一个实施例中，步骤S2中的卡尔曼滤波设计为GPS/INS组合导航***通常由惯性导航***、GPS接收模块、导航计算机及相关配套电路构成。为了满足工程上的可实现性和计算量小的需求，采用的是位置/速度组合模式，即为松组合模式，其工作方式主要体现在GPS对INS的辅助上。将GPS和INS的数据通过卡尔曼滤波进行处理，卡尔曼滤波实际上是一种最优估计的过程，其作用是估计***的状态。在本组合导航***中，利用卡尔曼滤波进行估计的主要对象是导航误差参数。卡尔曼滤波是递推计算的，除了需要知道***的状态方程、量测方程以及***噪声和测量噪声的统计特性外，还需知道初始状态估值和初始状态估计均方误差。
取***的状态方程和量测方程分别为：
Figure BDA0002294138030000061
Z＝HX+V
其中，X为***状态量；F为***矩阵；G为***噪声分布矩阵；W为***噪声矩阵；Z为观测量；H为观测矩阵；V为量测噪声阵。
根据以上的状态方程、量测方程以及噪声满足的条件，则离散卡尔曼滤波器的状态估计过程按以下五个基本方程求解：
状态一步预测方程：
Figure BDA0002294138030000062
状态估计方程：
Figure BDA0002294138030000063
滤波增益方程：
Figure BDA0002294138030000064
一步预测均方差方程：
Figure BDA0002294138030000065
估计均方差方程：
Figure BDA0002294138030000066
式中:
Figure BDA0002294138030000067
是滤波器利用上一时刻估计值
Figure BDA0002294138030000068
对tk时刻的状态量Xk的一步预测值；
Figure BDA0002294138030000069
是卡尔曼滤波器利用状态量和量测量对此刻状态量Xk的最终估计值；P阵对应各步预测的均方误差；Kk为增益矩阵，是使状态量估计值的均方误差达到最小的最佳增益矩阵。实际应用中，首先要给定***状态量的初值X0和均方误差阵的初值P0，再进行递推计算，即根据tk时刻的量测信息Zk和上一时刻tk-1的状态估计值
Figure BDA00022941380300000610
得到tk时刻的状态估计
Figure BDA00022941380300000611
在一个实施例中，当无人车辆速度处于静态或准静态或低速行进时，利用所设置的速度阈值判断其行驶状态。当速度低于所设置的速度阈值时，采用开环卡尔曼滤波进行导航解算。当速度高于所设置的速度阈值时，采用闭环卡尔曼滤波进行导航解算；
如图3所示，在本实施例中，开环卡尔曼滤波为：当车辆实时速度小于速度阈值时，采用开环卡尔曼滤波设计，即开环校正结构。估计出的姿态误差、速度误差和位置误差，用于在组合算法的每次迭代中校正惯性导航***的导航参数，但是并不反馈给INS。此时，组合导航的输出只反映了卡尔曼滤波器估计的效果，而最终输出的导航结果是由INS捷联惯性导航算法计算得到的导航结果。校正后的惯性导航参数
Figure BDA0002294138030000071
和
Figure BDA0002294138030000072
构成组合导航输出。校正后的参数是由原始惯性导航参数
Figure BDA0002294138030000073
和
Figure BDA0002294138030000074
获得：
Figure BDA0002294138030000075
Figure BDA0002294138030000076
Figure BDA0002294138030000077
其中，
Figure BDA0002294138030000078
和
Figure BDA0002294138030000079
分别是校正后的从载体b系到导航n系的惯性导航中姿态参数、速度参数和位置参数，这些导航参数是由卡尔曼滤波估计得到的。
Figure BDA00022941380300000710
和
Figure BDA00022941380300000711
分别是校正后的从载体b系到导航n系的原始惯性导航中姿态参数、速度参数和位置参数。
Figure BDA00022941380300000712
和
Figure BDA00022941380300000713
分别是从载体b系到导航n系的姿态误差、速度误差和位置误差。为抑制姿态信息中方位角振荡问题，此时，卡尔曼滤波器中的P阵对应各步预测的均方误差设置为0，即P阵不反馈给INS；
如图4所示，闭环卡尔曼滤波为：当车辆实时速度大于速度阈值时，采用闭环卡尔曼滤波设计，即闭环校正结构。借助于组合算法，GPS用于辅助INS，估计出的姿态误差、速度误差和位置误差，被反馈到惯性导航处理器，用来校正惯性导航参数本身，反馈可以出现在每次的卡尔曼滤波器迭代，或者更长的间隔时间。在每次反馈校正后，卡尔曼滤波器的姿态误差、速度误差和位置误差估计设置成初始值。此时，不存在独立的未校正惯性导航参数。在闭环组合中，只有经过校正的惯性导航参数，其校正方程为：
Figure BDA00022941380300000714
Figure BDA00022941380300000715
Figure BDA00022941380300000716
其中，(+)和(-)分别表示校正前和校正后的惯性导航参数；
在闭环结构中，IMU测量作为导航方程的输入，卡尔曼滤波器估计的陀螺误差和加速度计误差，均被用于反馈校正IMU测量。与姿态、速度和位置校正不同，在导航方程的每个迭代周期中，都必须进行加速度计和陀螺的校正，且卡尔曼滤波必须周期性地更新这些陀螺误差和加速度误差。
在一个实施例中，滤波器中的P阵对应方位角的均方误差设置为300"。
在一个实施例中，所述GPS/INS组合导航***中的惯性导航***包括：相互正交的三轴光纤陀螺(FOG)和三轴加速度计，其中FOG的精度为0.1°/h，加速度计的精度为300μg。
本发明提出了一种用于车载方位开环水平姿态角闭环的航姿方法，利用卡尔曼滤波对GPS/INS组合导航***进行导航解算。当无人车辆速度处于静态或准静态或低速行进时，利用所设置的速度阈值判断其行驶状态。当速度低于所设置的速度阈值时，采用开环卡尔曼滤波进行导航解算。当速度高于所设置的速度阈值时，采用闭环卡尔曼滤波进行导航解算。由此消除由于姿态信息的方位角振荡所带来的误差，提高无人驾驶导航***的导航精度。
最后所应说明的是，以上实施例仅用以说明本发明的技术方案而非限制。尽管参照实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，对本发明的技术方案进行修改或者等同替换，都不脱离本发明技术方案的精神和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (7)

1.一种用于车载方位开环水平姿态角闭环的航姿方法，其特征在于，包括：
S1、通过GPS/INS组合导航***获得车辆的导航参数和导航参数误差，所述导航参数包括车辆进行姿态、速度和位置；
S2、根据预设的阈值速度，选择开环卡尔曼滤波和闭环卡尔曼滤波中的一种卡尔曼滤波方式；
S3、将步骤S1中获取的导航参数和导航参数误差代入步骤S2中的选择的卡尔曼滤波方式进行导航计算，输出导航结果。
2.如权利要求1所述的一种用于车载方位开环水平姿态角闭环的航姿方法，其特征在于，所述步骤S2中的卡尔曼滤波的设计为：
取***的状态方程和量测方程分别为：
Figure FDA0002294138020000011
Z＝HX+V
其中，X为***状态量；F为***矩阵；G为***噪声分布矩阵；W为***噪声矩阵；Z为观测量；H为观测矩阵；V为量测噪声阵；
根据以上的状态方程、量测方程以及噪声满足的条件，则离散卡尔曼滤波器的状态估计过程按以下五个基本方程求解：
状态一步预测方程：
Figure FDA0002294138020000012
状态估计方程：
Figure FDA0002294138020000013
滤波增益方程：
Figure FDA0002294138020000014
一步预测均方差方程：
Figure FDA0002294138020000015
估计均方差方程：
Figure FDA0002294138020000016
式中:Φk,k-1是一步状态转移矩阵；Zk是***观测量方程的输出量；Hk是观测矩阵；Pk/k-1是一步预测的均方差；Rk是测量方程的协方差；Γk-1是噪声驱动矩阵；
Figure FDA0002294138020000021
是滤波器利用上一时刻估计值
Figure FDA0002294138020000022
对tk时刻的状态量Xk的一步预测值；
Figure FDA0002294138020000023
是卡尔曼滤波器利用状态量和量测量对此刻状态量Xk的最终估计值；P阵对应各步预测的均方误差；Kk为增益矩阵，是使状态量估计值的均方误差达到最小的最佳增益矩阵。
3.如权利要求1所述的一种用于车载方位开环水平姿态角闭环的航姿方法，其特征在于，所述步骤S2包括：
当速度低于预设的速度阈值时，采用开环卡尔曼滤波进行导航解算；当速度高于预设的速度阈值时，采用闭环卡尔曼滤波进行导航解算。
4.如权利要求3所述的一种用于车载方位开环水平姿态角闭环的航姿方法，其特征在于，所述步骤S2中的开环卡尔曼滤波为：
将GPS/INS组合导航***组合导航***的姿态误差、速度误差和位置误差，用于在开环卡尔曼滤波的每次迭代中校正惯性导航***的导航参数，但是并不反馈给INS；而最终输出的导航结果是由INS捷联惯性导航算法计算得到的导航结果，即校正后的惯性导航参数
Figure FDA0002294138020000024
和
Figure FDA0002294138020000025
构成组合导航输出；
校正后的参数是由原始惯性导航参数
Figure FDA0002294138020000026
和
Figure FDA0002294138020000027
获得：
Figure FDA0002294138020000028
Figure FDA0002294138020000029
Figure FDA00022941380200000210
其中，
Figure FDA00022941380200000211
和
Figure FDA00022941380200000212
分别是校正后的从载体b系到导航n系的惯性导航中姿态参数、速度参数和位置参数，该导航参数由卡尔曼滤波估计得到，卡尔曼滤波器中的P阵对应各步预测的均方误差设置为0，即P阵不反馈给INS；
Figure FDA00022941380200000213
和
Figure FDA00022941380200000214
分别是校正后的从载体b系到导航n系的原始惯性导航中姿态参数、速度参数和位置参数；
Figure FDA00022941380200000215
和
Figure FDA00022941380200000216
分别是从载体b系到导航n系的姿态误差、速度误差和位置误差。
5.如权利要求3所述的一种用于车载方位开环水平姿态角闭环的航姿方法，其特征在于，所述步骤S2中的闭环卡尔曼滤波为：
将GPS/INS组合导航***组合导航***的姿态误差、速度误差和位置误差，反馈到惯性导航处理器，用来校正惯性导航参数本身，反馈可以出现在每次的卡尔曼滤波器迭代；在每次反馈校正后，卡尔曼滤波器的姿态误差、速度误差和位置误差估计设置成初始值，在闭环组合中，只有经过校正的惯性导航参数，其校正方程为：
Figure FDA0002294138020000031
Figure FDA0002294138020000032
Figure FDA0002294138020000033
其中，(+)和(-)分别表示校正前和校正后的惯性导航参数；
在闭环结构中，IMU测量作为导航方程的输入，卡尔曼滤波器估计的陀螺误差和加速度计误差，均被用于反馈校正IMU测量；与姿态、速度和位置校正不同，在导航方程的每个迭代周期中，都必须进行加速度计和陀螺的校正，且卡尔曼滤波必须周期性地更新陀螺误差和加速度误差。
6.如权利要求2所述的一种用于车载方位开环水平姿态角闭环的航姿方法，其特征在于，所述卡尔曼滤波器中的P阵对应方位角的均方误差设置为300"。
7.如权利要求1所述的一种用于车载方位开环水平姿态角闭环的航姿方法，其特征在于，所述GPS/INS组合导航***中的惯性导航***包括：相互正交的三轴光纤陀螺(FOG)和三轴加速度计，其中FOG的精度为0.1°/h，加速度计的精度为300μg。
CN201911193389.5A 2019-11-28 2019-11-28 一种用于车载方位开环水平姿态角闭环的航姿方法 Pending CN110864688A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911193389.5A CN110864688A (zh) 2019-11-28 2019-11-28 一种用于车载方位开环水平姿态角闭环的航姿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911193389.5A CN110864688A (zh) 2019-11-28 2019-11-28 一种用于车载方位开环水平姿态角闭环的航姿方法

Publications (1)

Publication Number Publication Date
CN110864688A true CN110864688A (zh) 2020-03-06

Family

ID=69657472

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911193389.5A Pending CN110864688A (zh) 2019-11-28 2019-11-28 一种用于车载方位开环水平姿态角闭环的航姿方法

Country Status (1)

Country Link
CN (1) CN110864688A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112197767A (zh) * 2020-10-10 2021-01-08 江西洪都航空工业集团有限责任公司 一种在线改进滤波误差的滤波器设计方法
CN112874529A (zh) * 2021-02-05 2021-06-01 北京理工大学 基于事件触发状态估计的车辆质心侧偏角估计方法及***

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100097268A1 (en) * 2008-10-21 2010-04-22 Texas Instruments Incorporated Tightly-coupled gnss/imu integration filter having calibration features
CN102095424A (zh) * 2010-12-06 2011-06-15 国营红峰机械厂 一种适合车载光纤航姿***的姿态测量方法
CN105606094A (zh) * 2016-02-19 2016-05-25 北京航天控制仪器研究所 一种基于mems/gps组合***的信息条件匹配滤波估计方法
CN106679657A (zh) * 2016-12-06 2017-05-17 北京航空航天大学 一种运动载体导航定位方法及装置
CN109459044A (zh) * 2018-12-17 2019-03-12 北京计算机技术及应用研究所 一种gnss双天线辅助的车载mems惯导组合导航方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100097268A1 (en) * 2008-10-21 2010-04-22 Texas Instruments Incorporated Tightly-coupled gnss/imu integration filter having calibration features
CN102095424A (zh) * 2010-12-06 2011-06-15 国营红峰机械厂 一种适合车载光纤航姿***的姿态测量方法
CN105606094A (zh) * 2016-02-19 2016-05-25 北京航天控制仪器研究所 一种基于mems/gps组合***的信息条件匹配滤波估计方法
CN106679657A (zh) * 2016-12-06 2017-05-17 北京航空航天大学 一种运动载体导航定位方法及装置
CN109459044A (zh) * 2018-12-17 2019-03-12 北京计算机技术及应用研究所 一种gnss双天线辅助的车载mems惯导组合导航方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112197767A (zh) * 2020-10-10 2021-01-08 江西洪都航空工业集团有限责任公司 一种在线改进滤波误差的滤波器设计方法
CN112874529A (zh) * 2021-02-05 2021-06-01 北京理工大学 基于事件触发状态估计的车辆质心侧偏角估计方法及***
CN112874529B (zh) * 2021-02-05 2022-07-08 北京理工大学 基于事件触发状态估计的车辆质心侧偏角估计方法及***

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN109946732B (zh) 一种基于多传感器数据融合的无人车定位方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN105509738B (zh) 基于惯导/多普勒雷达组合的车载定位定向方法
CN103743414B (zh) 一种里程计辅助车载捷联惯导***行进间初始对准方法
CN107132563B (zh) 一种里程计结合双天线差分gnss的组合导航方法
Fakharian et al. Adaptive Kalman filtering based navigation: An IMU/GPS integration approach
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位***
CN108180925A (zh) 一种里程计辅助车载动态对准方法
CN109855617A (zh) 一种车辆定位方法、车辆定位装置及终端设备
CN112505737B (zh) 一种gnss/ins组合导航方法
CN103777220A (zh) 基于光纤陀螺、速度传感器和gps的实时精确位姿估计方法
CN105987696A (zh) 一种低成本车辆自动驾驶设计实现方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN111399023B (zh) 基于李群非线性状态误差的惯性基组合导航滤波方法
CN110006421B (zh) 一种基于mems和gps的车载导航方法及***
CN111006675B (zh) 基于高精度重力模型的车载激光惯导***自标定方法
CN112066983B (zh) 惯性/里程计组合导航滤波方法、电子设备及存储介质
CN112697138A (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN109959374A (zh) 一种行人惯性导航全时全程逆向平滑滤波方法
CN110864688A (zh) 一种用于车载方位开环水平姿态角闭环的航姿方法
Gao et al. An integrated land vehicle navigation system based on context awareness
Liu et al. Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN113503892A (zh) 一种基于里程计和回溯导航的惯导***动基座初始对准方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20200306