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

Abstract

The present invention provides a guidance control method for an unmanned self-propelled vehicle. The guidance control method uses an automatic guidance device to obtain the position of the center of the vehicle body to establish a vehicle coordinate system, and converts the coordinate system to establish a local coordinate system, which can calculate The shortest distance from the center of the car body to one of the target points on 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 according to the distance from the center of the car body to the center of any steering wheel By calculating the angle between the current heading of the car body and the center of the steering wheel, the direction of the car body to turn can be determined, and the rotation radius and angle of the two steering wheels can be obtained according to the cosine law, so that the steering drive system can be controlled according to the calculated rotation angle and speed. The two steering wheels are steered to the corresponding positions, so as to realize the guidance and control of the unmanned self-propelled vehicle to follow the pre-planned target path, and it does not require a large number of complex calculations or a long processing cycle, which can effectively improve the overall navigation efficiency.

Description

Guidance control method of unmanned self-propelled vehicle

The present invention provides a guidance control method, in particular to a guidance control method of an unmanned vehicle with dual steering wheels.

According to this, the shortage of labor resources and labor costs caused by the current wave of declining birthrates in the world have been increasing year by year, and the industry has gradually transformed from labor-intensive to technology-intensive industries. Based on the continuous increase in operating costs, how to reduce the costs of various items has become a 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 mark as the positioning and position correction of the vehicle operation, in addition to the automatic driving of the vehicle body along the pre-planned guide path, and because there is no need to lay tracks, brackets and other fixing devices in the active area, it will not be 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 vehicle body of the unmanned self-propelled vehicle includes two steering wheels for driving and controlling steering and at least two auxiliary runners, and the position and attitude of the vehicle body are positioned by the automatic guidance device, and Generate a predetermined target path. When the automatic guidance device obtains the position of the center of the vehicle body (such as coordinates and attitude angles), the vehicle coordinate system is established, and the coordinate system is carried out. Convert to establish a local coordinate system, you can 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 According to the distance between the center of the car body and the center of any steering wheel, the angle between the current heading of the car body and the center of the steering wheel can be calculated, and the direction in which the car body needs to turn can be determined. The steering drive system can control the two steering wheels to turn to the corresponding position according to the calculated turning angle and speed, so as to realize the guidance control of the unmanned self-propelled vehicle to follow the predetermined planned target path, and does not need a lot of complex calculations or long time. The processing cycle can effectively improve the overall navigation efficiency.

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 radius of rotation R from the center of the vehicle body to the target point are known, it is possible to obtain :

The θ _a is 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; d is the distance from the center of the vehicle body to the center of any steering wheel.

Then, according to the angle θ _S between the current heading of the vehicle body and the line connecting the center of the vehicle body to the target point, the direction in which the vehicle body needs to turn can be determined. Assuming that θ _S is greater than 0, according to the cosine law, it can be obtained:

Assuming that θ _S is less than 0, according to the law of cosines, we can get:

R _lf is the rotation radius of the two steering wheels; R _rr is the rotation radius of at least two auxiliary runners; θ _lf is the required turning angle when the two steering wheels turn along the rotation radius R _lf ; θ _rr is at least two The required turning angle for each auxiliary wheel to turn along the turning radius R _rr .

In addition, since the angular rate of the center of the vehicle body moving in a circle at the same rate is equal to the angular rate of the two steering wheels (ie ω=ω _lf =ω _rr ), and according to the speed V of the center of the vehicle body, the radius of rotation R and the angular rate ω The relationship between (ie V=R*ω) can be obtained:

Wherein the V _f is the angular velocity of the two steering wheels; V _r is the angular velocity of the at least two auxiliary runners.

Therefore, the automatic guidance device can first obtain the rotation radius required by the two steering wheels of the vehicle body when traveling, and push back to obtain the rotation angle required by the two steering wheels, and then obtain two The angular speed of the steering wheel can be controlled by the steering drive system to control the steering of the two steering wheels to the corresponding position, and travel at different angles and speeds (ie speed difference) when turning, so that the car body can stably maintain the predetermined target. on the path.

Another object of the present invention is to first rotate the Y and _L axes of the vehicle body in the local coordinate system to the target straight line formed by connecting the starting point to the target point when the attitude of the two steering wheels of the vehicle body is to move forward in translation. The included angle θ is parallel, and the two steering wheels rotate to the included angle θ. Assuming that the target line is divided into two segments, and the included angle θ is used as the reference, the control quantities of the two steering wheels can be calculated as θ+arctan(x/y) respectively; when When the attitude of the two steering wheels of the vehicle body is turning, if the attitude deviation of the vehicle body is greater than the deviation of the set angle, the two steering wheels will multiply the control amount by the minus sign according to the attitude direction of the vehicle body to be corrected, so as to provide the direction of the vehicle body. When turning left or right, the control amount of one steering wheel is θ+arctan(x/y), and the control amount of the other steering wheel is -[θ+arctan(x/y)].

