CN110763224A - Navigation method and navigation system for automatic guided transport vehicle - Google Patents

Navigation method and navigation system for automatic guided transport vehicle Download PDF

Info

Publication number
CN110763224A
CN110763224A CN201911108227.7A CN201911108227A CN110763224A CN 110763224 A CN110763224 A CN 110763224A CN 201911108227 A CN201911108227 A CN 201911108227A CN 110763224 A CN110763224 A CN 110763224A
Authority
CN
China
Prior art keywords
navigation
angle
guided vehicle
data
control
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201911108227.7A
Other languages
Chinese (zh)
Inventor
董朝轶
樊志强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Inner Mongolia University of Technology
Original Assignee
Inner Mongolia University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Inner Mongolia University of Technology filed Critical Inner Mongolia University of Technology
Priority to CN201911108227.7A priority Critical patent/CN110763224A/en
Publication of CN110763224A publication Critical patent/CN110763224A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0259—Control of position or course in two dimensions specially adapted to land vehicles using magnetic or electromagnetic means

Abstract

The invention provides a navigation method and a navigation system for an automatic guided transport vehicle, wherein the navigation method comprises the following steps: acquiring original navigation data, removing zero offset of the original data, and acquiring the current running direction of the automatic guided vehicle; obtaining the deviation angle between the current running direction of the automatic guided vehicle and the preset track direction through the position of preset equipment, and calibrating the navigation angle; forming a navigation track according to the mileage data and the calibrated navigation angle; and controlling the motion of the automatic guided transporting vehicle based on the navigation track and the control algorithm. The method adopts inertial navigation as a leading method and a magnetic nail correction method to improve the precision, and some complex control algorithms, such as: fuzzy control, fuzzy PID, self-adaptive control, robust control and the like can be realized on the AGV, the tracking precision of the AGV is improved, the control precision of various control algorithms can be compared, and the intellectualization of factory production is greatly improved.

Description

