Guidance control method for unmanned self-driving vehicles
Through the guidance control method of the dual-rudder-wheel unmanned self-driving vehicle, the automatic guidance device and cosine theorem calculate the turning direction and rotation radius, and control the steering wheel, solving the problems of low navigation efficiency and unsmooth path of the traditional automatic guidance vehicle. Efficient navigation control is achieved.
Patent Information
- Application Number
- CN202110620239.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-06-03
- Publication Date
- 2024-12-06
- Estimated Expiration
- 2041-06-03
AI Technical Summary
Traditional automatic guide vehicles require a lot of complex calculations during path planning and navigation, resulting in inefficient navigation and unsmooth paths, which are prone to deviations from the predetermined paths.
The guidance control method of the dual-rudder wheel unmanned self-driving vehicle is adopted. The coordinate system is established in the center of the vehicle body through the automatic guidance device, the shortest distance and angle are calculated, the turning direction and rotation radius are judged using the cosine theorem, and the steering wheel is controlled to achieve a smooth path. Guide and adjust the speed and rotation radius through PID control and inverse kinematic calculation.
It improves navigation efficiency, reduces computing complexity and processing cycles, ensures that the vehicle body moves stably along the predetermined path, and reduces the risk of path deviation.
Smart Images

Figure CN115431969B_ABST
Abstract
Description
Technical Field
[0001] The present invention provides a guidance control method, in particular to a guidance control method for an unmanned self-propelled vehicle with dual steering wheels. Background Art
[0002] The current global trend of declining birth rates has led to a shortage of labor resources and rising labor costs year by year, and has gradually transformed from a labor-intensive industry to a technology-intensive industry. As various operating costs continue to rise, how to reduce various costs has become the key to whether a company can make a profit. With the introduction of automation technology, the rapid development of the Internet of Things and artificial intelligence, smart manufacturing and smart factories have gradually been applied to industrial production and manufacturing, and more and more tasks have been replaced by industrial robots to solve the problem of labor shortage.
[0003] However, an Automated Guided Vehicle (AGV) or unmanned transport vehicle is a transport vehicle equipped with an electromagnetic or optical automatic guidance device and a combination of environmental perception, path planning and decision-making, and unmanned automatic control. It belongs to the category of wheeled mobile robots (WMR-Wheeled Mobile The main function of an automatic guided vehicle is to automatically walk and stop at a designated location or workstation under the monitoring of a computer or vehicle-mounted system according to the path planning and operation requirements, and complete a series of operation functions. Generally, an automatic guided vehicle can control its route through a vehicle-mounted system or computer, or can use indicator marks (such as electromagnetic tracks, reflective sheets with reflective properties, paint or ribbon positioning marks) set up on walls, pillars or the ground along its route as a guide, and install electromagnetic or optical sensors (such as electromagnetic sensors, visual sensors, ultrasonic sensors or laser sensors, etc.) on the automatic guided vehicle to detect the indicator marks as the positioning and position correction of the vehicle operation. In addition to allowing the vehicle body to automatically travel along the predetermined planned guide path, and because there is no need to lay tracks, brackets and other fixing devices in the activity area, it will not be restricted by the site, road and space. Therefore, automatic guided vehicles are widely used in automatic transportation of materials, surveillance and patrol of warehouses, and operations in harmful places.
[0004] Most of the traditional automatic guided vehicles' path planning is a path composed of lines connecting points in a grid, and the above-mentioned guidance method is used to enable the automatic guided vehicle to move along the predetermined path. However, the above-mentioned guidance method requires a large number of complex calculations to capture physical markers or features in the environment in order to determine the direction and speed that the automatic guided vehicle should go. The calculation processing cycle is long, which reduces the overall navigation efficiency. In addition, the path planning is not a smooth curve, which causes the automatic guided vehicle to have less smooth turns during the movement. The actual path of the vehicle will deviate from the predetermined path, and it is necessary to continuously compare and correct the position and heading, which is the direction that the industry is eager to study and improve. Summary of the invention
[0005] The main purpose of the present invention is that the body of the unmanned self-propelled vehicle includes two steering wheels for driving and controlling steering and at least two auxiliary rotating wheels, and the automatic guidance device locates the position and posture of the vehicle body, and generates a predetermined 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.) to establish a vehicle coordinate system, and performs coordinate system conversion to establish a local coordinate system, the shortest distance from the center of the vehicle body to one of the target points in the predetermined planned target path, the turning angle between the center of the vehicle body and the target point, and the turning radius from the center of the vehicle body to the target point can be calculated, and then the angle between the current heading of the vehicle body and the center of the steering wheel is calculated according to the distance from the center of the vehicle body to the center of any steering wheel, so that the direction in which the vehicle body needs to turn can be determined, and the turning radius and turning angle of the two steering wheels are obtained according to the cosine theorem, so that the steering drive system can control the two steering wheels to turn to the corresponding position according to the calculated turning angle and rate, thereby realizing the guidance control of the unmanned self-propelled vehicle to run along the predetermined planned target path, and does not require a large number of complex calculations or a long processing cycle, which can effectively improve the overall navigation efficiency.
[0006] The secondary purpose of the present invention is that since the geometric relationship between the center of the vehicle body and the two steering wheels is fixed, and when the speed V of the center of the vehicle body and the rotation radius R from the center of the vehicle body to the target point are known, it can be obtained:
[0007]
[0008] where θ a is the angle between the current heading of the vehicle and the line from the center of the vehicle to the center of any steering wheel; d is the distance from the center of the vehicle to the center of any steering wheel.
[0009] Then, the angle θ between the current heading of the vehicle and the line connecting the center of the vehicle to the target point can be calculated. S , determine the direction in which the vehicle needs to turn, assuming θ S is greater than 0, according to the cosine theorem we can get:
[0010]
[0011]
[0012]
[0013]
[0014] Assume θ S is less than 0, according to the cosine theorem we can get:
[0015]
[0016]
[0017]
[0018]
[0019] The R lf is the turning radius of the front steering wheel; R rr is the rotation radius of the rear steering wheel; θ lf The front steering wheel rotates along the radius R lf The turning angle required for turning; θ rr The rear steering wheel rotates along the radius R rr The required turning angle.
[0020] In addition, since the angular velocity of the center of the vehicle body when it moves in a circular motion at a constant rate is equal to the angular velocity of the two steering wheels (i.e. ω = ω lf =ω rr ), and according to the relationship between the velocity V of the center of the vehicle, the rotation radius R and the angular velocity ω (i.e. V = R * ω), we can get:
[0021]
[0022]
[0023] The V f is the speed of the front steering wheel; V r is the speed of the rear steering wheel.
[0024] Therefore, the automatic guidance device can first calculate the rotation radius required for the two steering wheels of the vehicle body to move, and then infer the required turning angle of the two steering wheels, and then obtain the speed of the two steering wheels according to the relationship between the speed, rotation radius and angular speed. The two steering wheels can be controlled to turn to the corresponding position through the steering drive system, and move at different turning angles and speeds (i.e., speed difference) when turning, so that the vehicle body can stably remain on the predetermined planned target path.
[0025] Another object of the present invention is to first adjust the Y position of the vehicle body in the local coordinate system to the position of the Y position of the vehicle body in the local coordinate system when the two steering wheels of the vehicle body are in the posture of translational forward movement. L The axis rotates to an angle θ that is parallel to the target straight line formed by the line connecting the starting point to the target point, and the two steering wheels rotate to the angle θ. Assuming that the target straight line is divided into two segments and taking the angle θ as the reference, the control amounts of the two steering wheels can be calculated to be θ+arctan(x / y) respectively; when the posture of the two steering wheels of the vehicle body is turning, if the posture deviation of the vehicle body is greater than the deviation of the set angle, the control amount of the two steering wheels is multiplied by a negative sign according to the posture direction to be corrected of the vehicle body, so that when the vehicle body turns left or right, 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)].
[0026] Another object of the present invention is that when the steering drive system completes the controlled steering of the two steering wheel angles, 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 based on the error to obtain the corrected speed and rotation radius of the vehicle body, and then use inverse kinematics to calculate the speed or acceleration required for the vehicle body to move to the target path in a reverse manner, so that the steering drive system can control the two steering wheels 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. BRIEF DESCRIPTION OF THE DRAWINGS
[0027] Figure 1 Schematic diagram of the unmanned self-driving vehicle system of the present invention.
[0028] Figure 2 The figure is a flowchart of the steps of a preferred embodiment of the present invention.
[0029] Figure 3 It is a schematic diagram of coordinate system transformation of the position and posture of the vehicle body of the present invention.
[0030] Figure 4 It is a schematic diagram of the algorithm for controlling the steering wheel steering drive of the vehicle body of the present invention.
[0031] Figure 5 A block diagram of the target path closed-loop guidance control of the present invention.
[0032] Figure 6 Schematic diagram of the present invention for correcting the turning radius of a vehicle body (I).
[0033] Figure 7 Schematic diagram (II) of correcting the turning radius of a vehicle body according to the present invention.
[0034] Figure 8 It is a schematic diagram of the straight line control of the vehicle body of the present invention.
[0035] Fig. 9It is a schematic diagram of the posture direction of the vehicle body relative to the target path of the present invention.
[0036] Fig.10 This is a schematic diagram of steering control when the vehicle body of the present invention is in a translational forward posture.
[0037] Fig.11 It is a schematic diagram of the posture of the vehicle body of the present invention being corrected to turn left.
[0038] Fig.12 It is a schematic diagram of the posture correction of the vehicle body of the present invention to turn right.
[0039] Explanation of the reference numerals: 1-unmanned self-propelled vehicle; 11-vehicle body; 111-steering wheel; 111a-left front wheel; 111b-right rear wheel; 112-turning wheel; 12-automatic guidance device; 121-sensor module; 122-path planning unit; 13-steering drive system. DETAILED DESCRIPTION
[0040] In order to achieve the above-mentioned purpose and its effect, the technical means and detailed structure adopted by the present invention are illustrated in detail with reference to the preferred embodiment of the present invention, and its structure and function are as follows for a complete understanding.
[0041] See also Figure 1 to Figure 4 As shown, they are respectively a schematic diagram of the unmanned self-propelled vehicle system of the present invention, a flow chart of steps of a preferred embodiment, a schematic diagram of coordinate system conversion of the position and posture of the vehicle body, and a schematic diagram of an algorithm for controlling the steering wheel steering drive of the vehicle body. It can be clearly seen from the figure that the unmanned self-propelled vehicle 1 of the present invention includes a vehicle body 11, an automatic guiding device 12 and a steering drive system 13, and the wheel module below the vehicle body 11 includes two steering wheels 111 (i.e., driving wheels for driving and controlling steering) arranged diagonally in front and rear directions, and the rotating shafts of the two steering wheels 111 are respectively connected to two other diagonally arranged rotating wheels 112 (i.e., driven wheels for carrying or assisting steering), but it is not limited to this, and two or more rotating wheels 112 can also be separately arranged at appropriate positions of the vehicle body 11, and after the automatic guiding device 12 receives the task instruction issued by the control management center through the communication interface, it can control the steering drive system 13 through the vehicle system or the vehicle controller to drive the wheel module, so that the vehicle body 11 runs along the predetermined planned target path, so as to form an automatic guided vehicle (AGV), an autonomous mobile robot (Automated Mobile Robot, AMR) or a mobile carrier, etc.
[0042] In the present embodiment, the unmanned self-propelled vehicle 1 uses the steering wheel 111 on the left front side of the vehicle body 11 as the left front wheel 111a, and the steering wheel 111 on the right diagonally rear side of the vehicle body 11 as the right rear wheel 111b for driving and controlling steering, but the present invention is not limited thereto. The positions of the two steering wheels 111 can also be changed to a right front wheel and a left rear wheel diagonally arranged according to the actual design of the vehicle body 11, and then cooperated with two rotating wheels 112 arranged at another diagonal side of the vehicle body 11 or rotating wheels 112 at other appropriate positions to carry or assist steering. In order to make the vehicle body 11 have better stability, two rotating wheels 112 (such as fork wheels) can also be installed on the cargo fork of the vehicle body 11 to play a supporting role, so as to form a forklift-type automatic guided vehicle, a tow plate-type or a stacker-type automatic guided vehicle suitable for carrying or transferring heavy-loaded goods.
[0043] In addition, the steering wheel 111 used in the body 11 of the unmanned self-driving 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, an encoder, etc.), and has a driving and controlling steering, and can realize the linear movement and steering functions of the body 11 with two degrees of freedom, and 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 (Inertial Measurement Unit, IMU), etc.] and an external sensor [such as a laser sensor, an optical radar (Light Detection and Ranging, LiDAR) scanner, an ultrasonic (Sonar) sensor or a 3D vision sensor (3D Camera), etc., and the internal sensor locates the position and posture of the vehicle body 11, so that the automatic guidance device 12 can use the environmental information obtained by the external sensor to correct the position or posture based on this positioning, and the path planning unit 122 adopts an algorithm to plan the path of the vehicle movement according to the pre-set and perform navigation / guidance, and the path navigation / guidance control method 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 planned target path, thereby realizing the positioning and position control of the vehicle body 11.
[0044] Specifically, the fixed path navigation / guidance control of the unmanned self-driving vehicle 1 uses physical markers (such as electromagnetic tracks, magnetic tapes, reflective sheets, etc.) set up on the moving path as a guide, and the sensor module 121 of the automatic guidance device 12 detects the markers to locate the position and posture of the vehicle body 11, so as to run along the target path predetermined and 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). The virtual path navigation / guidance control of the unmanned self-driving vehicle 1 does not have physical markers, and the configuration map data of the moving path of the vehicle body 11 is stored in the database or the 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 can determine the predetermined planned target path by itself, including but not limited to inertial navigation (Inertial Navigation), laser navigation (Laser Navigation). There are many ways of path navigation / guidance control for the unmanned self-driving vehicle 1, such as ultrasonic navigation, visual navigation or geographic navigation [such as global positioning system navigation (Global Position System)], which will not be described in detail here.
[0045] like Figure 2 As shown, the guidance control method adopted by the unmanned self-propelled vehicle system of the present invention includes the following implementation steps:
[0046] (S101) The automatic guidance device 12 of the unmanned self-driving 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.
[0047] (S102) Calculate the shortest distance from the center of the vehicle body 11 to one of the target points in the predetermined target path, the turning angle between the current heading of the vehicle body 11 and the line connecting the center of the vehicle body 11 to the target point, and the turning radius from the center of the vehicle body 11 to the target point.
[0048] (S103) Obtain the speed of the center of the vehicle body 11, and calculate the angle between the current heading of the vehicle body 11 and the center of the steering wheel 111 based on the distance from the center of the vehicle body 11 to the center of any steering wheel 111.
[0049] (S104) Determine the turning direction of the vehicle body 11, and obtain the rotation radius of the two steering wheels 111 and the turning angle required for turning according to the cosine theorem.
[0050] (S105) The speeds of the two steering wheels 111 are obtained based on the relationship between the speed of the center of the vehicle body 11, the rotation radius and the angular velocity.
[0051] (S106) The steering drive system 13 controls the two steering wheels 111 to turn to corresponding positions according to the calculated turning angle and speed, so that the center of the vehicle body 11 can stably move forward along the target path.
[0052] (S107) The automatic guidance device 12 calculates the error between the current position and rotation radius of the vehicle body 11 and the target path, and uses PID control to obtain the corrected speed and rotation radius, and then uses inverse kinematics to back-calculate the speed of the vehicle body 11, so that the steering drive system 13 can control the two steering wheels 111 to correct and adjust the current position and rotation radius of the vehicle body 11.
[0053] It can be clearly seen from the figure and the above-mentioned implementation steps that the unmanned self-propelled vehicle 1 of the following specification of the present invention is preferably implemented by taking a forklift-type automatic guided vehicle as an example, and using the sensor module 121 of the automatic guided device 12 to locate the position and posture of the vehicle body 11, and the path planning unit 122 generates a predetermined planned target path. Since the driving mechanism of the vehicle body 11 mainly uses the two steering wheels 111 (i.e., driving wheels) arranged diagonally at the front and rear ends with a steering function, and cooperates with the two rotating wheels 112 (i.e., driven wheels) arranged at the other diagonal ends for operation, the actual moving path trajectory is only related to the steering angle or heading angle of the steering wheel 111. Therefore, as long as the steering angle or heading angle of the steering wheel 111 is controlled, the path guidance control of the unmanned self-propelled vehicle 1 can be achieved.
[0054] In this embodiment, in order to use the automatic guiding device 12 to first establish a global coordinate system (Global Coordinate System) in the environment where the unmanned self-driving vehicle 1 is located (such as Figure 3 X in G Y G coordinate plane), and obtain the coordinates (X C , Y C ) as the center point C, and the target point P as one of the target points of the predetermined target path, and the predetermined target path includes a straight path and a curved path to establish a vehicle coordinate system (such as X GM Y GM Coordinate plane), and then use the rotation matrix to transform the coordinate system to establish a local coordinate system (Local Coordinate System) (such as X L Y L Coordinate plane) we can get:
[0055]
[0056] The θ is the current attitude angle of the vehicle body 11, which can be expressed as the X GM or Y GM Axis rotates to the local coordinate system X L or Y L The angle of the axis; X C is the Y coordinate system of the global coordinate system G Axis and vehicle coordinate system Y GM The distance between the axes; Y C is the X coordinate system of the global coordinate system G Axis and vehicle coordinate system X GM Axis spacing; X G , Y G is the coordinates (X, Y) of a target point P in the planned target path in the global coordinate system; L , Y L is the coordinates (X, Y) of the target point P in the local coordinate system to locate the current position and posture of the vehicle body 11.
[0057] According to the geometric relationship of the right triangle, we can get:
[0058]
[0059]
[0060]
[0061] Where D is the center point C (X C , Y C ) to the target point P(X L , Y L )’s shortest path distance; θ S Y is the local coordinate system L The angle of the axis rotating clockwise to the target point P can be expressed as the angle between the current heading of the vehicle body 11 and the line connecting the center of the vehicle body 11 (i.e., the center point C) to the target point P; since the two steering wheels 111 (i.e., the left front wheel 111a and the right rear wheel 111b) diagonally arranged at the front and rear of the vehicle body 11 are in a direction consistent with the current heading of the vehicle body 11, at D, θ S If it is known, R can be obtained as the rotation radius from the center of the vehicle body 11 to the target point P according to the geometric relationship.
[0062] like Figure 4As shown, after the automatic guidance device 12 obtains the current position (such as x, y, θ) of the center of the vehicle body 11, the distance D of the shortest path 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 wheel 111 is d, it can be obtained:
[0063]
[0064]
[0065] Wherein W is a fixed distance from the center of the vehicle body 11 to the center of any steering wheel 111 (i.e., the left front wheel 111a or the right rear wheel 111b) in the horizontal direction; L is a fixed distance from the center of the vehicle body 11 to the center of any steering wheel 111 in the vertical direction; θ a is the Y coordinate of the vehicle body 11 in the local coordinate system L The angle of the axis rotating counterclockwise to the center of any steering wheel 111 can be expressed as the angle between the current heading of the vehicle body 11 and the line connecting the center of the vehicle body 11 to the center of any steering wheel 111; d is the fixed distance from the center of the vehicle body 11 to the center of any steering wheel 111.
[0066] Then, the angle (i.e., turning angle or heading angle) θ between the current heading of the vehicle body 11 and the line connecting the center of the vehicle body 11 to the target point P can be calculated. S , determine the direction in which the vehicle body 11 needs to turn, assuming that the turning angle θ of the vehicle body 11 is determined S When is greater than 0 (i.e. the positive sign indicates the direction of counterclockwise turning), according to the cosine theorem, we can get:
[0067]
[0068]
[0069]
[0070]
[0071] Assume that the turning angle θ of the vehicle body 11 is determined S When it is less than 0 (i.e. the negative sign indicates the direction of clockwise turning), according to the cosine theorem, we can get:
[0072]
[0073]
[0074]
[0075]
[0076] The R lf is the distance from the center of the front steering wheel 111 (i.e., the left front wheel 111a) to the center of the circle O around the center of the vehicle body 11 with a rotation radius R; rr θ is the distance from the center of the rear steering wheel 111 (i.e., the right rear wheel 111b) to the center of the circle O around the center of the vehicle body 11 with a rotation radius R; lf The front steering wheel 111 (i.e., the left front wheel 111a) rotates along the rotation radius R lf The turning angle or heading angle required when turning; θ rr The rear steering wheel 111 (i.e., the right rear wheel 111b) rotates along the rotation radius R rr The turning angle or heading angle required when making a turn.
[0077] In addition, since the angular velocity of the center of the vehicle body 11 when it moves in a circular motion at a constant rate is equal to the angular velocity of the front and rear steering wheels 111 (i.e., ω = ω lf =ω rr ), and when the current speed V of the center of the vehicle body 11 is known, according to the relationship between the speed V (i.e., average speed) of the center of the vehicle body 11, the rotation radius R and the angular speed ω (i.e., V = R * ω), it can be obtained:
[0078]
[0079]
[0080] The V f V is the speed of the front steering wheel 111 (i.e., the left front wheel 111a); r is the speed of the rear steering wheel 111 (i.e., the right rear wheel 111b). Therefore, the automatic guiding device 12 can first obtain the required rotation radius R of the two steering wheels 111 (i.e., the left front wheel 111a and the right rear wheel 111b) of the vehicle body 11 when traveling along a straight path or a curved path. lf , R rr , and then the required turning angle θ of the two steering wheels 111 is obtained by back-calculation lf ,θ rr Then, the speed V required by the two steering wheels 111 is obtained according to the relationship between the speed, the rotation radius and the angular speed. f 、V r, the two steering wheels 111 of the vehicle body 11 can be controlled to turn to the corresponding positions through the steering drive system 13, and when the vehicle body 11 turns, it can move at different angles and speeds (i.e., speed difference), so that the center of the vehicle body 11 follows a straight path or a curved path, thereby achieving guidance control of the unmanned self-driving vehicle 1 to run along a predetermined target path. In addition, the steering drive algorithm adopted by the built-in processor of the on-board controller or the automatic guidance device 12 does not require a large number of complex calculations or a long calculation processing cycle, and the calculation performance requirements for the on-board controller or processor are relatively reduced, which can also effectively improve the overall navigation efficiency.
[0081] Please refer to Figure 5 to Figure 7 As shown, there are respectively a block diagram of the closed-loop guidance control of the target path of the present invention, a schematic diagram of correcting the rotation radius of the vehicle body turning (I), and a schematic diagram of correcting the rotation radius of the vehicle body turning (II). 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 to form a closed-loop control process, thereby realizing the control adjustment of the periodic cycle of the vehicle body 11.
[0082] When the vehicle 11 moves along the target path, the automatic guidance device 12 can perform coordinate conversion on the current position and rotation radius of the vehicle 11 and calculate the error between the current position and rotation radius of the vehicle 11 and the target path. d and error R ), and then PID control is performed based on the error to obtain the corrected speed V* and rotation radius R* of the vehicle 11. Then, inverse kinematics can be used to calculate the speed V or acceleration A required for the vehicle 11 to move to the target path in a reverse manner, such as the speed V of the steering wheel 111 (i.e., the left front wheel 111a) in front of the vehicle 11. f , the speed V of the rear steering wheel 111 (i.e., the right rear wheel 111b) r , and the acceleration A of the front and rear steering wheels 111 f , A r , so that the steering drive system 13 can control the two steering wheels 111 to correct and adjust the current position and rotation radius of the vehicle body 11, and repeatedly correct it so that the moving state of the vehicle body 11 conforms to the desired target path until the path guidance control of the vehicle body 11 is completed.
[0083] In this embodiment, the predetermined target path generated by the automatic guidance device 12 can guide the vehicle 11 to run along a straight path or a curved path, and give the vehicle 11 a current position and a rotation radius, and can continuously detect the error between the current position and rotation radius of the vehicle 11 and the target path, wherein the error dis the error between the current position of the vehicle 11 and the final position of the target path, and error R =RR false is the error between the rotation radius of the center of the vehicle body 11 and the rotation radius caused by the deviation of the vehicle body 11. Then, different algorithms of PID control can be used to calculate the corrected speed V* of the vehicle body 11 = K PR *(error d ), and the corrected rotation radius of the vehicle body 11 R*=K PR *(R-error R ), where K PR is the gain (proportional coefficient).
[0084] When the vehicle body 11 is traveling on a curved path, the turning range of the vehicle body 11 can be changed by adjusting the rotation radius. For example, when the rotation radius of the center of the vehicle body 11 becomes larger, it means that the vehicle body 11 has deviated to the outside of the curved path, and the turning range of the vehicle body 11 adjusted to change should become smaller; in other words, when the rotation radius of the vehicle body 11 becomes smaller, it means that the vehicle body 11 has deviated to the inside of the curved path, and the turning range of the vehicle body 11 adjusted to change should become larger. As for the turning direction of the center of the vehicle body 11, it can be determined in the algorithm. Therefore, the rotation radius of the center of the vehicle body 11 can be used as a variation to correct the steering of the vehicle body 11, so that when the vehicle body 11 deviates from the curved path, the deviation can be corrected quickly and accurately, and the vehicle body 11 can be stably maintained on the predetermined planned target path.
[0085] Please also see Figure 8 to Figure 12 As shown, they are respectively a schematic diagram of the straight line control of the vehicle body of the present invention, a schematic diagram of the posture direction of the vehicle body relative to the target path, a schematic diagram of the steering control of the vehicle body in which the posture is a translational forward movement, a schematic diagram of the vehicle body's posture correction to turn left, and a schematic diagram of the vehicle body's posture correction to turn right. It can be clearly seen from the figure that the predetermined planned target path generated by the above-mentioned automatic guidance device 12 is divided into multiple line segments using multiple target points P0~P9, and the multiple line segments are connected to form a straight line path trajectory, 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 end point of the straight line path, and is divided according to a predetermined length according to the straight line equation y=ax+b. When the vehicle body 11 is moving, according to the vehicle coordinate system, the first target point P1 is on the left side of the vehicle body 11 (such as Fig. 9 As shown), the included angle [arctan(x / y)] between the center of the vehicle body 11 and the target point P1 is calculated as the control of the two steering wheels 111 of the vehicle body 11. The turning direction of the vehicle body 11 can also be determined by this. Generally, the counterclockwise turning direction is taken as a positive sign, and the clockwise turning direction is taken as a negative sign.
[0086] In this embodiment, the unmanned self-driving vehicle 1 takes the two steering wheels 111 disposed diagonally at the front and rear of the vehicle body 11 as the right front wheel and the left rear wheel, and takes the distance from the center of the vehicle body 11 (i.e., the center point C) to the target point P1 as the hypotenuse of the right triangle, and the point where the opposite side and the adjacent side of the right triangle intersect perpendicularly as the starting point S (e.g., Fig.10 As shown in FIG. 1 , when the vehicle body 11 moves forward while tracking the target point P1 on the straight path, the Y coordinate of the vehicle body 11 in the local coordinate system is firstly L The axis rotates counterclockwise until it is parallel to the target straight line formed by the line connecting the starting point S to the target point P1, which can be expressed as the current heading of the vehicle body 11 rotates to an angle θ that is parallel to the target straight line, and the two steering wheels 111 (i.e., the right front wheel and the left rear wheel) of the vehicle body 11 are rotated to the angle θ in a translational manner. Assuming that the target straight line is divided into two segments and the angle θ is used as a reference, the control amounts of the two steering wheels 111 of the vehicle body 11 can be calculated according to the geometric relationship of the right triangle as θ+arctan(x / y), where x is the distance from the center of the vehicle body 11 to the starting point S, and y is the distance from the starting point S to the midpoint M of the target straight line.
[0087] In addition, when the vehicle body 11 is turning, if the attitude deviation of the vehicle body 11 is greater than the set angle, the two steering wheels 111 (i.e., the right front wheel and the left rear wheel) need to multiply their control amounts by a negative sign according to the attitude direction to be corrected by the vehicle body 11. For example, when the vehicle body 11 is turning left (such as Fig.11 As shown), 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 right (as shown Fig.12 As shown), 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 move forward along the target path.
[0088] The above detailed description is only for a preferred feasible embodiment of the present invention, and the embodiment is not used to limit the scope of the present invention. Any other equivalent changes and modifications made without departing from the technical spirit disclosed by the present invention should be included in the scope covered by the present invention.
[0089] In summary, the guidance and control method of the unmanned self-propelled vehicle of the present invention can indeed achieve its efficacy and purpose when used.
Claims
1. A guidance and control method for an unmanned self-propelled vehicle, characterized in that: The unmanned self-propelled vehicle comprises a vehicle body, an automatic guidance device and a steering drive system, and the vehicle body comprises two steering wheels and at least two auxiliary rotating wheels arranged at front and rear diagonals for driving and controlling steering. The automatic guidance device is used to locate the position and posture of the vehicle body and generate a predetermined target path for the steering drive system to drive the steering wheel of the vehicle body to run along the target path. The guidance 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 the global coordinate system, and obtains the current attitude angle of the vehicle body to perform coordinate system conversion to establish a local coordinate system; (B) Calculate the shortest distance D from the center of the vehicle to one of the target points on the target path, the angle θ between the current heading of the vehicle and the line connecting the center of the vehicle to the target point S , and the rotation radius R from the center of the vehicle body to the target point; (C) Obtain the speed V of the center of the vehicle body, and calculate the angle θ between the current heading of the vehicle body and the line connecting the center of the vehicle body to the center of any steering wheel based on the distance d from the center of the vehicle body to the center of any steering wheel. a ; (D) According to the turning angle θ of the vehicle body S Determine the direction in which the vehicle turns, assuming θ S When is greater than 0, according to the cosine theorem: Assume that the vehicle's turning angle θ S When is less than 0, according to the cosine theorem: Where R lf is the turning radius of the front steering wheel; R rr is the rotation radius of the rear steering wheel; θ lf The front steering wheel rotates along the radius R lf The turning angle required for turning; θ rr The radius of rotation of the rear steering wheel is R rr The turning angle required when turning; (E) Since the angular velocity of the center of the vehicle body in a constant circular motion is equal to the angular velocity of the two steering wheels, according to the relationship between the velocity V of the center of the vehicle body, the rotation radius R and the angular velocity, we can obtain: Where V f is the speed of the front steering wheel; V r is the speed of the rear steering wheel; (F) The steering drive system is based on the calculated turning angle θ lf ,θ rr and rate V f 、V r The two steering wheels are controlled to turn to corresponding positions so that the center of the vehicle body moves forward along the target path.
2. The guidance control method of the unmanned self-propelled vehicle according to claim 1, characterized in that: The automatic guidance device comprises a sensor module for locating the position and posture of the vehicle body and a path planning unit for generating the target path.
3. The guidance control method of the unmanned self-propelled vehicle according to claim 1, characterized in that: This step (A) is to use the rotation matrix to transform the coordinate system: Where θ is the current attitude angle of the vehicle; X C Y is the global coordinate system G Axis and vehicle coordinate system Y GM Axis spacing; Y C is the X coordinate system of the global coordinate system G Axis and vehicle coordinate system X GM Axis spacing; X G , Y G is the coordinate of the target point in the global coordinate system; X L , Y L is the coordinate of the target point in the local coordinate system.
4. The guidance control method of the unmanned self-propelled vehicle according to claim 1, characterized in that: This step (B) is obtained based on the geometric relationship of the right triangle: Where D is the shortest distance from the center of the vehicle to the target point; θ S is the angle between the current heading of the vehicle and the line from the center of the vehicle to the target point; R is the rotation radius from the center of the vehicle to the target point.
5. The guidance control method of the unmanned self-propelled vehicle according to claim 1, characterized in that: The step (C) is obtained based on the geometric relationship between the center of the vehicle body and the two steering wheels: Where W is the horizontal distance from the center of the vehicle body to the center of any steering wheel; L is the vertical distance from the center of the vehicle body to the center of any steering wheel; θ a is the angle between the current heading of the vehicle and the line from the center of the vehicle to the center of any steering wheel; d is the distance from the center of the vehicle to the center of any steering wheel.
6. The guidance control method of the unmanned self-propelled vehicle according to claim 1, characterized in that: In step (D), when the two steering wheels of the vehicle body are in a translational forward position, the vehicle body is first moved in the Y direction of the local coordinate system. L The axis is rotated to an angle θ that is parallel to a target straight line formed by a line from a starting point to the target point, and the two steering wheels are rotated to the angle θ in a translational manner. Assuming that the target straight line is divided into two segments, and taking the angle θ as a reference, the control amounts of the two steering wheels are calculated according to the geometric relationship of the right triangle as θ+arctan(x / y), where x is the distance from the center of the vehicle to the starting point, and y is the distance from the starting point to the midpoint of the target straight line.
7. The guidance control method of the unmanned self-propelled vehicle according to claim 6, characterized in that: When the posture of the two steering wheels of the vehicle body is to turn, if the posture deviation of the vehicle body is greater than the deviation of the set angle, the control amount of the two steering wheels is multiplied by a negative sign according to the posture direction of the vehicle body to be corrected, so that when the vehicle body turns left or right, 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)].
8. The guidance control method of the unmanned self-propelled vehicle according to claim 1, characterized in that: This step (F) completes the steering of the two steering wheels of the vehicle body, and then proceeds to the next step: (G) The automatic guidance device calculates the error between the current position and rotation radius of the vehicle body and the target path, and uses PID control to obtain the corrected speed and rotation radius of the vehicle body based on the error, and then uses inverse kinematics to calculate the speed or acceleration required for the vehicle body to move to the target path in a reverse manner, so that the steering drive system controls the two steering wheels 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.
Citation Information
Patent Citations
Double-steering-wheel automatic guided vehicle trajectory tracking control method and system
CN111273551A
Unmanned carrying automated guided vehicle and guiding method thereof
CN111776990A
Steering wheel and control method thereof
CN111845919A
Car following control method and device
CN112445214A
Unmanned vehicle of traveling wheel independent steering and travel control method therefor
JP2010117804A
