CN115437362A - Guidance control method of unmanned self-propelled vehicle - Google Patents

Abstract

The invention provides a guiding control method of an unmanned self-propelled vehicle, the unmanned self-propelled vehicle comprises a vehicle body, an automatic guiding device and a steering driving system, and the vehicle body comprises a steering wheel for driving and controlling steering and at least two rotating wheels.

Description

Guiding control method of unmanned self-propelled vehicle

Technical Field

The invention provides a path guiding method, in particular to a guiding control method of a single-steering-wheel unmanned self-propelled vehicle.

Background

Nowadays, the shortage of labor resources and labor costs caused by the global wave of minority is promoted year by year, and the labor is gradually transformed into a technology-intensive industry, and based on the increasing of various operation costs, how to reduce the costs of various items becomes a key for the profitability of enterprises, and with the introduction of automation technologies and the rapid development of the internet of things and artificial intelligence, intelligent manufacturing and intelligent factories are gradually applied to industrial production ends and manufacturing ends, and more tasks are replaced by industrial robots to solve the problem of the shortage of labor resources.

However, automated Guided vehicles (automated Guided Vehicle; AGV) or unmanned transport Vehicle, which is a transport Vehicle equipped with an Automatic guidance device such as electromagnetic or optical type, and integrating functions such as environmental sensing, path planning decision and unmanned Automatic control, and belongs to the field of a Wheeled Mobile Robot (WMR-Wheeled Mobile Robot), and its main functions are that under the monitoring of a computer or a Vehicle-mounted system, the Vehicle automatically travels and stops to a designated place or workstation according to path planning and operation requirements, and a series of operation functions are completed.

Most of the conventional automated guided vehicles are planned to be paths formed by lines connected in a grid type point-to-point manner, and the automated guided vehicles can advance along a predetermined path by using the guidance method, but the guidance method needs to use a large amount of complex operations to capture physical marks or features in the environment to determine the direction and speed of the automated guided vehicle, the operation processing period is long, and the overall navigation efficiency is reduced.

Disclosure of Invention

The main purpose of the present invention is that the vehicle body of the unmanned self-propelled vehicle comprises a steering wheel for driving and controlling steering and at least two auxiliary wheels, the automatic guiding device positions the vehicle body in position and attitude, and generates a predetermined planned target path, when the automatic guiding device obtains the position (such as coordinates, attitude angles, etc.) of the vehicle body center to establish a vehicle coordinate system, and performs coordinate system conversion to establish a local coordinate system, so as to calculate the shortest distance from the vehicle body center to a target point in the predetermined planned target path, the included angle between the vehicle body center and the target point, and the rotation radius from the vehicle body center to the target point, and then calculate the rotation radius and the rotation angle required by the steering wheel to the target point, so that the steering driving system can control the steering wheel to steer to the corresponding position according to the rotation angle, thereby realizing the guidance control of the unmanned self-propelled vehicle to follow the predetermined planned target path, and without a large number of complex operations or long processing period, and effectively improving the overall navigation efficiency.

The secondary objective of the present invention is to calculate the error between the current position and the turning radius of the vehicle body and the target path by the automatic guidance device when the steering driving system completes the steering control of the steering wheel angle, and obtain the speed and the turning radius of the vehicle body after the vehicle body is corrected by the PID control according to the error, and then calculate the speed or the acceleration required by the vehicle body moving to the target path by the inverse kinematics in a reverse thrust manner, so that the steering driving system can control the steering wheel to correct and adjust the current position and the turning radius of the vehicle body until the path guidance control of the vehicle body is completed.

Drawings

Fig. 1 is a schematic view of an unmanned self-propelled vehicle system of the present invention.

FIG. 2 is a flowchart illustrating steps of a preferred embodiment of the present invention.

Fig. 3 is a schematic diagram of coordinate system conversion of the position and attitude of the vehicle body according to the present invention.

FIG. 4 is a schematic diagram of an algorithm for controlling steering driving of a steering wheel of a vehicle body according to the invention.

FIG. 5 is a block diagram of closed loop steering control of the target path according to the present invention.

