CN104597905B - Route tracking method for magnetic navigation AGV - Google Patents

Abstract

The invention discloses a path tracking method of a magnetic navigation AGV. The path tracking method includes: step 1, judging whether there are signal outputs at the two points in the middle of the horizontal detection sensor and all points of the longitudinal sensor; step 2, based on step 1, if it is directly skipped to step 7; step 3, based on step 2, If not, then judge which bits of the horizontal sensor have signal output; step 4, based on step 3, judge whether the vertical signal output point has the first point at the same time, if it jumps directly to step 7; step 5; step 6, after The calculated control quantity is output to the two control motors to perform the deviation correction process, and judge whether to enter the approach attitude; step 7, if the condition of the approach attitude is satisfied, the speed of one of the two driving wheels of the car body remains unchanged and the other One wheel reverses at its original speed. The path tracking method greatly improves the control accuracy of the AGV.

Description

Path Tracking Method of Magnetic Navigation AGV

technical field

The invention relates to the field of automatic navigation cars, in particular to a path tracking method of a magnetic navigation AGV.

Background technique

AGV (Automated Guided Vehicle) refers to a transport vehicle equipped with automatic guidance equipment such as electromagnetic or optical, capable of driving along a specified path, with safety protection and various transplanting functions, belonging to wheeled mobile robots category. Most of the existing AGV magnetic navigation control methods use single-row or double-row sensor arrays.

Although such a sensor arrangement can also make the AGV run along the magnetic track, due to the distance between the Hall elements on the sensor, the calculated distance deviation and angle deviation have certain errors; most of the current correction processes are cumbersome. AGV has many state divisions. Although it can correct the deviation in real time, it increases the computational tasks of the controller; when a simple single-row sensor is used, when the AGV is in the middle position, there is no accurate sign to reflect the angular deviation of the AGV at this time. It is possible to cause the AGV to go over the tape, causing the AGV to do a slight wobble near the magnet wire.

The information disclosed in this Background section is only for enhancing the understanding of the general background of the present invention and should not be taken as an acknowledgment or any form of suggestion that the information constitutes the prior art that is already known to those skilled in the art.

Contents of the invention

The object of the present invention is to provide a path tracking method of magnetic navigation AGV with simple and reasonable steps. The path tracking method of this magnetic navigation AGV adopts the sensor group of T-shaped layout, and judges the position of the AGV through the signal output bits of the comprehensive horizontal sensor and the vertical sensor. Attitude: Use different control strategies to bring the AGV into a balanced state under different attitudes, and effectively detect the angular deviation of the AGV when it is approaching the attitude, which greatly improves the control accuracy of the AGV and also improves the dynamic stability.

In order to achieve the above object, the present invention provides a path tracking method for a magnetic navigation AGV. The magnetic navigation AGV adopts a horizontal detection sensor and a vertical sensor in a T-shaped layout, and judges the magnetic navigation AGV by integrating the signal output bits of the horizontal sensor and the vertical sensor. The attitude of the AGV, using different control strategies in different attitudes to bring the AGV into a balanced state;

The path tracking method includes the following steps: step 1, judge whether there are signal outputs at the middle two points of the horizontal detection sensor and all points of the longitudinal sensor; step 2, based on step 1, if not, then judge whether the middle two points of the horizontal detection sensor point and the first point of the vertical detection sensor has signal output and the vertical detection sensor has no signal output point; if it is, jump directly to step 7; step 3, based on step 2, if not, then judge which bits of the horizontal sensor There is signal output; step 4, based on step 3, at the same time judge whether the longitudinal signal output point has the first point, if it jumps directly to step 7; step 5, based on step 4, if the conclusion is no, then calculate the The distance deviation and angle deviation of the vehicle body are calculated, and the deviation correction radius and the motor speed increment of the vehicle body are calculated at the same time; step 6, the calculated control amount is output to the two control motors to perform the deviation correction process, and it is judged whether to enter the approach attitude; step 7. If the condition of approach attitude is satisfied, the speed of one of the two driving wheels of the car body remains unchanged and the other wheel reverses at the original speed until all points of the longitudinal detection sensor have signal output. The rotating speed of driving wheel returns to the speed that the car body of setting advances.

