CN115431969A

CN115431969A - Guidance control method of unmanned self-propelled vehicle

Info

Publication number: CN115431969A
Application number: CN202110620239.9A
Authority: CN
Inventors: 李明俊; 张咏信; 赖昇昊
Original assignee: Usun Technology Co Ltd
Current assignee: Usun Technology Co Ltd
Priority date: 2021-06-03
Filing date: 2021-06-03
Publication date: 2022-12-06

Abstract

The invention provides a guidance control method of an unmanned self-propelled vehicle, which comprises the steps of obtaining the position of the center of a vehicle body by an automatic guidance device to establish a vehicle coordinate system, converting the coordinate system to establish a local coordinate system, calculating the shortest distance from the center of the vehicle body to a target point in a preset planned target path, the corner between the center of the vehicle body and the target point and the rotating radius from the center of the vehicle body to the target point, calculating the included angle between the current course of the vehicle body and the center of a steering wheel according to the distance from the center of the vehicle body to the center of any steering wheel, judging the direction of the vehicle body needing to turn, obtaining the rotating radius and the corner of two steering wheels according to the cosine law, and controlling the two steering wheels to turn to the corresponding positions by a steering driving system according to the calculated corner and speed, thereby realizing the guidance control of the unmanned self-propelled vehicle running along the preset planned target path without a large number of complex operations or longer processing period and effectively improving the whole navigation efficiency.

Description

Guiding control method of unmanned self-propelled vehicle

Technical Field

The invention provides a guide control method, in particular to a guide control method of a double-rudder wheel unmanned self-propelled vehicle.

Background

Nowadays, the shortage of labor resources and the labor cost caused by the global wave of minority carrier wave are promoted year by year, and the industry is gradually transformed from labor intensive to technology intensive, and based on the continuous rise of various operation costs, how to reduce the cost of each item becomes the key for the profitability of enterprises, and with the introduction of automation technology, the rapid development of 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, so as to solve the problem of the shortage of labor resources.

However, an Automated Guided Vehicle (AGV), or an Automated Guided Vehicle (AGV), refers to a Vehicle equipped with an electromagnetic or optical Automatic guiding device, and integrating functions of environmental sensing, route planning decision, unmanned Automatic control, and the like, and belongs to the field of a wheel-type Mobile Robot (WMR-wheel Mobile Robot), and the main functions of the AGV include automatically walking and parking to a specified location or workstation according to route planning and operation requirements under monitoring of a computer or a Vehicle-mounted system, and performing a series of operation functions.

Most of the conventional automated guided vehicles are designed to be a path formed by lines connected in a grid-type point-to-point manner, and the automated guided vehicle can advance along a predetermined path by using the above guidance method, but the above 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 that the automated guided vehicle should travel, the operation processing period is long, but the overall navigation efficiency is reduced, and the path planning is not a smooth curve, so that the automated guided vehicle has an uneven turn in the traveling process, the actual traveling path deviates from the predetermined path, and the position and course comparison and correction need to be continuously performed, i.e., the direction that the industry is eagerly to research and improve is located.

Disclosure of Invention

The invention mainly aims to provide a vehicle body of an unmanned self-propelled vehicle, which comprises two steering wheels and at least two auxiliary steering wheels, wherein the steering wheels are used for driving and controlling steering, an automatic guiding device is used for positioning the position and the posture of the vehicle body, a preset planned target path is generated, when the automatic guiding device acquires the position (such as coordinates, posture angles and the like) of the center of the vehicle body to establish a vehicle coordinate system, the coordinate system is converted to establish a local coordinate system, the shortest distance from the center of the vehicle body to one target point of the preset planned target path, the corner between the center of the vehicle body and the target point and the rotating radius from the center of the vehicle body to the target point can be calculated, the included angle between the current center of the vehicle body and the center of the steering wheels can be calculated according to the distance from the center of the vehicle body to the center of any one steering wheel, the direction of the vehicle body needing to be turned can be judged, the rotating radius and the rotating angle of the two steering wheels can be obtained according to the cosine law, so that a steering driving system can control the two steering wheels to steer to the corresponding positions according to the calculated rotating angle and speed, thereby realizing the guiding control of the unmanned self-propelled vehicle running along the preset target path without a large amount of complex calculation or a long processing cycle, and effectively improving the whole navigation efficiency.

The secondary purpose of the invention is that the geometrical relationship between the vehicle body center and the two steering wheels is fixed, and under the condition that the speed V of the vehicle body center and the rotating radius R from the vehicle body center to a target point are known, the following can be obtained:

wherein θ is _a The included angle between the current course of the vehicle body and a connecting line from the center of the vehicle body to the center of any steering wheel is set; d is the distance from the center of the vehicle body to the center of any steering wheel.

Then, the included angle theta between the current course of the vehicle body and a connecting line from the center of the vehicle body to the target point can be determined _S Determining the direction in which the vehicle body needs to turn, assuming θ _S To be greater than 0, according to the cosine theorem then:

let θ be _S To be less than 0, according to the cosine theorem, one can then obtain:

wherein R is _lf The radius of rotation of the front steering wheel; r is _rr The rotation radius of the rear steering wheel; theta _lf For the front steering wheel along the radius of rotation R _lf The required turning angle during turning; theta.theta. _rr For the rear steering wheel along the radius of rotation R _rr The required turning angle when turning.

Further, since the angular velocity when the vehicle body center circularly moves at a constant velocity is equal to the angular velocity of the two steering wheels (i.e., ω = ω) _lf ＝ω _rr ) And the relationship between the speed V of the vehicle body center, the rotation radius R and the angular velocity omega (namely V = R multiplied by omega) can be obtained as follows:

wherein the V _f The speed of the front steering wheel; v _r The speed of the rear steering wheel.

Therefore, the automatic guiding device can firstly obtain the rotation radius required by the two steering wheels of the vehicle body when the two steering wheels travel, and push back to obtain the rotation angle required by the two steering wheels, and then obtain the speed of the two steering wheels according to the relation among the speed, the rotation radius and the angular speed, so that the two steering wheels can be controlled to steer to the corresponding position through the steering driving system, and the vehicle body can stably keep on the preset planned target path when the vehicle body travels at different rotation angles and speeds (namely speed difference).

Another purpose of the invention is to firstly carry out Y-coordinate of the vehicle body in a local coordinate system when the postures of the two steering wheels of the vehicle body advance in a translational way _L The axis rotates to an included angle theta parallel to a target straight line formed by a connecting line from the starting point to the target point, the two steering wheels rotate to the included angle theta, the target straight line is assumed to be divided into two sections, and the included angle theta is taken as a reference, so that the control quantity of the two steering wheels can be calculated to be theta + arctan (x/y) respectively; when the postures of the two steering wheels of the vehicle body are turning,if the attitude deviation of the vehicle body is larger than the deviation of the set angle, multiplying the control quantity by the negative sign according to the attitude direction to be corrected of the vehicle body by two steering wheels so that when the vehicle body turns left or right, the control quantity of one steering wheel is theta + arctan (x/y), and the control quantity of the other steering wheel is- [ theta + arctan (x/y)]。

The present invention also aims at controlling the steering of the steering driving system to complete the steering of the steering wheel, calculating the error between the current position and the rotating radius of the vehicle body and the target path with the automatic guide device, obtaining the corrected speed and the rotating radius of the vehicle body with PID control based on the error, calculating the speed or the acceleration required by the vehicle body to move to the target path with inverse kinematics in a reverse thrust manner, and controlling the steering driving system to control the two steering wheels to correct and adjust the current position and the rotating radius of the vehicle body until the path guide 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 the coordinate system transformation 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 (two) 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 steering control of the present invention in which the vehicle body is in a translational forward position.

Fig. 11 is a schematic view of the vehicle body of the present invention with its attitude corrected to turn left.

Fig. 12 is a schematic view of the vehicle body of the present invention with the attitude thereof corrected to a right turn.

Description of reference numerals: 1-unmanned self-propelled vehicle; 11-a vehicle body; 111-a steering wheel; 111 a-left front wheel; 111 b-the right rear 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 the unmanned self-propelled vehicle system, a flowchart of steps in a preferred embodiment, a schematic diagram of coordinate system conversion of position and posture of a vehicle body, and a schematic diagram of an algorithm of steering driving of a vehicle body control steering wheel, 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 two steering wheels 111 (i.e., driving wheels for driving and controlling steering) arranged diagonally in front and behind, and a rotating shaft of the two steering wheels 111 is connected to two rotating wheels 112 (i.e., driven wheels for bearing or assisting steering) arranged diagonally in front and behind, respectively, but not limited thereto, two or more than two rotating wheels 112 may be separately arranged at appropriate positions of the vehicle body 11, and after the automatic guiding device 12 receives a task instruction from a control management center through a communication interface, the vehicle system or a vehicle controller 13 may be operated to drive the vehicle body 11 to follow a predetermined planned target path, so as to form an automatic guided vehicle (Automated Mobile Robot), AGV, mobile vehicle (Mobile Robot, mobile vehicle or Mobile vehicle).