FIG. 6 is a schematic view (I) of the present invention for correcting the turning radius of the vehicle body.

FIG. 7 is a schematic view (II) of the present invention for correcting the turning radius of the vehicle body.

Fig. 8 is a schematic view of the vehicle body of the present invention performing linear control.

Fig. 9 is a schematic view of the attitude direction of the vehicle body with respect to the target path of the invention.

FIG. 10 is a schematic view of the vehicle body guided to the next target point (I) according to the present invention.

Fig. 11 is a schematic view of the vehicle body guided to the next target point according to the present invention (two).

Fig. 12 is a schematic view showing the length of the vehicle body switching target point according to the present invention.

Description of reference numerals: 1-unmanned self-propelled vehicle; 11-a vehicle body; 111-a steering wheel; 112-a wheel; 12-automatic guiding means; 121-a sensor module; 122-a path planning unit; 13-steering drive system.

Detailed Description

To achieve the above objects and other advantages and in accordance with the purpose of the invention, as embodied and broadly described herein, the following description is made in terms of the preferred embodiment of the present invention.

Referring to fig. 1 to 4, which are a schematic diagram of an unmanned self-propelled vehicle system, a step flow diagram of a preferred embodiment, a schematic diagram of coordinate system conversion of a position and a posture of a vehicle body, and a schematic diagram of an algorithm of steering driving of a steering wheel controlled by the vehicle body, respectively, as can be clearly seen from the diagrams, the unmanned self-propelled vehicle 1 of the present invention includes a vehicle body 11, an automatic guiding device 12, and a steering driving system 13, and a vehicle wheel module under the vehicle body 11 includes a steering wheel 111 (i.e., a driving wheel for driving and controlling steering) and at least two wheels 112 (i.e., a driven wheel for carrying or assisting steering), and the automatic guiding device 12 receives a task instruction from a control management center through a communication interface, and then can control the steering driving system 13 through a vehicle-mounted system or a vehicle-mounted controller to drive the vehicle body 11 to follow a predetermined planned target path, so as to form an Automatic Guided Vehicle (AGV), an Autonomous Mobile Robot (AMR), or a Mobile vehicle, etc.

In the embodiment, the vehicle body 11 of the unmanned autonomous vehicle 1 uses a front steering wheel 111 for driving and controlling steering, and uses two rotating wheels (e.g. fork wheels) 112 at the rear of the vehicle body 11 for carrying or assisting steering, in order to make the vehicle body 11 have better stability, more than two rotating wheels 112 may be installed to provide a fork for carrying to function as a support, so as to form a forklift type automatic guided vehicle suitable for heavy-duty cargo transportation or transfer, but not limited thereto, and may also be a trawl type or a forklift type automatic guided vehicle.

In addition, the steering wheel 111 used by the vehicle body 11 of the unmanned autonomous vehicle 1 may be a horizontal steering wheel or a vertical steering wheel, and includes a driving wheel, a driving Unit (e.g., a driving motor, a gear box, etc.) and a steering mechanism (e.g., a steering motor, an encoder, etc.), and has driving and steering control functions, and the automatic guidance device 12 includes a sensor module 121 and a path planning Unit 122, where the sensor module 121 includes an internal sensor [ e.g., an encoder, an Inertial Measurement Unit (IMU), etc. ] and an external sensor [ e.g., a laser sensor, an optical radar (radar) scanner, an ultrasonic (Sonar) sensor, or a 3D vision sensor (3D Camera), etc. ] loaded on the vehicle body 11, and the internal sensor positions and positions the attitude of the vehicle body 11, so that the automatic guidance device 12 can correct the position or attitude based on the positioning by using environmental information obtained by the external sensor, and perform position or attitude correction for the vehicle body 11 and perform positioning control according to a predetermined guidance path of the driving wheel 111, and a guidance control system for guiding the vehicle body to control a target path and a predetermined navigation path along a predetermined guidance route 13.

