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

Abstract

The present invention provides a guidance and control method for an unmanned self-propelled vehicle. The unmanned self-propelled vehicle includes a vehicle body, an automatic guiding device and a steering drive system, and the vehicle body includes a steering wheel for driving and controlling steering and at least two steering wheels. The path guidance method is that the automatic guidance device first obtains the position of the center of the vehicle body to establish a vehicle coordinate system, and then converts the coordinate system to establish a local coordinate system. The shortest distance of the point, the angle between the center of the car body and the target point, and the radius of rotation from the center of the car body to the target point, and then calculate the radius and angle of rotation required for the steering wheel of the car body to turn to the target point, so that the steering drive system can The calculated turning angle controls the steering wheel to turn to the corresponding position, so as to realize the guidance and control of the unmanned self-propelled vehicle to follow the predetermined planned target path, and does not require a large number of complex calculations or long processing cycles, which can effectively improve the overall performance. Navigation efficiency.

Description

Guidance control method of unmanned self-propelled vehicle

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

According to this, the shortage of labor resources and labor costs caused by the current wave of global low birthrate have been increasing year by year, and the industry has gradually transformed from labor-intensive to technology-intensive industries. Based on the continuous increase of various operating costs, how to reduce the cost of various items has become a problem. The key to whether an enterprise can make a profit, and with the introduction of automation technology, the rapid development of the Internet of Things and artificial intelligence, smart manufacturing and smart factories have been gradually applied to industrial production and manufacturing, and more and more tasks are being Replaced by industrial robots to solve the problem of labor shortages.

However, Automatic Guided Vehicle (AGV) or unmanned guided vehicle refers to an automatic guided vehicle equipped with electromagnetic or optical type, and integrates functions such as environmental perception, path planning decision-making and unmanned automatic control. The transport vehicle belongs to the category of wheeled mobile robot (WMR-Wheeled Mobile Robot). Complete a series of operation functions. Generally, the automatic guided vehicle can control its travel route through the on-board system or computer, or can use the indicator marks (such as electromagnetic track, reflective Special reflector, paint or ribbon and other positioning marks) as guidance, and equipped with electromagnetic or optical sensors (such as electromagnetic sensors, visual sensors, ultrasonic sensors or laser sensors, etc.) on the automatic guided vehicle , using the detection indicator as the positioning and position correction of the vehicle operation, in addition to the automatic driving of the vehicle body along the predetermined guide path, and because there is no need to lay tracks, brackets and other fixing devices in the active area, it will not Due to the limitation of site, road and space, automatic guided vehicles are widely used in automatic transportation of materials, monitoring and patrolling of warehouses and operations in harmful places.

Most of the path planning of traditional automatic guided vehicles is a path composed of lines connected point-to-point in a grid format, and the above-mentioned guidance method is used to enable the automatic guided vehicle to move along the predetermined path. The navigation method needs to use a large number of complex operations to capture the physical markers or features in the environment, so as to determine the direction and speed of the automatic guided vehicle. The operation processing cycle is long, which reduces the overall navigation efficiency. The planning is not a smooth curve, which causes the automatic guided vehicle to have a relatively uneven turn in the process of traveling, and the actual traveling path will deviate from the predetermined path, so it is necessary to continuously compare and correct the position and heading , which is the direction of research and improvement that those engaged in this industry are eager to study and improve.

Therefore, in view of the above-mentioned deficiencies, the inventor collected relevant information, through various evaluations and considerations, and with years of experience in this industry, continuous trial production and modification, before designing this kind of unmanned self-propelled vehicle guide. The invention patent of the control method was born.

The main purpose of the present invention is that the body of the unmanned self-propelled vehicle includes a steering wheel for driving and controlling steering and at least two auxiliary runners, and the position and attitude of the body are positioned and generated by the automatic guidance device. Pre-planned target path, when the automatic guidance device obtains the position of the center of the vehicle body (such as coordinates and attitude angles, etc.), the vehicle coordinate system is established, and the coordinate system is Convert to establish a local coordinate system to calculate the shortest distance from the center of the car body to one of the target points in the predetermined target path, the angle between the center of the car body and the target point, and the radius of rotation from the center of the car body to the target point, and then calculate The rotation radius and rotation angle required by the steering wheel to turn to the target point are obtained, so that the steering drive system can control the steering wheel to the corresponding position according to the rotation angle, so as to realize the guidance and control of the unmanned self-propelled vehicle to follow the predetermined planned target path. And it does not need a lot of complicated operations or a long processing cycle, which can effectively improve the overall navigation efficiency.

