CN109141410A - The Multi-sensor Fusion localization method of AGV integrated navigation - Google Patents
The Multi-sensor Fusion localization method of AGV integrated navigation Download PDFInfo
- Publication number
- CN109141410A CN109141410A CN201810825951.0A CN201810825951A CN109141410A CN 109141410 A CN109141410 A CN 109141410A CN 201810825951 A CN201810825951 A CN 201810825951A CN 109141410 A CN109141410 A CN 109141410A
- Authority
- CN
- China
- Prior art keywords
- agv
- pitch
- cos
- sin
- roll
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- 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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
Landscapes
- Engineering & Computer Science (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Geology (AREA)
- Navigation (AREA)
Abstract
The invention discloses a kind of Multi-sensor Fusion localization methods of AGV integrated navigation, which comprises determines coordinate: obtaining the carrier coordinate system (b) and geographic coordinate system (n) of AGV robot；Data processing: attitude algorithm is carried out using quaternion differential equation；It obtains data: the angular speed of AGV robot, the acceleration of operation and magnetic field strength is obtained by gyroscope, angular speed meter and magnetometer respectively；Filtering and noise reduction: using complementary filter algorithm, corrects quaternary number by accelerometer and magnetometer measures value to correct posture.The present invention provides a kind of Multi-sensor Fusion localization method of AGV integrated navigation, and linearisation degree and precision is made to increase, and reduces the calculation amount and error of full attitude algorithm algorithm.
Description
Technical field
The present invention relates to homing guidance robotic technology fields more particularly to a kind of multisensor of AGV integrated navigation to melt
Close localization method.
Background technique
AGV is the abbreviation of (Automated Guided Vehicle), i.e., " automated guided vehicle ", refers to equipped with electricity
Magnetically or optically equal homing guidances device, it can be travelled along defined guide path, have safeguard protection and various transfer function
The transport vehicle of energy.AGV integrated navigation technology generally passes through the positioning of two steps, and the first step is coarse positioning, low using flexible height, precision
Guide mode Primary Location determines the current location of AGV；Second step is fine positioning, passes through technology maturation, high reliablity, cost
Low guide mode carries out error correction, improves the positioning accuracy of AGV.The important directions that integrated navigation develops as airmanship
One of, it finds broad application at present in fields such as automobile, navigation, Aeronautics and Astronautics.
The core of the navigation guide technology of AGV is AGV attitude algorithm, is determining carrier coordinate system and geographic coordinate system
Between position relation, currently used algorithm is divided into Eulerian angles differential equation method, direction cosines differential equation method and quaternary number
Differential equation method.Wherein, Euler's horn cupping is although relatively simple, but triangulo operation is difficult, the algorithm when pitch angle is near 90 degree
Degradation failure phenomenon will be will appear, it is impossible to be used in full attitude algorithm；Though direction cosine method can avoid the equation that Euler's horn cupping generates
Degradation phenomena, but nine differential equations need to be resolved, it is computationally intensive；The linearisation degree and precision of quaternary number attitude algorithm compared with
Height, and can realize full attitude algorithm.But with quaternary number attitude algorithm method, gyroscope measurement angular speed can generate accumulation and miss
Difference is easy diverging, and accelerometer and magnetometer are not likely to produce accumulated error, but real-time accuracy is poor.Therefore, a kind of line is designed
Property degree and precision are higher, and the full attitude algorithm algorithm that can reduce calculation amount is that there is an urgent need to solve by those skilled in the art
Certainly the technical issues of.
Summary of the invention
The purpose of the present invention is to provide a kind of Multi-sensor Fusion localization method of AGV integrated navigation, make to linearize journey
Degree and precision increase, and reduce the calculation amount and error of full attitude algorithm algorithm.
Technical solution used by the Multi-sensor Fusion localization method of AGV integrated navigation disclosed by the invention is:
A kind of Multi-sensor Fusion localization method of AGV integrated navigation, which comprises
It determines coordinate: obtaining the carrier coordinate system (b) and geographic coordinate system (n) of AGV robot；
Data processing: attitude algorithm is carried out using quaternion differential equation；
Obtain data: by gyroscope, angular speed meter and magnetometer obtain respectively AGV robot angular speed, operation
Acceleration and magnetic field strength；
Filtering and noise reduction: using complementary filter algorithm, corrects quaternary number by accelerometer and magnetometer measures value to correct
Posture.
Preferably, in the acquisition data step, the gyroscope detects X, Y, Z using three independent MEMS
The capacitance variations of axis export voltage value relevant to angular speed variation.
Preferably, in the filtering and noise reduction step, the value pair that is measured by gyroscope, accelerometer and magnetometer
Pitch angle, roll angle, course angle and the initialization of quaternary number, calculation formula are as follows:
q0_init=cos (0.5*roll0)*cos(0.5*pitch0)*cos(0.5*yaw0)-
sin(0.5*roll0)*sin(0.5*pitch0)*sin(0.5*yaw0)
q1_init=cos (0.5*roll0)*sin(0.5*pitch0)*cos(0.5*yaw0)-
sin(0.5*roll0)*cos(0.5*pitch0)*sin(0.5*yaw0)
q2_init=sin (0.5*roll0)*cos(0.5*pitch0)*cos(0.5*yaw0)+
cos(0.5*roll0)*sin(0.5*pitch0)*sin(0.5*yaw0)
q3_init=cos (0.5*roll0)*cos(0.5*pitch0)*sin(0.5*yaw0)+
sin(0.5*roll0)*sin(0.5*pitch0)*cos(0.5*yaw0)
Wherein, ax, ay, az are the axle acceleration value that accelerometer measures, and mx, my, mz is the three axis magnetic that gyroscope measures
Induction value.
Preferably, in the data processing step, using the fourth order Runge-Kutta algorithm in numerical integration.Differential
Equation is as follows:
Wherein Q (t) is attitude quaternion,Angular speed for b system with respect to n system, quaternary number solution are as follows:
Indicate geographic coordinate system (n) to the attitude matrix carrier coordinate system (b) by the quaternary number solved are as follows:
The attitude angle of carrier is calculated according to the quaternion algebra value and attitude matrix for cutting upper solution are as follows:
Pitch=arcsinC32, Pitch ∈ (- 90 °, 90 °)
Roll ∈ (- 180 °, 180 °)
Yaw ∈ (- 180 °, 180 °).
Preferably, the method also includes two-wheel differentials to drive AGV motion model algorithm, comprising:
Obtain AGV position of form center P, course angle θ, spacing d between two drive shafts, driving wheel radius r must point between centers line
M, coordinate are (xM,yM) P and M distance l, straight line PM and the central axes AGV angle β, two-wheeled angular velocity of rotation is wLAnd wR.；
Kinematics operation: M spot speed and angular speed are released by left and right two-wheeled angular speed and radius of wheel；
Motion model is established.
Preferably, in the kinematics operation step, calculation formula are as follows:
The position coordinates of P point can be released by it with positional relationship, calculation formula are as follows:
xP=xM+lcos(θ+β)
yP=yM+lsin(θ+β)
The position equation of P point obtains the speed of P point, calculation formula to time derivation are as follows:
Preferably, in the motion model establishment step, motion model formula:
Preferably, the method also includes assisted location methods, comprising:
Correction mark is laid with to geographic coordinate system (n)；
Each the correction mark being scanned across；
Corresponding mark data is updated, range deviation e is obtained and is removed.
Preferably, the correction of the laying is identified as two dimensional code.
Preferably, the corresponding mark data of the update obtains range deviation e and removes in step, the distance of AGV
Deviation e are as follows:
Wherein: x is the corresponding actual range of each pixel.As e>o, vehicle is on the right side of leading line, as e<o, vehicle
On the left of leading line, to correct position and the course angle of AGV.
The beneficial effect of the Multi-sensor Fusion localization method of AGV integrated navigation disclosed by the invention is: by quaternary number
Method carries out attitude algorithm, using the fourth order Runge-Kutta algorithm in numerical integration, simplifies the calculation amount of full attitude algorithm algorithm；With
This corrects quaternary number by accelerometer and magnetometer measures value simultaneously, using complementary filter algorithm to correct posture, makes linear
Change degree and precision increase, and reduce full attitude algorithm Algorithm Error.
Detailed description of the invention
Fig. 1 is the flow chart of the Multi-sensor Fusion localization method of AGV integrated navigation of the present invention.
Fig. 2 is AGV robot information flow relational graph of the present invention.
Fig. 3 is AGV robot wireless communication module ESP8266 of the present invention.
Fig. 4 is AGV robot industry controller and sensor MPU9255 of the present invention.
Fig. 5 is the Multi-sensor Fusion localization method complementary filter attitude rectification algorithm flow of AGV integrated navigation of the present invention
Figure.
Fig. 6 is the Multi-sensor Fusion localization method pose monitoring and test result of AGV integrated navigation of the present invention.
Fig. 7 is two-wheel differential driving AGV motion model algorithm of the present invention.
Fig. 8 is the motion model of AGV robot of the present invention.
Fig. 9 is assisted location method of the present invention.
Figure 10 is image in 2 D code of the present invention and parameter definition schematic diagram.
Specific embodiment
The present invention is further elaborated and is illustrated with Figure of description combined with specific embodiments below:
Referring to FIG. 1, a kind of Multi-sensor Fusion localization method of AGV integrated navigation, which comprises
Step S101: it determines coordinate: obtaining the carrier coordinate system (b) and geographic coordinate system (n) of AGV robot；
Step S102: attitude algorithm data processing: is carried out using quaternion differential equation；
Step S103: data are obtained: obtains the angle speed of AGV robot respectively by gyroscope, angular speed meter and magnetometer
Degree, the acceleration of operation and magnetic field strength；
Step S104: filtering and noise reduction: using complementary filter algorithm, corrects quaternary by accelerometer and magnetometer measures value
Number is to correct posture.
Referring to FIG. 2, specifically, this method is mainly used in AGV robot field.AGV robot in the present embodiment
Communication module be all by serial ports RS232 with the external world communicated, adopted between AGV onboard system and host computer scheduling system
Use wireless LAN communication.
It since transmitted data amount is less between upper computer and lower computer, is occupied to reduce system resource, the present embodiment selection
UDP (User Datagram Protocol) agreement in OSI (Open System Interconnection) reference model into
Row wireless module transmits data.What the present embodiment hardware selected is the wireless mould of ESP8266 serial ports wifi of current Technical comparing maturation
Block, as shown in Figure 3.It is mainly used to the stability that test receives the number pick that AGV is sent, IP, voltage including AGV, speed at this stage
The relevant informations such as degree, operating status, position.The programming of UDP communication module includes that server and client side's two parts form, main logical
It crosses UDP control block and related skilful use interface function realizes data-transformation facility.
To improve attitude measurement accuracy, gyroscope needs that accelerometer or magnetometer is combined to carry out attitude detection.This implementation
Example is using the 9 axis attitude detection digital sensors produced based on the LPC2388 industrial control unit (ICU) designed and IrwenSense company
MPU925 realizes AGV inertial navigation technology, as shown in Figure 4.
In the present embodiment, gyroscope utilizes Coriolis effect, is become by the capacitor that three independent MEMS detect X, Y, Z axis
Change, exports voltage relevant to angular speed variation；Accelerometer utilizes principle of inertia, by capacitor on three axis of measurement come detection axis
The degree of deviation；Magnetometer utilizes Hall effect, electromagnetism intensity of the detection earth's magnetic field in X, Y, Z axis.
In step s 102, using the fourth order Runge-Kutta algorithm in numerical integration.The differential equation is as follows:
Wherein Q (t) is attitude quaternion,Angular speed for b system with respect to n system, quaternary number solution are as follows:
Indicate geographic coordinate system (n) to the attitude matrix carrier coordinate system (b) by the quaternary number solved are as follows:
The attitude angle of carrier is calculated according to the quaternion algebra value and attitude matrix for cutting upper solution are as follows:
Pitch=arcsinC32, Pitch ∈ (- 90 °, 90 °)
Roll ∈ (- 180 °, 180 °)
Yaw ∈ (- 180 °, 180 °).
Since gyroscope measurement angular speed can generate accumulated error, it is easy diverging；And accelerometer and magnetometer are not easy to produce
Raw accumulated error, but real-time accuracy is poor, and the two is appropriate for complementary filter.The principle of attitude rectification is carried out in the present embodiment
It is: in another coordinate system and theoretical when vector n is by there are after the transformation of the spin matrix of error for determining vector n
Value generates deviation, corrects spin matrix by this deviation, and spin matrix by quaternary array at therefore quaternary can be corrected
Number, to correct posture.
The direction cosine matrix nCb being made of quaternary number indicates that the transition matrix from b system to n, bCn are indicated from n system to b's
Transition matrix, the two transposition each other.Accelerometer and magnetometer are measuring tool and carrier, and the present embodiment passes through the two devices
The error for characterizing spin matrix exists, and then corrects error by complementary filter algorithm, i.e. amendment quaternary number, complementary filter amendment
The specific solution process of posture is as shown in Figure 5.
Wherein, the theoretical output of gravity is transformed into body coordinate system in n system, the value of the vector and measurement there are error,
Gyro is corrected by the difference；Similarly the bCn that the quaternary number of the magnetic strength last time of measurement is obtained is transformed by magnetometer
Geographic coordinate system, the component of x/y plane are transformed into body coordinate system as accurate output using nCb after being synthesized in x-axis,
There are errors for the vector and the value of measurement, correct magnetometer by the difference.
In step S104, the value measured by gyroscope, accelerometer and magnetometer is to pitch angle, roll angle, course
Angle and the initialization of quaternary number, calculation formula are as follows:
q0_init=cos (0.5*roll0)*cos(0.5*pitch0)*cos(0.5*yaw0)-
sin(0.5*roll0)*sin(0.5*pitch0)*sin(0.5*yaw0)
q1_init=cos (0.5*roll0)*sin(0.5*pitch0)*cos(0.5*yaw0)-
sin(0.5*roll0)*cos(0.5*pitch0)*sin(0.5*yaw0)
q2_init=sin (0.5*roll0)*cos(0.5*pitch0)*cos(0.5*yaw0)+
cos(0.5*roll0)*sin(0.5*pitch0)*sin(0.5*yaw0)
q3_init=cos (0.5*roll0)*cos(0.5*pitch0)*sin(0.5*yaw0)+
sin(0.5*roll0)*sin(0.5*pitch0)*cos(0.5*yaw0)
Wherein, ax, ay, az are the axle acceleration value that accelerometer measures, and mx, my, mz is the three axis magnetic that gyroscope measures
Induction value.
Using MPU9255, testing level moves left and right and tilts two kinds of situations with pitch respectively, and sample frequency is
125Hz exports course angle by the data that complementary filter merges amendment gyroscope, accelerometer and electronic compass measure
(Yaw), pitch angle skilful (Pitch), roll angle (Roll) with sampling number variation, as shown in Figure 6.
When horizontal checkout, course angle, pitch angle pace of change are very fast, and roll angle is basicly stable；Pitch inclining test
When, roll angle, pitch angle pace of change are very fast, and course angle is basicly stable.Test result shows two kinds of equal energy of situation lower sensor
Pose is followed to change faster, lower deviation is within 1 degree on waveform when attitude stabilization, the significant effect of complementary filter algorithm, energy
Enough meet the requirement of real-time monitoring AGV pose.
Referring to FIG. 7, the technical program further includes two-wheel differential driving AGV motion model algorithm, comprising:
Step S201: spacing d between AGV position of form center P, course angle θ, two drive shafts, driving wheel radius r, between centers are obtained
In line must point M, coordinate be (xM,yM) P and M distance l, straight line PM and the central axes AGV angle β, two-wheeled angular velocity of rotation is wLWith
wR.；
Specifically, AGV motion model, P is AGV position of form center, and θ is platform course angle, d spacing, r between two drive shafts
For driving wheel radius, M be between centers line must point, coordinate is (xM,yM) P and M distance be l, straight line PM and the central axes AGV angle
For β, two-wheeled angular velocity of rotation is wLAnd wR.To analyze AGV motion state.In the present embodiment, preset: 1. car body is rigid body；
2. horizontal in-plane moving；3. left and right wheels stress is identical, and indeformable；4. wheel face and contact surface are point contact；5. driving caliper scale
It is very little identical.
Step S202: M spot speed and angular speed kinematics operation: are released by left and right two-wheeled angular speed and radius of wheel；
Referring to FIG. 8, specifically, M spot speed and angular speed can be released by left and right two-wheeled angular speed and radius of wheel in figure,
Calculation formula are as follows:
The position coordinates of P point can be released by it with positional relationship, calculation formula are as follows:
xP=xM+lcos(θ+β)
yP=yM+lsin(θ+β)
The position equation of P point obtains the speed of P point, calculation formula to time derivation are as follows:
Step S203: motion model is established.
Specifically, obtaining motion model formula by above formula simultaneous:
By kinematics model formula it is found that P and M distance is l and the central axes straight line PM, AGV angle is β and radius of wheel
(r) in situation known to, system is dispatched to AGV real-time speed (xP,yP) with the control of angular speed (θ) two drives of control can be passed through
Angular speed (the w of driving wheelL,wR) realize.
Referring to FIG. 9, the technical program further includes assisted location method, comprising:
Step S301: correction mark is laid with to geographic coordinate system (n)；
Step S302: each the correction mark being scanned across；
Step S303: updating corresponding mark data, obtains range deviation e and removes.
Specifically, accumulated error can be generated at any time by increasing when inertial navigation is used alone, therefore for needing for a long time
It runs and the AGV system corrected is shut down in inconvenience, need to correct the position AGV using other auxiliary positioning modes.The present embodiment draws
Enter planar bar code technology, will include that the two dimensional code of location information pastes corresponding ground coordinate points, by vehicle-mounted industrial camera
It takes image in 2 D code identifying processing and carries out location navigation., know that the absolute position of two dimensional code can calculate AGV's using oneself
Absolute position, to correct position and the course angle of AGV.
After industrial camera collects image in 2 D code, by identifying that reading position information completes positioning to two dimensional code, then
According to the coordinate determination deviation parameter of two dimensional code characteristic point in the picture, ACV pose is obtained.Image in 2 D code shows with parameter definition
It is intended to, as shown in Figure 10.
In Figure 10 white box region be two dimensional code background area, black color dots be car body center, α be ACV center line with
The deviation of directivity of two dimensional code center line, e are the range deviation of the two.Assuming that A in Fig. 2, B, tetra- vertex C, D are in image coordinate system
Coordinate in OUV is respectively (ax, ay)、(bx, by)、(cx, cy)、(dx, dy), car body center point coordinate is (ox, oy) lower right corner picture
Element is that maximum pixel coordinate is (u, v), then:
As α>0, direction of traffic left avertence, when α<0, direction of traffic right avertence.It is square for Y-axis upwards using the lower left corner as origin
To establishing coordinate system to the right for X-axis positive direction, then the coordinate (x, y) of A, B, C, D in OXY coordinate system are as follows:
Two dimensional code center line equation is established according to the new coordinate in vertex, from the above, it can be seen that center line slope is cot α, and PFor on line a bit, according to point slope form equation, center line equation can be obtained are as follows:
It willBring formula intoCenter line withIntersection point Q x coordinate are as follows:
The then range deviation e of AGV are as follows:
Wherein: x is the corresponding actual range of each pixel.As e>o, vehicle is on the right side of leading line, as e<o, vehicle
On the left of leading line, to correct position and the course angle of AGV.
The present invention provides a kind of Multi-sensor Fusion localization method of AGV integrated navigation, carries out posture by Quaternion Method
It resolves, using the fourth order Runge-Kutta algorithm in numerical integration, simplifies the calculation amount of full attitude algorithm algorithm；At the same time, it adopts
Quaternary number is corrected to correct posture by accelerometer and magnetometer measures value with complementary filter algorithm, makes linearisation degree and essence
Degree increases, and reduces full attitude algorithm Algorithm Error.
Finally it should be noted that the above embodiments are merely illustrative of the technical solutions of the present invention, rather than the present invention is protected
The limitation of range is protected, although explaining in detail referring to preferred embodiment to the present invention, those skilled in the art are answered
Work as understanding, it can be with modification or equivalent replacement of the technical solution of the present invention are made, without departing from the reality of technical solution of the present invention
Matter and range.
Claims (10)
1. a kind of Multi-sensor Fusion localization method of AGV integrated navigation, which is characterized in that the described method includes:
It determines coordinate: obtaining the carrier coordinate system (b) and geographic coordinate system (n) of AGV robot；
Data processing: attitude algorithm is carried out using quaternion differential equation；
It obtains data: obtaining the acceleration of the angular speed of AGV robot, operation respectively by gyroscope, angular speed meter and magnetometer
Degree and magnetic field strength；
Filtering and noise reduction: using complementary filter algorithm, corrects quaternary number by accelerometer and magnetometer measures value to correct posture.
2. the Multi-sensor Fusion localization method of AGV integrated navigation as described in claim 1, which is characterized in that the acquisition
In data step, using the capacitance variations of three independent MEMS detection X, Y, Z axis, output and angular speed change the gyroscope
Relevant voltage value.
3. the Multi-sensor Fusion localization method of AGV integrated navigation as claimed in claim 2, which is characterized in that the filtering
It denoises in step, the value measured by gyroscope, accelerometer and magnetometer is to pitch angle, roll angle, course angle and quaternary number
Initialization, calculation formula are as follows:
q0_init=cos (0.5*roll0)*cos(0.5*pitch0)*cos(0.5*yaw0)-
sin(0.5*roll0)*sin(0.5*pitch0)*sin(0.5*yaw0)
q1_init=cos (0.5*roll0)*sin(0.5*pitch0)*cos(0.5*yaw0)-
sin(0.5*roll0)*cos(0.5*pitch0)*sin(0.5*yaw0)
q2_init=sin (0.5*roll0)*cos(0.5*pitch0)*cos(0.5*yaw0)+
cos(0.5*roll0)*sin(0.5*pitch0)*sin(0.5*yaw0)
q3_init=cos (0.5*roll0)*cos(0.5*pitch0)*sin(0.5*yaw0)+
sin(0.5*roll0)*sin(0.5*pitch0)*cos(0.5*yaw0)
Wherein, Roll is roll angle, Pitch is that pitch angle is skilful, Yaw is course angle, and ax, ay, az is the axis that accelerometer measures
Acceleration value, mx, my, mz are the three axis magnetic induction intensity values that gyroscope measures.
4. the Multi-sensor Fusion localization method of AGV integrated navigation as described in claim 1, which is characterized in that the data
In processing step, using the fourth order Runge-Kutta algorithm in numerical integration.The differential equation is as follows:
Wherein Q (t) is attitude quaternion,Angular speed for b system with respect to n system, quaternary number solution are as follows:
Indicate geographic coordinate system (n) to the attitude matrix carrier coordinate system (b) by the quaternary number solved are as follows:
The attitude angle of carrier is calculated according to the quaternion algebra value and attitude matrix for cutting upper solution are as follows:
Pitch=arcsinC32, Pitch ∈ (- 90 °, 90 °)
5. the Multi-sensor Fusion localization method of AGV integrated navigation according to any one of claims 1-4, which is characterized in that
The method also includes two-wheel differentials to drive AGV motion model algorithm, comprising:
Obtain AGV position of form center P, course angle θ, spacing d between two drive shafts, driving wheel radius r, between centers line must point M, sit
It is designated as (xM,yM) P and M distance l, straight line PM and the central axes AGV angle β, two-wheeled angular velocity of rotation is wLAnd wR；
Kinematics operation: M spot speed and angular speed are released by left and right two-wheeled angular speed and radius of wheel；
Motion model is established.
6. the Multi-sensor Fusion localization method of AGV integrated navigation as claimed in claim 5, which is characterized in that the movement
It learns in calculation step, calculation formula are as follows:
The position coordinates of P point can be released by it with positional relationship, calculation formula are as follows:
xp=xM+lcos(θ+β)
yP=yM+lsin(θ+β)
The position equation of P point obtains the speed of P point, calculation formula to time derivation are as follows:
7. the Multi-sensor Fusion localization method of AGV integrated navigation as claimed in claim 6, which is characterized in that the movement
In model foundation step, motion model formula:
8. the Multi-sensor Fusion localization method of AGV integrated navigation according to any one of claims 1-4, which is characterized in that
The method also includes assisted location methods, comprising:
Correction mark is laid with to geographic coordinate system (n)；
Each the correction mark being scanned across；
Corresponding mark data is updated, range deviation e is obtained and is removed.
9. the Multi-sensor Fusion localization method of AGV integrated navigation as claimed in claim 8, which is characterized in that the laying
Correction be identified as two dimensional code.
10. the Multi-sensor Fusion localization method of AGV integrated navigation as claimed in claim 8, which is characterized in that the update
Corresponding mark data obtains range deviation e and removes in step, the range deviation e of AGV are as follows:
Wherein: x is the corresponding actual range of each pixel.As e>o, vehicle is on the right side of leading line, and as e<o, vehicle is being led
On the left of course line, to correct position and the course angle of AGV.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810825951.0A CN109141410B (en) | 2018-07-25 | 2018-07-25 | Multi-sensor fusion positioning method for AGV (automatic guided vehicle) combined navigation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201810825951.0A CN109141410B (en) | 2018-07-25 | 2018-07-25 | Multi-sensor fusion positioning method for AGV (automatic guided vehicle) combined navigation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109141410A true CN109141410A (en) | 2019-01-04 |
| CN109141410B CN109141410B (en) | 2020-09-01 |
Family
ID=64798929
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201810825951.0A Active CN109141410B (en) | 2018-07-25 | 2018-07-25 | Multi-sensor fusion positioning method for AGV (automatic guided vehicle) combined navigation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109141410B (en) |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110398251A (en) * | 2019-08-16 | 2019-11-01 | 北京邮电大学 | A trackless navigation AGV positioning system and positioning method based on multi-sensor fusion |
| CN111007863A (en) * | 2019-12-06 | 2020-04-14 | 广州市申迪计算机系统有限公司 | Method and device for measuring course angle of robot and storage medium |
| CN111060126A (en) * | 2019-12-31 | 2020-04-24 | 东软睿驰汽车技术（沈阳）有限公司 | Positioning method and device and vehicle |
| CN111323043A (en) * | 2020-03-26 | 2020-06-23 | 深圳市创客火科技有限公司 | Sensor data processing method and system |
| CN111443735A (en) * | 2020-03-25 | 2020-07-24 | 浙江大华技术股份有限公司 | Method, device and system for maintaining posture of vehicle-mounted pan-tilt camera |
| CN111590572A (en) * | 2020-05-15 | 2020-08-28 | 深圳国信泰富科技有限公司 | Robot posture updating method and system |
| CN113494910A (en) * | 2020-04-02 | 2021-10-12 | 广州汽车集团股份有限公司 | Vehicle positioning method and device based on UWB positioning and storage medium |
| CN113752883A (en) * | 2021-08-11 | 2021-12-07 | 镇江默勒电器有限公司 | An AGV material distribution vehicle positioning system based on high-speed information communication |
| CN115560756A (en) * | 2022-08-26 | 2023-01-03 | 北京开拓航宇导控科技有限公司 | Miniature self-seeking missile strapdown navigation method under launching coordinate system |
| CN116592876A (en) * | 2023-07-17 | 2023-08-15 | 北京元客方舟科技有限公司 | Positioning device and positioning method thereof |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120265441A1 (en) * | 2009-11-24 | 2012-10-18 | Marcel Gregorius Anthonius Ruizenaar | Navigation system, navigation device, navigation server, vehicle provided with a navigation device, group of such vehicles and navigation method |
| CN103900569A (en) * | 2014-03-28 | 2014-07-02 | 哈尔滨工程大学 | Method for measuring integrated navigation attitude of micro-inertia navigation, DGPS (Differential Global Positioning System) and electronic compass |
| CN106225784A (en) * | 2016-06-13 | 2016-12-14 | 国家海洋局第二海洋研究所 | Based on low cost Multi-sensor Fusion pedestrian's dead reckoning method |
| CN107560613A (en) * | 2017-08-15 | 2018-01-09 | 江苏科技大学 | Trajectory Tracking System and method in robot chamber based on nine axle inertial sensors |
-
2018
- 2018-07-25 CN CN201810825951.0A patent/CN109141410B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20120265441A1 (en) * | 2009-11-24 | 2012-10-18 | Marcel Gregorius Anthonius Ruizenaar | Navigation system, navigation device, navigation server, vehicle provided with a navigation device, group of such vehicles and navigation method |
| CN103900569A (en) * | 2014-03-28 | 2014-07-02 | 哈尔滨工程大学 | Method for measuring integrated navigation attitude of micro-inertia navigation, DGPS (Differential Global Positioning System) and electronic compass |
| CN106225784A (en) * | 2016-06-13 | 2016-12-14 | 国家海洋局第二海洋研究所 | Based on low cost Multi-sensor Fusion pedestrian's dead reckoning method |
| CN107560613A (en) * | 2017-08-15 | 2018-01-09 | 江苏科技大学 | Trajectory Tracking System and method in robot chamber based on nine axle inertial sensors |
Cited By (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110398251A (en) * | 2019-08-16 | 2019-11-01 | 北京邮电大学 | A trackless navigation AGV positioning system and positioning method based on multi-sensor fusion |
| CN111007863B (en) * | 2019-12-06 | 2023-05-02 | 广州市申迪计算机系统有限公司 | Robot course angle measuring method, device and storage medium |
| CN111007863A (en) * | 2019-12-06 | 2020-04-14 | 广州市申迪计算机系统有限公司 | Method and device for measuring course angle of robot and storage medium |
| CN111060126A (en) * | 2019-12-31 | 2020-04-24 | 东软睿驰汽车技术（沈阳）有限公司 | Positioning method and device and vehicle |
| CN111060126B (en) * | 2019-12-31 | 2022-06-07 | 东软睿驰汽车技术（沈阳）有限公司 | Positioning method and device and vehicle |
| CN111443735A (en) * | 2020-03-25 | 2020-07-24 | 浙江大华技术股份有限公司 | Method, device and system for maintaining posture of vehicle-mounted pan-tilt camera |
| CN111443735B (en) * | 2020-03-25 | 2023-10-24 | 浙江大华技术股份有限公司 | Method, device and system for maintaining posture of vehicle-mounted pan-tilt camera |
| CN111323043A (en) * | 2020-03-26 | 2020-06-23 | 深圳市创客火科技有限公司 | Sensor data processing method and system |
| CN113494910A (en) * | 2020-04-02 | 2021-10-12 | 广州汽车集团股份有限公司 | Vehicle positioning method and device based on UWB positioning and storage medium |
| CN111590572A (en) * | 2020-05-15 | 2020-08-28 | 深圳国信泰富科技有限公司 | Robot posture updating method and system |
| CN111590572B (en) * | 2020-05-15 | 2021-05-04 | 深圳国信泰富科技有限公司 | Robot posture updating method and system |
| CN113752883A (en) * | 2021-08-11 | 2021-12-07 | 镇江默勒电器有限公司 | An AGV material distribution vehicle positioning system based on high-speed information communication |
| CN113752883B (en) * | 2021-08-11 | 2024-06-28 | 镇江默勒电器有限公司 | AGV material delivery car positioning system based on high-speed information communication |
| CN115560756A (en) * | 2022-08-26 | 2023-01-03 | 北京开拓航宇导控科技有限公司 | Miniature self-seeking missile strapdown navigation method under launching coordinate system |
| CN116592876A (en) * | 2023-07-17 | 2023-08-15 | 北京元客方舟科技有限公司 | Positioning device and positioning method thereof |
| CN116592876B (en) * | 2023-07-17 | 2023-10-03 | 北京元客方舟科技有限公司 | Positioning device and positioning method thereof |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109141410B (en) | 2020-09-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109141410A (en) | The Multi-sensor Fusion localization method of AGV integrated navigation | |
| CN107389028B (en) | A three-dimensional coordinate conversion method and device based on coordinate projection | |
| Xiong et al. | G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving | |
| JP4184351B2 (en) | Method and apparatus for determining geomagnetism using compass and method and apparatus for determining azimuth angle of moving object | |
| CN205066775U (en) | High accuracy movement track detection device | |
| JP2005140789A (en) | Method and system for estimating traveling direction of movable body | |
| CN103712622B (en) | The gyroscopic drift estimation compensation rotated based on Inertial Measurement Unit and device | |
| CN108759822B (en) | Mobile robot 3D positioning system | |
| CN109186597A (en) | A kind of localization method of the indoor wheeled robot based on double MEMS-IMU | |
| CN108535789A (en) | A kind of foreign matter identifying system based on airfield runway | |
| CN112862818B (en) | Underground parking lot vehicle positioning method combining inertial sensor and multi-fisheye camera | |
| CN106979779A (en) | A kind of unmanned vehicle real-time attitude measuring method | |
| CN107228663A (en) | The alignment system and method for a kind of automatical pilot transportation vehicle | |
| CN113703446A (en) | Magnetic nail-based guidance vehicle navigation method and scheduling system | |
| CN108955683A (en) | Localization method based on overall Vision | |
| CN207540557U (en) | A kind of device pinpoint in short-term for AGV trolleies | |
| Sorensen et al. | On-line optical flow feedback for mobile robot localization/navigation | |
| Peng et al. | A novel algorithm based on nonlinear optimization for parameters calibration of wheeled robot mobile chasses | |
| CN110030995A (en) | The intelligent carriage control method and system of blending image sensor and inertial sensor | |
| CN109375631A (en) | A kind of system and method for realizing high speed magnetic navigation identification | |
| Ye et al. | A vision-based guidance method for autonomous guided vehicles | |
| JP5748174B2 (en) | Method and apparatus for measuring relative posture of moving object | |
| CN209764110U (en) | Intelligent trolley control system fusing image sensor and inertial sensor | |
| Tao et al. | A fast and accurate camera-IMU calibration method for localization system | |
| CN116972844A (en) | Mobile robot indoor positioning system and method based on ArUco array |
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 |