Specifically, the fixed path Navigation/Guidance control of the unmanned self-propelled vehicle 1 is to use physical markers (such as electromagnetic tracks, magnetic tapes, reflectors, etc.) set on a moving path as Guidance, and the sensor module 121 of the automatic Guidance device 12 detects the markers to Position and posture the vehicle body 11, so as to run along a target path planned by the path planning unit 122, including but not limited to direct coordinate Guidance (i.e., cartesian coordinate Guidance), electromagnetic Guidance (Wire Guidance), magnetic Tape Guidance (Magnetic Tape Guidance) or Optical Guidance (Optical Guidance), while the virtual path Navigation/Guidance control of the unmanned self-propelled vehicle 1 has no physical markers, so as to store the configuration map data of the moving path of the vehicle body 11 in a database or map library route data in the automatic Guidance device 12, and the sensor module 121 detects the Position and posture of the vehicle body 11, so that the path planning unit 122 determines the predetermined target path by itself, including but not limited to Inertial Navigation (Navigation), laser Navigation (Laser Navigation), or Visual Navigation (Global Navigation) or Navigation (Navigation), and the Navigation System is not limited to do so as Navigation.

As shown in fig. 2, the method for guiding a path used by the unmanned self-propelled vehicle system of the present invention includes the following steps:

(S101) the automated guided device 12 of the unmanned autonomous vehicle 1 first obtains the position of the center of the vehicle body 11 to establish a vehicle coordinate system in the global coordinate system, and performs coordinate system conversion to establish a local coordinate system.

(S102) calculating the shortest distance from the center of the vehicle body 11 to one target point of a preset planned target path, and calculating the included angle between the center of the vehicle body 11 and the target point and the rotating radius from the center of the vehicle body 11 to the target point according to the geometric relationship.

(S103) a turning radius and a turning angle required for the steering wheel 111 of the vehicle body 11 to turn to the target point are calculated.

(S104) the steering drive system 13 controls the steering wheel 111 of the vehicle body 11 to steer to a corresponding position based on the calculated steering angle.

(S105) the automated guidance device 12 calculates the error between the current position and the radius of rotation of the vehicle body 11 and the target path, obtains the corrected speed and the radius of rotation by using PID control, and calculates the speed of the vehicle body 11 by using inverse kinematics to reverse the speed, so that the steering driving system 13 can control the steering wheel 111 to correct and adjust the current position and the radius of rotation of the vehicle body 11.

As is apparent from the above description and the above description, the unmanned autonomous moving vehicle 1 of the present invention is preferably implemented as a forklift type automated guided vehicle, and the sensor module 121 of the automated guided device 12 is used to position and posture of the vehicle body 11, and the path planning unit 122 generates a predetermined planned target path, because the driving mechanism of the vehicle body 11 mainly uses the front steering wheel 111 (i.e. the driving wheel) to have the steering function, and operates in cooperation with the rear two steering wheels 112 (i.e. the driven wheels), and the actual moving path trajectory is only related to the rotation angle or the course angle of the front steering wheel 111, the path guidance control of the unmanned autonomous moving vehicle 1 can be realized by only controlling the rotation angle or the course angle of the steering wheel 111.

In the present embodiment, a Global Coordinate System (X in fig. 3) is first established in the environment where the unmanned self-propelled vehicle 1 is located by using the automatic guiding device 12 _G Y _G Coordinate plane), and obtains coordinates (X) of the center of the vehicle body 11 (i.e., the geometric center of the steering wheel 111) in the global coordinate system _C ，Y _C ) As the center point C, and the target point P as one of the target points of the predetermined planned target path, and the predetermined planned target path includes a straight path and a curved path to establish a vehicle coordinate system (e.g., X) _GM Y _GM Coordinate plane), then a Local Coordinate System (e.g., X) is established by transforming the Coordinate System using the rotation matrix _L Y _L Coordinate plane) can be found:

where θ is the current attitude angle of the vehicle body 11, and can be expressed as X of the vehicle coordinate system _GM Or Y _GM Rotation of the axis to X of the local coordinate system _L Or Y _L The angle of the shaft; x _C Is a Y of a global coordinate system _G Axis and Y of vehicle coordinate system _GM The spacing of the shafts; y is _C Is X of a global coordinate system _G X of axes and vehicle coordinate system _GM The spacing of the shafts; x _G ，Y _G Coordinates (X, Y) of a target point P in the global coordinate system for a predetermined planned target path; x _L ，Y _L Coordinates (X, Y) of the target point P in the local coordinate system are obtained to locate the current position and attitude of the vehicle body 11.

