An unguided turning algorithm for AGV based on inertial navigation

Abstract

The present invention discloses an AGV non-guided turning algorithm based on inertial navigation, which belongs to the field of AGV control and includes the following steps: Step 1, setting guide information media at the turning point and the turning point, and calculating the preset turning radius and the preset turning angle; Step 2, when the AGV scans the guide information media at the turning point, calculate the lateral offset and angular offset of the turning center of the AGV body before the AGV enters the turn; Step 3, calculate the turning radius of the turning point; Step 4, calculate the angle value required for the AGV steering wheel according to the turning radius of the turning point and the spacing size of the AGV tires; Step 5, when the steering wheel rotation angle of the AGV reaches the requirement, calculate the corrected turning radius according to the change in the center value of the body and the change in the yaw angle; Step 6, during the steering wheel turning process, keep the steering wheel angle unchanged. Step 7, return the rudder. The purpose of the present invention is to provide an AGV non-guided turning algorithm based on inertial navigation, which can ensure that the AGV will not deviate when turning.

Description

Technical Field

[0001] The present invention relates to the field of AGV control, and in particular to an AGV unguided turning algorithm based on inertial navigation. Background Art

[0002] An automatic guided vehicle (AGV) is an industrial vehicle that automatically drives along a set route. Currently, AGVs are mostly guided by setting guide information media such as magnetic nails on the vehicle's running path. That is, the guide information media is detected by the sensors on the vehicle, and then the information is processed to control the vehicle to drive along the guided route. During the guidance process, when encountering a curve, if the guide information media is laid along the curve, the equipment and laying costs will increase because the path is a curve; if the guide information media is only set at the entrance and exit of the curve, such as Figure 1 As shown, the posture of the AGV cannot always face the preset turning point B. It may deviate to the left or to the right. If no adjustment is made and the AGV turns according to the predetermined path, the turning angle will not change. When the AGV reaches the preset turning point C, it will deviate from the preset value, causing the AGV to deviate from the end point E or be unable to continue walking due to failure to recognize the preset guide information media. Summary of the invention

[0003] In view of the problems existing in the prior art, the present invention provides an AGV unguided turning algorithm based on inertial navigation, which can ensure that the AGV will not deviate when turning without a guiding information medium, including the following steps:

[0004] Step 1: Set up guidance information media at the entry and exit points, and calculate the preset turning radius R and the preset turning angle W. The purpose is to provide a reference point for the AGV to turn, ensuring clear navigation basis during the exit, entry and turning process.

[0005] Step 2: When the AGV scans the guidance information medium of the turning point, the lateral offset TraOffset and the angular offset θ before the AGV enters the turn are calculated. By calculating the offset in real time, data support is provided for subsequent posture adjustment.

[0006] Step 3: Calculate the turning radius R1 by using the preset turning radius R, the preset turning angle W, the lateral offset TraOffset and the angle offset θ, and dynamically adjust the turning radius so that the AGV can make precise turns according to the actual offset.

[0007] Step 4: Calculate the angle value Wt that the AGV steering wheel needs to turn according to the turning radius R1, and record the value of Wt to ​​ensure that the steering wheel rotation angle matches the turning radius to improve the accuracy of the turn.

[0008] Step 5. When the steering wheel rotation angle of the AGV reaches Wt, the coordinate information of the AGV will change. At this time, the corrected turning radius R2 is calculated based on the change in the vehicle center value ΔX, ΔY and the change in the yaw angle Δψ. By correcting the turning radius in real time, the path accuracy during the turning process is further ensured.

[0009] Step 6: During the steering wheel turning process, keep correcting the turning radius and keep the steering wheel angle unchanged to maintain stability during the turning process and reduce deviation caused by changes in the steering wheel angle.

[0010] Step 7: When reaching the exit point, turn the rudder according to the symmetry principle of the turning degree to ensure that the AGV can smoothly return to the center after exiting the turn and continue to travel along the predetermined path.

[0011] Furthermore, the AGV deflection direction is judged when calculating the turning radius R1. If the deflection direction is inward, when calculating the turning path of the AVG, in addition to calculating the turning radius R1, the forward distance MoveDis needs to be calculated. If the deflection direction is outward, the forward distance does not need to be calculated. According to the different deflection directions, the turning path is dynamically adjusted to improve the flexibility and accuracy of the turning.

[0012] Furthermore, when the deflection direction is inward, the calculation formula of the AGV forward movement distance MoveDis is:

[0013]

[0014] When the deflection direction is inward, the calculation formula of R1 is:

[0015]

[0016] When the deflection direction is outward, the calculation formula of R1 is:

[0017]

[0018] In the above calculation formula,

[0019] Furthermore, the calculation formula for the corrected turning radius R2 is:

[0020]

[0021] During the operation, b=W-θ, and Δψ is the change in yaw angle.

[0022] Furthermore, when the AGV stops and waits during a turn, the angle value of the gyroscope at the moment the AGV stops is recorded. When the AGV restarts, the error caused by the drift during the parking process of the AGV is eliminated to reduce the error accumulation during the parking process and ensure that the AGV can continue to drive accurately after restarting.

[0023] The beneficial effects of the present invention are as follows: no information media such as magnetic nails are set on the turning path, and information media are only set at the entrance and exit of the curve and the straight path, thereby reducing the difficulty of laying the path and reducing the laying time and cost of the path.

[0024] At the same time, without setting up guidance information media on the turning path, the posture of the AGV when exiting the turn can be ensured by calculating the gyroscope and other data when entering, turning and exiting the turn, avoiding the deviation of the AGV when turning, and improving the navigation performance of the AGV while reducing costs; during the posture adjustment process, the posture and turning radius are adjusted in real time to improve navigation accuracy, while adapting to different path changes and improving the robustness of the entire system. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 This is a schematic diagram of the posture of the AGV when it deviates outward.

[0026] Figure 2 This is a schematic diagram of the posture of the AGV when it is biased inside. DETAILED DESCRIPTION

[0027] In order to enable those skilled in the art to better understand the technical solution, the technical solution is described in detail below in conjunction with the embodiments. The description in this section is only exemplary and explanatory and should not have any limiting effect on the scope of protection of the present invention.

[0028] Embodiment 1, an AGV unguided turning algorithm based on inertial navigation, comprising the following steps:

[0029] Step 1 Figure 1 , Figure 2 Point A is the starting point of the AGV, point B is the turning point, point C is the turning point, point D is the intermediate guide point, point E is the end point, and point O is the center point of the turning circle. Points A, B, C, D, and E are all equipped with guide information media. The box shows the posture of the AGV when it scans the magnetic nail at point B. Figure 1 This is an indication of outward deviation, that is, the midpoint M and C of the AGV head are placed on both sides of point B. Figure 2 This is an illustration of the inner deviation, that is, the midpoint M and point C of the AGV head are located on the same side of point B. In this embodiment, the guide information medium is a magnetic nail. Extend the line between DC and AB, and their intersection point is Q. The lengths between BQ, CQ and BC can be measured by a ruler, and then the preset turning radius R and the preset turning angle W can be calculated by the triangle side length and angle formula.

[0030] Step 2. When the AGV scans the magnetic nail at point B, the lateral offset TraOffset and angular offset θ before the AGV enters the bend are calculated. To this end, the AGV center line L can be made first. The intersection of the center line L and the AGC path before entering the bend (i.e., the line connecting points AB) is N. The lateral offset TraOffset (i.e., the line connecting points BM) and angular offset θ can be obtained through the sensor on the AGV.

[0031] Step 3: Calculate the turning radius R1 of the curve by using the preset turning radius R, the preset turning angle W, the lateral offset TraOffset and the angle offset θ.

[0032] When calculating the turning radius R1, the AGV deflection direction is first determined. If the deflection direction is inward, when calculating the turning path of the AVG, in addition to calculating the turning radius R1, the forward distance MoveDis needs to be calculated. If the deflection direction is outward, the forward distance does not need to be calculated.

[0033] When the deflection direction is inward, the calculation formula of the AGV forward distance MoveDis is:

[0034]

[0035] When the deflection direction is inward, the calculation formula of R1 is:

[0036]

[0037] When the deflection direction is outward, the calculation formula of R1 is:

[0038]

[0039] In the above calculation formula,

[0040] Step 4: Calculate the angle value Wt that the AGV steering wheel needs to turn according to the turning radius R1, and record the value of Wt;

[0041] Step 5: When the steering wheel rotation angle of the AGV reaches Wt, the coordinate information of the AGV will change. At this time, the corrected turning radius R2 is calculated based on the change ΔX, ΔY of the center of the vehicle in the unit time interval t and the change Δψ of the yaw angle;

[0042] The calculation formula for R2 is:

[0043]

[0044] In the operation, b = W - θ, Δψ is the change in yaw angle;

[0045] The values ​​of ΔX and ΔY are calculated by encoder integration. The specific calculation method is: first calculate the change x' and change y' of the center of the vehicle body at the point value t' in the unit time interval. The known quantities are the AGV wheelbase L, the actual AGV speed V, the AGV steering wheel angle β and the current AGV yaw angle ψ, thus obtaining:

[0046]

[0047] In the operation, ψ is the AGV yaw angle, and its value is obtained by analyzing the gyroscope data on the AGV;

[0048] Then the change of the AGV body center value at time 0-t is:

[0049]

[0050] i refers to any point value in the time range of 0-t. Its initial value is 0 and its maximum value is equal to t. In order to obtain the yaw angle change Δψ, also within the time range of 0-t:

[0051] Δψ=ψ t -ψ0.

[0052] Step 6: During the steering wheel turning process, keep the steering wheel angle unchanged, calculate the current turning angle through the gyroscope, and continue driving.

[0053] Step 7. When the AGV is about to turn to the target angle, according to the principle of symmetry, the rudder is turned back when the angle change value of the steering process into the corner is the same. At this time, it can be ensured that the straight line can accurately scan the next two magnetic nails after the corner to ensure that it can continue to move. That is, when the AGV is about to reach point C, according to the principle of symmetry, the rudder is turned back when the angle change value of the steering process into the corner is the same. At this time, it can be ensured that the straight line can accurately scan the magnetic nails at C and D after the corner to ensure that it can move to point E.

[0054] When the AGV stops and waits during a turn, the angle value of the gyroscope at the moment the AGV stops is recorded to obtain ψ 初 After the waiting time is over, the yaw angle obtained by the gyroscope is ψ 当 When the AGV restarts, the error caused by the drift during the AGV parking process is eliminated. The calculation method for elimination is to use ψ 当 Subtract ψ 初 The obtained value is the current yaw angle ψ. The parking waiting error elimination mechanism reduces the error accumulation caused by parking and improves the long-term stability of the system.

[0055] It should be noted that, in this article, the terms "include", "comprises" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. Specific examples are used in this article to illustrate the principle and implementation of the technical solution of the present invention. The above examples are only used to help understand the method of the present invention and its core idea. The above is only a preferred implementation of the present invention. It should be pointed out that due to the limitations of textual expression and the objective existence of infinite specific structures, for ordinary technicians in this technical field, without departing from the principle of the present invention, several improvements, modifications or changes can be made, and the above technical features can also be combined in an appropriate manner; these improvements, modifications, changes or combinations, or the direct application of the concept and technical solution of the present invention to other occasions without improvement, should be regarded as the protection scope of the present invention.

Claims

1. An AGV unguided turning algorithm based on inertial navigation, characterized in that: The following steps are involved: Step 1, setting a guidance information medium at a turning point and a turning point, and calculating a preset turning radius R and a preset turning angle W; Step 2: When the AGV scans the guidance information medium of the turning point, the lateral offset TraOffset and the angular offset θ of the turning center of the AGV before the AGV enters the turn are calculated; Step 3, calculating the turning radius R1 of the turn by using the preset turning radius R, the preset turning angle W, the lateral offset TraOffset and the angle offset θ; Step 4: Calculate the angle value Wt that the AGV steering wheel needs to turn according to the turning radius R1, and record the value of Wt; Step 5: When the steering wheel rotation angle of the AGV reaches Wt, the coordinate information of the AGV changes. At this time, the corrected turning radius R2 is calculated according to the changes in the center value of the vehicle body ΔX, ΔY and the change in the yaw angle Δψ; Step 6: When the steering wheel is turning, keep the steering wheel angle unchanged and continue driving; Step 7: When the AGV approaches the target angle, according to the principle of symmetry, the rudder is turned back when the angle change value of the turning process is the same; When calculating the turning radius R1, the AGV deflection direction is judged. If the deflection direction is inward, when calculating the turning path of the AVG, in addition to calculating the turning radius R1, the forward distance MoveDis needs to be calculated. If the deflection direction is outward, the forward distance does not need to be calculated. When the deflection direction is inward, the calculation formula of the AGV forward distance MoveDis is: When the deflection direction is inward, the calculation formula of R1 is: When the deflection direction is outward, the calculation formula of R1 is: In the above calculation formula, The calculation formula for the corrected turning radius R2 is: Where b = W-θ, Δψ is the change in yaw angle; ΔX and ΔY are the changes in the value of the vehicle center in the unit time interval t. The calculation method is to calculate the changes x' and y' of the vehicle center at the point value t' in the unit time interval. The known quantities are the AGV wheelbase L, the actual AGV speed V, the AGV steering wheel angle β and the current AGV yaw angle ψ, thus obtaining: Where ψ is the AGV yaw angle, and its value is obtained by analyzing the gyroscope data on the AGV; Then the change of the AGV body center value at time 0-t is: i refers to any point value in the time range of 0-t. Its initial value is 0 and its maximum value is equal to t.

2. The AGV unguided turning algorithm based on inertial navigation according to claim 1 is characterized in that: When the AGV stops and waits during a turn, the gyroscope value at the moment the AGV stops is recorded. When the AGV restarts, the error caused by the drift during the AGV parking process is eliminated.

Images