Navigation method and navigation system for automatic guided transport vehicle
Technical Field
The invention relates to the technical field of automatic guided vehicles, in particular to a navigation method and a navigation system of an automatic guided vehicle.
Background
AGVs are an Automated Guided Vehicle, which is an abbreviation used for Automated Guided vehicles, and refer to a Vehicle equipped with an automatic guidance device such as an electromagnetic, optical, or gyro device, capable of traveling along a predetermined guidance path, and having safety protection and various transfer functions. Compared with other equipment commonly used in material conveying, the AGV has the advantages that fixing devices such as rails and supporting frames do not need to be laid in the moving area of the AGV, and the AGV is not limited by sites, roads and spaces. Therefore, in the automatic logistics system, the automation and the flexibility can be fully embodied, and the efficient, economical and flexible unmanned production is realized. At present, the main navigation mode of a factory is magnetic navigation, and the more popular navigation modes in recent years are radar navigation, inertial navigation and visual navigation. Emerging navigation approaches have great potential, and since the technology is not so mature, there are many insufficient places worth further research.
The single magnetic navigation precision is not high, and the maintenance of magnetic stripe is more frequent, damages easily, and single laser radar easily receives the influence of weather, light, and single inertial navigation gyroscope can accumulate the error, and single vision can face the damage of reference coordinate point, and the technique is not very mature yet, is in the stage of research and development.
Disclosure of Invention
The invention aims to provide a navigation method and a navigation system of an automatic guided vehicle, which aim to solve the problems that the single-magnet navigation precision of the existing automatic guided vehicle is not high, the maintenance of a magnetic strip is frequent, and a single inertial navigation gyroscope can accumulate errors.
According to a first aspect of the present invention, there is provided an automated guided vehicle navigation method, comprising:
acquiring original navigation data, removing zero offset of the original navigation data, and acquiring the current running direction of the automatic guided vehicle;
obtaining the deviation angle between the current running direction of the automatic guided vehicle and the preset track direction according to the position of preset equipment, and calibrating the navigation angle;
forming a navigation track according to the mileage data and the calibrated navigation angle;
and controlling the motion of the automatic guided transporting vehicle based on the navigation track and the control algorithm.
Optionally, the raw data includes raw data of a gyroscope, raw data of an accelerometer, and raw data of a magnetometer on the automated guided vehicle;
correspondingly, the acquiring of the original navigation data, and the removing of the zero offset of the original navigation data to obtain the current running direction of the automated guided vehicle includes:
and solving out an attitude angle as an estimator from the data quaternion of the gyroscope without zero offset, and performing Kalman filtering by taking the angle in the original data measured by the accelerometer and the magnetometer as an observed quantity to obtain the attitude angle of the automatic guided vehicle.
Optionally, the forming a navigation track according to the mileage data and the calibrated navigation angle includes:
the position and the posture of the automatic guided vehicle at the moment K are (X (K), y (K) and phi (K)), phi is an included angle between the heading and the self-set X-axis direction, and the posture at the moment K +1 is as follows:
Figure BDA0002271955500000021
wherein Δ d (k) is the incremental mileage obtained by the encoder; delta phi (k) is measured by a gyroscope to obtain an increment angle, so that the positions and postures of all sampling moments are calculated through iteration to form a navigation track.
Optionally, the motion control of the automated guided vehicle based on the navigation track and the control algorithm includes:
taking the position point of the automatic guided vehicle at the current moment and the given position point as the input of a control regulator to obtain an increment angle as the output;
the deviation angle is converted into motion input, and the attitude of the automatic guided vehicle is adjusted by using a control algorithm.
Optionally, the control algorithm includes: a fuzzy control algorithm, a fuzzy PID algorithm, an adaptive control algorithm and a robust control algorithm.
Optionally, the calculating an attitude angle by using the data quaternion without zero offset of the gyroscope as an estimator, and performing kalman filtering by using an angle in raw data measured by the accelerometer and the magnetometer as an observed quantity to obtain the attitude angle of the automated guided vehicle includes:
Figure BDA0002271955500000031
wherein: x is the number ofkIs a state vector; y iskIs an observation vector; a is a state transition matrix from k-1 to k; u. ofkInputting a control vector for the system; b is a gain matrix of the input control vector; h is a gain matrix from the state quantity to the observed quantity; w is akIs the input noise; v. ofkTo observe the noise. The input noise and the observation noise are subjected to normal distribution, the covariance of the input noise is Q, the covariance of the observation noise is R, and then the Kalman filtering recursion formula is as follows:
pre-estimation of state quantities:
Figure BDA0002271955500000032
error covariance pre-estimation:
Pk|k-1＝APk-1AT+Q (4)
updating Kalman gain:
Kk＝Pk|k-1HT[HPk|k-1HT+R]-1(5)
updating the current state estimated value:
Figure BDA0002271955500000033
error covariance update:
Pk＝(I-KkH)Pk|k-1(7)
wherein:
Figure BDA0002271955500000034
for the actual value X of the current statek(ii) an estimate of (d);
Figure BDA0002271955500000035
is based on k-1 pairingsk times of pre-estimation; a is a state transition matrix from k-1 times to k times; b is a gain matrix of the human input control vector; h is a gain matrix from the state quantity to the observed quantity; y iskObserved quantity is k times; kkTo observe the deviation
Figure BDA0002271955500000041
The correction weighting of (1), namely a Kalman gain matrix is a key factor directly influencing the accuracy of state estimation; pkAn error covariance matrix of the current state estimation value; pk|k-1Is a pre-estimated error covariance matrix; and I is an identity matrix.
Optionally, before forming the navigation track according to the mileage data and the calibrated navigation angle, the method further includes:
acquiring an attitude angle obtained through quaternion resolving and Kalman filtering, acquiring preset equipment acquired by a magnetic sensor and distributed on a navigation route, and acquiring the position of the preset equipment, so that an encoder calculates mileage according to the pulse number of a driving motor, and acquiring mileage data.
Optionally, the step of taking the position point of the automated guided vehicle at the current moment and the given position point as inputs of the control regulator to obtain an incremental angle as an output includes:
if the given coordinate at time k is (x)m(k),ym(k) And the actual position and posture are (x (k), y (k)), and the output deviation adopts a polar coordinate mode as follows:
Figure BDA0002271955500000042
Δφ＝atan(y(k)-ym(k))(x(k)-xm(k)) (8)
according to a second aspect of the present invention, there is provided a navigation system for performing the automated guided vehicle navigation method described above, comprising:
the system comprises a magnetic navigation module, an inertial navigation module, a card reader module, an obstacle avoidance module, a human-computer interaction module, a microcontroller module and a motion control module;
the magnetic navigation module, the inertial navigation module, the card reader module, the obstacle avoidance module, the human-computer interaction module and the motion control module are all connected with the microcontroller module.
The invention provides a navigation method and a navigation system of an automatic guided transport vehicle, the method adopts inertial navigation as the leading and magnetic nail correction methods to improve the precision, and some complex control algorithms, such as: fuzzy control, fuzzy PID, self-adaptive control, robust control and the like can be realized on the AGV, the tracking precision of the AGV is improved, the control precision of various control algorithms can be compared, and the intellectualization of factory production is greatly improved.
Drawings
Other features, objects and advantages of the invention will become more apparent upon reading of the detailed description of non-limiting embodiments made with reference to the following drawings:
fig. 1 is a schematic flow chart illustrating an automated guided vehicle navigation method according to an embodiment of the present invention;
fig. 2 is a schematic flow chart illustrating development of an automated guided vehicle integrated navigation system according to an embodiment of the present invention;
FIG. 3 is a schematic diagram of a magnetic pin arrangement according to an embodiment of the present invention;
FIG. 4 is a schematic diagram of a magnetic pin calibration provided in accordance with an embodiment of the present invention;
FIG. 5 is a schematic diagram of dead reckoning according to an embodiment of the present invention;
FIG. 6 is a schematic diagram of a motion control provided by an embodiment of the present invention;
fig. 7 is a schematic diagram of the overall control of the automated guided vehicle according to an embodiment of the present invention;
fig. 8 is a structural diagram of an automated guided vehicle system according to an embodiment of the present invention.
Detailed Description
The present invention is described in further detail below with reference to the attached drawing figures.
Fig. 1 is a schematic flow chart of an automated guided vehicle navigation method according to an embodiment of the present invention, as shown in fig. 1, the method includes:
101. acquiring original navigation data, removing zero offset of the original navigation data, and acquiring the current running direction of the automatic guided vehicle;
102. obtaining the deviation angle between the current running direction of the automatic guided vehicle and the preset track direction according to the position of preset equipment, and calibrating the navigation angle;
in this embodiment, the predetermined equipment is a device that can be marked, and this embodiment is described by taking a magnetic nail as an example.
103. Forming a navigation track according to the mileage data and the calibrated navigation angle;
104. and controlling the motion of the automatic guided transporting vehicle based on the navigation track and the control algorithm.
The method provided by the embodiment adopts the inertial navigation as the leading part and the magnetic nail correction method to improve the precision, and some complex control algorithms, such as: fuzzy control, fuzzy PID, self-adaptive control, robust control and the like can be realized on the AGV, the tracking precision of the AGV is improved, the control precision of various control algorithms can be compared, and the intellectualization of factory production is greatly improved.
The raw data in step 101 comprises raw data of a gyroscope, raw data of an accelerometer and raw data of a magnetometer on an automated guided vehicle;
correspondingly, the acquiring of the original navigation data, and the removing of the zero offset of the original navigation data to obtain the current running direction of the automated guided vehicle includes:
and solving out an attitude angle as an estimator from the data quaternion of the gyroscope without zero offset, and performing Kalman filtering by taking the angle in the original data measured by the accelerometer and the magnetometer as an observed quantity to obtain the attitude angle of the automatic guided vehicle.
Motion control of the automated guided vehicle based on the navigation track and the control algorithm in step 104 comprises:
taking the position point of the automatic guided vehicle at the current moment and the given position point as the input of a control regulator to obtain an increment angle as the output;
the deviation angle is converted into motion input, and the attitude of the automatic guided vehicle is adjusted by using a control algorithm.
Before forming a navigation track from the mileage data and the calibrated navigation angle in step 103, the method further comprises:
acquiring an attitude angle obtained through quaternion resolving and Kalman filtering, acquiring preset equipment acquired by a magnetic sensor and distributed on a navigation route, and acquiring the position of the preset equipment, so that an encoder calculates mileage according to the pulse number of a driving motor, and acquiring mileage data.
The method of the above example will be described in detail below by means of specific procedures.
The detailed steps are as follows:
the AGV combined navigation system based on inertial navigation and magnetic nail correction is built by utilizing an aluminum square tube and an iron sheet to build a suggested frame, installing wheels and a motor, installing an encoder on the motor, installing a magnetic sensor on a central axis in front of the AGV, installing an RFID card reader at the center of a vehicle body, installing a microcontroller and a motor in the vehicle body, and connecting circuits of all parts.
The STM32 collects inertial sensor data, acquires raw data of the gyroscope, accelerometer, magnetometer, and de-zero biases the raw data.
And solving out an attitude angle as an estimator by using a data quaternion of the gyroscope with zero offset, and performing Kalman filtering by using angles measured by the accelerometer and the magnetometer as observed quantities.
Figure BDA0002271955500000071
Wherein: x is the number ofkIs a state vector; y iskIs an observation vector; a is a state transition matrix from k-1 to k; u. ofkInputting a control vector for the system; b is a gain matrix of the input control vector; h is a gain matrix from the state quantity to the observed quantity; w is akIs the input noise; v. ofkTo observe the noise. The input noise and the observation noise are both subject to normal distribution, and the covariance of the input noiseThe difference is Q, and the covariance of the observed noise is R, then the kalman filter recursion formula can be summarized into 5 core formulas:
pre-estimation of state quantities:
Figure BDA0002271955500000072
error covariance pre-estimation:
Pk|k-1＝APk-1AT+Q (3)
updating Kalman gain:
Kk＝Pk|k-1HT[HPk|k-1HT+R]-1(4)
updating the current state estimated value:
Figure BDA0002271955500000081
error covariance update:
Pk＝(I-KkH)Pk|k-1(6)
wherein:
Figure BDA0002271955500000082
for the actual value X of the current statek(ii) an estimate of (d);is based on k-1 pre-estimation of k times; a is a state transition matrix from k-1 times to k times; b is a gain matrix of the human input control vector; h is a gain matrix from the state quantity to the observed quantity; y iskObserved quantity is k times; kkTo observe the deviation
Figure BDA0002271955500000084
The correction weighting of (1), namely a Kalman gain matrix is a key factor directly influencing the accuracy of state estimation; pkAn error covariance matrix of the current state estimation value; pk|k-1Is a pre-estimated error covariance matrix; and I is an identity matrix.
(4) Navigation data acquisition, STM32 gather the attitude angle that obtains through quaternion resolving and Kalman filtering, and the magnetic sensor gathers the magnetism nail that distributes on the navigation route, acquires the position of magnetism nail, and the encoder can calculate the mileage according to driving motor's the pulse number.
The calculation formula of the encoder is as follows: length corresponding to one pulse sent by encoder is circle rate diameter/encoder line number
Length corresponding to one pulse sent by encoder is wheel circumference/encoder line number is pulse equivalent
(5) Performing secondary calibration on the magnetic nails, and calibrating the magnetic nails back to the preset track by the magnetic sensor according to the acquired positions of the magnetic nails to obtain the deviation angle between the current running direction of the AGV and the preset track direction; and if the deviation angle exceeds the set range, determining that the AGV runs off the rails.
The calibration principle of the magnetic nail is as follows: a pair of magnetic nails is arranged at the straight line of the running track, and the distance between the two magnetic nails is D. As shown in fig. 3, a1, a2 are arranged magnetic pegs. The magnetic sensors detect the positions of the A1 and A2 magnetic nails respectively, the 16 detection points of the magnetic sensor are used, as shown in FIG. 4, the distance between a pair of magnetic nails and the center of the magnetic sensor is detected, so that the deviation angle between the current running direction of the AGV and the preset track direction is calculated, and the calibration is carried out when the angle is within the set range. The larger the distance between the pair of magnetic nails is, the smaller the resolution of the measurement angle is, the larger the distance between the magnetic nails cannot be, otherwise, the phenomenon that two magnetic nails cannot be read simultaneously occurs, and if the second magnetic nail is not read, the calibration angle is considered to be large, namely, the AGV derails.
Deviation angle:
φ＝arcsin(|dA1-dA2|D) (7)
(6) and (3) dead reckoning, namely forming a polar coordinate relation according to the mileage data obtained by the encoder and the angle data of the inertial navigation after calibration, and calculating the positions and postures of all sampling moments to form a dead reckoning.
The track estimation is as shown in fig. 5, the track estimation forms a polar coordinate relationship according to the mileage data of the encoder and the calibrated gyroscope data, and if the position and the posture of the AGV at the time K are (X), (K), y (K), and phi (K)), where phi is an included angle between the heading and the X-axis direction set by the AGV, the posture at the time K +1 is:
x(k+1)＝x(k)+ΔdcosΔφ(k)
y(k+1)＝y(k)+ΔdsinΔφ(k) (8)
φ(k+1)＝φ(k)+Δφ(k)
wherein Δ d (k) is the incremental mileage obtained by the encoder; delta phi (k) is measured by a gyroscope to obtain an increment angle, and the positions and postures of all sampling moments are calculated through iteration so as to form a track.
(7) And (4) adjusting by using a controller, wherein the position point at the current moment and the given position point are used as the input of the control adjuster, and the increment angle is obtained as the output.
If the coordinates given at time k are (x) in the control regulationm(k),ym(k) And the actual position and posture are (x (k), y (k)), and the output deviation is expressed in a polar coordinate system at this time
Δφ＝atan(y(k)-ym(k))(x(k)-xm(k)) (9)
If the input is not deviated, the actual track of the AGV is superposed with the given track, namely delta phi is equal to 0; on the contrary, if the delta theta is not zero, deviation exists, the deviation rectifying algorithm adopts a control algorithm, if the angle of the AGV is consistent after adjustment, but the coordinates are inconsistent, namely x is inconsistent, so that the AGV is inevitably superposed with the given coordinates after running for a period of time; however, if the AGV has not reached the previous set point in the current period adjustment, the AGV's offset input is based on the current set point.
(8) And motion control, namely converting the deviation angle into motion input, and adjusting the posture of the AGV by utilizing a PID control algorithm, wherein the control algorithm can be fuzzy control, fuzzy PID control, self-adaptive control and robust control. FIG. 6 is an AGV control schematic. Motion control needs to be controlled according to an AGV motion model, and the control mode has two kinds: the AGV is a differential model, then the differential speed of the left motor and the right motor is controlled according to the angle deviation, and the AGV is a steering wheel, then the angle of the steering wheel is directly controlled. Both options are available depending on the design of the AGV chassis mechanics.
The AGV based on inertial navigation and magnetic pin correction utilizes an inertial navigation related algorithm to design a low-cost inertial component AGV, can improve navigation precision by using a gyroscope with higher precision on the basis of algorithm realization, has better performance, does not depend on external signals, can output attitude and position information in real time and high precision, can autonomously perform positioning and navigation, and is not limited by regions. And the vehicle body is designed with six wheels of automatic guiding transport vehicle driven by differential speed, two driving wheels and four driven wheels. The control system takes STM32 as a core, and realizes the driving steering of the AGV by controlling a direct current brushless motor and processing a gyroscope signal. Analyzing the structure of the AGV, establishing a mathematical motion model, providing a dead-reckoning formula, and forming an operation route; the motion of the AGV is controlled through a straight line walking deviation rectifying algorithm, the motion direction angle is calculated through the angular rate fed back by the gyroscope signal, the running direction angle of the AGV is obtained, and therefore the running direction of the automatic guided transport vehicle is corrected. Aiming at the accumulated error of a navigation system consisting of a gyroscope and an encoder, a secondary accurate positioning mode is adopted, namely, a magnetic nail and a magnetic sensor are adopted to perform secondary positioning on the AGV on the basis of primary positioning of gyroscope orientation and encoder feedback. A pair of magnetic nails is arranged on a running path at intervals, and the magnetic nails are detected by a magnetic sensor and subjected to pose correction.
The method has the main advantages of high positioning precision, flexibility, easy combination with other navigation modes, and good application prospect in the combined navigation mode of inertial navigation and magnetic nails. The AGV has the characteristics of high automation integration level, good flexibility, high reliability, automatic navigation operation, convenience in installation and use and the like, and is frequently applied to places with frequent material transportation, such as tobacco, automobile manufacturing, warehouse logistics, airports, docks and the like in developed countries such as Europe and America, so the inertial navigation AGV researched by the text has a certain engineering application value, and is particularly easy to realize in some industrial fields.
An automated guided AGV based on a gyroscope combines gyroscope orientation, encoder feedback, and secondary accurate positioning of a magnetic sensor and magnetic pins. In order to calibrate the inertial navigation system and the accumulated error of the encoder and mark the moving path of the robot, a magnetic pin is arranged at intervals on the path. And controlling the robot to move straightly between the two magnetic nails according to the heading provided by the inertial navigation system, calibrating the inertial navigation heading error and the robot position error according to the magnetic nail information at the magnetic nail position, and adjusting the heading of the robot. The card reader module realizes the functions of the station, such as starting and stopping, loading and unloading, turning and the like. Fig. 7 is a control schematic diagram for describing in detail whether the underlying controller drives the motor to perform navigation after obtaining the deviation signal by reading data of the magnetic sensor and the inertial navigation sensor, or the function of a part of the upper computer, which is not described in detail herein.
Fig. 8 shows a schematic diagram of a navigation system for performing the automated guided vehicle navigation method described above, comprising:
the system comprises a magnetic navigation module, an inertial navigation module, a card reader module, an obstacle avoidance module, a human-computer interaction module, a microcontroller module and a motion control module;
the magnetic navigation module, the inertial navigation module, the card reader module, the obstacle avoidance module, the human-computer interaction module and the motion control module are all connected with the microcontroller module.
The implementation of all modules in the system is the same as the modules in the above automated guided vehicle navigation method, and the detailed description of the embodiment is omitted.
It should be noted that the present invention may be implemented in software and/or in a combination of software and hardware, for example, as an Application Specific Integrated Circuit (ASIC), a general purpose computer or any other similar hardware device. In some embodiments, the software program of the present invention may be executed by a processor to implement the above steps or functions. Also, the software programs (including associated data structures) of the present invention can be stored in a computer readable recording medium, such as RAM memory, magnetic or optical drive or diskette and the like. Further, some of the steps or functions of the present invention may be implemented in hardware, for example, as circuitry that cooperates with the processor to perform various steps or functions.
It will be evident to those skilled in the art that the invention is not limited to the details of the foregoing illustrative embodiments, and that the present invention may be embodied in other specific forms without departing from the spirit or essential attributes thereof. The present embodiments are therefore to be considered in all respects as illustrative and not restrictive, the scope of the invention being indicated by the appended claims rather than by the foregoing description, and all changes which come within the meaning and range of equivalency of the claims are therefore intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim concerned. Furthermore, it is obvious that the word "comprising" does not exclude other elements or steps, and the singular does not exclude the plural. A plurality of units or means recited in the apparatus claims may also be implemented by one unit or means in software or hardware. The terms first, second, etc. are used to denote names, but not any particular order.