Another object of the present invention is that when the steering drive system completes the steering control 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 the error according to the error. PID control can obtain the corrected speed and rotation radius of the car body, and then use inverse kinematics to calculate the speed or acceleration required for the car body to move to the target path by means of reverse thrust, so that the steering drive system can control 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.

1: Unmanned self-propelled vehicles

11: Body

111: steering wheel

111a: Left front wheel

111b: Right rear 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 of steering control in which the posture of the vehicle body according to the present invention is to move forward in translation.

[FIG. 11] is a schematic diagram showing the posture of the vehicle body according to the present invention being corrected to turn to the left.

[FIG. 12] is a schematic diagram of the present invention when the posture of the vehicle body is corrected to turn to the right.

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 two steering wheels 111 arranged diagonally in the front and rear (that is, the driving wheels that drive and control the steering), and the shafts of the two steering wheels 111 are respectively connected with another two runners 112 arranged diagonally (that is, the ones that carry or assist the steering). Driven wheels), but not limited to this, two or more runners 112 can also be separately provided at appropriate positions of the vehicle body 11, and the automatic guiding device 12 receives the information issued by the control management center through the communication interface. After the task command, the steering drive 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), autonomous mobile robot ( Auto mated Mobile Robot, AMR) or mobile vehicle, etc.

In this embodiment, the unmanned self-propelled vehicle 1 uses the steering wheel 111 on the front and left side of the vehicle body 11 as the left front wheel 111a, and the steering wheel 111 on the right diagonally behind the vehicle body 11 as the right rear wheel 111b. Drive and control steering, but it is not limited to this. According to the actual design of the vehicle body 11 , the positions of the two steering wheels 111 can be changed to the right front wheel and the left rear wheel diagonally arranged, and then matched with the other diagonal of the vehicle body 11 . Two runners 112 or runners 112 at other suitable positions are provided for carrying or assisting steering. In order to make the vehicle body 11 have better stability, two runners can also be installed on the forks of the vehicle body 11. Wheels 112 (such as fork wheels) play a supporting role to form a forklift-type automatic guided vehicle, pallet-type or stacker-type automatic guided vehicle suitable for handling or transferring heavy loads.

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 system of the unmanned self-propelled vehicle 1 uses the physical markers (such as electromagnetic tracks, magnetic tapes, reflective sheets, etc.) established on the moving path as guidance, and The position and attitude of the vehicle body 11 are positioned by the sensor module 121 of the automatic guidance device 12 to detect the markings, so as 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 Guidance), electromagnetic guidance (Wire Guidance), magnetic tape guidance (Magnetic Tape Guidance) or optical guidance (Optical Guidance), while unmanned vehicle 1 virtual path navigation/guidance control If there is no physical mark, the configuration diagram 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 performs the position and attitude of the vehicle body 11. Detect, so that the path planning unit 122 can determine the target path of the predetermined planning, including but not limited to inertial navigation (Inertial Navigation), laser navigation (Laser 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 path navigation/guidance control of the unmanned self-propelled vehicle 1, which will not be repeated here.

As shown in Figure 2, the guidance control 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 in the predetermined target path, the current heading of the vehicle body 11 and the turning angle between the center of the vehicle body 11 and the target point, and the center of the vehicle body 11 to the target The radius of rotation of the point.

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

( S104 ) Determine the turning direction of the vehicle body 11 , and obtain the rotational radii of the two steering wheels 111 and the required turning angle according to the cosine law.

( S105 ) The speed of the two steering wheels 111 is obtained according to the relationship between the speed of the center of the vehicle body 11 , the radius of rotation and the angular speed.

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

(S107) The automatic guidance device 12 calculates the current position of the vehicle body 11, the error between the rotation radius and the target path, and uses the PID control to obtain the corrected speed and rotation radius, and then calculates the vehicle body 11 by inverse kinematics. speed, so that the steering driving system 13 can control the two steering wheels 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 vehicle) in the global coordinate system as the center point C, and the target point P as one of the target points of the predetermined planned target path, and the The pre-planned 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 convert the coordinate system to establish a local coordinate system (Local Coordinate System) (such as X _L Y _L coordinate plane) can be obtained:

The θ is the current attitude angle of the vehicle body 11, which can be expressed as the angle from the X _GM or Y _GM axis of the vehicle coordinate system to the _{XL or Y L axis} of the local coordinate system; X _C is the Y _G axis of the global coordinate system The distance from 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; X _G , Y _G are the predetermined planned target paths. One of the target points P is at Coordinates (X, Y) in the global coordinate 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:

The D is the distance from the center point C (X _C , Y _C ) of the vehicle body 11 in the local coordinate system to the shortest path of the target point P (X _L , Y _L ); θ _S is the Y _L axis of the local coordinate system The angle rotated 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 (ie, the center point C) to the target point P; The current heading of the two steering wheels 111 (ie the left front wheel 111a and the right rear wheel 111b ) and the current heading of the vehicle body 11 are in the same and parallel directions. When D and θ _S are known, according to the geometric relationship, it can be obtained that R is the vehicle body 11 The radius of rotation from the center to the target point P.

As shown in FIG. 4 , after the automatic guidance device 12 obtains the current position of the center of the vehicle body 11 (eg X, Y, θ), the distance D of the shortest path and the speed V, the center of the vehicle body 11 and the two steering wheels 111 The geometric relations of , are all 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:

Wherein, W is the distance from the center of the vehicle body 11 to the center of any steering wheel 111 (ie, the left front wheel 111a or the right rear wheel 111b) in the horizontal direction; L is the 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 angle at which the Y _L axis of the vehicle body 11 in the local coordinate system rotates counterclockwise to the center of any steering wheel 111, which can be expressed as the current heading of the vehicle body 11 and the center of the vehicle body 11 to any steering wheel 111 The included angle between the center lines; d is the fixed distance from the center of the vehicle body 11 to the center of any steering wheel 111 .

Then, according to the 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 (ie the turning angle or the heading angle) θ _S , the direction in which the vehicle body 11 needs to turn can be determined, and it is assumed that the turning angle of the vehicle body 11 is determined. When θ _S is greater than 0 (that is, the positive sign is the direction of the counterclockwise turn), according to the law of cosines, it can be obtained:

Assuming that the turning angle θ _S of the vehicle body 11 is judged to be less than 0 (that is, the negative sign is the direction of clockwise turning), according to the law of cosines, it can be obtained:

The R _lf is the distance from the center of the two steering wheels 111 (ie the left front wheel 111 a ) to the center O of the vehicle body 11 surrounded by the rotation radius R; R _rr is the at least two auxiliary wheels 111 (ie the right rear wheel 111 b ) The distance from the center to the center _O of the vehicle body 11 _surrounded by the _rotation radius R; The turning angle or heading angle required when the two auxiliary runners 111 (ie, the right rear wheel 111b) turn along the turning radius _Rrr .

In addition, since the angular velocity when the center of the vehicle body 11 moves in a circle at a constant rate is equal to the angular velocity of the front and rear steering wheels 111 (ie ω=ω _lf =ω _rr ), and the current speed V at the center of the vehicle body 11 is known In this case, according to the relationship between the speed of the center of the vehicle body 11 (that is, the average speed) V, the radius of rotation R and the angular rate ω (that is, V=R*ω), it can be obtained:

The V _f is the angular velocity of the two steering wheels 111 (ie the left front wheel 111 a ); V _r is the angular velocity of the at least two auxiliary runners 111 (ie the right rear wheel 111 b ). Therefore, the automatic guidance device 12 can first obtain the rotation radii R _lf and R _rr required when the two steering wheels 111 of the vehicle body 11 (ie the left front wheel 111 a and the right rear wheel 111 b ) follow a straight path or a curved path, and return the After the rotation angles θ _lf and θ _rr required by the two steering wheels 111 are obtained, then the angular velocities V _f and V _r required by the two steering wheels 111 can be obtained according to the relationship between the speed, the radius of rotation and the angular rate, which can be driven by steering The system 13 controls the two steering wheels 111 of the car body 11 to turn to corresponding positions, and can travel at different angles and speeds (ie speed differences) when the car body 11 turns, so that the center of the car body 11 follows a straight path or a curved path, In this way, the guidance control of the unmanned self-propelled vehicle 1 to follow the predetermined planned target path is realized, and the steering driving algorithm adopted by the built-in processor of the on-board controller or the automatic guidance device 12 does not need to go through a large number of complex calculations or comparisons. The long computing processing cycle relatively reduces the computing performance requirements of the on-board controller or processor, and 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 vehicle body 11, and then inverse kinematics (Inverse Kinematics) can be used to calculate the vehicle body in a reverse way. 11 The speed V or acceleration A required to move to the target path, such as the speed V _f of the two steering wheels 111 (ie the left front wheel 111a ) of the vehicle body 11 , the speed of the at least two auxiliary wheels 111 (ie the right rear wheel 111b ) The velocity V _r , and the accelerations A _f and _Ar of the front and rear steering wheels 111 enable the steering drive system 13 to control the two steering wheels 111 to correct and adjust the current position and rotation radius of the vehicle body 11 , so that the vehicle body 11 moves through repeated corrections. The state conforms to the desired target path until the path guidance control of the vehicle body 11 is completed.