The secondary purpose of the present invention is that when the steering drive system completes the steering control of the steering wheel angle, the automatic guidance device can calculate the error between the current position of the vehicle body, the rotation radius and the target path, and use PID control according to the error. The corrected speed and rotation radius of the car body can be obtained, and then the inverse kinematics is used to calculate the speed or acceleration required for the car body to move to the target path, so that the steering drive system can control the steering wheel to correct and adjust the current speed of the car body. position and rotation radius until the path guidance control of the vehicle body is completed.

1: Unmanned self-propelled vehicles

11: Body

111: steering wheel

112: runner

12: Automatic Guidance Device

121: Sensor module

122: Path Planning Unit

13: Steering drive system

[Fig. 1] is a schematic diagram of the unmanned vehicle system of the present invention.

[Fig. 2] is a flow chart of the steps of a preferred embodiment of the present invention.

[FIG. 3] is a schematic diagram of the coordinate system transformation of the position and posture of the vehicle body according to the present invention.

[Fig. 4] is a schematic diagram of the algorithm of the present invention for the steering and driving of the steering wheel controlled by the vehicle body.

[FIG. 5] is a block diagram of the target path closed-loop guidance control of the present invention.

[Fig. 6] is a schematic diagram (1) of the present invention for correcting the turning radius of the vehicle body.

[Fig. 7] is a schematic diagram (2) of the present invention for correcting the turning radius of the vehicle body.

[Fig. 8] is a schematic diagram of the linear control of the vehicle body of the present invention.

[Fig. 9] is a schematic diagram of the attitude direction of the vehicle body relative to the target path of the present invention.

[Fig. 10] is a schematic diagram (1) of the present invention guiding the vehicle body to the next target point.

[Fig. 11] is a schematic diagram (2) of the present invention guiding the vehicle body to the next target point.

[FIG. 12] is a schematic diagram of the length of the vehicle body switching target point of the present invention.

In order to achieve the above-mentioned purpose and effect, the technical means and detailed structure adopted by the present invention are drawn to illustrate the structure and function of the preferred embodiment of the present invention in detail as follows, so as to be fully understood.

Please refer to Figures 1 to 4, which are a schematic diagram of the unmanned vehicle system of the present invention, a flow chart of the steps of a preferred embodiment, a schematic diagram of the coordinate system conversion of the position and attitude of the vehicle body, and the vehicle body control steering wheel. A schematic diagram of the steering driving algorithm, it can be clearly seen from the figure that the unmanned vehicle 1 of the present invention includes a vehicle body 11 , an automatic guiding device 12 and a steering driving system 13 , and a wheel module under the vehicle body 11 It includes a steering wheel 111 (that is, the driving wheel that drives and controls the steering) and at least two runners 112 (that is, the driven wheel that carries or assists the steering), and the automatic guidance device 12 receives the task command issued by the control management center through the communication interface Afterwards, the steering and driving system 13 can be controlled by the vehicle-mounted system or the vehicle-mounted controller to drive the wheel module, so that the vehicle body 11 runs along the predetermined target path, so as to form an automatic guided vehicle (AGV), an autonomous mobile robot (Automated Mobile Robot, AMR) or mobile vehicle, etc.