Claims (9)

1. An automated guided vehicle navigation method, comprising:
acquiring original navigation data, removing zero offset of the original navigation data, and acquiring the current running direction of the automatic guided vehicle;
obtaining the deviation angle between the current running direction of the automatic guided vehicle and the preset track direction according to the position of preset equipment, and calibrating the navigation angle;
forming a navigation track according to the mileage data and the calibrated navigation angle;
and controlling the motion of the automatic guided transporting vehicle based on the navigation track and the control algorithm.
2. The method of claim 1, wherein the raw data comprises raw data for a gyroscope, raw data for an accelerometer, and raw data for a magnetometer on an automated guided vehicle;
correspondingly, the acquiring of the original navigation data, and the removing of the zero offset of the original navigation data to obtain the current running direction of the automated guided vehicle includes:
and solving out an attitude angle as an estimator from the data quaternion of the gyroscope without zero offset, and performing Kalman filtering by taking the angle in the original data measured by the accelerometer and the magnetometer as an observed quantity to obtain the attitude angle of the automatic guided vehicle.
3. The method of claim 1, wherein forming a navigation track based on the mileage data and the calibrated navigation angle comprises:
the position and the posture of the automatic guided vehicle at the moment K are (X (K), y (K) and phi (K)), phi is an included angle between the heading and the self-set X-axis direction, and the posture at the moment K +1 is as follows:
Figure FDA0002271955490000011
wherein Δ d (k) is the incremental mileage obtained by the encoder; delta phi (k) is measured by a gyroscope to obtain an increment angle, so that the positions and postures of all sampling moments are calculated through iteration to form a navigation track.
4. The method of claim 1, wherein the motion controlling the automated guided vehicle based on the navigation track and the control algorithm comprises:
taking the position point of the automatic guided vehicle at the current moment and the given position point as the input of a control regulator to obtain an increment angle as the output;
the deviation angle is converted into motion input, and the attitude of the automatic guided vehicle is adjusted by using a control algorithm.
5. The method of claim 4, wherein the control algorithm comprises: a fuzzy control algorithm, a fuzzy PID algorithm, an adaptive control algorithm and a robust control algorithm.
6. The method of claim 2, wherein solving the deskewed data quaternion of the gyroscope for attitude angles as an estimator and performing Kalman filtering using the angles in the raw data measured by the accelerometer and magnetometer as observations to obtain attitude angles for the automated guided vehicle comprises:
Figure FDA0002271955490000021
wherein: x is the number ofkIs a state vector; y iskIs an observation vector; a is a state transition matrix from k-1 to k; u. ofkInputting a control vector for the system; b is a gain matrix of the input control vector; h is a gain matrix from the state quantity to the observed quantity; w is akIs the input noise; v. ofkTo observe the noise. The input noise and the observation noise are subjected to normal distribution, the covariance of the input noise is Q, the covariance of the observation noise is R, and then the Kalman filtering recursion formula is as follows:
pre-estimation of state quantities:
Figure FDA0002271955490000022
error covariance pre-estimation:
Pk|k-1＝APk-1AT+Q (4)
updating Kalman gain:
Kk＝Pk|k-1HT[HPk|k-1HT+R]-1(5)
updating the current state estimated value:
Figure FDA0002271955490000031
error covariance update:
Pk＝(I-KkH)Pk|k-1(7)
wherein:
Figure FDA0002271955490000032
for the actual value X of the current statek(ii) an estimate of (d);
Figure FDA0002271955490000033
is based on k-1 pre-estimation of k times; a is a state transition matrix from k-1 times to k times; b is a gain matrix of the human input control vector; h is a gain matrix from the state quantity to the observed quantity; y iskObserved quantity is k times; kkTo observe the deviation
Figure FDA0002271955490000034
The correction weighting of (1), namely a Kalman gain matrix is a key factor directly influencing the accuracy of state estimation; pkAn error covariance matrix of the current state estimation value; pk|k-1Is a pre-estimated error covariance matrix; and I is an identity matrix.
7. The method of claim 2, wherein prior to forming a navigation track based on the mileage data and the calibrated angle of navigation, the method further comprises:
acquiring an attitude angle obtained through quaternion resolving and Kalman filtering, acquiring preset equipment acquired by a magnetic sensor and distributed on a navigation route, and acquiring the position of the preset equipment, so that an encoder calculates mileage according to the pulse number of a driving motor, and acquiring mileage data.
8. The method of claim 4, wherein said taking the current time position point of the automated guided vehicle and the given position point as inputs to a control regulator, deriving as an output an incremental angle, comprises:
if the given coordinate at time k is (x)m(k),ym(k) And the actual position and posture are (x (k), y (k)), and the output deviation adopts a polar coordinate mode as follows:
Δφ＝a tan(y(k)-ym(k))/(x(k)-xm(k)) (8)
9. a navigation system for performing the automated guided vehicle navigation method of any one of claims 1-8, comprising:
the system comprises a magnetic navigation module, an inertial navigation module, a card reader module, an obstacle avoidance module, a human-computer interaction module, a microcontroller module and a motion control module;
the magnetic navigation module, the inertial navigation module, the card reader module, the obstacle avoidance module, the human-computer interaction module and the motion control module are all connected with the microcontroller module.
CN201911108227.7A 2019-11-13 2019-11-13 Navigation method and navigation system for automatic guided transport vehicle Pending CN110763224A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911108227.7A CN110763224A (en) 2019-11-13 2019-11-13 Navigation method and navigation system for automatic guided transport vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911108227.7A CN110763224A (en) 2019-11-13 2019-11-13 Navigation method and navigation system for automatic guided transport vehicle

