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

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

Info

Publication number
WO2019228437A1
WO2019228437A1 PCT/CN2019/089204 CN2019089204W WO2019228437A1 WO 2019228437 A1 WO2019228437 A1 WO 2019228437A1 CN 2019089204 W CN2019089204 W CN 2019089204W WO 2019228437 A1 WO2019228437 A1 WO 2019228437A1
Authority
WO
WIPO (PCT)
Prior art keywords
mobile vehicle
time
state
attitude
speed
Prior art date
Application number
PCT/CN2019/089204
Other languages
English (en)
French (fr)
Inventor
陈勇
龚建飞
陈凤梧
张哲�
Original Assignee
浙江亚特电器有限公司
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 浙江亚特电器有限公司 filed Critical 浙江亚特电器有限公司
Priority to US16/965,608 priority Critical patent/US11566901B2/en
Publication of WO2019228437A1 publication Critical patent/WO2019228437A1/zh

Links

Images

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

Definitions

  • the invention belongs to the field of navigation, and particularly relates to a combined navigation method for a mobile vehicle.
  • GPS has the advantages of mature technology and low component prices, GPS cannot meet the needs of high-precision positioning of mobile vehicles in a small area of work, and therefore cannot be based on GPS high-precision route planning.
  • the present invention provides a combined navigation method for a mobile vehicle for improving positioning accuracy.
  • the present invention provides an integrated navigation method for a mobile vehicle, the integrated navigation method includes:
  • the satellite navigation elements installed on the mobile vehicle are used to estimate the running state of the mobile vehicle in real time to obtain the running state error estimation value, and the operating parameters of the mobile vehicle are estimated based on the running state error estimation value. Make corrections
  • the moving route of the mobile vehicle is controlled.
  • the inertial navigation element installed on the mobile vehicle obtains the motion parameters of the mobile vehicle, and calculates the motion posture of the mobile vehicle based on the motion parameters, including:
  • the movement attitude of the mobile vehicle is calculated based on the obtained speed information and position information.
  • the calculating the speed information of the mobile vehicle based on the inertial navigation element includes:
  • the angular velocity information of the mobile vehicle is obtained based on the gyroscope in the inertial navigation element, the linear velocity information of the mobile vehicle is obtained based on the accelerometer, and the speed calculation is performed on a pair of mobile vehicles based on the formula
  • Is the carrier speed in the navigation coordinate system at time k I is the identity matrix
  • ⁇ k is the angular change of the navigation system relative to the inertial system at times k-1 to k
  • X is the cross product operation
  • Is the attitude rotation matrix at time k-1 Is the carrier speed increment under the k-1 to k time machine system
  • ⁇ v scul is the paddle effect speed compensation amount
  • ⁇ v cot is the rotation effect speed compensation amount
  • Is the speed increase caused by gravity and Grioli Is the speed increase caused by gravity and Grioli
  • the calculating the speed information of the mobile vehicle based on the inertial navigation element includes:
  • ⁇ k is the equivalent rotation vector from k-1 to k in the navigation system, and ⁇ k is the equivalent rotation vector caused by the earth's rotation from k-1 to k, Is the position quaternion at time k;
  • the updated position is directly converted by using the updated quaternion.
  • the relationship between the quaternion and the position is
  • L is the latitude of the current position and ⁇ is the longitude of the current position.
  • the calculating the movement posture of the mobile vehicle based on the obtained speed information and position information includes:
  • Equation 3 Construct a kinematic solution quaternion as shown in Equation 3:
  • ⁇ k is the equivalent rotation vector from k-1 to k in the navigation system, and ⁇ k is the equivalent rotation vector in the machine system. Is the attitude quaternion at time k;
  • attitude angle The relationship between the attitude matrix and the direction angle can be used to obtain the attitude angle:
  • the satellite navigation element installed on the mobile vehicle can make a real-time estimation of the mobile vehicle's operating state, and the mobile vehicle's Correction of operating parameters, including:
  • Kalman filtering method is used to estimate the state quantity and state covariance matrix in real time. During the real-time estimation process, the operating parameters of the mobile vehicle are corrected according to the estimated error of the running state.
  • the prediction of the state quantity state covariance matrix from time n-1 to time n includes:
  • n-1 is the state transition matrix from time -1 to time n; Q is the system noise matrix; ⁇ t Update the period for the inertial measurement unit.
  • the Kalman filtering method is used to perform real-time estimation of the state quantity and the state covariance matrix, and the feedback and correction of the operating parameters of the mobile vehicle based on the estimated error of the operating state during the real-time estimation process include:
  • Kalman filtering is used for real-time estimation, and the expression of the filtering gain is shown in Equation 5.
  • K n P n
  • K n is the filter gain at time n
  • H n is the observation matrix at time n, which is obtained from the relationship between the observation and the state quantity
  • R n is the observation noise covariance matrix at time n
  • Z n is the observation and measurement at time n , that is, the position and speed difference between the inertial measurement unit and GNSS;
  • P n is the state covariance matrix at time n
  • I is the identity matrix
  • the current position, speed, and attitude are corrected using the estimated state quantities, and the estimated gyro and accelerometer zero offsets are used to correct the output information of the next cycle.
  • it also includes an operation of time synchronizing the data obtained by the inertial navigation element and the satellite navigation element.
  • the combined filtering estimation of GNSS / INS and vehicle kinematics model is used to realize real-time high-sensitivity high-precision positioning, speed and attitude.
  • the GNSS / INS / vehicle integrated navigation method realizes the complementary advantages of GNSS and INS, and combines with the vehicle kinematics model to provide high-sensitivity, high-precision position, speed, and attitude information in real time, which can meet the complex requirements of mobile vehicles. Requirements for continuous and stable operation in the environment.
  • FIG. 1 is a schematic flowchart of an integrated navigation method for a mobile vehicle provided by the present invention
  • FIG. 2 is a detailed flowchart of step 12 provided by the present invention.
  • FIG. 3 is a schematic flowchart of a mode using only INS / vehicle integrated navigation provided by the present invention.
  • the present invention provides an integrated navigation method for a mobile vehicle. As shown in FIG. 1, the integrated navigation method includes:
  • the satellite navigation elements installed on the mobile vehicle are used to estimate the running state of the mobile vehicle in real time to obtain the estimated error of the running state, and the mobile vehicle is based on the estimated error of the running state.
  • the integrated navigation method proposed in this application is used for navigation, path planning, etc. of a mobile vehicle.
  • a Global Positioning System GPS
  • this application proposes the use of global navigation satellites.
  • System Global Navigation System, GNSS
  • Inertial Navigation System Inertial Navigation System, INS
  • the position and speed difference between the inertial guide and the GNSS is used as the observation, and the position, speed, and The attitude and inertial device errors of the INS are used as state quantities, and Kalman filter estimation is performed to obtain a 15-dimensional state quantity error correction value.
  • the carrier is at a standstill at this time, the current speed is known to be zero.
  • the difference between the zero speed and the speed obtained from the solution is used as the observation. Further Kalman filtering estimation is performed to obtain the state correction amount, and then the platform error and IMU error are corrected by feedback. .
  • the walking motor drive of the lawnmower updates the vehicle motor rotation speed information
  • the speed difference between the inertial navigation vehicle and the vehicle is used as the observation to perform Kalman filtering estimation to obtain the state Error correction amount, used for feedback correction platform error and IMU error.
  • the IMU update rate is relatively fast.
  • the inertial navigation update information is the carrier's position, speed, and attitude information.
  • the invention uses inertial navigation equipment to realize the attitude calculation of the carrier.
  • Inertial guidance can provide high-precision and high-sensitivity carrier attitude in a short time, provide the attitude angle of the carrier in real time, and provide accurate attitude information for the automatic operation of the lawn mower.
  • the present invention combines the high-precision position information updated in real time by GNSS and the information calculated by the inertial device. It makes up for the shortcomings of GNSS and inertial navigation systems, and provides high-precision, high-sensitivity position, speed, and carrier attitude information in real time, and provides accurate navigation information for the automatic operation of lawn mowers.
  • the invention uses zero speed correction and heading lock technology when the carrier is stationary. When the carrier is at a standstill, the heading of the IMU is not observable. Using zero speed correction and heading lock can well maintain the current position and attitude information, especially the heading information of the IMU.
  • the present invention uses INS / vehicle combined filter estimation to restrict the three-dimensional speed of the vehicle, that is, NHC (non-integrity constraint).
  • NHC non-integrity constraint
  • the use of NHC can well assist the heading.
  • the NHC can better suppress the divergence of the INS and provide a more accurate position in a short time.
  • Speed and attitude information can effectively solve the problem of poor accuracy in complex environments such as signal occlusion in a short time.
  • the combined filtering estimation of GNSS / INS and vehicle kinematics model is used to realize real-time high-sensitivity high-precision positioning, speed and attitude.
  • the GNSS system has high positioning accuracy, but the data has low real-time performance, and is easily affected by signal strength. The accuracy drops sharply in complex environments such as weak signals, which cannot meet the requirements of mobile vehicles for continuous and reliable operations in complex environments.
  • the INS system has high accuracy and good real-time performance in a short time, but the error increases with the accumulation of time.
  • the kinematics model of a moving vehicle can be simplified as a two-wheeled vehicle model, and some degrees of freedom of the moving vehicle can be constrained using the vehicle's motion characteristics.
  • the GNSS / INS / vehicle integrated navigation method realizes the complementary advantages of GNSS and INS, and combines with the vehicle kinematics model to provide high-sensitivity, high-precision position, speed, and attitude information in real time, which can meet the complex requirements of mobile vehicles. Requirements for continuous and stable operation in the environment.
  • step 11 includes the following content:
  • step 111 includes:
  • the angular velocity information of the mobile vehicle is obtained based on the gyroscope in the inertial navigation element, the linear velocity information of the mobile vehicle is obtained based on the accelerometer, and the speed calculation is performed on a pair of mobile vehicles based on the formula
  • Is the carrier speed in the navigation coordinate system at time k I is the identity matrix
  • ⁇ k is the angular change of the navigation system relative to the inertial system at times k-1 to k
  • X is the cross product operation
  • Is the attitude rotation matrix at time k-1 Is the carrier speed increment under the k-1 to k time machine system
  • ⁇ v scul is the paddle effect speed compensation amount
  • ⁇ v cot is the rotation effect speed compensation amount
  • Is the speed increase caused by gravity and Grioli Is the speed increase caused by gravity and Grioli
  • Step 112 includes:
  • ⁇ k is the equivalent rotation vector from k-1 to k in the navigation system, and ⁇ k is the equivalent rotation vector caused by the earth's rotation from k-1 to k, Is the position quaternion at time k;
  • step 113 specifically includes:
  • Equation 3 Construct a kinematic solution quaternion as shown in Equation 3:
  • ⁇ k is the equivalent rotation vector from k-1 to k in the navigation system, and ⁇ k is the equivalent rotation vector in the machine system.
  • attitude angle The relationship between the attitude matrix and the direction angle can be used to obtain the attitude angle:
  • step 12 includes:
  • the difference between the position and velocity obtained by the inertial navigation solution and the position and velocity obtained from the GNSS difference is used as the observation measurement.
  • KALMAN filtering is used to estimate the state error in real time. Feedback correction of inertial instrumentation errors.
  • the revised navigation information is sent to the on-board computer for loop control of the automatic operation of the intelligent lawn mower, and instructions are issued. The technical process is shown in Figure 2.
  • State transition matrix calculation 15 positions of position error, speed error, platform attitude angle, and bias error of gyroscope and meter are selected as state quantities for filtering estimation, ie
  • the state transition matrix can be obtained as follows:
  • F 1 state transition matrix between position and position
  • F 2 state transition matrix between position and speed
  • F 3 state transition matrix between speed and position
  • F 4 state transition matrix between speed and velocity
  • F 5 state transition matrix between speed and attitude
  • F 6 state transition matrix between speed and table bias error
  • F 7 state transition matrix between attitude and attitude
  • F 8 difference between attitude and gyro bias error State transition matrix
  • F 9 state transition matrix between gyro bias error and gyro bias error
  • F 10 state transition matrix between table bias error and table bias error.
  • the above state transition matrix can be derived from the relationship between states.
  • the state quantity state covariance matrix for the prediction of time n-1 to time n in step 121 includes:
  • n-1 is the state transition matrix from time -1 to time n; Q is the system noise matrix; ⁇ t Update the period for the inertial measurement unit.
  • step 122 includes:
  • Kalman filtering is used for real-time estimation, and the expression of the filtering gain is shown in Equation 5.
  • K n P n
  • K n is the filter gain at time n
  • H n is the observation matrix at time n, which is obtained from the relationship between the observation and the state quantity
  • R n is the observation noise covariance matrix at time n
  • Z n is the observation and measurement at time n , that is, the position and speed difference between the inertial measurement unit and GNSS;
  • P n is the state covariance matrix at time n
  • I is the identity matrix
  • the current position, speed, and attitude are corrected using the estimated state quantities, and the estimated gyro and accelerometer zero offsets are used to correct the IMU output information for the next cycle.
  • the three-dimensional speed of the carrier can be restricted at zero speed, and the heading of the carrier can be restricted at the same time, that is, the combined filtering is further performed in the three-dimensional speed and the one-dimensional heading.
  • the basic principle of the combined filtering is the same as above, but the GNSS speed is processed at zero speed, and the gyro heading drift is limited to little or no drift, which can well suppress the position and heading drift over time.
  • it also includes an operation of time synchronizing the data obtained by the inertial navigation element and the satellite navigation element.
  • the GNSS information is updated more slowly than the IMU information.
  • the IMU is not necessarily updated synchronously, so the time of the IMU and GNSS must be strictly synchronized.
  • the GNSS PPS signal has a period of 1s, and the error is generally 20ns.
  • the system crystal starts timing.
  • the time at the PPS plus the system timing is the update time of the IMU.
  • the crystal of the system at each PPS is re-timed, which is equivalent to calibrating the crystal every 1 s, so even a crystal with poor accuracy can meet the high-precision time synchronization requirements.
  • the GNSS information is extrapolated to the moment where the currently updated IMU information is located.
  • the dynamic size it is possible to choose whether to extrapolate at a uniform speed or at a uniform acceleration.
  • the extrapolation of uniform speed means that when the lag of GNSS information is known, according to the time difference obtained by calibration, the GNSS information obtained after a certain period of time is maintained as the information synchronized with the IMU information while maintaining the speed of the moving vehicle; Extrapolation means that when the lag of GNSS information is known, according to the time difference obtained by calibration, uniformly accelerate the travel speed of the mobile vehicle, and obtain the GNSS information after a certain period of time under the premise of uniform acceleration to synchronize with the IMU information. Information to ensure the synchronization of the data in the two types of information.
  • the time synchronization between IMU, GNSS and odometer is realized by soft correction method.
  • the IMU and the auxiliary information have a time asynchronous problem.
  • the present invention uses a high-precision 1PPS (pulse per second) signal period to calibrate the processor timing system.
  • a low-accuracy crystal oscillator can be used to obtain a high-precision synchronization time.
  • the invention provides a combined navigation method for a mobile vehicle, which comprises using an inertial navigation element mounted on the mobile vehicle to obtain a motion parameter of the mobile vehicle, and calculating a motion posture of the mobile vehicle based on the motion parameter calculation; Movement attitude, using satellite navigation elements installed on the mobile vehicle to estimate the running state of the mobile vehicle in real time, to obtain the estimated error value of the running state, and to correct the operating parameters of the mobile vehicle based on the estimated error value of the running state;
  • the revised navigation information controls the running route of the mobile vehicle.
  • the dynamic initial alignment has fast convergence speed and high accuracy. It can effectively assist heading information and provide a more accurate attitude angle.
  • the technology can well suppress IMU divergence and provide higher accuracy position, velocity and attitude information.
  • the integrated navigation method proposed in this application in addition to using the GNSS / INS and the vehicle kinematics model given above, also provides a method using only INS / vehicle integrated navigation as shown in FIG. 3.
  • the vehicle kinematics model of the moving vehicle can be simplified as a two-wheel model.
  • the front-right lower coordinate system is established after the center of the axis is the origin.
  • the speeds of the right and down axes at the center of the rear axis of the vehicle are zero ,
  • the speed of the forward shaft can be obtained according to the real-time rotation speed conversion of the wheel motor, thereby obtaining the speed vector of the vehicle in the front right lower coordinate system.
  • Selecting the speed error of the vehicle's three axes as the observation measurement for the INS / vehicle integrated navigation can well assist the inertial measurement unit's heading and constrain the speed and position of the vehicle's three axes. Especially when the GNSS information is lost, the rapid divergence of the inertial measurement unit can be suppressed, providing high navigation accuracy in a short time, and effectively solving the problem of poor navigation accuracy in a complex environment such as GNSS signal encountering occlusion in a short time.
  • NHC constraint The basic principle of NHC constraint is similar to the above-mentioned integrated navigation, except that the state view measurement is the three-axis speed error of the vehicle in the front right lower coordinate system, and it can only constrain the vehicle's 3-dimensional speed.
  • the inertial measurement unit (IMU) error compensation inertial navigation solution state transition matrix calculation, one-step prediction, feedback correction, and time synchronization and IMU error compensation and inertial navigation solution, state transition matrix calculation, and prediction State quantity from time 1 to time n
  • n-1 , time synchronization, and feedback correction are the same.
  • the difference is that the measured Zn has only a three-dimensional velocity difference and no three-dimensional position difference.
  • time synchronization is simplified to a judgment standard of time difference of 50ms.

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

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