In the present embodiment, the steering wheel 111 on the left side in front of the vehicle body 11 is used as the left front wheel 111a, and the steering wheel 111 on the right side of the opposite corners behind the vehicle body 11 is used as the right rear wheel 111b for driving and controlling steering, but the present invention is not limited thereto, and the positions of the two steering wheels 111 may be changed to be diagonally arranged between the right front wheel and the left rear wheel according to the actual design of the vehicle body 11, and then two wheels 112 arranged on the opposite corners of the vehicle body 11 or other wheels 112 in other suitable positions may be used for carrying or assisting steering, in order to make the vehicle body 11 have better stability, two wheels 112 (such as fork wheels) may be mounted on the forks of the vehicle body 11 to play a role of supporting, so as to form a forklift type automatic guided vehicle, a trailer type automatic guided vehicle, or a stacker type automatic guided vehicle suitable for carrying or transferring heavy loads.

In addition, the steering wheel 111 used in 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 guiding device 12 includes a sensor module 121 and a path planning Unit 122, wherein 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 (Light Detection and Ranging, liDAR) 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 vehicle body 11, so that the automatic guiding device 12 can perform position or attitude correction using environmental information obtained by the external sensor on the basis of the positioning, and perform position and attitude correction of the vehicle body by using a fixed navigation algorithm for controlling the vehicle body 11 and the driving path navigation system according to a predetermined guiding path and a predetermined navigation algorithm for controlling the vehicle body navigation and the path.

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 guidance control method adopted by the above unmanned autonomous vehicle system of the present invention includes the following implementation 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 a target point in a preset planned target path, the corner between the current course of the vehicle body 11 and a connecting line 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.

(S103) acquiring the speed of the center of the vehicle body 11, and calculating an included angle between the current heading of the vehicle body 11 and the center of any steering wheel 111 according to the distance from the center of the vehicle body 11 to the center of any steering wheel 111.

(S104) the turning direction of the vehicle body 11 is determined, and the turning radii of the two steering wheels 111 and the turning angle required for turning are obtained according to the cosine law.

(S105) the velocity of the two steering wheels 111 is obtained from the relationship among the velocity of the center of the vehicle body 11, the radius of rotation, and the angular velocity.

(S106) the steering driving system 13 controls the two steering wheels 111 to steer to the corresponding positions according to the calculated steering angle and speed, so that the center of the vehicle body 11 can stably follow the target path.

(S107) the automatic guiding device 12 calculates the error between the current position and the rotating radius of the vehicle body 11 and the target path, obtains the corrected speed and the rotating radius by adopting PID control, and calculates the speed of the vehicle body 11 by adopting inverse kinematics backstepping, so that the steering driving system 13 can control two steering wheels 111 to correct and adjust the current position and the rotating radius of the vehicle body 11.

As is apparent from the above description and the above implementation steps, 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 two steering wheels 111 (i.e. driving wheels) disposed diagonally in front and back to have a steering function, and operates in cooperation with two turning wheels 112 (i.e. driven wheels) disposed diagonally in the other direction, and the path trajectory actually moved is only related to the turning angle or heading angle of the steering wheels 111, so that the path guidance control of the unmanned autonomous moving vehicle 1 can be realized by only controlling the turning angle or heading angle of the steering wheels 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 vehicle) 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 result in:

where θ is the current attitude angle of the vehicle body 11, and can be expressed asX of the vehicle coordinate system _GM Or Y _GM Rotation of axes to X of local coordinate system _L Or Y _L The angle of the shaft; 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 (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:

wherein 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 distance of the shortest path of (c); theta.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 rotation angle between the current course of the vehicle body 11 and the line from the center (i.e. the center point C) of the vehicle body 11 to the target point P; two steering wheels 111 (namely a left front wheel 111a and a right rear wheel 111 b) which are arranged diagonally in front and back of the vehicle body 11 are parallel to the current heading of the vehicle body 11 in a consistent manner at D and theta _S In a known situation, the rotation radius from the center of the vehicle body 11 to the target point P can be obtained according to the geometric relationship.

As shown in fig. 4, after the automatic guiding device 12 obtains the current position (e.g. x, y, θ) of the center of the vehicle body 11, the shortest distance D and the speed V, since the geometric relationship between the center of the vehicle body 11 and the two steering wheels 111 is fixed, and when V and R are known and the distance from the center of the vehicle body 11 to the geometric center of the steering wheels 111 is D, it can obtain:

wherein W is a fixed distance in the horizontal direction from the center of the vehicle body 11 to the center of any one of the steering wheels 111 (i.e., the left front wheel 111a or the right rear wheel 111 b); l is a fixed distance in the vertical direction from the center of the vehicle body 11 to the center of any one of the steering wheels 111; theta _a For the Y of the vehicle body 11 in the local coordinate system _L The angle of the shaft rotating counterclockwise to the center of any one of the steering wheels 111 can be represented as the included angle between the current heading of the vehicle body 11 and the connecting line from the center of the vehicle body 11 to the center of any one of the steering wheels 111; d is a fixed distance from the center of the vehicle body 11 to the center of any one of the steering wheels 111.

Then, the angle theta can be determined according to the current course of the vehicle body 11 and the included angle (i.e. the rotation angle or the course angle) between the center of the vehicle body 11 and the connecting line P between the target point and the target point _S The direction in which the vehicle body 11 needs to turn is determined, and the turning angle θ of the vehicle body 11 is assumed to be determined _S Greater than 0 (i.e., the positive sign is the direction of the counterclockwise turn), according to the cosine law, it can be obtained that:

suppose that the turning angle theta of the vehicle body 11 is judged _S To be less than 0 (i.e. negative sign is the direction of clockwise turn), then according to the cosine theorem:

wherein R is _lf A distance from the center of the front steering wheel 111 (i.e., the left front wheel 111 a) to a center O of the vehicle body 11 around the center with a radius of rotation R; r is _rr A distance from a center O of the rear steering wheel 111 (i.e., the right rear wheel 111 b) to the center of the vehicle body 11 with a radius of rotation R; theta _lf The front steering wheel 111 (i.e. the left front wheel 111 a) along the radius of rotation R _lf The required turn angle or course angle during turning; theta _rr The rear steering wheel 111 (i.e., the right rear wheel 111 b) along the radius of rotation R _rr The required turn angle or course angle when turning.

Further, since the angular velocity at which the center of the vehicle body 11 circularly moves at a constant velocity is equal to the angular velocity of the front and rear steering wheels 111 (i.e., ω = ω) _lf ＝ω _rr ) And, in the case where the current speed V at the center of the vehicle body 11 is known, from the relationship (i.e., V = R · ω) between the speed V at the center of the vehicle body 11 (i.e., the average velocity), the radius of rotation R, and the angular velocity ω:

wherein the V _f Is the speed of the front steering wheel 111 (i.e., the left front wheel 111 a); v _r Is the velocity of the rear steering wheel 111 (i.e., the right rear wheel 111 b). Therefore, the automatic guide device 12 can determine the turning radius R required when the two steering wheels 111 (i.e., the left front wheel 111a and the right rear wheel 111 b) of the vehicle body 11 travel along a straight path or a curved path _lf 、R _rr And pushed back to obtain the rotation angle theta required by the two steering wheels 111 _lf 、θ _rr Then, the speed V required by the two steering wheels 111 is obtained according to the relation among the speed, the rotation radius and the angular speed _f 、V _r The steering driving system 13 can control the two steering wheels 111 of the vehicle body 11 to steer to corresponding positions, and the vehicle body 11 can travel at different turning angles and speeds (i.e. speed difference) when turning, so that the center of the vehicle body 11 can follow a straight path or a curved path, thereby realizing guidance control of the unmanned autonomous vehicle 1 to run along a predetermined planned target path, and the steering driving algorithm adopted by the built-in processor of the vehicle-mounted controller or the automatic guidance device 12 does not need to go through a large amount of complex operations 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 automatic guide deviceThe device 12 converts the coordinates of the current position and the radius of rotation of the vehicle body 11, and calculates the error (error) between the current position and the radius of rotation of the vehicle body 11 and the target path _d And error _R ) Then, the speed V and the radius of rotation R of the vehicle body 11 after correction are obtained by performing PID control based on the error amount, and the speed V or the acceleration a required for the vehicle body 11 to move to the target path, for example, the speed V of the steering wheel 111 (i.e., the left front wheel 111 a) in front of the vehicle body 11, can be calculated by Inverse Kinematics (Inverse Kinematics) in a reverse manner _f Velocity V of the rear steering wheel 111 (i.e., the right rear wheel 111 b) _r And the acceleration A of the front and rear steering wheels 111 _f 、A _r The steering drive system 13 may control the two steering wheels 111 to adjust the current position and the turning radius of the vehicle body 11, and thus repeatedly adjust the moving state of the vehicle body 11 to 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 vehicle body 11 the current position and the rotation radius, and can continuously detect the error between the current position, the rotation radius and the target path of the vehicle body 11, wherein the error is generated by the error device 12 _d Error is an amount of error in the linear distance between the current position of the vehicle body 11 and the final position of the target path _R ＝R-R _false The error between the turning radius of the center of the vehicle body 11 and the turning radius caused by the deviation of the vehicle body 11 is calculated by performing different algorithms of PID control to obtain the corrected speed V x = K of the vehicle body 11 _PR ＊(error _d ) And the corrected radius of rotation R of the vehicle body 11 x = K _PR ＊(R-error _R ) Wherein the K is _PR Is the amount of gain (scaling factor).

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.