According to the geometric relationship of the right triangle, the following can be obtained:

where D is the center point C (X) of the vehicle body 11 in the local coordinate system _C ，Y _C ) To the target point P (X) _L ，Y _L ) The shortest path distance of (d); theta _S As Y of the local coordinate system _L The angle of the clockwise rotation of the shaft to the target point P can be represented as the included angle between the center of the vehicle body 11 or the center point C and the target point P, that is, the rotation angle or the course angle of the steering wheel 111 of the vehicle body 11 turning to the target point P; because the current heading angle of the steering wheel 111 of the vehicle body 11 is the same as the Y of the vehicle body 11 in the local coordinate system _L The axes are kept consistent at D, theta _S In a known case, the rotation radius from the center of the vehicle body 11 (i.e., the geometric center of the steering wheel 111) to the target point P can be obtained according to the geometric relationship.

As shown in fig. 4, when the automatic guidance device 12 acquires the current position (i.e., the center point C) of the center of the vehicle body 11 or the geometric center of the steering wheel 111, the distance D, and the speed V, the deviation amount between the current position and the target route can be calculated, and when V and D are known, the following can be obtained from the geometric relationship between the right triangle:

wherein R is a radius of rotation of the center of the vehicle body 11 (i.e., the geometric center of the steering wheel 111) to the target point P; l is a fixed distance between the geometric center of the steering wheel 111 at the front of the vehicle body 11 and the midpoint M of the central line of the two rear rotating wheels 112; w is a variable distance from the midpoint M of the line connecting the centers of the two rotating wheels 112 to the origin O of the rotation radius of the center of the vehicle body 11; theta _S The steering driving system 13 can steer the steering wheel 111 of the vehicle body 11 to the turning angle or the course angle of the target point P according to theta _S The deviation value of the steering wheel 111 of the vehicle body 11 is controlled to steer to a corresponding position, so that the guidance control of the unmanned autonomous vehicle 1 running along a preset planned target path is realized, and a steering driving algorithm adopted by a processor built in the vehicle-mounted controller or the automatic guidance device 12 does not need to be subjected to a large amount of complex operation or a long operation processing period, so that the requirement on the operation performance of the vehicle-mounted controller or the processor is relatively reduced, and the overall navigation efficiency can be effectively improved.

Please refer to fig. 5 to 7, which respectively show a block diagram of the target path closed loop guidance control, a schematic diagram (a) of the turning radius of the vehicle body, and a schematic diagram (b) of the turning radius of the vehicle body, as can be clearly seen from the diagrams, the unmanned autonomous vehicle 1 of the present invention can perform PID (proportional, integral, and derivative) control according to the moving state of the vehicle body 11 and the predetermined planned target path generated by the automatic guidance device 12 to form a closed loop control flow, thereby realizing the control adjustment of the vehicle body 11 in a periodic cycle.

When the vehicle body 11 moves along the target path, the automated guidance device 12 can perform coordinate transformation between the current position and the turning radius of the vehicle body 11, and calculate an error amount (error) between the current position and the turning radius of the vehicle body 11 and the target path _d And error _R ) Then, PID control is performed based on the error amount to obtain a corrected speed V and a corrected turning radius R of the vehicle body 11, and the speed V or the acceleration a required for the vehicle body 11 to move to the target path is calculated in a reverse manner using Inverse Kinematics (Inverse Kinematics), so that the steering drive system 13 can control the steering wheels 111 to correct and adjust the current position and the turning radius of the vehicle body 11, and thus the correction is repeated to make the moving state of the vehicle body 11 conform to the desired target path until the path guidance control of the vehicle body 11 is completed.