In this embodiment, the vehicle body 11 of the unmanned vehicle 1 is equipped with a steering wheel 111 in front for driving and steering control, and cooperates with two runners (such as 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 runners 112 can also be installed to provide the supporting role of the fork, so as to form a suitable for heavy cargo. Forklift-type automatic guided vehicle for handling or transfer, but not limited to this, it can also be a pallet-type or stacker-type automatic guided vehicle.

In addition, the steering wheel 111 used in the body 11 of the unmanned vehicle 1 can be a horizontal steering wheel or a vertical steering wheel, and includes a driving wheel, a driving unit (such as a driving motor, a gear box, etc.) and a steering mechanism (such as a steering motor, encoder, etc.) It is composed of a driving and steering control, and can realize the linear movement and steering functions of the vehicle body 11 with two degrees of freedom. The automatic guidance device 12 includes a sensor module 121 and a path planning unit 122, wherein The sensor module 121 includes an internal sensor (such as an encoder, an inertial measurement unit (IMU), etc.) mounted on the vehicle body 11 and an external sensor (such as a laser sensor, an optical radar, etc.) (Light Detection and Ranging, LiDAR) scanner, ultrasonic (Sonar) sensor or 3D vision sensor (3D Camera), etc.], and the position and attitude of the vehicle body 11 are positioned by the internal sensor, so that the automatic The guiding device 12 can use the environmental information obtained by the external sensor to correct the position or attitude on the basis of this positioning, and the path planning unit 122 uses an algorithm to plan the moving path of the vehicle body according to the preset and perform navigation/ Guidance, and the path navigation/guidance control can be a fixed path or a virtual path, so that the steering drive system 13 can drive the steering wheel 111 according to the predetermined target path, so as to realize the positioning and position control of the vehicle body 11 .

Specifically, the fixed path navigation/guidance control of the unmanned self-propelled vehicle 1 uses physical markers (such as electromagnetic tracks, magnetic tapes, reflective sheets, etc.) established on the moving path as guidance, and is sensed by the automatic guidance device 12 The sensor module 121 detects the marker to locate the position and attitude of the vehicle body 11 to run along the target path predetermined by the path planning unit 122, including but not limited to direct coordinate guidance (ie, Cartesian coordinate guidance, Cartesian coordinate guidance, Cartesian coordinate guidance, Cartesian coordinate guidance, Cartesian coordinate guidance, Cartesian Guidance), Electromagnetic Guidance (Wire Guidance), Magnetic Tape Guidance, or Optical Guidance (Optica) l Guidance), while the virtual route navigation/guidance control of the unmanned self-propelled vehicle 1 has no physical mark, and stores the configuration map data of the moving route of the vehicle body 11 in the database or the map library route data in the automatic guidance device 12 , and the sensor module 121 detects the position and attitude 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, Laser Navigation Navigation) or ultrasonic navigation, visual navigation (Visual Navigation) or geographic navigation (such as Global Positioning System Navigation (Global Position System)], but there are many ways of route navigation/guidance control of the unmanned vehicle 1, and I will not make one here. Repeat.

As shown in FIG. 2, the path guidance method adopted by the above-mentioned unmanned vehicle system of the present invention includes the following implementation steps:

(S101) The automatic guidance device 12 of the unmanned 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 transformation to establish a local coordinate system.

(S102) Calculate the shortest distance from the center of the vehicle body 11 to one of the target points of the predetermined target path, and calculate the angle between the center of the vehicle body 11 and the target point, and the radius of rotation from the center of the vehicle body 11 to the target point according to the geometric relationship .

( S103 ) The rotation radius and the rotation angle required for steering the steering wheel 111 of the vehicle body 11 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 according to the calculated rotation angle.

(S105) The automatic guidance device 12 calculates the current position of the vehicle body 11, the amount of error between the rotation radius and the target path, and uses PID control to obtain the corrected speed and rotation radius , and then use inverse kinematics to calculate the speed of the vehicle body 11 , so that the steering drive system 13 can control the steering wheel 111 to correct and adjust the current position and rotation radius of the vehicle body 11 .

It can be clearly seen from the figure and the above-mentioned implementation steps that the preferred implementation of the unmanned vehicle 1 in the following description of the present invention takes a forklift-type automatic guided vehicle as an example, and uses the sensor model of the automatic guided device 12. The group 121 locates the position and attitude of the vehicle body 11, and the path planning unit 122 generates a predetermined target path. Because the driving mechanism of the vehicle body 11 mainly utilizes the front steering wheel 111 (ie the driving wheel) with a steering function , and cooperates with the two rear wheels 112 (ie, driven wheels) to operate, the actual moving path trajectory is only related to the rotation angle or heading angle of the front steering wheel 111, so as long as the steering wheel 111 is controlled. Path guidance control of self-propelled vehicle 1.