Referring to fig. 8 to 12, a schematic diagram of the vehicle body performing linear control, a schematic diagram of the vehicle body in the attitude direction relative to the target path, a schematic diagram of the vehicle body steering control in which the attitude of the vehicle body is translational forward, a schematic diagram of the vehicle body correcting to turn left and a schematic diagram of the vehicle body correcting to turn right are shown, respectively, it can be clearly seen from the drawings that the target path generated by the automatic guidance device 12 is a predetermined planned target path, which is divided into a plurality of line segments by a plurality of target points P0 to P9, and the plurality of line segments are connected to form a straight line path trajectory, wherein the target point P0 can be represented as a start point of the straight line path, the target point P9 can be represented as a final point of the straight line path, and is divided by a predetermined length according to the straight line equation y = ax + b, when the vehicle body 11 travels, the first target point P1 is viewed from the vehicle coordinate system to the left 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 P1 is determined as the control of the two steering wheels 111 of the vehicle body turning, and the vehicle body turning direction can be a clockwise direction, and the turning direction can also be determined as a negative sign, and the turning direction of the vehicle body is a clockwise.

In the present embodiment, the unmanned autonomous vehicle 1 takes two steering wheels 111 disposed diagonally front and back of the vehicle body 11 as a right front wheel and a left rear wheel as an example, and takes the distance from the center (i.e., the center point C) of the vehicle body 11 to the target point P1 as the hypotenuse of the right triangle, and the point where the opposite side of the right triangle intersects with the adjacent side vertically as the starting point S (as shown in fig. 10), when the vehicle body 11 tracks the target point P1 on the straight path and moves forward in a translation manner, the Y of the vehicle body 11 in the local coordinate system is first used _L The axle rotates counterclockwise to be parallel to the target straight line formed by the connection line from the starting point S to the target point P1, which can be expressed as that the current course of the vehicle body 11 rotates to an included angle theta parallel to the target straight line, and two steering wheels 111 (namely a right front wheel and a left rear wheel) of the vehicle body 11 are enabledAfter the vehicle body is rotated to the included angle θ in a translation manner, assuming that the target straight line is divided into two sections, and taking the included angle θ as a reference, the control quantities of the two steering wheels 111 of the vehicle body 11 can be calculated according to the geometric relationship of the right triangle and are respectively θ + arctan (x/y), where x is a distance from the center of the vehicle body 11 to the starting point S, and y is a distance from the starting point S to the midpoint M of the target straight line.

Further, when the attitude deviation of the vehicle body 11 is larger than the deviation of the set angle when the vehicle body 11 is turning, the control amounts of the two steering wheels 111 (i.e., the right front wheel and the left rear wheel) need to be multiplied by a negative sign according to the attitude direction to be corrected by the vehicle body 11, and for example, when the vehicle body 11 is turning left (as shown in fig. 11), the control amount of the right front wheel is θ + arctan (x/y), and the control amount of the left rear wheel is- [ θ + arctan (x/y) ]; similarly, when the vehicle body 11 is turning to the right (as shown in fig. 12), the control amount of the right front wheel is θ + arctan (x/y) and the control amount of the left rear wheel is θ + arctan (x/y), so that the center of the vehicle body 11 can stably follow the target path.