In the present embodiment, the predetermined planned target path generated by the automatic guiding device 12 can guide the vehicle body 11 to follow a straight path or a curved path, and give the current position and the turning radius of the vehicle body 11, and can continuously detect the error amount between the current position, the turning radius and the target path of the vehicle body 11, wherein error is _d Error is an error amount of a linear distance between the current position of the vehicle body 11 and the final position of the target path _R ＝R-R _false The corrected speed V x = K of the vehicle body 11 can be calculated by performing algorithms different from PID control on the error between the rotation radius of the center of the vehicle body 11 and the rotation radius generated by the deviation of the vehicle body 11 _PR ＊(error _d ) And the corrected rotation radius R + = K of the vehicle body 11 _PR ＊(R-error _R ) In which K is _PR Is the amount of gain.

When the vehicle body 11 travels in a curved path, the turning width of the vehicle body 11 can be changed by adjusting the turning radius, for example, when the turning radius of the center of the vehicle body 11 becomes larger, which means that the vehicle body 11 has deviated to the outer side of the curved path, the turning width adjusted and changed by the vehicle body 11 becomes smaller; in other words, when the turning radius of the vehicle body 11 becomes smaller, which indicates that the vehicle body 11 has deviated to the inside of the curved path, the turning range of the adjustment change of the vehicle body 11 becomes larger, and the turning direction of the center of the vehicle body 11 can be determined in the algorithm, so that the turning direction of the vehicle body 11 can be corrected by using the turning radius of the center of the vehicle body 11 as the variation, so that the vehicle body 11 can quickly and accurately correct the deviation and stably maintain on the predetermined planned target path when deviating the curved path.

Please refer to fig. 8 to 12, which are a schematic diagram of the linear control of the vehicle body, a schematic diagram of the attitude direction of the vehicle body relative to the target path, a schematic diagram (a) of the vehicle body guiding to the next target point, a schematic diagram (b) of the vehicle body guiding to the next target point, and a schematic diagram of the length of the vehicle body switching target point, as can be clearly seen from the diagrams, the target path generated by the automatic guidance device 12 is divided into a plurality of line segments by using a plurality of target points P0 to P9, and the plurality of line segments are connected to form a linear path trajectory, where the target point P0 can be represented as a start point of the linear path, the target point P9 can be represented as a final point of the linear path, and is divided by a predetermined length according to a linear equation y = ax + b, when the vehicle body 11 travels, the first target point P0 is on the left side of the vehicle body 11 as shown in fig. 9, and an included angle [ arctan (x/y) ] between the center of the vehicle body 11 and the target point P0 can be calculated as the control of the steering of the vehicle body 11, and the direction of the turning can also be determined as the clockwise turning or counterclockwise direction of the vehicle body 11, where the turning direction is a clockwise direction, and a counterclockwise direction.

In the embodiment, when the vehicle 11 tracks a plurality of target points P0 to P9 divided on the straight-line path, if the center of the vehicle 11 exceeds the target point, the next target point is tracked (as shown in fig. 10 to 11), and as to whether the target point exceeds the target point, based on whether the y-axis direction vector of the target point of the vehicle coordinates itself with respect to the vehicle 11 is less than 0, if the y-axis direction vector of the target point with respect to the vehicle 11 is determined to be less than 0, the tracking from the next target point is started, for example, if the positioning point n projected from the center of the vehicle 11 onto the straight-line path is located between the target point P2 and the target point P3, the y-axis direction vector of the target point P2 on the straight-line path with respect to the vehicle 11 is 2-n less than 0, that is y less than 0, and if the y-axis direction vector of the target point P3 on the straight-line path with respect to the vehicle 11 is 3-n greater than 0, it can be expressed that y is greater than 0, and a function operation is performed in conjunction with program code, where the target point on the straight-line path is y = x-line with y = n-line-n, if y = x-n is calculated to be less than 0, then the result of the target point P3 is greater than 0, that the final tracking from the target point P11 is calculated, and the third target point is calculated, that is three target point P3, that is equal to be the target point P11, and the target point is calculated from the target point is equal to be greater than the target point is equal to be the target point.