Description

用于移动载具的组合导航方法
本申请要求于2018年6月1日提交中国专利局、申请号201810556653.6、申请名称为“用于移动载具的组合导航方法”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本发明属于导航领域，特别涉及用于移动载具的组合导航方法。
背景技术
随着经济的快速发展，生活水平的不断提高，人们对环境的绿化要求也越来越高，导致大量的草坪需要更多的人员来维护。智能家用移动载具的出现可以解放双手提高工作效率，有效降低家用草坪的维护成本，减少传统移动载具的尾气排放、高噪音等问题，使得割草能够更加智能、环保，控制更为简单，实现减少劳动力、降低成本的目标。
当前在移动载具中使用的导航系统主要是GPS，虽然GPS具有技术成熟、组件价格低廉的优势，但是GPS无法适应移动载具在较小区域工作范围内高精度定位的需求，进而无法实现基于GPS的高精度路线规划。
发明内容
为了解决现有技术中存在的缺点和不足，本发明提供了用于提高定位精确性的用于移动载具的组合导航方法。
为了达到上述技术目的，本发明提供了用于移动载具的组合导航方法，所述组合导航方法包括：
利用安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态；
结合已得到的运动姿态，利用安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，得到运行状态误差估计值，并基于运行状态误差估计值对移动载具的运行参数进行修正；
基于修正后的导航信息对移动载具的运行路线进行控制。
可选的，所述令安装在移动载具上的惯性导航元件获取移动载具的运动参数，根据运动参数计算得到移动载具的运动姿态，包括：
基于惯性导航元件计算移动载具的速度信息；
基于惯性导航元件计算移动载具的位置信息；
基于得到的速度信息和位置信息进行计算得到移动载具的运动姿态。
可选的，所述基于惯性导航元件计算移动载具的速度信息，包括：
基于惯性导航元件中的陀螺仪获取移动载具的角速度信息，基于加速度计获取移动载具的线速度信息，结合公式一对移动载具进行速度解算
Figure PCTCN2019089204-appb-000001
式中
Figure PCTCN2019089204-appb-000002
为k时刻导航坐标系下的载体速度，I为单位矩阵，ξ k为导航系相对于惯性系在k-1到k时刻的角度变化量，X为叉乘运算，
Figure PCTCN2019089204-appb-000003
为k-1时刻的姿态旋转矩阵，
Figure PCTCN2019089204-appb-000004
为k-1到k时刻机体系下的载体速度增量，Δv scul为划桨效应速度补偿量，Δv cot为旋转效应速度补偿量，
Figure PCTCN2019089204-appb-000005
为由重力和格里奥利引起的速度增量；
可选的，所述基于惯性导航元件计算移动载具的速度信息，包括：
结合公式二基于惯性导航元件对移动载具进行位置解算，
Figure PCTCN2019089204-appb-000006
式中
Figure PCTCN2019089204-appb-000007
ζ k为导航系下k-1到k时刻的等效旋转矢量，ξ k为k-1到k时刻地球自转引起的等效旋转矢量，
Figure PCTCN2019089204-appb-000008
为k时刻的位置四元数；
利用更新后的四元数直接转换得到更新后的位置，四元数与位置之间关系为
Figure PCTCN2019089204-appb-000009
其中L为当前位置纬度，λ为当前位置经度。
可选的，所述基于得到的速度信息和位置信息进行计算得到移动载具的运动姿态，包括：
构建如公式三所示的运动姿态解算四元数：
Figure PCTCN2019089204-appb-000010
其中
Figure PCTCN2019089204-appb-000011
ζ k为导航系下k-1到k时刻的等效旋转矢量，φ k为机体系下的等效旋转矢量，
Figure PCTCN2019089204-appb-000012
为k时刻的姿态四元数；
利用更新后的姿态四元数直接转换得到如公式四所示的姿态矩阵
Figure PCTCN2019089204-appb-000013
Figure PCTCN2019089204-appb-000014
其中
Figure PCTCN2019089204-appb-000015
为姿态矩阵方向余弦形式，q x为姿态四元数
Figure PCTCN2019089204-appb-000016
的第x维；
利用姿态矩阵与方向角之间的关系可以得到姿态角：
Figure PCTCN2019089204-appb-000017
其中r、p、h为翻转角、俯仰角、航向角，
Figure PCTCN2019089204-appb-000018
为姿态矩阵
Figure PCTCN2019089204-appb-000019
的第x/y维，根据上述姿态角转换矩阵初始化初始姿态阵。
可选的，所述结合已得到的运动姿态，令安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，基于实时估计得到的运行状态误差估计值对移动载具的运行参数进行修正，包括：
选取位置误差、速度误差、平台姿态角以及陀螺和加表的零偏误差作为滤波估计的状态量，预测n-1时刻到n时刻的状态量状态协方差矩阵；
使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对移动载具的运行参数进行反馈修正。
可选的，所述预测n-1时刻到n时刻的状态量状态协方差矩阵，包括：
构建状态量表达式
Figure PCTCN2019089204-appb-000020
构建状态量表达式P n|n-1＝φ n|n-1P n-1φ n|n-1 T+(φ n|n-1Q+Qφ n|n-1 T)δt/2，
其中，
Figure PCTCN2019089204-appb-000021
为n-1时刻状态量X的估计值；P n-1：n-1时刻状态协方差矩阵；φ n|n-1为-1时刻到n时刻状态转移矩阵；Q为系统噪声矩阵；δt为惯性测量单元更新周期。
可选的，所述使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对移动载具的运行参数进行反馈修正，包括：
使用Kalman滤波法进行实时估计，滤波增益表达式如公式五所示
K n＝P n|n-1H n T(H nP n|n-1H n T+R n) -1  公式五
其中K n为n时刻的滤波增益；H n为n时刻的观测矩阵，由观测量与状态量之间的关系得到，R n为n时刻观测噪声协方差矩阵；
根据公式六、公式七进行实时估计
Figure PCTCN2019089204-appb-000022
其中
Figure PCTCN2019089204-appb-000023
为n时刻的状态估计量，Z n为n时刻的观测量即惯性测量单元与GNSS的位置速度差值；
P n＝(I-K nH n)P n|n-1(I-K nH n) T+K nR nK n T  公式七
其中P n为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 Navigation System，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包括：
基于惯性导航元件中的陀螺仪获取移动载具的角速度信息，基于加速度计获取移动载具的线速度信息，结合公式一对移动载具进行速度解算
Figure PCTCN2019089204-appb-000024
式中
Figure PCTCN2019089204-appb-000025
为k时刻导航坐标系下的载体速度，I为单位矩阵，ξ k为导航系相对于惯性系在k-1到k时刻的角度变化量，X为叉乘运算，
Figure PCTCN2019089204-appb-000026
为k-1时刻的姿态旋转矩阵，
Figure PCTCN2019089204-appb-000027
为k-1到k时刻机体系下的载体速度增量，Δv scul为划桨效应速度补偿量，Δv cot为旋转效应速度补偿量，
Figure PCTCN2019089204-appb-000028
为由重力和格里奥利引起的速度增量；
步骤112则包括：
结合公式二基于惯性导航元件对移动载具进行位置解算，
Figure PCTCN2019089204-appb-000029
式中
Figure PCTCN2019089204-appb-000030
ζ k为导航系下k-1到k时刻的等效旋转矢量，ξ k为k-1到k时刻地球自转引起的等效旋转矢量，
Figure PCTCN2019089204-appb-000031
为k时刻的位置四元数；
根据四元数与位置之间关系
Figure PCTCN2019089204-appb-000032
获取当前纬度L，和当前经度λ。
Figure PCTCN2019089204-appb-000033
Figure PCTCN2019089204-appb-000034
其中，
Figure PCTCN2019089204-appb-000035
为向量
Figure PCTCN2019089204-appb-000036
中的第k个元素。
基于已有的步骤111、112，步骤113具体包括：
构建如公式三所示的运动姿态解算四元数：
Figure PCTCN2019089204-appb-000037
其中
Figure PCTCN2019089204-appb-000038
ζ k为导航系下k-1到k时刻的等效旋转矢量，φ k为机体系下的等效旋转矢量，
Figure PCTCN2019089204-appb-000039
为k时刻的姿态四元数；
Figure PCTCN2019089204-appb-000040
表示B坐标系相对于A坐标系的四元数，n表示导航坐标系，b表示载体坐标系，
利用更新后的姿态四元数直接转换得到如公式四所示的姿态矩阵
Figure PCTCN2019089204-appb-000041
这里的转换步骤是成熟的算法，但是这种算法没有给出固定的名称，跟空间坐标系的旋转矩阵有关。
Figure PCTCN2019089204-appb-000042
其中
Figure PCTCN2019089204-appb-000043
为姿态矩阵方向余弦形式，q x为姿态四元数
Figure PCTCN2019089204-appb-000044
的第x维；
利用姿态矩阵与方向角之间的关系可以得到姿态角：
Figure PCTCN2019089204-appb-000045
其中r、p、h为翻转角、俯仰角、航向角，
Figure PCTCN2019089204-appb-000046
为姿态矩阵
Figure PCTCN2019089204-appb-000047
的第x/y维，根据上述姿态角转换矩阵初始化初始姿态阵。
可选的，步骤12，包括：
121、选取位置误差、速度误差、平台姿态角以及陀螺和加表的零偏误差作为滤波估计的状态量，预测n-1时刻到n时刻的状态量状态协方差矩阵；
122、使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对移动载具的运行参数进行反馈修正。
在实施中，将惯导解算获取的位置、速度与GNSS差分得到的位置、速度之间的差值作为观测量，利用KALMAN滤波实时估计状态误差，并利用状态误差估计值对平台位置速度和惯性仪器仪表误差进行反馈修正。将修正后的导航信息发送到机载计算机进行智能割草机自动作业的环路控制、指令下发等。其技术流程如图2所示。
IMU误差补偿和惯导解算：
根据Kalman滤波估计的IMU零偏误差或者初始给定的零偏误差，反馈修正IMU的实时输出量。
惯导解算部分参见前文中的惯导解算部分。
状态转移矩阵计算：选取位置误差、速度误差、平台姿态角以及陀螺和加表的零偏误差共15维作为滤波估计的状态量，即
Figure PCTCN2019089204-appb-000048
其中：
Figure PCTCN2019089204-appb-000049
分别对应车辆坐标系前右下三轴所对应的陀螺和加表误差，并不是对应IMU的XYZ三轴零偏误差，需要特别注意。
根据所选状态量之间的关系可以得到状态转移矩阵如下：
Figure PCTCN2019089204-appb-000050
其中：F 1：位置与位置之间状态转移矩阵；F 2：位置与速度之间状态转移矩阵；F 3：速度与位置之间状态转移矩阵；F 4：速度与速度之间状态转移矩阵；F 5：速度与姿态之间状态转移矩阵；F 6：速度与加表零偏误差之间状态转移矩阵；F 7：姿态与姿态之间状态转移矩阵；F 8：姿态与陀螺零偏误差之间状态转移矩阵；F 9：陀螺零偏误差与陀螺零偏误差之间状态转移矩阵；F 10：加表零偏误差与加表零偏误差之间状态转移矩阵。以上状态转移矩阵可以由状态之间关系推导。
步骤121中的预测n-1时刻到n时刻的状态量状态协方差矩阵，包括：
构建状态量表达式
Figure PCTCN2019089204-appb-000051
构建状态量表达式P n|n-1＝φ n|n-1P n-1φ n|n-1 T+(φ n|n-1Q+Qφ n|n-1 T)δt/2，
其中，
Figure PCTCN2019089204-appb-000052
为n-1时刻状态量X的估计值；P n-1：n-1时刻状态协方差矩阵；φ n|n-1为-1时刻到n时刻状态转移矩阵；Q为系统噪声矩阵；δt为惯性测量单元更新周期。
而步骤122则包括：
使用Kalman滤波法进行实时估计，滤波增益表达式如公式五所示
K n＝P n|n-1H n T(H nP n|n-1H n T+R n) -1  公式五
其中K n为n时刻的滤波增益；H n为n时刻的观测矩阵，由观测量与状态量之间的关系得到，R n为n时刻观测噪声协方差矩阵；
根据公式六、公式七进行实时估计
Figure PCTCN2019089204-appb-000053
其中
Figure PCTCN2019089204-appb-000054
为n时刻的状态估计量，Z n为n时刻的观测量即惯性测量单元与GNSS的位置速度差值；
P n＝(I-K nH n)P n|n-1(I-K nH n) T+K nR nK n T  公式七
其中P n为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时刻的状态量
Figure PCTCN2019089204-appb-000055
状态协方差矩阵P n|n-1、时间同步、反馈修正等步骤的内容一致，不同的是观测量Zn只有3维速度差，而没有3维位置差。并且对时间同步的要求简化为时间差50ms的判断标准。
上述实施例中的各个序号仅仅为了描述，不代表各部件的组装或使用过程中的先后顺序。
以上所述仅为本发明的实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

  1. 用于移动载具的组合导航方法，其特征在于，所述组合导航方法包括：
    利用安装在移动载具上的惯性导航元件获取所述移动载具的运动参数，根据所述运动参数计算得到所述移动载具的运动姿态；
    结合已得到的所述运动姿态，利用安装在所述移动载具上的卫星导航元件对所述移动载具的运行状态进行实时估计，得到运行状态误差估计值，并基于所述运行状态误差估计值对所述移动载具的运行参数进行修正；
    基于修正后的导航信息对所述移动载具的运行路线进行控制。
  2. 根据权利要求1所述的用于移动载具的组合导航方法，其特征在于，所述利用安装在移动载具上的惯性导航元件获取所述移动载具的运动参数，根据所述运动参数计算得到所述移动载具的运动姿态，包括：
    基于惯性导航元件计算移动载具的速度信息；
    基于所述惯性导航元件计算所述移动载具的位置信息；
    基于得到的所述速度信息和所述位置信息进行计算得到所述移动载具的运动姿态。
  3. 根据权利要求2所述的用于移动载具的组合导航方法，其特征在于，所述基于惯性导航元件计算移动载具的速度信息，包括：
    基于惯性导航元件中的陀螺仪获取移动载具的角速度信息，基于加速度计获取移动载具的线速度信息，结合公式一对移动载具进行速度解算
    Figure PCTCN2019089204-appb-100001
    式中
    Figure PCTCN2019089204-appb-100002
    为k时刻导航坐标系下的载体速度，I为单位矩阵，ξ k为导航系相对于惯性系在k-1到k时刻的角度变化量，X为叉乘运算，
    Figure PCTCN2019089204-appb-100003
    为k-1时刻的姿态旋转矩阵，
    Figure PCTCN2019089204-appb-100004
    为k-1到k时刻机体系下的载体速度增量，Δv scul为划桨效应速度补偿量，Δv cot为旋转效应速度补偿量，
    Figure PCTCN2019089204-appb-100005
    为由重力和格里奥利引起的速度增量。
  4. 根据权利要求2所述的用于移动载具的组合导航方法，其特征在于，所 述基于惯性导航元件计算移动载具的速度信息，包括：
    结合公式二基于惯性导航元件对移动载具进行位置解算，
    Figure PCTCN2019089204-appb-100006
    式中
    Figure PCTCN2019089204-appb-100007
    ζ k为导航系下k-1到k时刻的等效旋转矢量，ξ k为k-1到k时刻地球自转引起的等效旋转矢量，
    Figure PCTCN2019089204-appb-100008
    为k时刻的位置四元数；
    利用更新后的四元数直接转换得到更新后的位置，四元数与位置之间关系为
    Figure PCTCN2019089204-appb-100009
    其中L为当前位置纬度，λ为当前位置经度。
  5. 根据权利要求2所述的用于移动载具的组合导航方法，其特征在于，所述基于得到的所述速度信息和所述位置信息进行计算得到移动载具的运动姿态，包括：
    构建如公式三所示的运动姿态解算四元数：
    Figure PCTCN2019089204-appb-100010
    其中
    Figure PCTCN2019089204-appb-100011
    ζ k为导航系下k-1到k时刻的等效旋转矢量，φ k为机体系下的等效旋转矢 量，
    Figure PCTCN2019089204-appb-100012
    为k时刻的姿态四元数；
    利用更新后的姿态四元数直接转换得到如公式四所示的姿态矩阵
    Figure PCTCN2019089204-appb-100013
    Figure PCTCN2019089204-appb-100014
    其中
    Figure PCTCN2019089204-appb-100015
    为姿态矩阵方向余弦形式，q x为姿态四元数
    Figure PCTCN2019089204-appb-100016
    的第x维；
    利用姿态矩阵与方向角之间的关系可以得到姿态角：
    Figure PCTCN2019089204-appb-100017
    Figure PCTCN2019089204-appb-100018
    Figure PCTCN2019089204-appb-100019
    Figure PCTCN2019089204-appb-100020
    其中r、p、h为翻转角、俯仰角、航向角，
    Figure PCTCN2019089204-appb-100021
    为姿态矩阵
    Figure PCTCN2019089204-appb-100022
    的第x/y维，根据上述姿态角转换矩阵初始化初始姿态阵。
  6. 根据权利要求1所述的用于移动载具的组合导航方法，其特征在于，所述结合已得到的运动姿态，利用安装在移动载具上的卫星导航元件对移动载具的运行状态进行实时估计，得到运行状态误差估计值，并基于所述运行状态误差估计值对所述移动载具的运行参数进行修正，包括：
    选取位置误差、速度误差、平台姿态角以及陀螺和加表的零偏误差作为滤波估计的状态量，预测n-1时刻到n时刻的状态量状态协方差矩阵；
    使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对所述移动载具的运行参数进行反馈修正。
  7. 根据权利要求6所述的用于移动载具的组合导航方法，其特征在于，所述预测n-1时刻到n时刻的状态量状态协方差矩阵，包括：
    构建状态量表达式
    Figure PCTCN2019089204-appb-100023
    构建状态量表达式P n|n-1＝φ n|n-1P n-1φ n|n-1 T+(φ n|n-1Q+Qφ n|n-1 T)δt/2，其中，
    Figure PCTCN2019089204-appb-100024
    为n-1时刻状态量X的估计值；P n-1：n-1时刻状态协方差矩阵；φ n|n-1为-1时刻到n时刻状态转移矩阵；Q为系统噪声矩阵；δt为惯性测量单元更新周期。
  8. 根据权利要求6所述的用于移动载具的组合导航方法，其特征在于，所述使用Kalman滤波法对状态量和状态协方差矩阵进行实时估计，在实时估计过程中根据运行状态误差估计值对所述移动载具的运行参数进行反馈修正，包括：
    使用Kalman滤波法进行实时估计，滤波增益表达式如公式五所示
    K n＝P n|n-1H n T(H nP n|n-1H n T+R n) -1  公式五
    其中K n为n时刻的滤波增益；H n为n时刻的观测矩阵，由观测量与状态量之间的关系得到，R n为n时刻观测噪声协方差矩阵；
    根据公式六、公式七进行实时估计
    Figure PCTCN2019089204-appb-100025
    其中
    Figure PCTCN2019089204-appb-100026
    为n时刻的状态估计量，Z n为n时刻的观测量即惯性测量单元与GNSS的位置速度差值；
    P n＝(I-K nH n)P n|n-1(I-K nH n) T+K nR nK n T  公式七
    其中P n为n时刻的状态协方差矩阵，I为单位矩阵；
    滤波估计后利用估计得到的状态量对当前位置、速度以及姿态进行修正，同时把估计得到的陀螺仪、加速度计零偏对下一个周期的输出信息进行反馈修正。
  9. 根据权利要求1至8任一项所述的用于移动载具的组合导航方法，其特征在于，还包括对惯性导航元件和卫星导航元件得到的数据进行时间同步的操作。