Preferably, in the above technical solution, the state of the magnetic navigation AGV detected by the lateral detection sensor and the longitudinal sensor includes:

The first type of state: balanced attitude, which means that the approaching circle of the sensor group has signal output and all other detection units of the longitudinal sensor have signal output;

The second type of state: extreme attitude means that there is no signal output from the middle two points of the horizontal detection sensor and no signal output from the first point of the longitudinal detection sensor. The deviation correction strategy in this state is to calculate the distance of the AGV at this time deviation and angle deviation, and then calculate the deviation correction radius and control amount;

The third type of state: approaching attitude, at this time, both the middle two points of the horizontal detection sensor and the first point of the longitudinal detection sensor have signal output.

Preferably, in the above-mentioned technical solution, the longitudinal sensor coincides with the center line of the magnetic navigation AGV, and the center line of the magnetic navigation AGV refers to the center line passing through the midpoint of the axes of the two drive wheels in the plane of the trolley chassis and perpendicular to the axes of the two drive wheels. line; the axes of the two drive wheels pass through the first signal detection point of the longitudinal sensor.

Preferably, in the above technical solution, the distance deviation refers to the vertical distance between the point projected on the ground by the first Hall element of the longitudinal sensor and the direction line of the magnetic strip, and the angle deviation refers to the center of the AGV car body The angle between the line and the direction line of the magnetic stripe.

Preferably, in the above technical solution, the two middle points of the transverse sensor and the first point of the longitudinal sensor close to the transverse sensor are all on the circumference of the approximate circle, and the diameter of the approximate circle is the width of the magnetic tape.

Compared with the prior art, the present invention has the following beneficial effects: the path tracking method of the magnetic navigation AGV adopts the sensor group of the T-shaped layout, and judges the posture of the AGV through the signal output bits of the comprehensive lateral sensor and the longitudinal sensor; Using different control strategies to bring the AGV into a balanced state can effectively detect the angle deviation of the AGV when it is approaching the attitude, so that the control accuracy of the AGV is greatly improved, and the dynamic stability is also greatly improved.

Description of drawings

Fig. 1 is a kind of AGV automatic correction control flow chart provided by the present invention;

Fig. 2 is the layout of a T-shaped sensor provided by the present invention;

Fig. 3 is a schematic diagram of an AGV attitude during an extreme attitude provided by the present invention;

Fig. 4 is a schematic diagram of an AGV attitude when approaching an attitude provided by the present invention;

Fig. 5 is a schematic diagram of the posture of an AGV in a balanced position provided by the present invention.

detailed description

The specific embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, but it should be understood that the protection scope of the present invention is not limited by the specific embodiments.

Unless expressly stated otherwise, throughout the specification and claims, the term "comprise" or variations thereof such as "includes" or "includes" and the like will be understood to include the stated elements or constituents, and not Other elements or other components are not excluded.

As shown in Figure 1, the path tracking method of the magnetic navigation AGV according to the specific embodiment of the present invention comprises the following steps:

Step 1, judge whether there are signal outputs at the middle two points of the horizontal detection sensor and all points of the longitudinal sensor;

The sensor group (horizontal detection sensor and longitudinal sensor) used in the path tracking method of the magnetic navigation AGV is in a T-shaped layout; the attitude of the AGV is judged by integrating the signal output bits of the transverse sensor and the longitudinal sensor; different control strategies are used under different attitudes Make the AGV enter a balanced state; the state of the AGV detected by the two sensors includes the following aspects:

The first type of state: balanced attitude, means that the approach circle of the sensor group has signal output and all other detection units of the longitudinal sensor have signal output. At this time, the speed increment △V of the control motor is all zero.

The second type of state: extreme attitude, means that there is no signal output from the middle two points of the horizontal detection sensor and no signal output from the first point of the vertical detection sensor. The deviation correction strategy in this state is to calculate the distance deviation ed and angle deviation eα of the AGV at this time, and then calculate the deviation correction radius R and the control amount △V. Since the scanning frequency of the AGV controller is relatively large, it is The four parameters ed, eα, R, and ΔV in this correction stage vary according to the scanning frequency and the output point of the sensor signal.