In this embodiment, the automatic guidance device 12 is used to first establish a Global Coordinate System (such as the X _G Y _G coordinate plane in the third figure) in the environment where the unmanned vehicle 1 is located, and obtain the coordinates (X _C , Y _C ) of the center of the vehicle body 11 (ie, the geometric center of the steering wheel 111 ) in the global coordinate system as the center point C, and the target point P as one of the target points in the predetermined target path, and The predetermined target path includes a straight path and a curved path. After establishing the vehicle coordinate system (such as the X _GM Y _GM coordinate plane), the rotation matrix is used to convert the coordinate system to establish a Local Coordinate System (such as X _L Y _L coordinate plane) can be obtained:

Where θ is the current attitude angle of the vehicle body 11, which can be expressed as the angle between the X _GM or Y _GM axis of the vehicle coordinate system rotated to the _{XL or Y L axis} of the local coordinate system; X _C is the Y _G axis of the global coordinate system and the vehicle The distance between the Y _GM axes of the coordinate system; Y _C is the distance between the X _G axis of the global coordinate system and the X _GM axis of the vehicle coordinate system; X _G , Y _G are the predetermined target paths, and one of the target points P is in the global coordinates The coordinates (X, Y) in the system; X _L , Y _L are the coordinates (X, Y) of the target point P in the local coordinate system to locate the current position and attitude of the vehicle body 11 .

According to the geometric relationship of the right triangle, we can get:

D is the shortest path distance from the center point C (X _C , Y _C ) of the vehicle body 11 in the local coordinate system to the target point P (X _L , Y _L ); θ _S is the clockwise Y _L axis of the local coordinate system The angle of rotation to the target point P can be expressed as the angle between the center of the vehicle body 11 or the center point C and the target point P, that is, the turning angle or heading angle of the steering wheel 111 of the vehicle body 11 to the target point P; The current heading angle of the steering wheel 111 is consistent with the Y and _L axes of the vehicle body 11 in the local coordinate system. Under the condition that D and θ _S are known, according to the geometric relationship, it can be obtained that R is the center of the vehicle body 11 (that is, the geometric center of the steering wheel 111). ) to the radius of rotation of the target point P.

As shown in FIG. 4 , when the automatic guidance device 12 obtains the current position of the center of the vehicle body 11 or the geometric center of the steering wheel 111 (ie, the center point C), the distance D, and the speed V, the deviation between the current position and the target path can be calculated. quantity, and when V and D are known, according to the geometric relationship of the right triangle, it can be obtained:

where R is the radius of rotation from the center of the vehicle body 11 (ie, the geometric center of the steering wheel 111 ) to the target point P; 1 is the distance between the geometric center of the steering wheel 111 in front of the vehicle body 11 and the midpoint M of the line connecting the centers of the two rear wheels 112 Fixed distance; w is the variable distance between the midpoint M of the line connecting the centers of the two runners 112 to the origin O of the rotation radius of the center of the vehicle body 11; θ _S is the steering wheel 111 of the vehicle body 11 turning to the target point The turning angle or heading angle of P enables the steering drive system 13 to control the steering wheel 111 of the vehicle body 11 to turn to the corresponding position according to the deviation of θ _S , so as to realize the guidance for the unmanned self-propelled vehicle 1 to follow the predetermined planned target path. and the steering driving algorithm adopted by the built-in processor of the on-board controller or automatic guidance device 12 does not need to go through a large number of complex operations or a long processing cycle, which requires the computing performance of the on-board controller or processor. The relative reduction can also effectively improve the overall navigation efficiency.

Please refer to Figures 5 to 7, which are the block diagram of the closed-loop guidance control of the target path of the present invention, the schematic diagram (1) of correcting the turning radius of the car body, and the schematic diagram of correcting the turning radius of the car body. (2), it can be clearly seen from the figure that the unmanned self-propelled vehicle 1 of the present invention can perform PID (proportional, integral and differential) control according to the moving state of the vehicle body 11 and the predetermined planned target path generated by the automatic guidance device 12 , so as to form a closed-loop control process, so as to realize the control and adjustment of the periodic cycle of the vehicle body 11 .

