CN109059904A - 用于移动载具的组合导航方法 - Google Patents

用于移动载具的组合导航方法 Download PDF

Info

Publication number
CN109059904A
CN109059904A CN201810556653.6A CN201810556653A CN109059904A CN 109059904 A CN109059904 A CN 109059904A CN 201810556653 A CN201810556653 A CN 201810556653A CN 109059904 A CN109059904 A CN 109059904A
Authority
CN
China
Prior art keywords
mobile carrier
moment
state
navigation
matrix
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
CN201810556653.6A
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.)
Wuxi Calman Navigation Technology Co Ltd
Zhejiang YAT Electrical Appliance Co Ltd
Original Assignee
Wuxi Calman Navigation Technology Co Ltd
Zhejiang YAT Electrical Appliance 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 Wuxi Calman Navigation Technology Co Ltd, Zhejiang YAT Electrical Appliance Co Ltd filed Critical Wuxi Calman Navigation Technology Co Ltd
Priority to CN201810556653.6A priority Critical patent/CN109059904A/zh
Publication of CN109059904A publication Critical patent/CN109059904A/zh
Priority to PCT/CN2019/089204 priority patent/WO2019228437A1/zh
Priority to US16/965,608 priority patent/US11566901B2/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
    • 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
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01D—HARVESTING; MOWING
    • A01D34/00—Mowers; Mowing apparatus of harvesters
    • A01D34/006—Control or measuring arrangements
    • A01D34/008—Control or measuring arrangements for automated or remotely controlled operation
    • 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/183—Compensation of inertial measurements, e.g. for temperature effects
    • 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/20—Instruments for performing navigational calculations
    • 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/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B69/00—Steering of agricultural machines or implements; Guiding agricultural machines or implements on a desired track
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01B—SOIL WORKING IN AGRICULTURE OR FORESTRY; PARTS, DETAILS, OR ACCESSORIES OF AGRICULTURAL MACHINES OR IMPLEMENTS, IN GENERAL
    • A01B79/00—Methods for working soil
    • A01B79/005—Precision agriculture
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01D—HARVESTING; MOWING
    • A01D2101/00—Lawn-mowers

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental Sciences (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Mechanical Engineering (AREA)
  • Soil Sciences (AREA)
  • Navigation (AREA)

Abstract

本发明提供了用于移动载具的组合导航方法，属于导航领域，包括令安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态；结合已得到的运动姿态，令安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，基于实时估计得到的运行状态误差估计值对移动载具的运行参数进行修正；基于修正后的导航信息对移动载具的运行路线进行控制。通过本发明提供的GNSS/INS组合导航方法，动态初始对准收敛速度快、精度高。可以有效辅助航向信息，提供更高精度的姿态角。当GNSS信号丢失时，该技术可以很好地抑制IMU发散，提供较高精度的位置、速度和姿态信息。

Description

用于移动载具的组合导航方法
技术领域
本发明属于导航领域，特别涉及用于移动载具的组合导航方法。
背景技术
随着经济的快速发展，生活水平的不断提高，人们对环境的绿化要求也越来越高，导致大量的草坪需要更多的人员来维护。智能家用移动载具的出现可以解放双手提高工作效率，有效降低家用草坪的维护成本，减少传统移动载具的尾气排放、高噪音等问题，使得割草能够更加智能、环保，控制更为简单，实现减少劳动力、降低成本的目标。
当前在移动载具中使用的导航系统主要是GPS，虽然GPS具有技术成熟、组件价格低廉的优势，但是GPS无法适应移动载具在较小区域工作范围内高精度定位的需求，进而无法实现基于GPS的高精度路线规划。
发明内容
为了解决现有技术中存在的缺点和不足，本发明提供了用于提高定位精确性的用于移动载具的组合导航方法。
为了达到上述技术目的，本发明提供了用于移动载具的组合导航方法，所述组合导航方法包括：
令安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态；
结合已得到的运动姿态，令安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，基于实时估计得到的运行状态误差估计值对移动载具的运行参数进行修正；
基于修正后的导航信息对移动载具的运行路线进行控制。
可选的，所述令安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态，包括：
基于惯性导航元件计算移动载具的速度信息；
基于惯性导航元件计算移动载具的位置信息；
基于得到的速度信息和位置信息进行计算得到移动载具的运动姿态。
可选的，所述基于惯性导航元件计算移动载具的速度信息，包括：
基于惯性导航元件中的陀螺仪获取移动载具的角速度信息，基于加速度计获取移动载具的线速度信息，结合公式一对移动载具进行速度解算
式中为k时刻导航坐标系下的载体速度，I为单位矩阵，ζk为导航系相对于惯性系在k-1到k时刻的角度变化量，X为叉乘运算，为k-1时刻的姿态旋转矩阵，为k-1到k时刻机体系下的载体速度增量，△vscul为划桨效应速度补偿量，△vcot为旋转效应速度补偿量，为由重力和格里奥利引起的速度增量；
可选的，所述基于惯性导航元件计算移动载具的速度信息，包括：
结合公式二基于惯性导航元件对移动载具进行位置解算，
式中
ζk为导航系下k-1到k时刻的等效旋转矢量，ζk为k-1到k时刻地球自转引起的等效旋转矢量，为k时刻的位置四元数；
利用更新后的四元数直接转换得到更新后的位置，四元数与位置之间关系为
其中L为当前位置纬度，λ为当前位置经度。
可选的，所述基于得到的速度信息和位置信息进行计算得到移动载具的运动姿态，包括：
构建如公式三所示的运动姿态解算四元数：
其中
ζk为导航系下k-1到k时刻的等效旋转矢量，φk为机体系下的等效旋转矢量，为k时刻的姿态四元数；
利用更新后的姿态四元数直接转换得到如公式四所示的姿态矩阵
其中为姿态矩阵方向余弦形式，qx为姿态四元数的第x维；
利用姿态矩阵与方向角之间的关系可以得到姿态角：
其中r、p、h为翻转角、俯仰角、航向角，为姿态矩阵的第x/y维，根据上述姿态角转换矩阵初始化初始姿态阵。
可选的，所述结合已得到的运动姿态，令安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，基于实时估计得到的运行状态误差估计值对移动载具的运行参数进行修正，包括：
选取位置误差、速度误差、平台姿态角以及陀螺和加表的零偏误差作为滤波估计的状态量，预测n-1时刻到n时刻的状态量状态协方差矩阵；
使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对移动载具的运行参数进行反馈修正。
可选的，所述预测n-1时刻到n时刻的状态量状态协方差矩阵，包括：
构建状态量表达式
构建状态量表达式Pn|n-1＝φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2，
其中，为n-1时刻状态量X的估计值；Pn-1：n-1时刻状态协方差矩阵；φn|n-1为-1时刻到n时刻状态转移矩阵；Q为系统噪声矩阵；δt为惯性测量单元更新周期。
可选的，所述使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对移动载具的运行参数进行反馈修正，包括：
使用Kalman滤波法进行实时估计，滤波增益表达式如公式五所示
Kn＝Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1 公式五
其中Kn为n时刻的滤波增益；Hn为n时刻的观测矩阵，由观测量与状态量之间的关系得到，Rn为n时刻观测噪声协方差矩阵；
根据公式六、公式七进行实时估计
其中为n时刻的状态估计量，Zn为n时刻的观测量即惯性测量单元与GNSS的位置速度差值；
Pn＝(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn T 公式七
其中Pn为n时刻的状态协方差矩阵，I为单位矩阵；
滤波估计后利用估计得到的状态量对当前位置、速度以及姿态进行修正，同时把估计得到的陀螺仪、加速度计零偏对下一个周期的输出信息进行反馈修正。
可选的，还包括对惯性导航元件和卫星导航元件得到的数据进行时间同步的操作。
本发明提供的技术方案带来的有益效果是：
通过GNSS/INS和车辆运动学模型的组合滤波估计实现实时的高灵敏高精度定位、定速及定姿。该GNSS/INS/车辆组合导航方法实现了GNSS与INS的优势互补，同时与车辆运动学模型相结合，实时提供高灵敏度、高精度的位置、速度、姿态信息等，能够满足移动载具在复杂环境下连续稳定作业的要求。
附图说明
为了更清楚地说明本发明的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明提供的用于移动载具的组合导航方法的流程示意图；
图2是本发明提供的步骤12的详细流程示意图；
图3是本发明提供的仅使用INS/车辆组合导航的方式的流程示意图。
具体实施方式
为使本发明的结构和优点更加清楚，下面将结合附图对本发明的结构作进一步地描述。
实施例一
本发明提供了用于移动载具的组合导航方法，如图1所示，所述组合导航方法包括：
11、令安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态；
12、结合已得到的运动姿态，令安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，基于实时估计得到的运行状态误差估计值对移动载具的运行参数进行修正；
13、基于修正后的导航信息对移动载具的运行路线进行控制。
在实施中，本申请提出的组合导航方法用于移动载具的导航、路径规划等方面。在现有的移动载具导航中，常常使用的是全球定位系统(Global Positioning System，GPS)，因其在移动载具经常使用的小范围内定位精度不高，本申请提出了使用全球导航卫星系统(Global Navigation Satellite System，GNSS)以及惯性导航系统(Inertial NavigationSystem，INS)在内的组合导航方式。
解决方案如下：
当由惯性器件输出解算得到新的位置、速度、姿态信息，并且GNSS已更新的位置和速度信息时，将惯导与GNSS的位置、速度差值作为观测量，以载体的位置、速度、姿态及INS的惯性器件误差作为状态量，进行Kalman滤波估计，得到15维状态量的误差修正值。
如果此时载体处于运动状态，则直接利用估计得到的状态误差对平台的位置、速度、姿态以及陀螺和加表的零偏进行反馈修正；
如果此时载体处于静止状态，则已知当前速度为零，将零速与解算获得的速度的差值作为观测量，进一步进行Kalman滤波估计得到状态修正量，然后反馈修正平台误差和IMU误差。
当惯导解算得到新的位置、速度和姿态信息，并且割草机的行走电机驱动更新了车辆电机转速信息，则将惯导与车辆的速度差值作为观测量进行Kalman滤波估计，得到状态误差修正量，用于反馈修正平台误差和IMU误差。
IMU更新速率比较快，当只有惯导解算更新了新的位置、速度和姿态信息，而GNSS和车辆模型未更新信息时，惯导更新的信息就是载体的位置、速度和姿态信息。
本发明利用惯性导航设备实现载体的姿态解算。在短时间内惯导可以提供高精度、高灵敏度的载体姿态，实时提供载体的姿态角，为割草机的自动作业提供准确的姿态信息。
(b)本发明将GNSS实时更新的高精度位置信息和惯性设备解算的信息进行组合。弥补了GNSS和惯性导航系统各自的缺点，实时提供高精度、高灵敏度的位置、速度和载体姿态信息，为割草机的自动作业提供精确的导航信息。
(c)本发明在载体静止时，采用零速修正和航向锁定技术。当载体处于静止状态时，IMU的航向不具有可观测性，采用零速修正和航向锁定可以很好的保持当前位置、姿态信息尤其是IMU的航向信息。
(d)本发明采用INS/车辆组合滤波估计，对车辆的三维速度进行约束即NHC(非完整性约束)。结合惯性解算得到的导航信息与行走电机的实时转速信息，采用NHC可以很好地辅助航向，当GNSS信息丢失时，NHC可以较好地抑制INS发散，短时间内提供更高精度的位置、速度和姿态信息，有效解决短时间内信号遮挡等复杂环境下精度较差的问题。
通过GNSS/INS和车辆运动学模型的组合滤波估计实现实时的高灵敏高精度定位、定速及定姿。GNSS系统定位精度高，但是数据实时性低，并且容易受信号强度的影响，在弱信号等复杂环境下精度急剧下降，无法满足移动载具在复杂环境下进行连续可靠作业的要求。INS系统短时间内精度高、实时性好，但是误差随时间的累积而增大。移动载具的运动学模型可以简化为2轮车辆模型，利用车辆的运动特性可以约束移动载具的部分自由度。该GNSS/INS/车辆组合导航方法实现了GNSS与INS的优势互补，同时与车辆运动学模型相结合，实时提供高灵敏度、高精度的位置、速度、姿态信息等，能够满足移动载具在复杂环境下连续稳定作业的要求。
具体的，针对步骤11的具体内容，包括如下内容：
111、基于惯性导航元件计算移动载具的速度信息；
112、基于惯性导航元件计算移动载具的位置信息；
113、基于得到的速度信息和位置信息进行计算得到移动载具的运动姿态。
在实施中，步骤111包括：
基于惯性导航元件中的陀螺仪获取移动载具的角速度信息，基于加速度计获取移动载具的线速度信息，结合公式一对移动载具进行速度解算
式中为k时刻导航坐标系下的载体速度，I为单位矩阵，ξk为导航系相对于惯性系在k-1到k时刻的角度变化量，X为叉乘运算，为k-1时刻的姿态旋转矩阵，为k-1到k时刻机体系下的载体速度增量，△vscul为划桨效应速度补偿量，△vcot为旋转效应速度补偿量，为由重力和格里奥利引起的速度增量；
步骤112则包括：
结合公式二基于惯性导航元件对移动载具进行位置解算，
式中
ζk为导航系下k-1到k时刻的等效旋转矢量，ζk为k-1到k时刻地球自转引起的等效旋转矢量，为k时刻的位置四元数；
根据四元数与位置之间关系
获取当前纬度L，和当前经度λ。
其中，为向量中的第k个元素。
基于已有的步骤111、112，步骤113具体包括：
构建如公式三所示的运动姿态解算四元数：
其中
ζk为导航系下k-1到k时刻的等效旋转矢量，φk为机体系下的等效旋转矢量，为k时刻的姿态四元数；表示B坐标系相对于A坐标系的四元数，n表示导航坐标系，b表示载体坐标系，
利用更新后的姿态四元数直接转换得到如公式四所示的姿态矩阵这里的转换步骤是成熟的算法，但是这种算法没有给出固定的名称，跟空间坐标系的旋转矩阵有关。
其中为姿态矩阵方向余弦形式，qx为姿态四元数的第x维；
利用姿态矩阵与方向角之间的关系可以得到姿态角：
其中r、p、h为翻转角、俯仰角、航向角，为姿态矩阵的第x/y维，根据上述姿态角转换矩阵初始化初始姿态阵。
可选的，步骤12，包括：
121、选取位置误差、速度误差、平台姿态角以及陀螺和加表的零偏误差作为滤波估计的状态量，预测n-1时刻到n时刻的状态量状态协方差矩阵；
122、使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对移动载具的运行参数进行反馈修正。
在实施中，将惯导解算获取的位置、速度与GNSS差分得到的位置、速度之间的差值作为观测量，利用KALMAN滤波实时估计状态误差，并利用状态误差估计值对平台位置速度和惯性仪器仪表误差进行反馈修正。将修正后的导航信息发送到机载计算机进行智能割草机自动作业的环路控制、指令下发等。其技术流程如图2所示。
IMU误差补偿和惯导解算：
根据Kalman滤波估计的IMU零偏误差或者初始给定的零偏误差，反馈修正IMU的实时输出量。
惯导解算部分参见前文中的惯导解算部分。
状态转移矩阵计算：选取位置误差、速度误差、平台姿态角以及陀螺和加表的零偏误差共15维作为滤波估计的状态量，即
其中：
分别对应车辆坐标系前右下三轴所对应的陀螺和加表误差，并不是对应IMU的XYZ三轴零偏误差，需要特别注意。
根据所选状态量之间的关系可以得到状态转移矩阵如下：
其中：F1：位置与位置之间状态转移矩阵；F2：位置与速度之间状态转移矩阵；F3：速度与位置之间状态转移矩阵；F4：速度与速度之间状态转移矩阵；F5：速度与姿态之间状态转移矩阵；F6：速度与加表零偏误差之间状态转移矩阵；F7：姿态与姿态之间状态转移矩阵；F8：姿态与陀螺零偏误差之间状态转移矩阵；F9：陀螺零偏误差与陀螺零偏误差之间状态转移矩阵；F10：加表零偏误差与加表零偏误差之间状态转移矩阵。以上状态转移矩阵可以由状态之间关系推导。
步骤121中的预测n-1时刻到n时刻的状态量状态协方差矩阵，包括：
构建状态量表达式
构建状态量表达式Pn|n-1＝φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2，
其中，为n-1时刻状态量X的估计值；Pn-1：n-1时刻状态协方差矩阵；φn|n-1为-1时刻到n时刻状态转移矩阵；Q为系统噪声矩阵；δt为惯性测量单元更新周期。
而步骤122则包括：
使用Kalman滤波法进行实时估计，滤波增益表达式如公式五所示
Kn＝Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1 公式五
其中Kn为n时刻的滤波增益；Hn为n时刻的观测矩阵，由观测量与状态量之间的关系得到，Rn为n时刻观测噪声协方差矩阵；
根据公式六、公式七进行实时估计
其中为n时刻的状态估计量，Zn为n时刻的观测量即惯性测量单元与GNSS的位置速度差值；
Pn＝(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn T 公式七
其中Pn为n时刻的状态协方差矩阵，I为单位矩阵；
滤波估计后利用估计得到的状态量对当前位置、速度以及姿态进行修正，同时把估计得到的陀螺仪、加速度计零偏对下一个周期的IMU输出信息进行反馈修正。
零速修正和航向锁定：
对于GNSS/INS组合导航，当载体静止时，虽然可以保持较高的位置精度，但是IMU的航向不具有可观性，载体航向会逐渐漂移，对于智能割草机或者其他对航向要求较高的领域是满足不了要求。
当载体处于静止状态时，可以对载体的三维速度按零速进行约束，同时约束载体航向，即在三维速度和一维航向上进一步进行组合滤波。
组合滤波的基本原理与上述相同，但是GNSS速度按零速处理，陀螺航向漂移限定为很小或者没有漂移，可以很好地抑制位置和航向随着时间的漂移。
可选的，还包括对惯性导航元件和卫星导航元件得到的数据进行时间同步的操作。
在实施中，GNSS信息相较于IMU信息更新较慢，当GNSS信息更新时，IMU不一定同步更新，所以要严格同步IMU与GNSS的时间。
GNSS的PPS信号是1s为周期，误差一般在20ns。当PPS来临时，系统晶振开始计时，在IMU更新时PPS处的时间加上系统计时即是IMU的更新时间。每个PPS处系统晶振都是重新计时，相当于每1s校准一次晶振，所以即便精度很差的晶振也满足高精度的时间同步要求。
GNSS信息更新时，把GNSS信息外推到当前最近更新的IMU信息所处的时刻，根据动态大小可选择是匀速外推还是匀加速外推。
这里的匀速外推是指在已得知GNSS信息滞后时，根据校准得到的时差，在保持移动载具行驶速度的前提下获取一定时间后的GNSS信息作为与IMU信息同步的信息；而匀加速外推则是指已得知GNSS信息滞后时，根据校准得到的时差，对移动载具的行驶速度进行匀加速处理，并在匀加速的前提下获取一定时间后的GNSS信息作为与IMU信息同步的信息，以保证两种信息中数据的同步。
采用软校正的方法实现IMU和GNSS和里程计的时间同步。在实时处理系统中，IMU与辅助信息存在时间异步问题，本发明利用高精度的1PPS(pulse per second)信号周期校准处理器计时系统，采用低精度的晶振可以获得高精度的同步时间。
本发明提供了用于移动载具的组合导航方法，包括令安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态；结合已得到的运动姿态，令安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，基于实时估计得到的运行状态误差估计值对移动载具的运行参数进行修正；基于修正后的导航信息对移动载具的运行路线进行控制。通过本发明提供的GNSS/INS组合导航方法，动态初始对准收敛速度快、精度高。可以有效辅助航向信息，提供更高精度的姿态角。当GNSS信号丢失时，该技术可以很好地抑制IMU发散，提供较高精度的位置、速度和姿态信息。
实施例二
本申请提出的组合导航方法，除了上述内容给出的使用GNSS/INS和车辆运动学模型以外，还给出如图3所示的仅使用INS/车辆组合导航的方式。移动载具的车辆运动学模型可以简化为2轮模型，以后轴中心为原点建立前右下坐标系，在理想情况下，在车辆后轴中心处右向和下向两个轴的速度为零，前向轴的速度可以根据车轮电机的实时转速转换获得，由此获得了车辆在前右下坐标系下的速度矢量。
选取车辆三轴的速度误差作为观测量进行INS/车辆组合导航，可以很好地辅助惯性测量单元航向，约束车辆三轴的速度和位置。尤其是当GNSS信息丢失时，可以抑制惯性测量单元的快速发散，在短时间内提供较高的导航精度，有效解决短时间内GNSS信号遭遇遮挡等复杂环境下产生的导航精度较差问题。
NHC约束的基本原理与上述组合导航相似，所不同的是状态观测量为车辆在前右下坐标系下的三轴速度误差，只能对车辆3维速度进行约束。
其中惯性测量单元(IMU)误差补偿惯导解算、状态转移矩阵计算、一步预测、反馈修正以及时间同步与实施例一中的IMU误差补偿和惯导解算、状态转移矩阵计算、预测n-1时刻到n时刻的状态量状态协方差矩阵Pn|n-1、时间同步、反馈修正等步骤的内容一致，不同的是观测量Zn只有3维速度差，而没有3维位置差。并且对时间同步的要求简化为时间差50ms的判断标准。
上述实施例中的各个序号仅仅为了描述，不代表各部件的组装或使用过程中的先后顺序。
以上所述仅为本发明的实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.用于移动载具的组合导航方法，其特征在于，所述组合导航方法包括：
令安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态；
结合已得到的运动姿态，令安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，基于实时估计得到的运行状态误差估计值对移动载具的运行参数进行修正；
基于修正后的导航信息对移动载具的运行路线进行控制。
2.根据权利要求1所述的用于移动载具的组合导航方法，其特征在于，所述令安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态，包括：
基于惯性导航元件计算移动载具的速度信息；
基于惯性导航元件计算移动载具的位置信息；
基于得到的速度信息和位置信息进行计算得到移动载具的运动姿态。
3.根据权利要求2所述的用于移动载具的组合导航方法，其特征在于，所述基于惯性导航元件计算移动载具的速度信息，包括：
基于惯性导航元件中的陀螺仪获取移动载具的角速度信息，基于加速度计获取移动载具的线速度信息，结合公式一对移动载具进行速度解算
式中为k时刻导航坐标系下的载体速度，I为单位矩阵，ξk为导航系相对于惯性系在k-1到k时刻的角度变化量，X为叉乘运算，为k-1时刻的姿态旋转矩阵，为k-1到k时刻机体系下的载体速度增量，△vscul为划桨效应速度补偿量，△vcot为旋转效应速度补偿量，为由重力和格里奥利引起的速度增量。
4.根据权利要求2所述的用于移动载具的组合导航方法，其特征在于，所述基于惯性导航元件计算移动载具的速度信息，包括：
结合公式二基于惯性导航元件对移动载具进行位置解算，
式中
ζk为导航系下k-1到k时刻的等效旋转矢量，ζk为k-1到k时刻地球自转引起的等效旋转矢量，为k时刻的位置四元数；
利用更新后的四元数直接转换得到更新后的位置，四元数与位置之间关系为
其中L为当前位置纬度，λ为当前位置经度。
5.根据权利要求2所述的用于移动载具的组合导航方法，其特征在于，所述基于得到的速度信息和位置信息进行计算得到移动载具的运动姿态，包括：
构建如公式三所示的运动姿态解算四元数：
其中
ζk为导航系下k-1到k时刻的等效旋转矢量，φk为机体系下的等效旋转矢量，为k时刻的姿态四元数；
利用更新后的姿态四元数直接转换得到如公式四所示的姿态矩阵
其中为姿态矩阵方向余弦形式，qx为姿态四元数的第x维；
利用姿态矩阵与方向角之间的关系可以得到姿态角：
其中r、p、h为翻转角、俯仰角、航向角，为姿态矩阵的第x/y维，根据上述姿态角转换矩阵初始化初始姿态阵。
6.根据权利要求1所述的用于移动载具的组合导航方法，其特征在于，所述结合已得到的运动姿态，令安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，基于实时估计得到的运行状态误差估计值对移动载具的运行参数进行修正，包括：
选取位置误差、速度误差、平台姿态角以及陀螺和加表的零偏误差作为滤波估计的状态量，预测n-1时刻到n时刻的状态量状态协方差矩阵；
使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对移动载具的运行参数进行反馈修正。
7.根据权利要求6所述的用于移动载具的组合导航方法，其特征在于，所述预测n-1时刻到n时刻的状态量状态协方差矩阵，包括：
构建状态量表达式
构建状态量表达式Pn|n-1＝φn|n-1Pn-1φn|n-1 T+(φn|n-1Q+Qφn|n-1 T)δt/2，
其中，为n-1时刻状态量X的估计值；Pn-1：n-1时刻状态协方差矩阵；φn|n-1为-1时刻到n时刻状态转移矩阵；Q为系统噪声矩阵；δt为惯性测量单元更新周期。
8.根据权利要求6所述的用于移动载具的组合导航方法，其特征在于，所述使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对移动载具的运行参数进行反馈修正，包括：
使用Kalman滤波法进行实时估计，滤波增益表达式如公式五所示
Kn＝Pn|n-1Hn T(HnPn|n-1Hn T+Rn)-1 公式五
其中Kn为n时刻的滤波增益；Hn为n时刻的观测矩阵，由观测量与状态量之间的关系得到，Rn为n时刻观测噪声协方差矩阵；
根据公式六、公式七进行实时估计
其中为n时刻的状态估计量，Zn为n时刻的观测量即惯性测量单元与GNSS的位置速度差值；
Pn＝(I-KnHn)Pn|n-1(I-KnHn)T+KnRnKn T 公式七
其中Pn为n时刻的状态协方差矩阵，I为单位矩阵；
滤波估计后利用估计得到的状态量对当前位置、速度以及姿态进行修正，同时把估计得到的陀螺仪、加速度计零偏对下一个周期的输出信息进行反馈修正。
9.根据权利要求1至8任一项所述的用于移动载具的组合导航方法，其特征在于，还包括对惯性导航元件和卫星导航元件得到的数据进行时间同步的操作。
CN201810556653.6A 2018-06-01 2018-06-01 用于移动载具的组合导航方法 Pending CN109059904A (zh)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN201810556653.6A CN109059904A (zh) 2018-06-01 2018-06-01 用于移动载具的组合导航方法
PCT/CN2019/089204 WO2019228437A1 (zh) 2018-06-01 2019-05-30 用于移动载具的组合导航方法
US16/965,608 US11566901B2 (en) 2018-06-01 2019-05-30 Integrated navigation method for mobile vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810556653.6A CN109059904A (zh) 2018-06-01 2018-06-01 用于移动载具的组合导航方法

Publications (1)

Publication Number Publication Date
CN109059904A true CN109059904A (zh) 2018-12-21

Family

ID=64819849

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810556653.6A Pending CN109059904A (zh) 2018-06-01 2018-06-01 用于移动载具的组合导航方法

Country Status (3)

Country Link
US (1) US11566901B2 (zh)
CN (1) CN109059904A (zh)
WO (1) WO2019228437A1 (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109855617A (zh) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 一种车辆定位方法、车辆定位装置及终端设备
CN109931926A (zh) * 2019-04-04 2019-06-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航算法
WO2019228437A1 (zh) * 2018-06-01 2019-12-05 浙江亚特电器有限公司 用于移动载具的组合导航方法
CN111190209A (zh) * 2019-10-21 2020-05-22 浙江亚特电器有限公司 一种割草机平行切割的方法、装置及割草机
CN112146561A (zh) * 2020-09-09 2020-12-29 无锡卡尔曼导航技术有限公司 一种霍尔角度传感器安装角度偏置的估计方法
CN112492943A (zh) * 2020-12-15 2021-03-16 塔里木大学 一种果园牵引式双肥箱变量施肥机及施肥方法
CN112577521A (zh) * 2020-11-26 2021-03-30 北京邮电大学 一种组合导航误差校准方法及电子设备
CN112729281A (zh) * 2020-12-18 2021-04-30 无锡卡尔曼导航技术有限公司 一种约束惯性/卫星组合导航静止时航向漂移的方法
CN113475215A (zh) * 2021-06-19 2021-10-08 北京正兴鸿业金属材料有限公司 利用激光测距和定位控制的无人剪草机
CN117570934A (zh) * 2024-01-15 2024-02-20 自然资源部第一海洋研究所 一种基于imu的gnss测波缺测补偿方法

Families Citing this family (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11592581B2 (en) * 2020-04-30 2023-02-28 Baidu Usa Llc Dual inertial measurement units for inertial navigation system
CN113008229B (zh) * 2021-02-26 2024-04-05 南京理工大学 一种基于低成本车载传感器的分布式自主组合导航方法
CN113029139B (zh) * 2021-04-07 2023-07-28 中国电子科技集团公司第二十八研究所 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN113188539A (zh) * 2021-04-27 2021-07-30 深圳亿嘉和科技研发有限公司 一种巡检机器人的组合定位方法
CN113758502B (zh) * 2021-09-24 2024-02-20 广东汇天航空航天科技有限公司 组合导航处理方法及装置
CN113865586B (zh) * 2021-09-29 2024-02-06 苏州挚途科技有限公司 安装角度的估计方法、装置和自动驾驶系统
CN113635892B (zh) * 2021-10-18 2022-02-18 禾多科技（北京）有限公司 车辆控制方法、装置、电子设备和计算机可读介质
CN114413929A (zh) * 2021-12-06 2022-04-29 阿波罗智能技术(北京)有限公司 定位信息的校验方法、装置、系统、无人车及存储介质
CN114413885B (zh) * 2021-12-22 2024-05-24 华人运通(上海)自动驾驶科技有限公司 基于多传感器融合定位的时间同步方法及系统
CN114413934B (zh) * 2022-01-20 2024-01-26 北京经纬恒润科技股份有限公司 一种车辆定位系统校正方法和装置
CN114509071B (zh) * 2022-04-20 2022-07-08 中国空气动力研究与发展中心低速空气动力研究所 一种风洞试验模型姿态测量方法
CN114895192B (zh) * 2022-05-20 2023-04-25 上海玫克生储能科技有限公司 基于卡尔曼滤波的soc估算方法、系统、介质及电子设备
CN115900701B (zh) * 2022-11-14 2023-10-31 中山大学 一种卫导拒止环境下的轨道车组合导航方法及装置
CN117647251A (zh) * 2024-01-30 2024-03-05 山东科技大学 基于观测噪声协方差矩阵的抗差自适应组合导航方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6424914B1 (en) * 2000-12-26 2002-07-23 American Gnc Corporation Fully-coupled vehicle positioning method and system thereof
CN101949703A (zh) * 2010-09-08 2011-01-19 北京航空航天大学 一种捷联惯性/卫星组合导航滤波方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361409A (zh) * 2000-12-23 2002-07-31 林清芳 增强型导航定位之方法及其系统
US8086405B2 (en) 2007-06-28 2011-12-27 Sirf Technology Holdings, Inc. Compensation for mounting misalignment of a navigation device
CN101788296B (zh) 2010-01-26 2011-11-16 北京航空航天大学 一种sins/cns深组合导航系统及其实现方法
CN102519463A (zh) 2011-12-13 2012-06-27 华南理工大学 一种基于扩展卡尔曼滤波的导航方法及装置
CN109059904A (zh) 2018-06-01 2018-12-21 浙江亚特电器有限公司 用于移动载具的组合导航方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6424914B1 (en) * 2000-12-26 2002-07-23 American Gnc Corporation Fully-coupled vehicle positioning method and system thereof
CN101949703A (zh) * 2010-09-08 2011-01-19 北京航空航天大学 一种捷联惯性/卫星组合导航滤波方法
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
施立涛等: "两种常用多传感器时间配准方法在匀加速模型下的改进", 《数字技术与应用》 *
李玉琴: "基于抛物线拟合的雷达网时间对准方法研究", 《科技视界》 *

Cited By (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019228437A1 (zh) * 2018-06-01 2019-12-05 浙江亚特电器有限公司 用于移动载具的组合导航方法
US11566901B2 (en) 2018-06-01 2023-01-31 Zhejiang Yat Electrical Appliance Co., Ltd Integrated navigation method for mobile vehicle
CN109855617A (zh) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 一种车辆定位方法、车辆定位装置及终端设备
CN109931926A (zh) * 2019-04-04 2019-06-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航算法
CN111190209A (zh) * 2019-10-21 2020-05-22 浙江亚特电器有限公司 一种割草机平行切割的方法、装置及割草机
CN112146561A (zh) * 2020-09-09 2020-12-29 无锡卡尔曼导航技术有限公司 一种霍尔角度传感器安装角度偏置的估计方法
CN112577521A (zh) * 2020-11-26 2021-03-30 北京邮电大学 一种组合导航误差校准方法及电子设备
CN112577521B (zh) * 2020-11-26 2023-11-17 北京邮电大学 一种组合导航误差校准方法及电子设备
CN112492943A (zh) * 2020-12-15 2021-03-16 塔里木大学 一种果园牵引式双肥箱变量施肥机及施肥方法
CN112729281A (zh) * 2020-12-18 2021-04-30 无锡卡尔曼导航技术有限公司 一种约束惯性/卫星组合导航静止时航向漂移的方法
CN113475215A (zh) * 2021-06-19 2021-10-08 北京正兴鸿业金属材料有限公司 利用激光测距和定位控制的无人剪草机
CN113475215B (zh) * 2021-06-19 2022-09-20 北京正兴鸿业金属材料有限公司 利用激光测距和定位控制的无人剪草机
CN117570934A (zh) * 2024-01-15 2024-02-20 自然资源部第一海洋研究所 一种基于imu的gnss测波缺测补偿方法
CN117570934B (zh) * 2024-01-15 2024-04-02 自然资源部第一海洋研究所 一种基于imu的gnss测波缺测补偿方法

Also Published As

Publication number Publication date
US11566901B2 (en) 2023-01-31
US20210041239A1 (en) 2021-02-11
WO2019228437A1 (zh) 2019-12-05

Similar Documents

Publication Publication Date Title
CN109059904A (zh) 用于移动载具的组合导航方法
CN107289933B (zh) 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
CN106950586A (zh) 用于农机作业的gnss/ins/车辆组合导航方法
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN201262709Y (zh) 小型无人直升飞机控制系统
CN105698822B (zh) 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法
CN103743395B (zh) 一种惯性重力匹配组合导航系统中时间延迟的补偿方法
US20210199438A1 (en) Heading initialization method for tilt rtk
CN110398257A (zh) Gps辅助的sins系统快速动基座初始对准方法
CN104501838B (zh) 捷联惯导系统初始对准方法
CN105203098A (zh) 基于九轴mems传感器的农业机械全姿态角更新方法
CN106507913B (zh) 用于管道测绘的组合定位方法
CN102768043B (zh) 一种无外观测量的调制型捷联系统组合姿态确定方法
CN106871928A (zh) 基于李群滤波的捷联惯性导航初始对准方法
CN105571578B (zh) 一种利用伪观测取代精密转台的原地旋转调制寻北方法
CN109596018A (zh) 基于磁测滚转角速率信息的旋转弹飞行姿态高精度估计方法
CN104713554A (zh) 一种基于mems惯性器件与安卓智能手机融合的室内定位方法
CN105865452B (zh) 一种基于间接卡尔曼滤波的移动平台位姿估计方法
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN109540130A (zh) 一种连采机惯性导航定位定向方法
CN108051866A (zh) 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法
CN105987696A (zh) 一种低成本车辆自动驾驶设计实现方法
CN109931955A (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN109884680A (zh) 基于多核dsp的北斗_sins紧组合导航系统及方法
CN107576327A (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: 20181221