The above detailed description is of a preferred embodiment of the present invention, which is not intended to limit the scope of the invention, but rather, all equivalent variations and modifications which do not depart from the technical spirit of the invention are intended to be included within the scope of the 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

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, two steering wheels and at least two auxiliary rotating wheels are arranged on the vehicle body in opposite angles, the steering wheels are used for driving and controlling steering, 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 wheels of the vehicle body to run along the target path, and the guiding control 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 to obtain the shortest distance D from the center of the vehicle body to a target point in the target path, and the rotation angle theta between the current course of the vehicle body and the line between the center of the vehicle body and the target point _S And a radius of rotation R from the center of the vehicle body to the target point;

(C) Obtaining the speed V of the center of the vehicle body, and calculating an included angle theta between the current course of the vehicle body and a connecting line between the center of the vehicle body and the center of any steering wheel according to the distance d from the center of the vehicle body to the center of any steering wheel _a ；

(D) According to the angle of rotation theta of the vehicle body _S Determining the direction of the vehicle body turning, assuming theta _S If the value is greater than 0, the following results are obtained according to the cosine theorem:

assume a rotation angle theta of the vehicle body _S When the value is less than 0, the following results are obtained according to the cosine law:

wherein R is _lf The rotating radius of the front steering wheel; r _rr The rotation radius of the rear steering wheel; theta _lf Along the radius of rotation R for the front steering wheel _lf The required turning angle during turning; theta _rr For the rear steering wheel along this radius of rotation R _rr The required turning angle during turning;

(E) Since the angular velocity of the vehicle body center circular motion at a constant velocity is equal to the angular velocities of the two steering wheels, it is obtained from the relationship among the velocity V of the vehicle body center, the radius of rotation R, and the angular velocity:

wherein V _f The speed of the front steering wheel; v _r The speed of the rear steering wheel;

(F) The steering driving system obtains the rotation angle theta according to the calculation _lf 、θ _rr And rate V _f 、V _r The two steering wheels are controlled to steer to corresponding positions, so that the center of the vehicle body moves forward along the target path.

2. The guidance control method of an unmanned autonomous vehicle as claimed in claim 1, wherein the automatic guidance device 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 θ is the current attitude angle of the vehicle body; 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 of the target point in a global coordinate system; x _L ，Y _L The coordinates of the target point in the local coordinate system.

4. The guidance control method of an unmanned autonomous vehicle as claimed in claim 1, wherein the step (B) obtains, based on a geometric relationship of a right triangle:

wherein D is the shortest distance from the center of the vehicle body to the target point; theta _S The current course of the vehicle body and the corner between the center of the vehicle body and the target point are the same; 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 (C) obtains, based on a geometrical relationship between the vehicle body center and the two steering wheels:

wherein W is the distance from the center of the vehicle body to the center of any steering wheel in the horizontal direction; l is the distance from the center of the vehicle body to the center of any steering wheel in the vertical direction; theta _a The included angle between the current course of the vehicle body and a connecting line from the center of the vehicle body to the center of any steering wheel is set; d is the distance from the center of the vehicle body to the center of any steering wheel.

6. The guidance control method for an unmanned autonomous vehicle as claimed in claim 1, wherein the step (D) is performed by first determining Y of the vehicle body in the local coordinate system when the positions of the two steered wheels of the vehicle body are advanced in translation _L The axis rotates to an included angle theta parallel to a target straight line formed by a connecting line from a starting point to the target point, the two steering wheels rotate to the included angle theta in a translation mode, the target straight line is divided into two sections, the included angle theta is used as a reference, and the control quantities of the two steering wheels are calculated according to the geometric relation of a right triangle and are respectively theta + arctan (x/y), wherein x is the distance from the center of the vehicle body to the starting point, and y is the distance from the starting point to the middle point of the target straight line.

7. The guidance control method of an unmanned autonomous vehicle as claimed in claim 6, wherein when the attitude of two steering wheels of the vehicle body is turning, if the attitude deviation of the vehicle body is larger than a deviation of a set angle, the control amount of one of the steering wheels is θ + arctan (x/y) and the control amount of the other steering wheel is- [ θ + arctan (x/y) ] when the vehicle body is turning left or turning right, the control amount of the two steering wheels is multiplied by a negative sign according to the attitude direction to be corrected of the vehicle body.

8. The guidance control method of an unmanned autonomous vehicle as claimed in claim 1, wherein the step (F) performs steering of two steering wheels of the vehicle body, and then performs the next step:

(G) 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 by adopting inverse kinematics in a reverse thrust mode, so that the steering driving system controls two steering wheels 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.