The third type of state: approaching attitude, at this time, both the middle two points of the horizontal detection sensor and the first point of the longitudinal detection sensor have signal output.

The longitudinal sensor of the sensor group coincides with the centerline of the dolly, and the centerline of the dolly refers to a line that passes through the midpoint of the axes of the two driving wheels in the plane of the dolly chassis and is perpendicular to the axes of the two driving wheels; The axis of the wheel passes the first signal detection point of the longitudinal sensor (Hall element);

With reference to accompanying drawing 2, take 8 bit magnetosensitive sensor as example, the layout of T-shaped sensor, the middle two points of horizontal sensor 6 and the first point 3 of one end of vertical sensor 2 near the horizontal sensor are on the same circle 7 , and the diameter of the circle 7 is the width of the magnetic tape. In the present invention, the whole composed of these three detection points is called an approximate circle; when the longitudinal sensor 2 is installed, it coincides with the center line 8 of the trolley 1, and the axes of the two driving wheels 5 pass through the approach of the longitudinal sensor 2. First point 3 on the lateral sensor end.

It is advisable that the width of the magnetic tape is exactly two points of signal output at the installation height of the sensor.

The sensor group sends the detected signals to the upper computer for analysis and processing, and the upper computer can be a PLC, a 51 series single-chip microcomputer or a motion controller.

Referring to accompanying drawing 3, after described host computer detects signal, first analyze whether there is signal output at the first point of vertical sensor, if not, then need to calculate the angular deviation _{e α} and distance deviation ed of AGV at this time, its They are calculated as follows:

e _d =L ₂ ·sin(e _α )

_In the above formula, L1 refers to the distance from the average midpoint of the signal output point of the horizontal sensor to the midpoint of the _two middle points, and L2 refers to the midpoint of the signal output point of the longitudinal sensor to its first point distance.

After the host computer calculates _{e α} and ed, it calculates the deviation correction radius R and the motor control amount ΔV in the following way:

In the above formula, L refers to the distance between the two wheels of the AGV, and V refers to the linear velocity of the car body moving.

With reference to Figure 4, after the above-mentioned deviation correction, the AGV will enter the approaching attitude. At this time, the host will give one of the motors a reverse rotation speed, so that the AGV will rotate at an angle until all the detection points of the longitudinal sensor have signal output. .

Combined with Figure 5, when the AGV enters a balanced state, only the two points in the middle of the horizontal sensor have signal output and all points of the longitudinal sensor have signal output. At this time, the signals from the host computer to the two motors are the same, and the AGV moves forward smoothly. .

The first point of the longitudinal sensor has no signal output, and the deviation correction strategy of the AGV under the attitude again is to calculate the distance deviation ed and angle deviation eα of the current AGV in real time, and then calculate the deviation correction radius R and the control amount ΔV, because the AGV The scanning frequency of the controller is relatively high, so the four parameters ed, eα, R, and ΔV in this correction stage are changed according to the scanning frequency and the output point of the sensor signal, which is conducive to the smooth entry of the trolley. a state.

Step 2, based on step 1, if not, then judge whether the middle two points of the horizontal detection sensor and the first point of the vertical detection sensor have signal output and the vertical detection sensor has no signal output point; if it is, jump directly to the step 7;

Step 3, based on step 2, if not, then judge which bits of the horizontal sensor have signal output;

Step 4, based on step 3, at the same time judge whether the vertical signal output point has the first point, if so, jump directly to step 7;

Step 5, based on step 4, if the conclusion is no, calculate the distance deviation ed and angle deviation eα of the vehicle body at this time, and simultaneously calculate the deviation correction radius R of the vehicle body and the motor speed increment ΔV;

The distance deviation refers to the vertical distance between the point projected on the ground by the first Hall element of the longitudinal sensor and the direction line of the magnetic stripe, and the angular deviation refers to the distance between the center line of the AGV body and the magnetic stripe. the angle between the direction lines;

Step 6, the calculated control quantity is output to the two control motors to execute the deviation correction process. And judge whether to enter the approaching attitude.