Publications (1)

Publication Number Publication Date
CN110763224A true CN110763224A (en) 2020-02-07

Family

ID=69337640

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911108227.7A Pending CN110763224A (en) 2019-11-13 2019-11-13 Navigation method and navigation system for automatic guided transport vehicle

Country Status (1)

Country Link
CN (1) CN110763224A (en)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111474938A (en) * 2020-04-30 2020-07-31 内蒙古工业大学 Inertial navigation automatic guided vehicle and track determination method thereof
CN111930126A (en) * 2020-08-20 2020-11-13 北京特种机械研究所 Navigation deviation rectifying method based on differential wheel set AGV
CN112550289A (en) * 2020-12-10 2021-03-26 珠海格力智能装备有限公司 Trajectory deviation rectifying method and device for autonomous navigation vehicle and vehicle control system
CN113320617A (en) * 2021-07-09 2021-08-31 北京优时科技有限公司 Six-wheel differential speed control method and six-wheel differential speed control device
CN113624260A (en) * 2021-08-26 2021-11-09 三一智矿科技有限公司 Odometer pulse equivalent calibration method and device, electronic equipment and storage medium
CN114252013A (en) * 2021-12-22 2022-03-29 深圳市天昕朗科技有限公司 AGV visual identification accurate positioning system based on wired communication mode

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102680997A (en) * 2012-05-31 2012-09-19 东南大学 Magnetic navigation-assisted global positioning system (GPS)/inertial navigation system (INS) combined navigation positioning system and control method thereof
CN103822633A (en) * 2014-02-11 2014-05-28 哈尔滨工程大学 Low-cost attitude estimation method based on second-order measurement update
CN105180934A (en) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 AVG inertial navigation method
CN105180930A (en) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 AGV inertial navigation system
EP3447654A1 (en) * 2017-08-25 2019-02-27 Institut Français des Sciences et Technologies des Transports, de l'Aménagement et des Réseaux Method for determining the trajectory of a moving object, program and device for implementing said method

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102680997A (en) * 2012-05-31 2012-09-19 东南大学 Magnetic navigation-assisted global positioning system (GPS)/inertial navigation system (INS) combined navigation positioning system and control method thereof
CN103822633A (en) * 2014-02-11 2014-05-28 哈尔滨工程大学 Low-cost attitude estimation method based on second-order measurement update
CN105180934A (en) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 AVG inertial navigation method
CN105180930A (en) * 2015-09-16 2015-12-23 成都四威高科技产业园有限公司 AGV inertial navigation system
EP3447654A1 (en) * 2017-08-25 2019-02-27 Institut Français des Sciences et Technologies des Transports, de l'Aménagement et des Réseaux Method for determining the trajectory of a moving object, program and device for implementing said method

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
陈伟: "基于四元数和卡尔曼滤波的姿态角估计算法研究与应用", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111474938A (en) * 2020-04-30 2020-07-31 内蒙古工业大学 Inertial navigation automatic guided vehicle and track determination method thereof
CN111930126A (en) * 2020-08-20 2020-11-13 北京特种机械研究所 Navigation deviation rectifying method based on differential wheel set AGV
CN111930126B (en) * 2020-08-20 2021-06-01 北京特种机械研究所 Navigation deviation rectifying method based on differential wheel set AGV
CN112550289A (en) * 2020-12-10 2021-03-26 珠海格力智能装备有限公司 Trajectory deviation rectifying method and device for autonomous navigation vehicle and vehicle control system
CN113320617A (en) * 2021-07-09 2021-08-31 北京优时科技有限公司 Six-wheel differential speed control method and six-wheel differential speed control device
CN113624260A (en) * 2021-08-26 2021-11-09 三一智矿科技有限公司 Odometer pulse equivalent calibration method and device, electronic equipment and storage medium
CN113624260B (en) * 2021-08-26 2024-02-27 三一智矿科技有限公司 Pulse equivalent calibration method and device for odometer, electronic equipment and storage medium
CN114252013A (en) * 2021-12-22 2022-03-29 深圳市天昕朗科技有限公司 AGV visual identification accurate positioning system based on wired communication mode
CN114252013B (en) * 2021-12-22 2024-03-22 深圳市天昕朗科技有限公司 AGV visual identification accurate positioning system based on wired communication mode