PCT/CN2019/089204 2018-06-01 2019-05-30 用于移动载具的组合导航方法 WO2019228437A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US16/965,608 US11566901B2 (en) 2018-06-01 2019-05-30 Integrated navigation method for mobile vehicle

Applications Claiming Priority (2)

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

Publications (1)

Publication Number Publication Date
WO2019228437A1 true WO2019228437A1 (zh) 2019-12-05

Family

ID=64819849

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2019/089204 WO2019228437A1 (zh) 2018-06-01 2019-05-30 用于移动载具的组合导航方法

Country Status (3)

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

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112577521A (zh) * 2020-11-26 2021-03-30 北京邮电大学 一种组合导航误差校准方法及电子设备
CN113008229A (zh) * 2021-02-26 2021-06-22 南京理工大学 一种基于低成本车载传感器的分布式自主组合导航方法
CN113029139A (zh) * 2021-04-07 2021-06-25 中国电子科技集团公司第二十八研究所 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN113188539A (zh) * 2021-04-27 2021-07-30 深圳亿嘉和科技研发有限公司 一种巡检机器人的组合定位方法
CN113758502A (zh) * 2021-09-24 2021-12-07 广东汇天航空航天科技有限公司 组合导航处理方法及装置

Families Citing this family (22)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109059904A (zh) * 2018-06-01 2018-12-21 浙江亚特电器有限公司 用于移动载具的组合导航方法
CN109855617A (zh) * 2019-02-28 2019-06-07 深圳市元征科技股份有限公司 一种车辆定位方法、车辆定位装置及终端设备
CN109931926B (zh) * 2019-04-04 2023-04-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航方法
CN110927766A (zh) * 2019-10-21 2020-03-27 浙江亚特电器有限公司 割草机的卫星导航和围线结合的平行切割方法
US11592581B2 (en) * 2020-04-30 2023-02-28 Baidu Usa Llc Dual inertial measurement units for inertial navigation system
CN112146561B (zh) * 2020-09-09 2021-04-02 无锡卡尔曼导航技术有限公司 一种霍尔角度传感器安装角度偏置的估计方法
CN112492943A (zh) * 2020-12-15 2021-03-16 塔里木大学 一种果园牵引式双肥箱变量施肥机及施肥方法
CN112729281A (zh) * 2020-12-18 2021-04-30 无锡卡尔曼导航技术有限公司 一种约束惯性/卫星组合导航静止时航向漂移的方法
CN113475215B (zh) * 2021-06-19 2022-09-20 北京正兴鸿业金属材料有限公司 利用激光测距和定位控制的无人剪草机
CN113865586B (zh) * 2021-09-29 2024-02-06 苏州挚途科技有限公司 安装角度的估计方法、装置和自动驾驶系统
CN113917504B (zh) * 2021-09-30 2024-08-13 浙江工业大学 一种针对可控智能载具的位置估计方法
CN113635892B (zh) * 2021-10-18 2022-02-18 禾多科技（北京）有限公司 车辆控制方法、装置、电子设备和计算机可读介质
CN114019915B (zh) * 2021-11-04 2024-08-16 山东大学 多移动机器人协同运动控制系统及其控制方法
CN114413929B (zh) * 2021-12-06 2024-10-15 阿波罗智能技术(北京)有限公司 定位信息的校验方法、装置、系统、无人车及存储介质
CN114413885B (zh) * 2021-12-22 2024-05-24 华人运通(上海)自动驾驶科技有限公司 基于多传感器融合定位的时间同步方法及系统
CN114413934B (zh) * 2022-01-20 2024-01-26 北京经纬恒润科技股份有限公司 一种车辆定位系统校正方法和装置
WO2023158580A1 (en) * 2022-02-16 2023-08-24 Motional Ad Llc Prediction error scenario mining for machine learning models
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 中山大学 一种卫导拒止环境下的轨道车组合导航方法及装置
CN117570934B (zh) * 2024-01-15 2024-04-02 自然资源部第一海洋研究所 一种基于imu的gnss测波缺测补偿方法
CN117647251A (zh) * 2024-01-30 2024-03-05 山东科技大学 基于观测噪声协方差矩阵的抗差自适应组合导航方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361409A (zh) * 2000-12-23 2002-07-31 林清芳 增强型导航定位之方法及其系统
US20090254279A1 (en) * 2007-06-28 2009-10-08 Shaowei Han Compensation for mounting misalignment of a navigation device
CN101788296A (zh) * 2010-01-26 2010-07-28 北京航空航天大学 一种sins/cns深组合导航系统及其实现方法
CN102519463A (zh) * 2011-12-13 2012-06-27 华南理工大学 一种基于扩展卡尔曼滤波的导航方法及装置
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN109059904A (zh) * 2018-06-01 2018-12-21 浙江亚特电器有限公司 用于移动载具的组合导航方法