Step 7, if the condition of approach attitude is met, the speed of one of the two driving wheels of the car body remains unchanged and the other wheel reverses at the original speed until all points of the longitudinal detection sensor have signal output, and the two The rotation speed of the first driving wheel returns to the set forward speed of the car body.

In summary, the path tracking method of the magnetic navigation AGV uses a sensor group with a T-shaped layout, and judges the attitude of the AGV by integrating the signal output bits of the horizontal sensor and the longitudinal sensor; in different attitudes, different control strategies are used to bring the AGV into a balanced state. Effectively solve the detection of the angle deviation of the AGV when it is approaching the attitude, so that the control accuracy of the AGV is greatly improved, and the dynamic stability is also greatly improved.

The foregoing descriptions of specific exemplary embodiments of the present invention have been presented for purposes of illustration and description. These descriptions are not intended to limit the invention to the precise form disclosed, and obviously many modifications and variations are possible in light of the above teaching. The exemplary embodiments were chosen and described in order to explain the specific principles of the invention and its practical application, thereby enabling others skilled in the art to make and use various exemplary embodiments of the invention, as well as various Choose and change. It is intended that the scope of the invention be defined by the claims and their equivalents.

Claims (3)

1. A path tracking method of a magnetic navigation AGV, characterized in that, The magnetic navigation AGV adopts a T-shaped layout of horizontal detection sensors and longitudinal sensors, and judges the attitude of the magnetic navigation AGV through the comprehensive signal output bits of the horizontal sensor and the longitudinal sensor, and uses different control strategies to bring the AGV into balance under different attitudes. state; The path tracing method includes the following steps: Step 1, judge whether there are signal outputs at the middle two points of the horizontal detection sensor and all points of the longitudinal sensor; Step 2, based on step 1, if not, then judge whether the middle two points of the horizontal detection sensor and the first point of the vertical detection sensor have signal output and the vertical detection sensor has no signal output point; if it is, jump directly to the step 7; Step 3, based on step 2, if not, then judge which bits of the horizontal sensor have signal output; Step 4, based on step 3, at the same time judge whether the vertical signal output point has the first point, if so, jump directly to step 7; Step 5, based on step 4, if the conclusion is no, calculate the distance deviation and angle deviation of the vehicle body at this time, and calculate the deviation correction radius and motor speed increment of the vehicle body at the same time; Step 6, the calculated control quantity is output to the two control motors to perform the deviation correction process, and judge whether to enter the approach attitude; Step 7, if the condition of approach attitude is met, the speed of one of the two driving wheels of the car body remains unchanged and the other wheel reverses at the original speed until all points of the longitudinal detection sensor have signal output, and the two The speed of the first driving wheel returns to the set forward speed of the car body; The state of the magnetic navigation AGV detected by the horizontal detection sensor and the longitudinal sensor includes: The first type of state: balanced attitude, which means that the approaching circle of the sensor group has signal output and all other detection units of the longitudinal sensor have signal output; The second type of state: extreme attitude means that there is no signal output from the middle two points of the horizontal detection sensor and no signal output from the first point of the longitudinal detection sensor. The deviation correction strategy in this state is to calculate the distance of the AGV at this time deviation and angle deviation, and then calculate the deviation correction radius and control amount; The third type of state: close to attitude, at this time, the middle two points of the horizontal detection sensor and the first point of the longitudinal detection sensor have signal output; The longitudinal sensor coincides with the center line of the magnetic navigation AGV, and the center line of the magnetic navigation AGV refers to a line passing through the midpoint of the axes of the two driving wheels in the plane of the trolley chassis and perpendicular to the axes of the two driving wheels; The axis of the wheel passes the first signal detection point of the longitudinal sensor. 2. The path tracking method of the magnetic navigation AGV according to claim 1, wherein the distance deviation refers to the vertical distance between the point where the first Hall element of the longitudinal sensor is projected onto the ground and the direction line of the magnetic strip Distance, the angle deviation refers to the angle between the center line of the AGV car body and the direction line of the magnetic strip. 3. The path tracking method of the magnetic navigation AGV according to claim 1, characterized in that, the two points in the middle of the lateral sensor and the first point of the longitudinal sensor close to the lateral sensor are all on the circumference of a circle close to the circle. The diameter is the width of the tape.