When the vehicle body 11 moves along the target path, the automatic guidance device 12 can perform coordinate transformation between the current position of the vehicle body 11 and the rotation radius, and calculate the error between the current position of the vehicle body 11, the rotation radius and the target path (error _d and error _R ), and then perform PID control according to the error amount to obtain the corrected speed V* and rotation radius R* of the car body 11, and then use inverse kinematics (Inverse Kinematics) to calculate the car body in a reverse way 11 to move to the required speed V or acceleration A on the target path, so that the steering drive system 13 can control the steering wheel 111 to correct and adjust the current position and rotation radius of the vehicle body 11, so that repeated corrections make the moving state of the vehicle body 11 meet the desired target path until the path guidance control of the vehicle body 11 is completed.

In this embodiment, the pre-planned target path generated by the automatic guidance device 12 can guide the vehicle body 11 to follow a straight path or a curved path, and give the current position and rotation radius of the vehicle body 11, and can continuously detect The error amount between the current position of the vehicle body 11, the radius of rotation and the target path, where error _d is the error amount of the linear distance between the current position of the vehicle body 11 and the final position of the target path, and error _R =RR _false is the center of the vehicle body 11. The error of the rotation radius caused by the deviation of the rotation radius and the vehicle body 11, and then different algorithms of PID control can be used to calculate the corrected speed of the vehicle body 11 V*=K _PR *(error _d ), and the vehicle body 11 corrected speed V*=K PR *(error d ) The rear rotation radius R*=K _PR *(R-error _R ), where K _PR is the gain amount.

When the vehicle body 11 travels on a curved path, the turning radius of the vehicle body 11 can be changed by adjusting the turning radius. In other words, when the turning radius of the vehicle body 11 becomes smaller, it means that the vehicle body 11 has shifted to the inside of the curved path, and the turning range of the vehicle body 11 adjustment and change should become larger. The turning direction of the center of the body 11 can be determined in the algorithm, so the rotation radius of the center of the body 11 can be used as a variation to correct the steering of the body 11, so that when the body 11 deviates from the curved path, it can quickly And accurately correct the deviation, and stably keep it on the pre-planned target path.

Please also refer to Figures 8 to 12, which are respectively the straightening of the vehicle body of the present invention. Schematic diagram of line control, schematic diagram of the attitude direction of the car body relative to the target path, schematic diagram of the car body guiding to the next target point (1), schematic diagram of the car body guiding to the next target point (2), and car body switching The schematic diagram of the length of the target point, it can be clearly seen from the figure that the target path of the predetermined plan generated by the above-mentioned automatic guidance device 12 is divided into a plurality of line segments by using the plurality of target points P0~P9, and the plurality of line segments are connected. A straight-line path trajectory is formed, wherein the target point P0 can be represented as the starting point of the straight-line path, and the target point P9 can be represented as the final point of the straight-line path, and is divided according to the predetermined length according to the straight-line equation y=ax+b. When the vehicle body 11 travels , according to the vehicle coordinate system, the first target point P0 is on the left side of the vehicle body 11 (as shown in Fig. 9), and the angle between the center of the vehicle body 11 and the target point P0 is calculated [arctan(x/y) ] can be used as the steering control of the vehicle body 11, and can also be used to determine whether the turning direction of the vehicle body 11 is counterclockwise or clockwise.

In this embodiment, when the vehicle body 11 tracks the plural target points P0~P9 divided on the straight line path, if the center of the vehicle body 11 exceeds the target point, the next target point will be tracked (as shown in Figs. 10-11). ), as for judging whether the target point is exceeded, it can be determined whether the y-axis direction vector of the vehicle body 11 from the target point of the vehicle coordinate itself is less than 0, and if the y-axis direction vector of the target point to the vehicle body 11 is less than 0 , the tracking will start from the next target point. For example, if the positioning point n projected from the center of the vehicle body 11 on the straight line path is located between the target point P2 and the target point P3, the target point P2 on the straight line path is the most important point for the vehicle. The y-axis direction vector of the body 11 is 2-n less than 0, that is to say, y is less than 0, and the y-axis direction vector of the target point P3 on the straight path to the vehicle body 11 is 3-n is greater than 0, it can be expressed as y If it is greater than 0, and perform function operation in combination with the code, where index is the target point on the straight line, y=index-n, if y is calculated to be less than 0, the preset index needs to be increased by 1 until y is greater than 0, That is, the final index calculation result is 3, and the car body 11 will start from the third A target point P3 starts tracking.

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 is still too large, it is easy to cause the steering angle of the vehicle body 11 [arctan(x/y)] to be too large, causing the vehicle body 11 to be offset Therefore, the length of the switching target point can be set by parameters (as shown in Figure 12), thereby ensuring that the vehicle body 11 can stably follow the target path.