Similar Documents

Publication Publication Date Title
CN110763224A (en) Navigation method and navigation system for automatic guided transport vehicle
CN109358340B (en) AGV indoor map construction method and system based on laser radar
Lee et al. Robust mobile robot localization using optical flow sensors and encoders
US11279045B2 (en) Robot pose estimation method and apparatus and robot using the same
Lee et al. Kinematic parameter calibration of a car-like mobile robot to improve odometry accuracy
Siagian et al. Mobile robot navigation system in outdoor pedestrian environment using vision-based road recognition
EP1548534B1 (en) Method and apparatus for using rotational movement amount of mobile device and computer-readable recording medium for storing computer program
Xiong et al. G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving
Choi et al. Enhanced SLAM for a mobile robot using extended Kalman filter and neural networks
Bento et al. Sensor fusion for precise autonomous vehicle navigation in outdoor semi-structured environments
Pfaff et al. Towards mapping of cities
CN101846734A (en) Agricultural machinery navigation and position method and system and agricultural machinery industrial personal computer
CN111474938A (en) Inertial navigation automatic guided vehicle and track determination method thereof
CN110702091A (en) High-precision positioning method for moving robot along subway rail
Hoang et al. Multi-sensor perceptual system for mobile robot and sensor fusion-based localization
CN108036792A (en) A kind of data fusion method of odometer for mobile robot and measurement pose
CN116337045A (en) High-speed map building navigation method based on karto and teb
Yin et al. Combinatorial inertial guidance system for an automated guided vehicle
Xu et al. Magnetic locating AGV navigation based on Kalman filter and PID control
CN115903857A (en) RFID-based unmanned grain surface inspection device and positioning method
CN110398251B (en) Trackless navigation AGV positioning system based on multi-sensor fusion and positioning method thereof
Kimura et al. Vehicle localization by sensor fusion of LRS measurement and odometry information based on moving horizon estimation
Krejsa et al. Fusion of local and global sensory information in mobile robot outdoor localization task
Lee Mobile robot localization using optical mice
CN112254728A (en) Method for enhancing EKF-SLAM global optimization based on key road sign

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