In this embodiment, the predetermined 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 vehicle body 11 the current position and rotation radius, and can continuously detect The error between the current position of the vehicle body 11, the radius of rotation and the target path, wherein the 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 vehicle body 11 The error of the rotation radius of the center and the rotation radius of the vehicle body 11 caused by the offset, 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 The corrected rotation radius R*=K _PR *(R-error _R ), wherein the K _PR is the gain amount (proportional coefficient).

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 accurate deviation correction, and stable maintenance on the pre-planned target path.

Please also refer to Figures 8 to 12, which are the schematic diagram of the vehicle body performing linear control, the schematic diagram of the attitude direction of the vehicle body relative to the target path, the steering control diagram of the vehicle body's attitude as translation forward, and the vehicle body according to the present invention. The posture of the body is corrected to a schematic diagram of turning to the left and the posture of the vehicle body is corrected to a schematic diagram of turning to the right. It can be clearly seen from the figures that the target path of the predetermined plan generated by the above-mentioned automatic guidance device 12 uses a plurality of target points. P0~P9 are divided into multiple line segments, and the multiple line segments are connected to form a straight path trajectory, wherein the target point P0 can be represented as the starting point of the straight path, and the target point P9 can be represented as the final point of the straight path. And according to the straight line equation y=ax+b is divided according to the predetermined length, when the vehicle body 11 is traveling, according to the vehicle coordinate system, the first target point P1 is on the left side of the vehicle body 11 (as shown in Fig. 9), and calculate The included angle [arctan(x/y)] between the center of the vehicle body 11 and the target point P1 is obtained as the control of the two steering wheels 111 of the vehicle body 11, and the turning direction of the vehicle body 11 can also be judged by this, which is generally counterclockwise The direction of the turn is taken as a positive sign, and the direction of a clockwise turn is taken as a negative sign.

In this embodiment, the unmanned vehicle 1 takes the two steering wheels 111 diagonally arranged in the front and rear of the vehicle body 11 as the right front wheel and the left rear wheel as an example, and the center of the vehicle body 11 (ie the center point C) is set to the target point The distance of P1 is used as the hypotenuse of the right triangle, and the point where the opposite side and the adjacent side of the right triangle intersect perpendicularly is used as the starting point S (as shown in Figure 10). When moving forward, first rotate the Y and _L axes of the vehicle body 11 in the local coordinate system counterclockwise to be parallel to the target 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 rotated to The included angle θ is parallel to the target line, and after the two steering wheels 111 of the vehicle body 11 (ie, the right front wheel and the left rear wheel) are rotated to the included angle θ in a translational manner, it is assumed that the target line is divided into two sections, and the included angle θ As a benchmark, according to the geometric relationship of the right triangle, the control quantities of the two steering wheels 111 of the vehicle body 11 can be calculated as θ+arctan(x/y), where x is the distance from the center of the vehicle body 11 to the starting point S, y is the distance from the starting point S to the midpoint M of the target line.