The above detailed description is for a preferred feasible embodiment of the present invention, but the embodiment is not intended to limit the scope of the present invention, and all other equivalent changes and modifications are completed without departing from the technical spirit disclosed in the present invention. Changes should be included in the patent scope covered by the present invention.

To sum up, the guidance control method of the unmanned self-propelled vehicle of the present invention can indeed achieve its effect and purpose when used, so the present invention is an invention with excellent practicability. I hope that the review committee will approve the case as soon as possible to protect the inventor's hard work. If the review committee has any doubts, please do not hesitate to send a letter for instructions. The inventor will make every effort to cooperate.

Claims (5)

A guidance control method for an unmanned self-propelled vehicle, the unmanned self-propelled vehicle includes a vehicle body, an automatic guidance device and a steering drive system, and the vehicle body includes a steering wheel for driving and controlling steering and at least two auxiliary steering The automatic guidance device is used to locate the position and attitude of the vehicle body, and generate a predetermined target path, so that the steering wheel of the steering drive system to drive the vehicle body can follow the target path. , the path guidance method includes the following steps: (A) the automatic guidance device obtains the position of the center of the vehicle body to establish a vehicle coordinate system in the global coordinate system, and obtains the current attitude angle of the vehicle body to convert the coordinate system to Establish a local coordinate system; (B) Calculate the shortest distance from the center of the vehicle body to one of the target points on the target path, and calculate the angle between the center of the vehicle body and the target point according to the geometric relationship, and the center of the vehicle body to the target (C) Obtain the speed of the center of the vehicle body, and calculate the rotation radius and angle required for the steering wheel of the vehicle body to turn to the target point according to the shortest distance:

,

,

D is the shortest distance; R is the radius of rotation 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 at least two auxiliary steering wheels; w is the at least two The distance from the midpoint of the steering wheel to the origin of the turning radius of the steering wheel; θ _S is the turning angle of the steering wheel to the target point; (D) The steering drive system controls the vehicle body according to the calculated turning angle The steering wheel is turned to the corresponding position. The guidance control method for an unmanned self-propelled vehicle as claimed in claim 1, wherein the automatic guidance device comprises a sensor module for locating the position and attitude of the vehicle body and a sensor module for generating the target path Path planning unit. The guidance control method for an unmanned self-propelled vehicle as claimed in claim 1, wherein the step (A) is to perform coordinate system transformation using a rotation matrix:

Among them, θ is the current attitude angle of the vehicle body; X _C is the distance between the Y _G axis of the global coordinate system and the Y _GM axis of the vehicle coordinate system; Y _C is the distance between the X _G axis of the global coordinate system and the X _GM axis of the vehicle coordinate system. Distance; X _G , Y _G are the coordinates of the target point in the global coordinate system; _{XL , Y L are} the coordinates of the target point in the local coordinate system. The guidance control method for an unmanned self-propelled vehicle as claimed in claim 1, wherein the calculation method of the step (B) is obtained according to the geometric relationship of a right triangle:

,

,

Where D is the shortest distance from the center of the vehicle body to the target point; θ _S is the angle between the center of the vehicle body and the target point; R is the radius of rotation from the center of the vehicle body to the target point. The guidance control method for an unmanned self-propelled vehicle as claimed in claim 1, wherein the step (D) completes the steering wheel steering of the vehicle body, and then executes the next step: (E) the automatic guidance device calculates the The amount of error between the current position, the radius of rotation and the target path, and according to the amount of error, PID control is used to obtain the corrected speed and radius of rotation of the vehicle body, and then inverse kinematics is used to calculate the movement of the vehicle body to The required speed or acceleration on the target path enables the steering driving system to control the steering wheel of the vehicle body to correct and adjust the current position and rotation radius of the vehicle body until the path guidance control of the vehicle body is completed.

Images