Family Cites Families (2)

* 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
CN101949703B (zh) * 2010-09-08 2012-11-14 北京航空航天大学 一种捷联惯性/卫星组合导航滤波方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361409A (zh) * 2000-12-23 2002-07-31 林清芳 增强型导航定位之方法及其系统
US20090254279A1 (en) * 2007-06-28 2009-10-08 Shaowei Han Compensation for mounting misalignment of a navigation device
CN101788296A (zh) * 2010-01-26 2010-07-28 北京航空航天大学 一种sins/cns深组合导航系统及其实现方法
CN102519463A (zh) * 2011-12-13 2012-06-27 华南理工大学 一种基于扩展卡尔曼滤波的导航方法及装置
CN106950586A (zh) * 2017-01-22 2017-07-14 无锡卡尔曼导航技术有限公司 用于农机作业的gnss/ins/车辆组合导航方法
CN109059904A (zh) * 2018-06-01 2018-12-21 浙江亚特电器有限公司 用于移动载具的组合导航方法

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112577521A (zh) * 2020-11-26 2021-03-30 北京邮电大学 一种组合导航误差校准方法及电子设备
CN112577521B (zh) * 2020-11-26 2023-11-17 北京邮电大学 一种组合导航误差校准方法及电子设备
CN113008229A (zh) * 2021-02-26 2021-06-22 南京理工大学 一种基于低成本车载传感器的分布式自主组合导航方法
CN113008229B (zh) * 2021-02-26 2024-04-05 南京理工大学 一种基于低成本车载传感器的分布式自主组合导航方法
CN113029139A (zh) * 2021-04-07 2021-06-25 中国电子科技集团公司第二十八研究所 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN113029139B (zh) * 2021-04-07 2023-07-28 中国电子科技集团公司第二十八研究所 基于运动检测的机场飞行区车辆差分北斗/sins组合导航方法
CN113188539A (zh) * 2021-04-27 2021-07-30 深圳亿嘉和科技研发有限公司 一种巡检机器人的组合定位方法
CN113758502A (zh) * 2021-09-24 2021-12-07 广东汇天航空航天科技有限公司 组合导航处理方法及装置
CN113758502B (zh) * 2021-09-24 2024-02-20 广东汇天航空航天科技有限公司 组合导航处理方法及装置

