CN107490803A - Using GPS and inertial navigation system to robot localization orientation method - Google Patents
Using GPS and inertial navigation system to robot localization orientation method Download PDFInfo
- Publication number
- CN107490803A CN107490803A CN201710447395.3A CN201710447395A CN107490803A CN 107490803 A CN107490803 A CN 107490803A CN 201710447395 A CN201710447395 A CN 201710447395A CN 107490803 A CN107490803 A CN 107490803A
- Authority
- CN
- China
- Prior art keywords
- robot
- gps
- information
- navigation system
- inertial navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
-
- G—PHYSICS
- G01—MEASURING; TESTING
- 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
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)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention discloses one kind using GPS and inertial navigation system to robot localization orientation method, including：Obtain the directed information and location information of inertial navigation system；Whether the signal quality for judging GPS every preset time meets preset value；If GPS signal quality meets preset value, GPS latitude and longitude information and directional angle information are obtained；Positioning and directing is carried out to robot with reference to the mode that the GPS got latitude and longitude information and directional angle information are modified to the directed information of inertial navigation system and location information that get.The technical program overcomes the error problem of electromagnetic interference, GPS existing for the orientation of electronic compass in the prior art, and precision is not as good as inertial navigation system in short-term, and the problems such as inertial navigation system long term drift.And when GPS fails in short-term, inertial navigation system can be relied on to be used as and positioned in short-term.
Description
Technical field
The present invention relates to technical field of navigation and positioning, more particularly to one kind to utilize GPS and inertial navigation system to robot localization
Orientation method.
Background technology
Inertial navigation system is the navigational parameter resolving system using gyro and accelerometer as Sensitive Apparatus, the system according to
Navigational coordinate system is established in the output of gyro, and speed and position of the carrier in navigational coordinate system are calculated according to accelerometer output
Put.
In the prior art, electronic compass and GPS (Global Positioning System, global positioning system) are utilized
Micro- inertial navigation system is aided in improve the method for navigation attitude accuracy.
Concrete technical scheme is as follows：
First, combined system is initially aligned using micro- inertial navigation, electronic compass, obtains carrier coordinate system b to navigation
Coordinate system n initial attitude matrix；And then the initial attitude value of carrier can be calculated.
Then, using the error equation of the position of micro- inertial navigation system, speed, posture and inertial sensor, expansion card is established
The state equation of Thalmann filter；The observational equation established respectively using electronic compass and GPS forms extended Kalman filter
Observational equation；The micro- inertial navigation system attitude error of real-time estimation is carried out using extended Kalman filter；Utilize obtained posture
Error is modified attitude matrix, and calculates the new attitude value of micro- inertial navigation system.
But above-mentioned technical proposal has following technological deficiency：
1) accuracy of electronic compass is not considered and under electromagnetic interference environment situations such as electronic compass failure, and electronic compass
Failure be difficult to be detected with technological means, therefore can not by electronic compass carry out angle initially be aligned.
2) do not consider that inertial navigation system starts zero bias amendment problem.When inertial navigation system zero bias are zero-speed input, inertial navigation output
Angular speed deviant, zero bias can influence the angle precision after angular speed integration.
3) the installation accuracy problem that equipment alignment error is brought is not considered.
4) MEMS (Micro-Electro-Mechanical System, MEMS) gyroscope operation principle is：
MEMS gyroscope utilizes Coriolis force --- the rotating object tangential force suffered when there is radial motion.MEMS gyroscope leads to
Often there is the removable capacitor board of both direction, the capacitor board of radial direction adds concussion voltage to force object to make radial motion, horizontal electricity
Hold the capacitance variations that plate measurement is brought due to horizontal Coriolis.Because Coriolis force is proportional to angular speed, by
The change of electric capacity can calculate angular velocity.If there is error in installation, then the Coriolis force of capacitive sensing is only to revolve
The component of Coriolis force caused by turning.So gyroscope measurement angular speed can have error.
Above-mentioned prior art may be referred to Application No.：CN201410121059.6, it is entitled " micro- inertial navigation with
The Chinese patent application file of DGPS and electronic compass integrated navigation attitude measurement method ".
The content of the invention
The present invention is aiming above mentioned problem, there is provided one kind is using GPS and inertial navigation system to robot localization orientation side
Method, comprise the following steps：
Obtain the directed information and location information of inertial navigation system；
Whether the signal quality for judging GPS every preset time meets preset value；
If GPS signal quality meets preset value, GPS latitude and longitude information and directional angle information are obtained；
With reference to the GPS got latitude and longitude information and directional angle information to the directed information of the inertial navigation system got
Positioning and directing is carried out to robot with the mode that location information is modified.
Optionally, if GPS signal quality is unsatisfactory for preset value, abandon this acquisition GPS latitude and longitude information and determine
To angle information, positioning and directing is carried out to robot using the directed information and location information of the inertial navigation system got.
Optionally, the directed information for obtaining inertial navigation system and location information comprise the following steps：
Initialize the zero bias value of gyroscope；Obtain the three axis angular rate information and 3-axis acceleration information of gyroscope；According to
3-axis acceleration information calculates carrier inclination angle；Three axis angular rate information of gyroscope are integrated to obtain Eulerian angles matrix letter
Breath, and calculate course angle spin matrix；Machine under inertial navigation system is calculated based on Eulerian angles matrix information and course angle spin matrix
The course angle of people；The feedback rotating speed of robotically-driven motor encoder is obtained, and is converted into the wheel speed of robot；Based on machine
The mobile linear velocity of the wheel speed calculation robot of device people；Determined to be based on machine under inertial navigation system according to moveable robot movement equation
The position coordinates of device people.
Optionally, the latitude and longitude information and directional angle information for obtaining GPS comprise the following steps：Read and parse nmea associations
View, to obtain GPS latitude and longitude informations and GPS directional angle information；Formula is just being calculated using gauss projection to turn GPS latitude and longitude informations
It is changed to the coordinate value of plane right-angle coordinate；
Optionally, default error threshold is exceeded according to the directed information of inertial navigation system and the accumulated error of location information in identification
In the case of value, directed information and location information based on the inertial navigation system got and the longitude and latitude for combining the GPS got
The mode that information and directional angle information are modified to it carries out positioning and directing to robot and comprised the following steps：
Respectively to the seat based on the position coordinates of robot, robot course angle, plane right-angle coordinate under inertial navigation system
Scale value and GPS directional angle information make Kalman filtering, to obtain the present coordinate values of robot and current course angle；Utilize
The present coordinate values of robot are converted into the latitude and longitude information of current location by gauss projection inversion formula.
Compared with prior art, technical solution of the present invention at least has the advantages that：
According to embodiments of the present invention, the directed information (i.e. the course angle of robot) and positioning for obtaining inertial navigation system respectively are believed
Cease (position coordinates of robot under inertial navigation system), whether the signal quality for judging GPS every preset time meets preset value, if
Meet condition, then obtain GPS latitude and longitude information (i.e. location information) and directional angle information, and believe using GPS longitude and latitude
Breath (i.e. location information) and directional angle information are modified to the directed information and location information of inertial navigation system, are overcome existing
Precision is not as good as inertial navigation system in short-term by the error problem of electromagnetic interference, GPS existing for electronic compass orientation in technology, and inertial navigation system
The problems such as long term drift.And when GPS fails in short-term, inertial navigation system can be relied on to be used as and positioned in short-term.
The zero bias value of gyroscope is initialized in start, avoids in motion process gyroscope from making zero bias and initialize to cause to miss
Difference, and the angle precision after the three axis angular rate information scores to gyroscope can be improved.Accelerated using three axles of gyroscope
Degree information can correct the alignment error of gyroscope.
Further, inertial navigation system is determined using GPS latitude and longitude information (i.e. location information) and directional angle information
During being modified to information and location information, using fixed to the coordinate information under inertial navigation system, robot course angle, GPS
Position information and directional angle information make Kalman filtering respectively, so as to the robot location stablized and angle information.
Brief description of the drawings
Fig. 1 is provided by the invention a kind of to utilize the specific implementation of GPS and inertial navigation system to robot localization orientation method
The schematic flow sheet of mode.
Embodiment
It is understandable to enable the above objects, features and advantages of the present invention to become apparent, below in conjunction with the accompanying drawings to the present invention
Embodiment be described in detail.
As shown in Fig. 1 it is provided by the invention it is a kind of using GPS and inertial navigation system to robot localization orientation method
The schematic flow sheet of embodiment.
Step S1：Obtain the directed information and location information of inertial navigation system；
Step S2：Whether the signal quality for judging GPS every preset time meets preset value；
Step S3：If GPS signal quality meets preset value, GPS latitude and longitude information and directional angle information are obtained；
Step S4：With reference to the GPS got latitude and longitude information and directional angle information to the inertial navigation system that gets
The mode that directed information and location information are modified carries out positioning and directing to robot.
Compared to prior art, the error that the embodiment of the present invention overcomes electromagnetic interference existing for electronic compass orientation is asked
Precision is not as good as inertial navigation system in short-term by topic, GPS, and the problems such as inertial navigation system long term drift.Meet in the signal quality for monitoring GPS
It is required that when (reaching preset value), GPS latitude and longitude information and directional angle information are obtained, and arrive the orientation according to inertial navigation system
The accumulated error of information and location information exceedes default error threshold (specifically can set the mistake according to different application scenarios
Poor threshold value) in the case of, using GPS latitude and longitude information (i.e. location information) and directional angle information to the orientation of inertial navigation system
Information and location information are modified.
Further, and if current GPS signal quality is unsatisfactory for preset value (or abnormal (or the signal(l)ing condition of signal
It is bad), then this latitude and longitude information and directional angle information for obtaining GPS is abandoned, is determined even if recognizing according to inertial navigation system
Exceed default error threshold to the accumulated error of information and location information, still using the inertial navigation system got directed information and
Location information carries out positioning and directing to robot.
In the present embodiment, the step S1 obtains the directed information of inertial navigation system and location information comprises the following steps：
Step S11：Initialize the zero bias value of gyroscope；
Step S12：Obtain the three axis angular rate information and 3-axis acceleration information of gyroscope；
Step S13：Carrier inclination angle is calculated according to 3-axis acceleration information；
Step S14：Three axis angular rate information of gyroscope are integrated to obtain Eulerian angles matrix information, and calculate boat
To angle spin matrix；
Step S15：The course of robot under inertial navigation system is calculated based on Eulerian angles matrix information and course angle spin matrix
Angle；
Step S16：The feedback rotating speed of robotically-driven motor encoder is obtained, and is converted into the wheel speed of robot；
Step S17：The mobile linear velocity of wheel speed calculation robot based on robot；
Step S18：Position coordinates based on robot under inertial navigation system is determined according to moveable robot movement equation.
In actual applications, it can obtain the directed information of inertial navigation system using MPU6050 inertial navigations module hardware and determine
Position information.
As described in step S2, whether the signal quality for judging GPS every preset time meets preset value.Wherein, it is described pre-
If time (such as 100 milliseconds) and preset value can be set according to practical application, do not limit herein.
Wherein, judge whether GPS signal quality meets that the mode of preset value is：According to GPS signal quality whether etc.
Determined in RTK fixed solutions, if GPS signal quality is equal to RTK fixed solutions, judge that GPS signal quality meets preset value；
Conversely, if GPS signal quality is not equal to RTK fixed solutions, judge that GPS signal quality is unsatisfactory for preset value.Wherein, RTK
Fixed solution is state of the art, for example, receive gps signal instrument can be the Big Dipper lead to company BDM680.RTK instrument
Device generally has single-point solution, float-solution and a fixed solution, and single-point solution is usually meter level (general more than one meter) error, and float-solution is tens lis
Rice (with time increase and decrease precision meeting more and more higher, precision is become better and better), fixed solution is typically all 1~2 centimetre, and these three solutions are to make
The parameter that will be shown when with the instrument.
In the step S3, the latitude and longitude information and directional angle information that obtain GPS comprise the following steps：
Step S31：Read and parse nmea agreements, to obtain GPS latitude and longitude informations and GPS directional angle information；
Step S32：The coordinate that formula is converted to GPS latitude and longitude informations plane right-angle coordinate is just being calculated using gauss projection
Value.
The step S4, (meet preset value in this GPS signal quality, and obtain GPS latitude and longitude information and determine
In the case of angle information), with reference to the GPS got latitude and longitude information and directional angle information to the inertial navigation system that gets
The mode that the directed information and location information of system are modified carries out positioning and directing to robot and comprised the following steps：
Step S41：Respectively to being sat based on the position coordinates of robot, robot course angle, flat square under inertial navigation system
The coordinate value and GPS directional angle information for marking system make Kalman filtering, to obtain the present coordinate values of robot and current boat
To angle；
Step S42：The present coordinate values of robot are converted into the longitude and latitude of current location using gauss projection inversion formula
Spend information.
In actual applications, STM32F407 mainboards can be used by the location information of inertial navigation system and directed information and GPS
Latitude and longitude information and directional angle information fusion, obtain the revised positional information of robot.
Specifically, as described in step S11, the zero bias value of gyroscope is initialized.
Those skilled in the art know, under robot inactive state, after gyroscope electrifying startup, detect the axle of robot three
Non-zero rotating speed, as speed zero bias be present.Speed zero bias are undulating value, and influence be present to robot angle calculation precision.
The method for determining gyroscope zero bias is as follows：
(1) ensure in the case where robot remains static, by iic bus, 1 is read every preset time (such as 10 seconds)
Three axle rate values of secondary gyroscope, it is respectively：Velox (i), veloy (i) and veloz (i), and each axle acquisition N groups (such as N
=100)；
(2) the zero bias value of the gyroscope of each axle is calculated respectively according to equation below：
Wherein, velox_zero is the speed zero bias value of gyroscope x-axis, and velox (i) is the speed that ith obtains gyroscope x-axis
Value；
Wherein, veloy_zero is the speed zero bias value of gyroscope y-axis, and veloy (i) is the speed that ith obtains gyroscope y-axis
Value；
Wherein, veloz_zero is the speed zero bias value of gyroscope z-axis, and veloz (i) is the speed that ith obtains gyroscope z-axis
Value.
As described in step S12, the three axis angular rate information and 3-axis acceleration information of gyroscope are obtained.
In the present embodiment, by iic bus from gyroscope device read gyroscope tri-axis angular rate value velox,
veloy、veloz.Acceleration magnitude accx, accy, accz are read from accelerometer device by iic bus.
As described in step S13, carrier inclination angle is calculated according to 3-axis acceleration information.
Specifically, equation below can be used to calculate carrier inclination angle：
Inclix=arctan (accy/accz)；Wherein, inclix is robot x-axis rotation inclination angle；
Incliy=arctan (accx/accz)；Wherein, incliy is robot y-axis rotation inclination angle；
Incliz=arctan (accy/accx)；Wherein, incliz is robot z-axis rotation inclination angle.
As described in step S14, three axis angular rate information of gyroscope are integrated to obtain Eulerian angles matrix information, and
Calculate course angle spin matrix.
Specifically, three axis angular rate information of gyroscope are integrated to obtain Eulerian angles matrix information using following public
Formula calculates：
Wherein, velo_robot_x (i) be gyroscope x-axis angular speed, velo_robot_y (i) be gyroscope y-axis angle speed
Degree, velo_robot_z (i) are gyroscope z-axis angular speed, and T is integration period.Above-mentioned integral formula turns to calculating to be discrete
Machine language.Wherein,
Velo_robot_x (i)=velox (i)-velox_zero；
Velo_robot_y (i)=veloy (i)-veloy_zero；
Velo_robot_z (i)=veloz (i)-veloz_zero；
Then, spin matrix R, spin matrix R=[cos (incliy) * cos (incliz), cos (incliz) * are calculated
Sin (inclix) * sin (incliy)-cos (inclix) * sin (incliz), sin (inclix) * sin (incliz)+cos
(inclix) * cos (incliz) * sin (incliy)] [cos (incliy) * sin (incliz), cos (inclix) * cos
(incliz)+sin (inclix) * sin (incliy) * sin (incliz), cos (inclix) * sin (incliy) * sin
(incliz)-cos (incliz) * sin (inclix)] [- sin (incliy), cos (incliy) * sin (inclix), cos
(inclix)*cos(incliy)]
Course angle spin matrix is the part in above-mentioned spin matrix R.
Course angle spin matrix Rheading=[cos (incliy) * sin (incliz), cos (inclix) * cos
(incliz)+sin (inclix) * sin (incliy) * sin (incliz), cos (inclix) * sin (incliy) * sin
(incliz)-cos(incliz)*sin(inclix)]]。
As described in step S15, robot under inertial navigation system is calculated based on Eulerian angles matrix information and course angle spin matrix
Course angle.
Calculating robot's course angle：
As described in step S16, the feedback rotating speed of robotically-driven motor encoder is obtained, and is converted into robot
Wheel speed.
Using the differential type of drive of two-wheel, the feedback rotating speed for obtaining motor encoder is motor for robot
Rotating speed.
Specifically, the feedback rotating speed of robotically-driven motor encoder is obtained by CAN.
Feedback rotating speed is converted into the wheel speed of robot using equation below：
Velo_wheel=motor_velo/Ratio_gear；
Wherein, the feedback that Ratio_gear is reductor speed reducing ratio, motor_velo is robotically-driven motor encoder
Rotating speed, the wheel speed that velo_wheel is robot.
As described in step S17, the mobile linear velocity of the wheel speed calculation robot based on robot.
Specifically, the mobile linear velocity of robot is calculated using equation below：
Robot_v=(velo_wheel_left+velo_wheel_right)/2；
Wherein, velo_wheel_left be the wheel speed of robot revolver, velo_wheel_right be robot right wheel
Wheel speed.
As described in step S18, the position based on robot under inertial navigation system is determined according to moveable robot movement equation
Coordinate.
Specifically, the x-axis coordinate based on robot under inertial navigation system is determined using following moveable robot movement equation
With y-axis coordinate：
Robot_x (k)=robot_x (k-1)+robot_v (k-1) * T*cos (heading (k-1))；
Robot_y (k)=robot_y (k-1)+robot_v (k-1) * T*sin (heading (k-1))；
Wherein, heading be robot course angle, T be controlling cycle；
Robot_x (k), robot_y (k) represent x-axis coordinate and y-axis coordinate of the robot at the k moment；
Robot_x (k-1), robot_y (k-1) represent x-axis coordinate and y-axis coordinate of the robot at the k-1 moment；
Robot_v (k-1) represents the mobile linear velocity at robot k-1 moment；
Heading (k-1) represents the robot course angle at robot k-1 moment.
Above-mentioned steps S11 to step S18 is the directed information and location information for obtaining inertial navigation system.
As described in step S31, read and parse nmea agreements, believed with obtaining GPS latitude and longitude informations with GPS directional angle
Breath.
Specifically, processor reads GPS nmea agreements by serial ports and parses nmea agreements.For example, specifying information
Including：The finger of GPS longitude information lon, GPS latitude information lat, GPS course angle gps heading and GPS signal quality
Show symbol gps qual.
As described in step S32, formula is just being calculated using gauss projection GPS latitude and longitude informations are converted into plane right-angle coordinate
Coordinate value.
Specifically, formula is just calculated using following gauss projection, respectively substituted into GPS longitude informations and latitude information in formula
B and l, to convert thereof into the coordinate value xg and yg of plane right-angle coordinate：
Above-mentioned steps S31 and step S32 is the latitude and longitude information and directional angle information for obtaining GPS.
Further, meet preset value in this GPS signal quality, and obtain GPS latitude and longitude information and orientation angle
In the case of spending information, using GPS latitude and longitude information and directional angle information to the directed information of the inertial navigation system got
Positioning and directing is carried out to robot with the mode that location information is modified.
As described in step S41, respectively to based on the position coordinates of robot, robot course angle, plane under inertial navigation system
The coordinate value and GPS directional angle information of rectangular coordinate system make Kalman filtering, with obtain the present coordinate values of robot and
Current course angle.
Those skilled in the art know that Kalman filter formulation is as follows：
Kalman.p=kalman.p+kalman.q；
Kalman.k (k)=kalman.p/ (kalman.p+kalman.r)；
Kalman.x (k)=kalman.x (k-1)+kalman.k* (kalman.z (k-1)-kalman.x (k-1))；
Kalman.p (k)=(1-kalman.k) * kalman.p；
Wherein, kalman.x be state variable, kalman.z be observation.
Kalman.p is that covariance estimation, kalman.q are that procedure activation noise covariance, kalman.k are remaining increasings
Benefit, kalman.r are observation noise covariances.
Specifically, the state variable using the x-axis coordinate value robot_x of robot under inertial navigation system as Kalman
The observation kalman.z (k-1) of kalman.x (k-1), the coordinate value xg of plane right-angle coordinate as Kalman, to obtain
The current X-axis coordinate value x of robot；
State variable kalman.x (k- using the y-axis coordinate value robot_y of robot under inertial navigation system as Kalman
1), observation kalman.zs (k-1) of the coordinate value yg of plane right-angle coordinate as Kalman, it is current to obtain robot
Y-axis coordinate value y；
State variable kalman.x (k-1) using the course angle heading of robot under inertial navigation system as Kalman,
GPS directional angle information is as the observation kalman.z (k-1) of Kalman to obtain the current course angle of robot.
As described in step S42, the present coordinate values of robot are converted into current location using gauss projection inversion formula
Latitude and longitude information.
Using following gauss projection inversion formula, respectively by the current Y-axis of the current X-axis coordinate value x of robot, robot
Coordinate value y substitutes into x and y in formula, to convert thereof into latitude information BlatWith longitude information llon
It should be noted that in above-mentioned steps S22 and step S32, it is existing that formula and inversion formula are just being calculated in gauss projection
There is theory, those skilled in the art can refer to the document theoretical on gauss projection, will not be repeated here.
In summary, using the technical program in the case where the signal quality for monitoring GPS meets preset value, using obtaining
Get GPS latitude and longitude information (i.e. location information) and directional angle information to the directed information of inertial navigation system that gets (i.e.
The course angle of robot) be modified with location information, so as to optimize GPS and inertial navigation system fusion method and process, with gram
Take error problem existing for electromagnetic interference and gyroscope (i.e. inertial navigation system) existing for electronic compass orientation in use
Existing trueness error problem.
Although the present invention is disclosed as above with preferred embodiment, it is not for limiting the present invention, any this area
Technical staff without departing from the spirit and scope of the present invention, may be by the methods and technical content of the disclosure above to this hair
Bright technical scheme makes possible variation and modification, therefore, every content without departing from technical solution of the present invention, according to the present invention
Any simple modifications, equivalents, and modifications made to above example of technical spirit, belong to technical solution of the present invention
Protection domain.
Claims (15)
1. one kind is using GPS and inertial navigation system to robot localization orientation method, it is characterised in that comprises the following steps：
Obtain the directed information and location information of inertial navigation system；
Whether the signal quality for judging GPS every preset time meets preset value；
If GPS signal quality meets preset value, GPS latitude and longitude information and directional angle information are obtained；
With reference to the GPS got latitude and longitude information and directional angle information to the directed information of inertial navigation system that gets and fixed
The mode that position information is modified carries out positioning and directing to robot.
2. as claimed in claim 1 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that if GPS
Signal quality be unsatisfactory for preset value, then this latitude and longitude information and directional angle information for obtaining GPS is abandoned, using getting
Inertial navigation system directed information and location information to robot carry out positioning and directing.
3. as claimed in claim 1 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that described
The directed information and location information for obtaining inertial navigation system comprise the following steps：
Initialize the zero bias value of gyroscope；
Obtain the three axis angular rate information and 3-axis acceleration information of gyroscope；
Carrier inclination angle is calculated according to 3-axis acceleration information；
Three axis angular rate information of gyroscope are integrated to obtain Eulerian angles matrix information, and calculate course angle spin moment
Battle array；
The course angle of robot under inertial navigation system is calculated based on Eulerian angles matrix information and course angle spin matrix；
The feedback rotating speed of robotically-driven motor encoder is obtained, and is converted into the wheel speed of robot；
The mobile linear velocity of wheel speed calculation robot based on robot；
Position coordinates based on robot under inertial navigation system is determined according to moveable robot movement equation.
4. as claimed in claim 3 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that obtain
GPS latitude and longitude information and directional angle information comprises the following steps：
Read and parse nmea agreements, to obtain GPS latitude and longitude informations and GPS directional angle information；
The coordinate value that formula is converted to GPS latitude and longitude informations plane right-angle coordinate is just being calculated using gauss projection.
5. as claimed in claim 4 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that be based on
The directed information and location information of the inertial navigation system got and the latitude and longitude information and directional angle letter for combining the GPS got
The mode for being modified it is ceased to comprise the following steps robot progress positioning and directing：
Respectively to the coordinate value based on the position coordinates of robot, robot course angle, plane right-angle coordinate under inertial navigation system
And GPS directional angle information makees Kalman filtering, to obtain the present coordinate values of robot and current course angle；
The present coordinate values of robot are converted into the latitude and longitude information of current location using gauss projection inversion formula.
6. as claimed in claim 5 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that described
The zero bias value of initialization gyroscope includes：
(1) in the case where robot remains static, by iic bus, three axle speed rates of i gyroscope are read every preset time
Value, it is respectively：Velox (i), veloy (i) and veloz (i), and each axle obtains N groups；
(2) the zero bias value of the gyroscope of each axle is calculated respectively according to equation below：
Wherein, velox_zero is the speed zero bias value of gyroscope x-axis, and velox (i) is the speed that ith obtains gyroscope x-axis
Value；
Wherein, veloy_zero is the speed zero bias value of gyroscope y-axis, and veloy (i) is the speed that ith obtains gyroscope y-axis
Value；
Wherein, veloz_zero is the speed zero bias value of gyroscope z-axis, and veloz (i) is the speed that ith obtains gyroscope z-axis
Value.
7. as claimed in claim 5 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that described
Calculating carrier inclination angle according to 3-axis acceleration information includes：
Carrier inclination angle is calculated using equation below：
Inclix=arctan (accy/accz)；Wherein, inclix is robot x-axis rotation inclination angle；
Incliy=arctan (accx/accz)；Wherein, incliy is robot y-axis rotation inclination angle；
Incliz=arctan (accy/accx)；Wherein, incliz is robot z-axis rotation inclination angle.
8. as claimed in claim 5 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that to top
Three axis angular rate information of spiral shell instrument are integrated to be calculated with obtaining Eulerian angles matrix information using equation below：
Wherein, velo_robot_x (i) be gyroscope x-axis angular speed, velo_robot_y (i) be gyroscope y-axis angular speed,
Velo_robot_z (i) is gyroscope z-axis angular speed, and T is integration period.
9. as claimed in claim 7 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that use
Equation below calculates course angle spin matrix：
Rheading=[cos (incliy) * sin (incliz), cos (inclix) * cos (incliz)+sin (inclix) *
sin(incliy)*sin(incliz),cos(inclix)*sin(incliy)*sin(incliz)-cos(incliz)*sin
(inclix)]]；Wherein, Rheading is course angle spin matrix.
10. as claimed in claim 5 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that described
The feedback rotating speed of robotically-driven motor encoder is obtained, and the wheel speed for being converted into robot includes：
The feedback rotating speed of robotically-driven motor encoder is obtained by CAN；
Feedback rotating speed is converted into the wheel speed of robot using equation below：
Velo_wheel=motor_velo/Ratio_gear；
Wherein, Ratio_gear is reductor speed reducing ratio, motor_velo is robotically-driven motor encoder feedback rotating speed,
Velo_wheel is the wheel speed of robot.
11. as claimed in claim 10 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that institute
Stating the mobile linear velocity of the wheel speed calculation robot based on robot includes：
The mobile linear velocity of robot is calculated using equation below：
Robot_v=(velo_wheel_left+velo_wheel_right)/2；
Wherein, the wheel that velo_wheel_left is the wheel speed of robot revolver, velo_wheel_right is robot right wheel
Speed.
12. as claimed in claim 11 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that root
Determine that the position coordinates based on robot under inertial navigation system includes according to moveable robot movement equation：
X-axis coordinate and y-axis coordinate based on robot under inertial navigation system are determined using following moveable robot movement equation：
Robot_x (k)=robot_x (k-1)+robot_v (k-1) * T*cos (heading (k-1))；
Robot_y (k)=robot_y (k-1)+robot_v (k-1) * T*sin (heading (k-1))；
Wherein, heading be robot course angle, T be controlling cycle；
Robot_x (k), robot_y (k) represent x-axis coordinate and y-axis coordinate of the robot at the k moment；
Robot_x (k-1), robot_y (k-1) represent x-axis coordinate and y-axis coordinate of the robot at the k-1 moment；
Robot_v (k-1) represents the mobile linear velocity at robot k-1 moment；
Heading (k-1) represents the robot course angle at robot k-1 moment.
13. as claimed in claim 12 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that profit
Formula is just being calculated with gauss projection GPS latitude and longitude informations are converted to the coordinate value of plane right-angle coordinate includes：
Formula is just calculated using following gauss projection, GPS longitude informations and latitude information are substituted into B and l in formula respectively, by it
It is converted into the coordinate value xg and yg of plane right-angle coordinate：
14. as claimed in claim 13 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that point
It is other to coordinate value and GPS based on the position coordinates of robot, robot course angle, plane right-angle coordinate under inertial navigation system
Directional angle information makees Kalman filtering, is included with obtaining the present coordinate values of robot and current course angle：
State variable kalman.x (k-1) using the x-axis coordinate value robot_x of robot under inertial navigation system as Kalman, put down
Observation kalman.zs (k-1) of the coordinate value xg of face rectangular coordinate system as Kalman, sat with obtaining the current X-axis of robot
Scale value x；
State variable kalman.x (k-1) using the y-axis coordinate value robot_y of robot under inertial navigation system as Kalman, put down
Observation kalman.zs (k-1) of the coordinate value yg of face rectangular coordinate system as Kalman, sat with obtaining the current Y-axis of robot
Scale value y；
State variable kalman.x (k-1) using the course angle of robot under inertial navigation system as Kalman, GPS directional angle letter
Cease as the observation kalman.z (k-1) of Kalman to obtain the current course angle of robot.
15. as claimed in claim 14 using GPS and inertial navigation system to robot localization orientation method, it is characterised in that profit
The present coordinate values of robot are converted into the latitude and longitude information of current location with gauss projection inversion formula to be included：
Using following gauss projection inversion formula, respectively by the current Y-axis coordinate of the current X-axis coordinate value x of robot, robot
Value y substitutes into x and y in formula, to convert thereof into latitude information BlatWith longitude information llon
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710447395.3A CN107490803A (en) | 2017-06-14 | 2017-06-14 | Using GPS and inertial navigation system to robot localization orientation method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710447395.3A CN107490803A (en) | 2017-06-14 | 2017-06-14 | Using GPS and inertial navigation system to robot localization orientation method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107490803A true CN107490803A (en) | 2017-12-19 |
Family
ID=60642964
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710447395.3A Pending CN107490803A (en) | 2017-06-14 | 2017-06-14 | Using GPS and inertial navigation system to robot localization orientation method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107490803A (en) |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108235736A (en) * | 2017-12-25 | 2018-06-29 | 深圳前海达闼云端智能科技有限公司 | Positioning method, cloud server, terminal, system, electronic device and computer program product |
| CN108387918A (en) * | 2018-01-18 | 2018-08-10 | 和芯星通（上海）科技有限公司 | A kind of pedestrian navigation method and cloud system server, storage medium, electronic equipment |
| CN108508471A (en) * | 2018-06-05 | 2018-09-07 | 广东纵行科技有限公司 | A kind of automatic driving vehicle localization method and device |
| CN109116845A (en) * | 2018-08-17 | 2019-01-01 | 华晟（青岛）智能装备科技有限公司 | Automated guided vehicle localization method, positioning system and homing guidance transportation system |
| CN109189079A (en) * | 2018-11-05 | 2019-01-11 | 南京理工大学 | Mobile Robotics Navigation control method based on GPS positioning |
| CN109901580A (en) * | 2019-03-13 | 2019-06-18 | 华南理工大学 | A kind of unmanned plane cooperates with unmanned ground robot follows diameter obstacle avoidance system and its method |
| CN110133694A (en) * | 2019-04-18 | 2019-08-16 | 同济大学 | The vehicle positioning method and system assisted based on the course double antenna GNSS and wheel speed |
| WO2020103696A1 (en) * | 2018-11-19 | 2020-05-28 | 苏州宝时得电动工具有限公司 | Automatic walking device and control method therefor |
| CN111267080A (en) * | 2020-02-18 | 2020-06-12 | 上海柴孚机器人有限公司 | Method for automatically correcting path of industrial robot |
| CN111267081A (en) * | 2020-02-18 | 2020-06-12 | 上海柴孚机器人有限公司 | Method for orienting an industrial robot |
| CN111337702A (en) * | 2020-03-24 | 2020-06-26 | 深圳市微秒控制技术有限公司 | Data processing system and method for optical incremental encoder |
| CN111811505A (en) * | 2020-08-27 | 2020-10-23 | 中国人民解放军国防科技大学 | Pedestrian seamless navigation positioning method and system based on intelligent device and MIMU |
| CN112097758A (en) * | 2019-06-18 | 2020-12-18 | 阿里巴巴集团控股有限公司 | Positioning method and device, robot positioning method and robot |
| CN112230256A (en) * | 2019-07-15 | 2021-01-15 | 苏州宝时得电动工具有限公司 | Autonomous robot, positioning calibration method and device thereof, and storage medium |
| WO2021098388A1 (en) * | 2019-11-20 | 2021-05-27 | 苏州宝时得电动工具有限公司 | Route generation method and apparatus, and lawn mower |
| CN114745698A (en) * | 2022-06-08 | 2022-07-12 | 深圳市永达电子信息股份有限公司 | Automatic alignment method and system for mobile communication vehicle and mobile communication vehicle |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102721970A (en) * | 2011-11-22 | 2012-10-10 | 武汉苍穹数码仪器有限公司 | Speech auxiliary smooth acquisition method capable of eliminating shelter influence |
| CN103207634A (en) * | 2013-03-20 | 2013-07-17 | 北京工业大学 | Data fusion system and method of differential GPS (Global Position System) and inertial navigation in intelligent vehicle |
| CN106356765A (en) * | 2016-11-11 | 2017-01-25 | 广东电网有限责任公司电力科学研究院 | Multitask load transmission line patrol robot and system |
-
2017
- 2017-06-14 CN CN201710447395.3A patent/CN107490803A/en active Pending
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102721970A (en) * | 2011-11-22 | 2012-10-10 | 武汉苍穹数码仪器有限公司 | Speech auxiliary smooth acquisition method capable of eliminating shelter influence |
| CN103207634A (en) * | 2013-03-20 | 2013-07-17 | 北京工业大学 | Data fusion system and method of differential GPS (Global Position System) and inertial navigation in intelligent vehicle |
| CN106356765A (en) * | 2016-11-11 | 2017-01-25 | 广东电网有限责任公司电力科学研究院 | Multitask load transmission line patrol robot and system |
Non-Patent Citations (3)
| Title |
|---|
| 宗光华 等: ""一种鲁棒的室外移动机器人定位方法"", 《北京航空航天大学学报》 * |
| 戴慧琼: ""关于特殊地形的测绘技术方案"", 《商 BUSINESS》 * |
| 胡胜豪: ""室外移动机器人的定位与运动控制研究"", 《中国优秀硕士学位论文全文数据库（电子期刊） 信息科技辑》 * |
Cited By (20)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108235736A (en) * | 2017-12-25 | 2018-06-29 | 深圳前海达闼云端智能科技有限公司 | Positioning method, cloud server, terminal, system, electronic device and computer program product |
| CN108235736B (en) * | 2017-12-25 | 2021-11-16 | 达闼机器人有限公司 | Positioning method, cloud server, terminal, system, electronic device and computer program product |
| CN108387918A (en) * | 2018-01-18 | 2018-08-10 | 和芯星通（上海）科技有限公司 | A kind of pedestrian navigation method and cloud system server, storage medium, electronic equipment |
| CN108508471A (en) * | 2018-06-05 | 2018-09-07 | 广东纵行科技有限公司 | A kind of automatic driving vehicle localization method and device |
| CN109116845A (en) * | 2018-08-17 | 2019-01-01 | 华晟（青岛）智能装备科技有限公司 | Automated guided vehicle localization method, positioning system and homing guidance transportation system |
| CN109189079B (en) * | 2018-11-05 | 2021-07-23 | 南京理工大学 | Mobile robot navigation control method based on GPS positioning |
| CN109189079A (en) * | 2018-11-05 | 2019-01-11 | 南京理工大学 | Mobile Robotics Navigation control method based on GPS positioning |
| WO2020103696A1 (en) * | 2018-11-19 | 2020-05-28 | 苏州宝时得电动工具有限公司 | Automatic walking device and control method therefor |
| CN109901580A (en) * | 2019-03-13 | 2019-06-18 | 华南理工大学 | A kind of unmanned plane cooperates with unmanned ground robot follows diameter obstacle avoidance system and its method |
| CN110133694A (en) * | 2019-04-18 | 2019-08-16 | 同济大学 | The vehicle positioning method and system assisted based on the course double antenna GNSS and wheel speed |
| CN110133694B (en) * | 2019-04-18 | 2023-11-03 | 同济大学 | Vehicle positioning method and system based on dual-antenna GNSS heading and wheel speed assistance |
| CN112097758A (en) * | 2019-06-18 | 2020-12-18 | 阿里巴巴集团控股有限公司 | Positioning method and device, robot positioning method and robot |
| CN112230256A (en) * | 2019-07-15 | 2021-01-15 | 苏州宝时得电动工具有限公司 | Autonomous robot, positioning calibration method and device thereof, and storage medium |
| CN112230256B (en) * | 2019-07-15 | 2024-04-09 | 苏州宝时得电动工具有限公司 | Autonomous robot, positioning calibration method and device thereof, and storage medium |
| WO2021098388A1 (en) * | 2019-11-20 | 2021-05-27 | 苏州宝时得电动工具有限公司 | Route generation method and apparatus, and lawn mower |
| CN111267081A (en) * | 2020-02-18 | 2020-06-12 | 上海柴孚机器人有限公司 | Method for orienting an industrial robot |
| CN111267080A (en) * | 2020-02-18 | 2020-06-12 | 上海柴孚机器人有限公司 | Method for automatically correcting path of industrial robot |
| CN111337702A (en) * | 2020-03-24 | 2020-06-26 | 深圳市微秒控制技术有限公司 | Data processing system and method for optical incremental encoder |
| CN111811505A (en) * | 2020-08-27 | 2020-10-23 | 中国人民解放军国防科技大学 | Pedestrian seamless navigation positioning method and system based on intelligent device and MIMU |
| CN114745698A (en) * | 2022-06-08 | 2022-07-12 | 深圳市永达电子信息股份有限公司 | Automatic alignment method and system for mobile communication vehicle and mobile communication vehicle |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107490803A (en) | Using GPS and inertial navigation system to robot localization orientation method | |
| Costanzi et al. | An attitude estimation algorithm for mobile robots under unknown magnetic disturbances | |
| CN102257358B (en) | Method for determining a heading in the direction of true north using an inertial measurement unit | |
| CN107655493B (en) | SINS six-position system-level calibration method for fiber-optic gyroscope | |
| CN108458714B (en) | Euler angle solving method without gravity acceleration in attitude detection system | |
| WO2016198009A1 (en) | Heading checking method and apparatus | |
| EP2510308B1 (en) | A method of determining heading by turning an inertial device | |
| CN103712622B (en) | The gyroscopic drift estimation compensation rotated based on Inertial Measurement Unit and device | |
| CN109073388B (en) | Gyromagnetic geographic positioning system | |
| CN106153069B (en) | Attitude rectification device and method in autonomous navigation system | |
| US11226203B2 (en) | Low cost INS | |
| CN106708088B (en) | Coordinate calculation method and device, flight control method and system and unmanned aerial vehicle | |
| CN106979779A (en) | A kind of unmanned vehicle real-time attitude measuring method | |
| JP2002090173A (en) | Inertia navigation system and its initial alignment method | |
| JP2001141507A (en) | Inertial navigation system | |
| EP4148383A2 (en) | Rotation measurement system using coriolis and euler forces | |
| Kaniewski et al. | Integrated system for heading determination | |
| CN105222764B (en) | The method that a kind of pair of inertia angular-rate sensor carries out earth rotation compensation | |
| CN101949702A (en) | Quick self-testing method for GNSS PVT quality by using MEMS accelerometer | |
| JP2011069633A (en) | Portable device | |
| Chenghao et al. | Application of correlation detection in rotation-modulation north-seeking based on MEMS gyroscope | |
| CN107289935B (en) | Indoor navigation algorithm suitable for wearable equipment | |
| Xu et al. | Anti-magnetic disturbance pedestrians navigation system based on MEMS inertial sensors | |
| CN115773750B (en) | Magnetic course angle measuring device and method | |
| US20230204358A1 (en) | Mems gyrocompass |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| RJ01 | Rejection of invention patent application after publication | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20171219 |