In addition, in order to avoid that the center of the vehicle body 11 is too close to the target point on the straight path and the X-axis direction has too large difference, the steering angle [ arctan (X/y) ] of the vehicle body 11 is easily too large, and the vehicle body 11 is deviated or shaken strongly, so the length of the target point can be switched by setting parameters (as shown in fig. 12), thereby ensuring that the vehicle body 11 can stably follow the target path.

The above detailed description is of a preferred embodiment of the present invention, and the present invention is not intended to be limited to the embodiment, but rather, all equivalent variations and modifications without departing from the spirit of the present invention are intended to be included within the scope of the present invention.

In summary, the guiding control method of the unmanned self-propelled vehicle of the present invention can achieve the efficacy and purpose thereof.

Claims (5)

1. A guiding control method of an unmanned self-propelled vehicle is characterized in that the unmanned self-propelled vehicle comprises a vehicle body, an automatic guiding device and a steering driving system, the vehicle body comprises a steering wheel used for driving and controlling steering and at least two auxiliary steering rotating wheels, the automatic guiding device is used for positioning the position and the posture of the vehicle body and generating a preset planned target path for the steering driving system to drive the steering wheel of the vehicle body to run along the target path, and the path guiding method comprises the following steps:

(A) The automatic guiding device obtains the position of the center of the vehicle body to establish a vehicle coordinate system in a global coordinate system, and obtains the current attitude angle of the vehicle body to carry out coordinate system conversion so as to establish a local coordinate system;

(B) Calculating the shortest distance from the center of the vehicle body to one target point of the target path, and calculating an included angle between the center of the vehicle body and the target point and a rotating radius from the center of the vehicle body to the target point according to a geometric relationship;

(C) Obtaining the speed of the center of the vehicle body, and calculating the rotating radius and the rotating angle required by the steering wheel of the vehicle body to turn to the target point according to the shortest distance:

wherein D is the shortest distance; r is the turning radius of the steering wheel to the target point; l is the distance from the center of the steering wheel to the midpoint of the line connecting the centers of the two rotating wheels; w is the distance from the midpoint of the two wheels to the origin of the turning radius of the steering wheel; theta _S Turning the steering wheel to the corner of the target point;

(D) And the steering driving system controls the steering wheel of the vehicle body to steer to a corresponding position according to the calculated turning angle.

2. The guidance control method for an unmanned autonomous vehicle as claimed in claim 1, wherein the automatic guidance apparatus comprises a sensor module for positioning a position and an attitude of the vehicle body and a path planning unit for generating the target path.

3. The guidance control method of an unmanned autonomous vehicle as claimed in claim 1, wherein the step (a) is a coordinate system conversion using a rotation matrix:

wherein theta is the current attitude angle of the vehicle body; x _C Y being a global coordinate system _G Axis and Y of vehicle coordinate system _GM The spacing of the shafts; y is _C Is X of a global coordinate system _G X of axes and vehicle coordinate system _GM The spacing of the shafts; x _G ，Y _G Coordinates of the target point in a global coordinate system; x _L ，Y _L Is local to the target pointCoordinates in a coordinate system.

4. The guidance control method of an unmanned autonomous vehicle as claimed in claim 1, wherein the step (B) is calculated in such a manner that the following is obtained based on the geometric relationship of right triangles:

wherein D is the shortest distance from the center of the vehicle body to the target point; theta _S Is the included angle between the center of the vehicle body and the target point; and R is the rotation radius from the center of the vehicle body to the target point.

5. The guidance control method of an unmanned autonomous vehicle as claimed in claim 1, wherein the step (D) performs steering of a steering wheel of the vehicle body, and then performs the next step:

(E) The automatic guiding device calculates the error amount between the current position and the rotating radius of the vehicle body and the target path, obtains the corrected speed and the rotating radius of the vehicle body by adopting PID control according to the error amount, and calculates the speed or the acceleration required by the vehicle body moving to the target path in a reverse thrust mode by adopting inverse kinematics so that the steering driving system controls the steering wheel of the vehicle body to correct and adjust the current position and the rotating radius of the vehicle body until the path guiding control of the vehicle body is completed.

Images