Also Published As

Publication number Publication date
US11566901B2 (en) 2023-01-31
CN109059904A (zh) 2018-12-21
US20210041239A1 (en) 2021-02-11

Similar Documents

Publication Publication Date Title
WO2019228437A1 (zh) 用于移动载具的组合导航方法
US11105633B2 (en) Navigation system utilizing yaw rate constraint during inertial dead reckoning
CN108051866B (zh) 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法
CN112859051B (zh) 激光雷达点云运动畸变的矫正方法
CN104121905B (zh) 一种基于惯性传感器的航向角获取方法
US20210199438A1 (en) Heading initialization method for tilt rtk
Sasani et al. Improving MEMS-IMU/GPS integrated systems for land vehicle navigation applications
CN106950586A (zh) 用于农机作业的gnss/ins/车辆组合导航方法
CN108594283B (zh) Gnss/mems惯性组合导航系统的自由安装方法
CN110702143B (zh) 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN107063241A (zh) 基于双gnss天线及单轴mems陀螺的前轮测角系统
CN102809377A (zh) 飞行器惯性/气动模型组合导航方法
CN105652306A (zh) 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法
CN111006675B (zh) 基于高精度重力模型的车载激光惯导系统自标定方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN115773751A (zh) 一种修正等效天向加计零位造成对准误差的方法
CN109596127A (zh) 一种无线电辅助航位推算的导航方法
CN105698790B (zh) 一种gnss-ins组合中的桥接方法
CN206540555U (zh) 基于双gnss天线及单轴mems陀螺的前轮测角系统
CN102997916A (zh) 一种自主提高定位定向系统惯性姿态解算精度的方法
CN211012986U (zh) 一种基于惯导技术的无人自主巡航车导航系统
CN111123381B (zh) 一种用于平台式重力仪减小水平加速度影响的方法
CN116793365A (zh) 基于时序神经网络辅助的农机定位导航方法
CN116519015A (zh) 一种基于相对距离约束的分布式协同导航方法及系统
CN104864868B (zh) 一种基于近距离地标测距的组合导航方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19811029

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 19811029

Country of ref document: EP

Kind code of ref document: A1