In addition, when the vehicle body 11 is turning, if the attitude deviation of the vehicle body 11 is greater than the deviation of the set angle, the two steering wheels 111 (ie the right front wheel and the left rear wheel) need to be adjusted according to the attitude direction of the vehicle body 11 to be corrected. The control amount is multiplied by the minus sign. For example, when the vehicle body 11 turns to the left (as shown in Figure 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 turns to the right (as shown in Fig. 12), the control amount of the right front wheel is -[θ+arctan(x/y)], 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 for a preferred feasible embodiment of the present invention, but the embodiment is not intended to limit the scope of the present invention. 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 (8)

A guidance control method for an unmanned self-propelled vehicle, the unmanned self-propelled vehicle includes a vehicle body, an automatic guide device and a steering drive system, and the vehicle body includes two steering wheels arranged diagonally at the front and rear for driving and controlling steering, and a steering wheel and a steering wheel. At least two auxiliary runners, the automatic guidance device is used for positioning the position and attitude of the vehicle body, and generating a predetermined planned path for the steering wheel of the vehicle body to be driven by the steering drive system. Running along the target path, the guidance control method includes the following steps: (A) the automatic guidance device obtains the position of the center of the vehicle body, establishes a vehicle coordinate system in the global coordinate system, and obtains the current attitude angle of the vehicle body Perform coordinate system transformation to establish a local coordinate system; (B) Calculate the shortest distance D from the center of the vehicle body to one of the target points on the target path, the current heading of the vehicle body and the line connecting 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 distance d from the center of the vehicle body to the center of any one of the steering wheels. The included angle θ _a 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; (D) judge the direction of the vehicle body turning according to the turning angle θ _S of the vehicle body, assuming that θ _S is greater than 0 time , according to the cosine law:

Assuming that the rotation angle θ _S of the car body is less than 0, according to the cosine law, we can get:

Wherein the R _lf is the rotation radius of the two steering wheels; R _rr is the rotation radius of at least two auxiliary runners; θ _lf is the required turning angle _when the two steering wheels turn along the rotation radius R _lf ; The required turning angle when the two auxiliary runners turn along the rotation radius R _rr ; (E) Since the angular velocity of the center of the car body moving in a circle at a constant rate is equal to the angular velocity of the two steering wheels, according to the angular velocity of the car body The relationship between the velocity V of the center, the radius of rotation R and the angular rate is:

Wherein the V _f is the angular velocity of the two steering wheels; V _r is the angular velocity of the at least two auxiliary runners; (F) the steering drive system is based on the calculated rotation angles θ _lf , θ _rr and rates V _f , V _r The two steering wheels are controlled to be turned to corresponding positions, so that the center of the vehicle body can follow the target path. The guidance control method for an unmanned 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 positioning the vehicle body. A path planning unit for generating the target path. 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:

The θ 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 X _G axis of the global coordinate system and the X _GM of the vehicle coordinate system. Distance between axes; 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 step (B) is obtained according to the geometric relationship of a right triangle:

Wherein D is the shortest distance from the center of the vehicle body to the target point; θ _S is the angle between the current heading of the vehicle body and the line connecting the center of the vehicle body to the target point; R is the center of the vehicle body to the target point radius of rotation. The guidance control method for an unmanned self-propelled vehicle as claimed in claim 1, wherein the step (C) is obtained according to the geometric relationship between the center of the vehicle body and the two steering wheels:

Wherein the W is the horizontal distance from the center of the vehicle body to the center of any one of the steering wheels; L is the distance from the center of the vehicle body to the center of the any one of the steering wheels in the vertical direction; θ _a is the current heading of the vehicle body and the center of the vehicle body The included angle between the line connecting the center of any one of the steering wheels; d is the distance from the center of the vehicle body to the center of any one of the steering wheels. The guidance control method for an unmanned self-propelled vehicle as claimed in claim 1, wherein in step (D), when the attitude of the two steering wheels of the vehicle body is to move forward in translation, the first step is to move the vehicle body in the local coordinate system. The Y and _L axes are rotated to an included angle θ parallel to a target line formed by a line connecting the starting point to the target point, and the two steering wheels are rotated to the included angle θ in a translational manner. Suppose the target line is divided into two sections , and based on the included angle θ, the control variables of the two steering wheels are calculated according to the geometric relationship of the right triangle as θ+arctan(x/y), where the x is the distance from the center of the vehicle body to the starting point , y is the distance from the starting point to the midpoint of the target line. According to the guidance control method of the unmanned self-propelled vehicle described in claim 6, when the attitude of the two steering wheels of the vehicle body is to turn, if the attitude deviation of the vehicle body is greater than the deviation of the set angle, the two steering wheels Each steering wheel multiplies its control amount by the minus sign according to the attitude direction of the vehicle body to be corrected, so that when the vehicle body is turning 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)]. The guidance control method for an unmanned self-propelled vehicle as claimed in claim 1, wherein the step (F) completes the steering of the two steering wheels of the vehicle body, and then performs the next step: (G) the automatic guidance device calculates the vehicle The current position of the car body, the error between the rotation radius and the target path, and according to the error amount, PID control is used to obtain the corrected speed and rotation radius of the car body, and then the inverse kinematics is used to calculate the car body in a reverse way. The speed or acceleration required to move to the target path, so that the steering drive system can control the two steering wheels of the car body to correct Adjust the current position and rotation radius of the vehicle body until the path guidance control of the vehicle body is completed.

Images