CN110926483B - Low-cost sensor combination positioning system and method for automatic driving - Google Patents
Low-cost sensor combination positioning system and method for automatic driving Download PDFInfo
- Publication number
- CN110926483B CN110926483B CN201911168614.XA CN201911168614A CN110926483B CN 110926483 B CN110926483 B CN 110926483B CN 201911168614 A CN201911168614 A CN 201911168614A CN 110926483 B CN110926483 B CN 110926483B
- Authority
- CN
- China
- Prior art keywords
- vehicle
- gps
- state
- kalman filter
- entering
- 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.)
- Active
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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
- G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational 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/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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
Abstract
The invention discloses a low-cost sensor combined positioning system for automatic driving, which is characterized in that a set of state machine is designed according to the positioning characteristics of a low-cost GPS by adopting a low-cost GPS and an IMU sensor and used for dynamically adjusting Kalman filter parameters, and then the current position, speed and course information of a vehicle is output through a prediction and state updating module of a Kalman filter. The invention also provides a low-cost sensor combination positioning method for automatic driving. The invention not only reduces the hardware cost of the positioning external member, but also facilitates the automatic driving marketization; and finally, the positioning precision of the whole system can reach a sub-meter level.
Description
Technical Field
The invention belongs to the field of automatic driving, and particularly relates to a low-cost sensor combination positioning system and method for automatic driving.
Background
Integrated navigation is an important research topic in the field of autopilot, and expensive high-precision GPS and IMU are commonly used at present. RTK (high precision GPS) suite equipment is tens of thousands of dollars in price; in addition, the mobile terminal processes satellite signals with the base station and the data processing center through the communication module, and the mobile terminal cannot work effectively in places with severe communication environments. The high-precision IMU is composed of a magnetometer, a gyroscope and an accelerometer and can acquire the attitude information of the current vehicle. The prices for industrial-grade IMUs range from thousands to tens of thousands of dollars. The conventional combined navigation equipment cannot realize the marketization of the automatic driving technology because the sensor is expensive and cannot be used.
Disclosure of Invention
The purpose of the invention is as follows: aiming at the problems in the prior art, the invention provides a low-cost sensor combined positioning system for automatic driving, which is low in cost and good in positioning effect.
The technical scheme is as follows: in order to achieve the aim, the invention provides a low-cost sensor combination positioning system for automatic driving, which comprises a vehicle speed information acquisition unit, a vehicle course information acquisition unit, a vehicle position information acquisition unit and a vehicle position positioning unit, wherein the vehicle speed information acquisition unit is used for acquiring real-time vehicle speed information of a vehicle; the vehicle course information acquisition unit is used for acquiring the course information of the vehicle in real time; the vehicle position information acquisition unit is a GPS and is used for acquiring vehicle position information in real time; the vehicle positioning unit comprises a state machine and a Kalman filter, the state machine adjusts parameters of the Kalman filter according to the parameters fed back by the GPS, and the Kalman filter positions the vehicle according to the vehicle information input by the vehicle speed information acquisition unit, the vehicle course information acquisition unit and the vehicle position information acquisition unit.
Further, the state machine comprises a restart state, a GPS invalid state, a GPS valid state and a parking/slow driving state, wherein in the restart state, a system covariance matrix P, a measurement noise covariance matrix R and a system state quantity matrix X in Kalman filter parameters are initialized; when the GPS is in an invalid state, the position information acquired by the GPS is not input into the Kalman filter; when the GPS is in an effective state, the position information collected by the GPS is input into a Kalman filter; when the vehicle is in a parking/slow running state, inputting the position information of the GPS into the Kalman filter once every 1 second; when the system is powered on, entering a restarting state, and when hdop > h _ mid or star _ num <10 in the restarting state, entering a GPS invalid state; entering a GPS-active state when hdop < ═ h _ mid and v _ car > v _ mid; entering a parking/slow-driving state when hdop < ═ h _ mid and v _ car < ═ v _ mid; when the GPS is in an invalid state, if hdop is less than h _ good, entering a GPS valid state; if the P is abnormal, entering a restarting state; when the GPS is in an effective state, if hdop is greater than h _ bad, entering an ineffective state of the GPS; if v _ car < v _ ba, entering a parking/slow driving state; if the P is abnormal, entering a restarting state; when in the parking/slow driving state, if v _ car is greater than v _ good, entering a GPS valid state; if hdop is greater than h _ bad, entering a GPS invalid state; if the P is abnormal, entering a restarting state; hdop is a horizontal accuracy factor, star _ num is a received star number, v _ car is a current vehicle speed read from the ODOM, P is a system covariance matrix of a kalman filter, h _ good, h _ mid and h _ bad are parameters for judging the positioning accuracy of the GPS, and v _ good, v _ bad and v _ mid are parameters for judging the speed of the vehicle. Therefore, the positioning precision can be effectively improved.
Wherein, h _ good ranges from 0.55 to 0.65, h _ mid ranges from 0.68 to 0.73, and h _ bad ranges from 0.75 to 0.77. v _ good ranges from 3 to 5m/s, v _ mid ranges from 1 to 2m/s, and v _ bad ranges from 0.3 to 0.5 m/s. Therefore, the robustness of the whole method is effectively guaranteed, the state switching rationality is guaranteed, and the positioning effect is better.
Further, the vehicle speed information acquisition unit adopts an ODOM arranged on the vehicle, and the ODOM acquires the advancing speed information and the rotating speed of the vehicle; the vehicle course information acquisition unit adopts an IMU (inertial measurement Unit) arranged at the position of a vehicle rotation center and mainly acquires and provides course information of the vehicle; the vehicle position information acquisition unit employs a GPS, which is provided on the roof of the vehicle and is located at the vehicle rotation center position, and mainly acquires and provides position information of the vehicle.
The invention also provides a combined positioning method based on the low-cost sensor combined positioning system for automatic driving, which comprises the following steps:
step 1: the vehicle speed information acquisition unit, the vehicle course information acquisition unit and the vehicle position information acquisition unit respectively start the acquired speed of the vehicle, the acquired course information of the vehicle and the acquired position information of the vehicle to the vehicle position positioning unit;
step 2: constructing a Kalman filter in a vehicle position positioning unit, and initializing or modifying Kalman filter parameters by a state machine in the vehicle position positioning unit according to the acquired vehicle position information;
and step 3: and the Kalman filter combines the prediction model and the updating model thereof to continuously iterate to obtain and output the final vehicle positioning information according to the received speed of the vehicle, the course information of the vehicle and the position information of the vehicle.
Further, the Kalman filter parameters comprise a system covariance matrix P, a measurement noise covariance matrix R, and a system state quantity matrix X (X, y, v, yaw),yawv,vscale,yawbias) And an observation matrixWherein x represents the abscissa of the vehicle input with the Kalman filter in the UTM coordinate system, y represents the ordinate of the vehicle input with the Kalman filter in the UTM coordinate system, v represents the speed of the vehicle input with the Kalman filter, yaw represents the heading angle of the vehicle input with the Kalman filter, and yaw represents the heading angle of the vehicle input with the Kalman filtervIndicating the course angular velocity, v, of the vehicle entering the Kalman filterscaleRepresenting a velocity scale factor, yaw, input to a Kalman filterbiasRepresenting course angle deviation of the input Kalman filter; z is a radical ofxAnd zyIndicating position information of GPS output, zyawIs the heading angle, z, of the IMU outputvSpeed information output for ODMO, zyawvIs the angular velocity of the heading angle output by the IMU. Velocity scale factor vscaleSolves the problem of inconsistency between the measured speed and the actual speed caused by the air pressure of the vehicle tires, the load of the vehicle, the uneven road surface and the like, and introduces course angle deviation yawbiasThe method solves the problem of unstable course angle caused by road magnetic field environment transformation, and the two state quantities can be dynamically adjusted according to observation data of a GPS and a prediction model of a Kalman filtering algorithm. The accuracy of vehicle location is more effectual improvement.
Further, the prediction model is:
xt=xt-1+vt*cos(yawt)*Δt;
yt=yt-1+vt*sin(yawt)*Δt;
in the above formula vtRepresenting vehicles at time tSpeed, rawtIndicating the heading angle, x, of the vehicle at time tt、ytRespectively representing the abscissa and ordinate, v, of the vehicle in the UTM coordinate system at time tt-1Representing the speed, yaw, of the vehicle at time t-1t-1Representing the heading angle, x, of the vehicle at time t-1t-1And yt-1Respectively, an abscissa and an ordinate of the vehicle in the UTM coordinate system at time t-1, at represents a time difference,representing the vehicle's speed scale factor at time t-1,indicating the heading angle deviation of the vehicle at time t-1,the angular velocity of the heading angle at time t-1.
Furthermore, the step 2 also comprises correcting the course information input into the Kalman filter by the vehicle course information acquisition unit by using the course information acquired by the GPS; according to the formula:
yawoffset=yawgps-yawimu;
yawinput=yawimu+yawoffest;
correcting course information input into a Kalman filter by a vehicle course information acquisition unit, wherein yawgpsWhen the GPS star number is more than or equal to 12, the vehicle speed is more than 5m/s, the horizontal precision factor is less than 0.6, the RMC message outputs the course angle, yawimuCourse angle, yaw, of imu outputoffsetIs an offset angle, yawinput is the yaw value input to the kalman filter. Therefore, the positioning precision of the system can be effectively improved.
Further, the method for initializing or modifying the kalman filter parameter in step 2 is as follows: setting a restarting state, a GPS invalid state, a GPS valid state and a parking/slow driving state, wherein in the restarting state, a system covariance matrix P, a measurement noise covariance matrix R and a system state quantity matrix X in Kalman filter parameters are initialized; when the GPS is in an invalid state, the position information acquired by the GPS is not input into the Kalman filter; when the GPS is in an effective state, the position information collected by the GPS is input into a Kalman filter; when the vehicle is in a parking/slow running state, inputting the position information of the GPS into the Kalman filter once every 1 second; when the system is powered on, entering a restarting state, and when hdop > h _ mid or star _ num <10 in the restarting state, entering a GPS invalid state; entering a GPS-active state when hdop < ═ h _ mid and v _ car > v _ mid; entering a parking/slow-driving state when hdop < ═ h _ mid and v _ car < ═ v _ mid; when the GPS is in an invalid state, if hdop is less than h _ good, entering a GPS valid state; if the P is abnormal, entering a restarting state; when the GPS is in an effective state, if hdop is greater than h _ bad, entering an ineffective state of the GPS; if v _ car < v _ ba, entering a parking/slow driving state; if the P is abnormal, entering a restarting state; when in the parking/slow driving state, if v _ car is greater than v _ good, entering a GPS valid state; if hdop is greater than h _ bad, entering a GPS invalid state; if the P is abnormal, entering a restarting state; hdop is a horizontal accuracy factor, star _ num is a received star number, v _ car is a current vehicle speed read from the ODOM, P is a system covariance matrix of a kalman filter, h _ good, h _ mid and h _ bad are parameters for judging the positioning accuracy of the GPS, and v _ good, v _ bad and v _ mid are parameters for judging the speed of the vehicle.
The working principle is as follows: the positioning scheme provided by the invention adopts a low-cost GPS and an IMU sensor, wherein course information (Yaw) output by the IMU, Position information (Position) provided by the GPS, speed (Velocity) and angular speed (Yaw _ rate) provided by the vehicle ODOM are input into an extended Kalman filter, a set of state machine is designed according to the positioning characteristic of the low-cost GPS and is used for dynamically adjusting Kalman filter parameters, and then the current Position, speed and course information of the vehicle are output through a prediction and state updating module of the Kalman filter. The ODOM information of the vehicle CAN be read from a CAN bus of the vehicle, and the speed and angular velocity values thereof are respectively obtained by summing, averaging and differencing pulse counters mounted on the left and right wheels. The invention also adds a speed scale factor and course angle deviation for correcting the speed and the course angle in real time; meanwhile, when the vehicle speed is high and the GPS positioning is stable, the course information of the IMU is corrected by the course information of the GPS.
Has the advantages that: compared with the prior art, the invention utilizes the low-cost GPS and IMU to reduce the hardware cost of the positioning suite, thereby facilitating the automatic driving marketization; meanwhile, the final positioning precision of the system can reach a sub-meter level through a state machine parameter adjusting mechanism.
Drawings
FIG. 1 is a schematic diagram of the system of the present invention;
FIG. 2 is a diagram illustrating the operation of a state machine;
fig. 3 is a comparison of the driving path of the vehicle according to the present invention and the prior art.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the examples of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
As shown in figure 1, the invention provides a low-cost sensor combination positioning system for automatic driving, which comprises a vehicle speed information acquisition unit, a vehicle course information acquisition unit, a vehicle position information acquisition unit and a vehicle position positioning unit, wherein the vehicle speed information acquisition unit, the vehicle course information acquisition unit and the vehicle position information acquisition unit send acquired information to the vehicle position positioning unit, and the vehicle position positioning unit introduces a speed scale factor vscaleYaw angle deviation yawbiasThe Kalman filter fuses the received information of the vehicle, so as to obtain the current positioning information of the vehicle, wherein the positioning information comprises the current speed, the course angle, and the abscissa and the ordinate under the UTM plane coordinate system.
The automatic driving vehicle comprises a vehicle speed information acquisition unit, a vehicle speed information acquisition unit and a vehicle speed information acquisition unit, wherein the vehicle speed information acquisition unit adopts an ODOM (open data object) arranged on the vehicle, the ODOM acquires the advancing speed information and the rotating speed of the vehicle, the advancing speed information and the rotating speed Can be read from an on-vehicle Can bus, the automatic driving vehicle contains the information, and the information is obtained through a left wheel pulse counter and a; the vehicle course information acquisition unit adopts an IMU (inertial measurement Unit) arranged at the position of a vehicle rotation center, mainly acquires and provides course information of a vehicle, and adopts an IMU of mti-3 type in the embodiment; the vehicle position information acquisition unit employs a GPS, which is provided on the roof of the vehicle and is located at the vehicle rotation center position, and which mainly acquires and provides position information of the vehicle, i.e., the longitude and latitude of the position where the vehicle is located, and in the present embodiment employs a GPS of model GN 200B.
The combined positioning method of the positioning system adopting the low-cost sensor for automatic driving disclosed by the embodiment specifically comprises the following steps:
step 1: the vehicle speed information acquisition unit, the vehicle course information acquisition unit and the vehicle position information acquisition unit respectively start the acquired speed of the vehicle, the acquired course information of the vehicle and the acquired position information of the vehicle to the vehicle position positioning unit;
step 2: constructing a Kalman filter in a vehicle position positioning unit, initializing or modifying Kalman filter parameters by the vehicle position positioning unit according to the acquired vehicle position information, wherein the Kalman filter parameters comprise a system covariance matrixMeasuring a noise covariance matrixThe system state quantity matrix X (X, y, v, yaw)v,vscale,yawbias) And an observation matrix Z (Z)x,zy,zyaw,zv) Wherein P isx_xRepresenting the variance value, p, of the abscissa of the vehicle in the UTM coordinate systemx_yRepresenting the covariance value, p, of the abscissa and ordinate of the vehicle in the UTM coordinate systemx_vRepresenting the covariance value, P, of the vehicle's abscissa and vehicle speed in the UTM coordinate systemx_yawIndicating that the vehicle is in UThe covariance value of the abscissa and the course angle in the TM coordinate system,represents the covariance value of the abscissa and the course angular velocity of the vehicle under the UTM coordinate system,represents the covariance value of the abscissa of the vehicle in the UTM coordinate system and the speed scale factor,covariance value, P, representing the deviation of the vehicle between the abscissa and the course angle in the UTM coordinate systemy_xRepresenting the covariance value, p, of the ordinate and abscissa of the vehicle in the UTM coordinate systemy_yRepresenting the variance value, p, of the longitudinal coordinate of the vehicle in the UTM coordinate systemy_vRepresenting the covariance value, p, of the vehicle's longitudinal coordinate in the UTM coordinate system and the vehicle's speedy_yawRepresents the covariance value of the longitudinal coordinate and the heading angle of the vehicle under the UTM coordinate system,represents the covariance value of the longitudinal coordinate and the course angular speed of the vehicle under the UTM coordinate system,represents the covariance value of the longitudinal coordinate of the vehicle in the UTM coordinate system and the speed scale factor,representing the covariance value, P, of the deviation between the longitudinal coordinate and the course angle of the vehicle in the UTM coordinate systemv_xRepresenting the covariance value, p, of the vehicle's speed with the abscissa in the UTM coordinate systemv_yRepresenting the covariance value, p, of the speed of the vehicle and the ordinate in the UTM coordinate systemv_vVariance value, p, representing vehicle speedv_yawRepresenting the covariance value of the vehicle speed and the heading angle,representing the covariance value of the vehicle speed and the heading angular velocity,representing the covariance value of the vehicle speed and the speed scale factor,covariance value, P, representing the deviation of vehicle speed from course angleyaw_xRepresenting the covariance value, p, of the course angle of the vehicle and the abscissa of the vehicle in the UTM coordinate systemyaw_yRepresenting the covariance value, p, of the course angle of the vehicle and the longitudinal coordinate of the vehicle in the UTM coordinate systemyaw_vRepresenting the covariance, p, of the course angle of the vehicle and the speed of the vehicleyaw_yawA variance value representing a heading angle of the vehicle,representing the covariance value, p, of the vehicle course angle and course angular velocityyaw_vsacleRepresenting the covariance value of the vehicle heading angle and the speed scale factor,a covariance value representing a deviation of a heading angle of the vehicle from the heading angle,represents the covariance value of the vehicle course angular velocity and the abscissa of the vehicle in the UTM coordinate system,represents the covariance value of the vehicle course angular speed and the vertical coordinate of the vehicle under the UTM coordinate system,representing the covariance value of the vehicle heading angular velocity and the vehicle velocity,representing the covariance value of the vehicle heading angular velocity and the vehicle heading angle,a variance value representing a heading angular velocity of the vehicle,representing the covariance value of the vehicle heading angular velocity and the velocity scaling factor,a covariance value representing a heading angular velocity of the vehicle and a heading angular deviation,representing the covariance value of the vehicle's speed scale factor and the vehicle's abscissa in the UTM coordinate system,represents the covariance value of the speed scale factor of the vehicle and the ordinate of the vehicle in the UTM coordinate system,representing the covariance value of the vehicle's speed scale factor and the vehicle speed,representing the covariance value of the vehicle's speed scale factor and the vehicle heading angle,representing the covariance value of the vehicle's speed scale factor and the vehicle's heading angular velocity,a variance value representing a speed scale factor of the vehicle,a covariance value representing a speed scale factor of the vehicle and a heading angle deviation of the vehicle,represents the covariance value of the deviation of the vehicle heading angle and the abscissa of the vehicle in the UTM coordinate system,representing the covariance value of the deviation of the vehicle course angle and the vertical coordinate of the vehicle in the UTM coordinate system,representing the covariance value of the angular deviation of the vehicle heading and the vehicle speed,representing the covariance value of the vehicle heading angle deviation and the vehicle heading angle,representing the covariance value of the vehicle heading angular deviation and the vehicle heading angular velocity,representing the covariance value of the vehicle heading angle deviation and the vehicle speed scale factor,a variance value representing a deviation of a vehicle heading angle; r isx_xRepresenting the variance of the GPS in lateral position, ry_yRepresenting the variance of the GPS position in the longitudinal direction, rv_vVariance, r, representing vehicle speedyaw_yawThe variance of the vehicle heading angle is represented,represents the variance of the vehicle heading angular velocity,a variance of a speed scale factor representing the vehicle,indicating vehicle navigationThe variance of the angular deviation, where the elements in the matrix R are variances set according to the characteristics of the sensor, which are related to the accuracy of the sensor,andis 0; x represents the abscissa of the vehicle input with the Kalman filter in the UTM coordinate system, y represents the ordinate of the vehicle input with the Kalman filter in the UTM coordinate system, v represents the velocity of the vehicle input with the Kalman filter, yaw represents the heading angle of the vehicle input with the Kalman filter, and yaw represents the heading angle of the vehicle input with the Kalman filtervIndicating the course angular velocity, v, of the vehicle entering the Kalman filterscaleRepresenting a velocity scale factor, yaw, input to a Kalman filterbiasRepresenting course angle deviation of the input Kalman filter; z is a radical ofxAnd zyRespectively representing the abscissa and ordinate, z, of the position information output by the GPS in the UTM coordinate systemyawIs the heading angle, z, of the IMU outputvFor the speed information output by the ODMO,is the angular velocity of the heading angle output by the IMU.
And step 3: and the Kalman filter combines the prediction model and the updating model thereof to continuously iterate to obtain and output the final vehicle positioning information according to the received speed of the vehicle, the course information of the vehicle and the position information of the vehicle. The method specifically comprises the following steps:
step 301: respectively predicting the value of each quantity in the system state quantity X at the next moment according to a CTRV prediction model in a Kalman filter; wherein, the CTRV prediction model is as follows:
xt=xt-1+vt*cos(yawt)*Δt;
yt=yt-1+vt*sin(yawt)*Δt;
in the above formula vtRepresenting the speed, yaw, of the vehicle at time ttIndicating the heading angle, x, of the vehicle at time tt、ytRespectively representing the abscissa and ordinate, v, of the vehicle in the UTM coordinate system at time tt-1Representing the speed, yaw, of the vehicle at time t-1t-1Representing the heading angle, x, of the vehicle at time t-1t-1And yt-1Respectively, an abscissa and an ordinate of the vehicle in the UTM coordinate system at time t-1, at represents a time difference,representing the vehicle's speed scale factor at time t-1,indicating the heading angle deviation of the vehicle at time t-1,the angular velocity of the heading angle at time t-1.
Step 302: the longitude and latitude data of the vehicle collected by the vehicle position information collecting unit are converted into UTM plane coordinates, and an updating model in the Kalman filter updates a system state quantity matrix X (X, y, v, yaw) according to position information, course and speed values output by a GPS (global positioning system), an IMU (inertial measurement unit) and an ODOM (odd object model)v,vscale,yawbias);
Wherein v isscaleThe regulation updating method comprises the following steps: position (z) from a prediction model and from GPS outputx,zy) And the speed z of the vehiclevV can be calculatedscaleIn the calculation of the prediction model, the speed of the system vehicle is calculated asWherein it contains the last oneVelocity v of the momentt-1And a scale factor vscaleWhile simultaneously varying the velocity vtInto formula xt=xt-1+vt*cos(yawt) Δ t and yt=yt-1+vt*sin(yawt) Δ t; in the updating model, the sensor inputs the position information (X, y) and the speed v of the GPS, and the v in the system variable X is updated according to the predicted value, the output value of the sensor and the noise covariance matrix R and the covariance matrix P thereofscaleFor correcting the vehicle speed.
yawbiasThe regulation updating method comprises the following steps: position (z) from a prediction model and from GPS outputx,zy) And IMU output heading angles yaw and yawvTo calculate yawbiasThe value of (c). The calculation formula of the course angle of the system vehicle in the calculation of the prediction model is as follows:which contains rawbias(ii) a Angle of course yawtInto formula xt=xt-1+vt*cos(yawt) Δ t and yt=yt-1+vt*sin(yawt) Δ t; in the updating module part, the sensor inputs the position information (X, y) of the GPS and the course angle and the angular speed output by the imu, and then the yaw in the system variable X is updated according to the predicted value, the output value of the sensor and the noise covariance matrix R and the covariance matrix P thereofbiasFor correcting the course angle of the vehicle.
Step 303: and continuously repeating the step 301 to the step 302, outputting the positioning information of the vehicle every 0.02 second, wherein the output positioning information is the current system state quantity matrix X.
When the GPS positioning accuracy is high and the vehicle speed is high, the course information of the IMU is corrected by the course information of the GPS positioning accuracy, and the course information input into the Kalman filter is corrected by the IMU. The formula is as follows:
yawoffset=yawgps-yawimu;
yawinput=yawimu+yawoffest;
in the above formulagpsWhen the GPS star number is more than or equal to 12, the vehicle speed is more than 5m/s, the horizontal precision factor is less than 0.6, the RMC message outputs the course angle, yawimuCourse angle, yaw, of imu outputoffsetIs an offset angle, yawinputThe value of yaw input to the kalman filter is the value of Z _ yaw.
The method for initializing or modifying the Kalman filter parameters in the step 2 comprises the following steps: and a set of state machine parameter adjusting method is set according to the different positioning accuracy of the GPS under the dynamic and static conditions, the parameters of the GPS such as the star number star _ num, the horizontal accuracy factor hdop and the like and the signal characteristics, and the observed value Z and the measurement noise covariance R of the system are dynamically set. The method mainly comprises four states of restarting, GPS ineffectiveness, GPS effectiveness and stopping/slow running, and the measurement noise covariance R in Kalman filtering is set according to the four states, and algorithm parameters are dynamically adjusted. The states are as follows:
and (4) restarting the state: initializing a system covariance matrix P, a measurement noise covariance matrix R and a system state quantity matrix X in Kalman filter parameters. During initialization, setting X and y in a system state quantity matrix X as longitude and latitude of a GPS (global positioning system) to be converted into horizontal and vertical coordinates under a UTM (universal time management) system, and setting other values as 0; the element value on the diagonal of the system covariance matrix P is set to 0.01, and the element value on the diagonal of the measurement noise covariance matrix R is set to 0.1.
GPS invalid state: when the vehicle passes through the signal shielding places such as bridge bottoms, tunnels and the like for a long time, the horizontal precision factor output by the GPS is higher, the number of received stars is less, the GPS is invalid, the position information acquired by the GPS is not input into a Kalman filter, and the value in the system variable X and the speed and course value output by the sensor are input into a prediction model for calculation.
GPS valid state: when the vehicle is in open road conditions or the urban road is running normally, the GPS positioning data can be used. However, when the user passes through a dense high-rise building or a place with a number of shelters, the positioning data of the GPS is unstable, and a shaking situation occurs. And at the moment, the GPS observation variance is dynamically adjusted according to the horizontal precision factor hdop, and the smaller hdop is, the smaller the measurement noise variance is. I.e. Hdop<At 0.55, rx_xAnd ry_ySet to 1.0; 0.55<Hdop<At 0.65, rx_xAnd ry_ySet to 1.5; 0.65<Hdop<At 0.68, rx_xAnd ry_ySet to 2.5; 0.68<Hdop<At 0.73, rx_xAnd ry_yIs set to 15; hdop>At 0.75, rx_xAnd ry_ySet to 150.
Parking/slow-driving state: when the slave vehicle speed is small, the positioning accuracy of the GPS is low. Inputting the position information of the GPS into the Kalman filter once every 1 second, simultaneously setting the measurement noise variance of the GPS, and converting the measurement noise covariance r of the GPSx_xAnd ry_yAre all set to 2.0. Wherein r isx_xAnd ry_yRepresenting the variance of the GPS in lateral and longitudinal position, respectively.
As shown in fig. 2, after the system is powered on, entering a restart state, and in the restart state, entering a GPS invalid state when hdop > h _ mid or star _ num < 10; entering a GPS-active state when hdop < ═ h _ mid and v _ car > v _ mid; when hdop < ═ h _ mid and v _ car < ═ v _ mid, the parking/slow-traveling state is entered.
When the GPS is in an invalid state, if hdop is less than h _ good, the positioning accuracy of the GPS is improved, and then the GPS enters into an effective state; if the P is abnormal, entering a restarting state;
when the GPS is in an effective state, if hdop is greater than h _ bad, the vehicle enters a tunnel or under a viaduct, and the GPS information is not available at the moment, so that the vehicle enters an ineffective GPS state; if v _ car < v _ ba, the vehicle speed is reduced, and then the vehicle enters a parking/slow running state; if the P is abnormal, entering a restarting state;
when the vehicle is in a parking/slow driving state, if v _ car > v _ good, which indicates that the vehicle speed is improved, the GPS positioning precision is increased, and the vehicle enters a GPS effective state; if hdop > h _ bad, it indicates that the GPS antenna is blocked, and then enters into the GPS invalid state. And if the P is abnormal, entering a restarting state.
Wherein hdop is a horizontal accuracy factor, star _ num is a received star number, v _ car is a current vehicle speed read from ODOM, P is a system covariance of a Kalman filter, h _ good, h _ mid and h _ bad are parameters for judging GPS positioning accuracy in the algorithm, the preferred range of h _ good is 0.55-0.65, the preferred range of h _ mid is 0.68-0.73, and the preferred range of h _ bad is 0.75-0.77; v _ good, v _ bad and v _ mid are used as the vehicle speed parameters judged in the algorithm, the preferable range of v _ good is 3-5m/s, the preferable range of v _ mid is 1-2m/s, and the preferable range of v _ bad is 0.3-0.5 m/s.
When the covariance P value of the system is not positive or the covariance values of the transverse direction and the longitudinal direction and other parameters have obvious asymmetric difference in the initialization or operation process of the system, the abnormal P is represented. For example, if the difference between the elements P _ x _ v and P _ y _ v in the system covariance matrix P is greater than 1000, it is considered that there is a significant asymmetric difference between the covariance values of the horizontal and vertical directions and other parameters, and the case is P anomaly.
As shown in fig. 3, a comparison graph of the positioning tracks when crossing an overpass on an urban road is shown, wherein the black track is the positioning result achieved by the present invention, and the gray track is the positioning result of DJI-A3. It is clear that the black track is relatively smooth and the grey track appears as a wobble at the bottom of the bridge.
Compared with DJI-A3 and TrimbleDB982 positioning effects in the prior art and related contents such as the cost of the whole system, the positioning method provided by the invention has the advantages that as shown in the table I, the positioning method provided by the invention reduces the hardware cost of the whole system, ensures the positioning accuracy of the system and can accurately position vehicle information no matter in a shielded or unshielded place.
Table 1:
Claims (5)
1. a low cost sensor cluster positioning system for autonomous driving, characterized by: the system comprises a vehicle speed information acquisition unit, a vehicle course information acquisition unit, a vehicle position information acquisition unit and a vehicle position positioning unit, wherein the vehicle speed information acquisition unit is used for acquiring real-time vehicle speed information of a vehicle; the vehicle course information acquisition unit is used for acquiring the course information of the vehicle in real time; the vehicle position information acquisition unit is a GPS and is used for acquiring vehicle position information in real time; the vehicle position positioning unit comprises a state machine and a Kalman filter, the state machine adjusts parameters of the Kalman filter according to the parameters fed back by the GPS, and the Kalman filter obtains final vehicle positioning information and outputs the final vehicle positioning information to position the vehicle by combining continuous iteration of a prediction model and an update model of the Kalman filter according to the vehicle information output by the vehicle speed information acquisition unit, the vehicle course information acquisition unit and the vehicle position information acquisition unit; the prediction model is as follows:
xt=xt-1+vt*cos(yawt)*Δt;
yt=yt-1+vt*sin(yawt)*Δt;
in the above formula vtRepresenting the speed, yaw, of the vehicle at time ttIndicating the heading angle, x, of the vehicle at time tt、ytRespectively representing the abscissa and ordinate, v, of the vehicle in the UTM coordinate system at time tt-1Representing the speed, yaw, of the vehicle at time t-1t-1Representing the heading angle, x, of the vehicle at time t-1t-1And yt-1Respectively, an abscissa and an ordinate of the vehicle in the UTM coordinate system at time t-1, at represents a time difference,representing the vehicle's speed scale factor at time t-1,indicating the heading angle deviation of the vehicle at time t-1,the angular velocity is the course angle at the time of t-1;
the state machine comprises a restarting state, a GPS invalid state, a GPS valid state and a parking/slow driving state, wherein in the restarting state, a system covariance matrix P, a measurement noise covariance matrix R and a system state quantity matrix X in Kalman filter parameters are initialized; when the GPS is in an invalid state, the position information acquired by the GPS is not input into the Kalman filter; when the GPS is in an effective state, the position information collected by the GPS is input into a Kalman filter; when the vehicle is in a parking/slow running state, inputting the position information of the GPS into the Kalman filter once every 1 second; when the system is powered on, entering a restarting state, and when hdop > h _ mid or star _ num <10 in the restarting state, entering a GPS invalid state; entering a GPS-active state when hdop < ═ h _ mid and v _ car > v _ mid; entering a parking/slow-driving state when hdop < ═ h _ mid and v _ car < ═ v _ mid; when the GPS is in an invalid state, if hdop is less than h _ good, entering a GPS valid state; if the P is abnormal, entering a restarting state; when the GPS is in an effective state, if hdop is greater than h _ bad, entering an ineffective state of the GPS; if v _ car < v _ bad, entering a parking/slow driving state; if the P is abnormal, entering a restarting state; when in the parking/slow driving state, if v _ car is greater than v _ good, entering a GPS valid state; if hdop is greater than h _ bad, entering a GPS invalid state; if the P is abnormal, entering a restarting state; hdop is a horizontal precision factor, star _ num is a received star number, v _ car is a current vehicle speed read from the ODOM, P is a system covariance matrix of a Kalman filter, h _ good, h _ mid and h _ bad are parameters for judging GPS positioning precision, and v _ good, v _ bad and v _ mid are parameters for judging vehicle speed; the vehicle speed information acquisition unit adopts an ODOM arranged on the vehicle, and the ODOM acquires the advancing speed information and the rotating speed of the vehicle; the vehicle course information acquisition unit adopts an IMU (inertial measurement Unit) arranged at the position of a vehicle rotation center and mainly acquires and provides course information of the vehicle; the vehicle position information acquisition unit employs a GPS, which is provided on the roof of the vehicle and is located at the vehicle rotation center position, and mainly acquires and provides position information of the vehicle.
2. A low cost sensor cluster positioning system for autonomous driving as claimed in claim 1, characterized by: h _ good ranges from 0.55 to 0.65, h _ mid ranges from 0.68 to 0.73, and h _ bad ranges from 0.75 to 0.77.
3. A low cost sensor cluster positioning system for autonomous driving as claimed in claim 1, characterized by: v _ good ranges from 3 to 5m/s, v _ mid ranges from 1 to 2m/s, and v _ bad ranges from 0.3 to 0.5 m/s.
4. A positioning method based on the low-cost sensor combination positioning system for automatic driving of claim 1, characterized in that: the method comprises the following steps:
step 1: the vehicle speed information acquisition unit, the vehicle course information acquisition unit and the vehicle position information acquisition unit respectively send the acquired speed of the vehicle, the acquired course information of the vehicle and the acquired position information of the vehicle to the vehicle position positioning unit;
step 2: constructing a Kalman filter in a vehicle position positioning unit, and initializing or modifying Kalman filter parameters by a state machine in the vehicle position positioning unit according to the acquired vehicle position information;
and step 3: the Kalman filter obtains and outputs final vehicle positioning information by combining the continuous iteration of a prediction model and an update model in the Kalman filter according to the received speed of the vehicle, the course information of the vehicle and the position information of the vehicle;
the method for initializing or modifying the kalman filter parameter in the step 2 comprises the following steps: setting a restarting state, a GPS invalid state, a GPS valid state and a parking/slow driving state, wherein in the restarting state, a system covariance matrix P, a measurement noise covariance matrix R and a system state quantity matrix X in Kalman filter parameters are initialized; when the GPS is in an invalid state, the position information acquired by the GPS is not input into the Kalman filter; when the GPS is in an effective state, the position information collected by the GPS is input into a Kalman filter; when the vehicle is in a parking/slow running state, inputting the position information of the GPS into the Kalman filter once every 1 second; when the system is powered on, entering a restarting state, and when hdop > h _ mid or star _ num <10 in the restarting state, entering a GPS invalid state; entering a GPS-active state when hdop < ═ h _ mid and v _ car > v _ mid; entering a parking/slow-driving state when hdop < ═ h _ mid and v _ car < ═ v _ mid; when the GPS is in an invalid state, if hdop is less than h _ good, entering a GPS valid state; if the P is abnormal, entering a restarting state; when the GPS is in an effective state, if hdop is greater than h _ bad, entering an ineffective state of the GPS; if v _ car < v _ bad, entering a parking/slow driving state; if the P is abnormal, entering a restarting state; when in the parking/slow driving state, if v _ car is greater than v _ good, entering a GPS valid state; if hdop is greater than h _ bad, entering a GPS invalid state; if the P is abnormal, entering a restarting state; hdop is a horizontal precision factor, star _ num is a received star number, v _ car is a current vehicle speed read from the ODOM, P is a system covariance matrix of a Kalman filter, h _ good, h _ mid and h _ bad are parameters for judging GPS positioning precision, and v _ good, v _ bad and v _ mid are parameters for judging vehicle speed;
the prediction model is as follows:
xt=xt-1+vt*cos(yawt)*Δt;
yt=yt-1+vt*sin(yawt)*Δt;
in the above formula vtRepresenting the speed, yaw, of the vehicle at time ttIndicating the heading angle, x, of the vehicle at time tt、ytRespectively representing the abscissa and ordinate, v, of the vehicle in the UTM coordinate system at time tt-1Representing the speed, yaw, of the vehicle at time t-1t-1Representing the heading angle, x, of the vehicle at time t-1t-1And yt-1Respectively, an abscissa and an ordinate of the vehicle in the UTM coordinate system at time t-1, at represents a time difference,representing the vehicle's speed scale factor at time t-1,indicating the heading angle deviation of the vehicle at time t-1,the angular velocity of the heading angle at time t-1.
5. The positioning method according to claim 4, characterized in that: the step 2 also comprises correcting the course information input into the Kalman filter by the vehicle course information acquisition unit by using the course information acquired by the GPS; according to the formula:
yawoffset=yawgps-yawimu;
yawinput=yawimu+yawoffest;
correcting course information input into a Kalman filter by a vehicle course information acquisition unit, wherein yawgpsWhen the GPS star number is more than or equal to 12, the vehicle speed is more than 5m/s, the horizontal precision factor is less than 0.6, the RMC message outputs the course angle, yawimuCourse angle, yaw, output for IMUoffsetIs an offset angle, yawinputIs the raw value input to the kalman filter.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911168614.XA CN110926483B (en) | 2019-11-25 | 2019-11-25 | Low-cost sensor combination positioning system and method for automatic driving |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201911168614.XA CN110926483B (en) | 2019-11-25 | 2019-11-25 | Low-cost sensor combination positioning system and method for automatic driving |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110926483A CN110926483A (en) | 2020-03-27 |
CN110926483B true CN110926483B (en) | 2020-12-25 |
Family
ID=69851986
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201911168614.XA Active CN110926483B (en) | 2019-11-25 | 2019-11-25 | Low-cost sensor combination positioning system and method for automatic driving |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110926483B (en) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112505718B (en) * | 2020-11-10 | 2022-03-01 | 奥特酷智能科技(南京)有限公司 | Positioning method, system and computer readable medium for autonomous vehicle |
CN115932924B (en) * | 2022-07-29 | 2023-09-05 | 润芯微科技(江苏)有限公司 | IMU-based auxiliary positioning method and system |
Family Cites Families (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR2898196B1 (en) * | 2006-03-01 | 2008-04-25 | Eurocopter France | HYBRID POSITIONING METHOD AND DEVICE |
US8473207B2 (en) * | 2008-10-21 | 2013-06-25 | Texas Instruments Incorporated | Tightly-coupled GNSS/IMU integration filter having calibration features |
CN102608641A (en) * | 2012-03-30 | 2012-07-25 | 江苏物联网研究发展中心 | Vehicle-mounted combined navigation system based on single-axis gyroscope and single-axis accelerometer and method |
CN103472459A (en) * | 2013-08-29 | 2013-12-25 | 镇江青思网络科技有限公司 | GPS (Global Positioning System)-pseudo-range-differential-based cooperative positioning method for vehicles |
CN105606094B (en) * | 2016-02-19 | 2018-08-21 | 北京航天控制仪器研究所 | A kind of information condition matched filtering method of estimation based on MEMS/GPS combined systems |
CN107436444A (en) * | 2017-06-23 | 2017-12-05 | 北京机械设备研究所 | A kind of vehicle multi-mode formula integrated navigation system and method |
CN108983270A (en) * | 2018-06-14 | 2018-12-11 | 兰州晨阳启创信息科技有限公司 | A kind of train security positioning system and method based on Multi-sensor Fusion |
CN108827292A (en) * | 2018-06-27 | 2018-11-16 | 四川大学 | The accurate method for locating speed measurement of combined navigation and system based on GNSS and ground base station |
CN109343095B (en) * | 2018-11-15 | 2020-09-01 | 众泰新能源汽车有限公司 | Vehicle-mounted navigation vehicle combined positioning device and combined positioning method thereof |
-
2019
- 2019-11-25 CN CN201911168614.XA patent/CN110926483B/en active Active
Also Published As
Publication number | Publication date |
---|---|
CN110926483A (en) | 2020-03-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109946732B (en) | Unmanned vehicle positioning method based on multi-sensor data fusion | |
CN110631593B (en) | Multi-sensor fusion positioning method for automatic driving scene | |
CN110940344B (en) | Low-cost sensor combination positioning method for automatic driving | |
Ahmed et al. | Accurate attitude estimation of a moving land vehicle using low-cost MEMS IMU sensors | |
CN110307836B (en) | Accurate positioning method for welt cleaning of unmanned cleaning vehicle | |
CN110160542B (en) | Method and device for positioning lane line, storage medium and electronic device | |
EP3109589B1 (en) | A unit and method for improving positioning accuracy | |
Georgy et al. | Modeling the stochastic drift of a MEMS-based gyroscope in gyro/odometer/GPS integrated navigation | |
CN104061899B (en) | A kind of vehicle side inclination angle based on Kalman filtering and angle of pitch method of estimation | |
CN113945206A (en) | Positioning method and device based on multi-sensor fusion | |
Fakharian et al. | Adaptive Kalman filtering based navigation: An IMU/GPS integration approach | |
CN109186597B (en) | Positioning method of indoor wheeled robot based on double MEMS-IMU | |
CN108931244B (en) | Inertial navigation error suppression method and system based on train motion constraint | |
CN112505737B (en) | GNSS/INS integrated navigation method | |
CN109343095B (en) | Vehicle-mounted navigation vehicle combined positioning device and combined positioning method thereof | |
CN107132563B (en) | Combined navigation method combining odometer and dual-antenna differential GNSS | |
CN110530361B (en) | Steering angle estimator based on agricultural machinery double-antenna GNSS automatic navigation system | |
Chen et al. | A novel fusion methodology to bridge GPS outages for land vehicle positioning | |
CN110926483B (en) | Low-cost sensor combination positioning system and method for automatic driving | |
US20230182790A1 (en) | Method for calculating an instantaneous velocity vector of a rail vehicle and corresponding system | |
Gao et al. | An integrated land vehicle navigation system based on context awareness | |
CN112433531A (en) | Trajectory tracking method and device for automatic driving vehicle and computer equipment | |
CN111678514A (en) | Vehicle-mounted autonomous navigation method based on carrier motion condition constraint and single-axis rotation modulation | |
CN106646569B (en) | Navigation positioning method and equipment | |
CN115451949A (en) | Vehicle positioning method based on wheel installation inertia measurement unit |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CP02 | Change in the address of a patent holder | ||
CP02 | Change in the address of a patent holder |
Address after: 210012 room 401-404, building 5, chuqiaocheng, No. 57, Andemen street, Yuhuatai District, Nanjing, Jiangsu Province Patentee after: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd. Address before: 211800 building 12-289, 29 buyue Road, Qiaolin street, Jiangbei new district, Pukou District, Nanjing City, Jiangsu Province Patentee before: AUTOCORE INTELLIGENT TECHNOLOGY (NANJING) Co.,Ltd. |