TW202242580A - Guidance control method for unmanned self-propelled vehicle effectively increasing the overall navigation efficiency - Google Patents

Abstract

This invention provides a guidance control method for an unmanned self-propelled vehicle. The guidance control method includes the following steps: obtaining a position of a center of a vehicle body by using an automatic guidance apparatus to establish a vehicle coordinate system, and performing coordinate system conversion to establish a local coordinate system, such that the shortest distance from the center of the vehicle body to one of target points of a planned target path, a turning angle between the center of the vehicle body and the target point, and a rotation radius from the center of the vehicle body to the target point are calculated; then calculating an included angle between a current heading of the vehicle body and centers of steering wheels according to the distance from the center of the vehicle body to the center of any steering wheel, so as to determine the direction of the vehicle body to turn to, and obtaining the rotation radius and the corner of the two steering wheels according to the cosine theorem, such that a steering drive system can control the two steering wheels to turn to the corresponding positions according to the calculated turning angle and speed, so as to realize the guidance control on the unmanned self-propelled vehicle to follow the planned target path without a lot of complex operations or a long processing period. Accordingly, the overall navigation efficiency can be effectively increased.

Description

Guidance and control method for unmanned self-propelled vehicles

The present invention provides a guidance control method, especially a guidance control method for an unmanned self-propelled vehicle with double steering wheels.

According to the current global wave of declining birthrates, the shortage of labor resources and the increase in labor costs are increasing year by year, and gradually transforming from labor-intensive to technology-intensive industries. Based on the continuous increase in various operating costs, how to reduce various costs has become an issue. 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 gradually been applied to industrial production and manufacturing, and more and more tasks are being used Replaced by industrial robots to solve the problem of shortage of labor resources.

However, Automatic Guided Vehicle (AGV), or unmanned guided vehicle, refers to a vehicle that is equipped with automatic guidance devices such as electromagnetic or optical, 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-Wbeeled Mobile Robot). Its main function is to automatically walk and dock to the designated place or workstation under the monitoring of the computer or vehicle system according to the path planning and operation requirements, and To complete a series of operating functions, the general automatic guided vehicle can control its traveling route through the on-board system or computer, or can use the indicator marks set up along the route on the wall, pillar or ground (such as electromagnetic track, reflective Specific reflective sheeting, paint or ribbon and other positioning marks) as a guide, and electromagnetic or optical sensors (such as electromagnetic sensors, visual sensors, ultrasonic sensors or laser sensors, etc.) are installed on the automatic guided vehicle , using the detection indicator mark as the positioning and position correction of the vehicle, in addition to enabling the vehicle body to automatically drive along the planned guiding path, and because there is no need to lay rails, brackets and other fixed devices in the activity area, it will not Restricted by sites, roads and spaces, automatic guided vehicles are widely used in automatic transportation of materials, surveillance and patrol of warehouses, and operations in hazardous places.

Most of the path planning of traditional automatic guided vehicles is a path composed of lines connected between point-to-point grids, and the above-mentioned guidance method is used to enable the automatic guided vehicle to advance along the predetermined path, but the above-mentioned guidance The guidance method needs to use a large number of complex calculations to extract entity marks or features in the environment to determine the direction and speed that the automatic guided vehicle should go. The calculation and processing cycle is long, which reduces the overall navigation efficiency, and the path The planning is not a smooth curve, which will cause the automatic guided vehicle to have rough turns during its travel, and its actual travel path will deviate from the predetermined path, which requires constant comparison and correction of position and heading , which is the direction that those engaged in this industry want to study and improve.

Therefore, in view of the above deficiencies, the inventor collected relevant information, after various evaluations and considerations, and based on years of experience accumulated in this industry, continuous trial production and modification, he designed the guide for this kind of unmanned self-propelled vehicle The invention patent of the control method was born.

The main purpose of the present invention is that the car body of the unmanned self-propelled vehicle includes two steering wheels and at least two auxiliary runners for driving and controlling steering, and the position and attitude of the car body are positioned by the automatic guiding device, and Generate the 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.), establish the vehicle coordinate system, and carry out the coordinate system By transforming and establishing the local coordinate system, the shortest distance from the center of the car body to one of the target points on the planned target path, the corner 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 can be calculated, 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 course of the car body and the center of the steering wheel can be obtained, and then the direction in which the car body needs to turn can be judged, and the rotation radius and rotation angle of the two steering wheels can be obtained according to the cosine law, so that The steering drive system can control the two steering wheels to steer to the corresponding position according to the calculated angle and speed, so as to realize the guidance control for the unmanned self-propelled vehicle to follow the planned target path without a lot of complicated calculations or long time The processing cycle can effectively improve the overall navigation efficiency.

The secondary purpose of the present invention is because the geometric relationship between the center of the car body and the two steering wheels is fixed, and when the velocity V of the center of the car body and the radius of rotation R from the center of the car body to the target point are known, it can be obtained :

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

Then, according to the angle θ _S between the current course 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 judged. Assuming that θ _S is greater than 0, according to the law of cosines, it can be obtained:

Assuming that θ _S is less than 0, according to the law of cosines, it can be obtained:

Among them, R _lf is the radius of rotation of the front steering wheel; R _rr is the radius of rotation of the rear steering wheel; θ _lf is the required turning angle of the front steering wheel along the radius of rotation R _lf ; θ _rr is the turning radius of the rear steering wheel along the radius R _rr required corner.

In addition, since the angular velocity of the center of the car body is equal to the angular rate of the two steering wheels (ie ω=ω _lf =ω _rr ), and according to the velocity V of the center of the car body, the radius of rotation R and the angular rate ω The relationship between (ie V=R*ω) can be obtained:

Among them, V _f is the speed of the front steering wheel; V _r is the speed of the rear steering wheel.

Therefore, the automatic guidance device can first obtain the rotation radius required by the two steering wheels of the car body when it is moving, and push back to obtain the rotation angle required by the two steering wheels, and then obtain the two according to the relationship between the speed, rotation radius and angular rate. The speed of the steering wheel can be controlled by the steering drive system to steer the two steering wheels to the corresponding position, and travel at different angles and speeds (that is, speed difference) when turning, so that the car body can be kept stably at the planned target on the path.

Another object of the present invention is to rotate the Y _L axis of the car 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 car body is translational advancement Parallel to the included angle θ, and the two steering wheels rotate to the included angle θ, assuming that the target line is divided into two segments, and based on the included angle θ, the control quantities of the two steering wheels can be calculated as θ+arctan(x/y); when When the attitude of the two steering wheels of the car body is turning, if the deviation of the attitude of the car 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 attitude direction of the car body to be corrected for the direction of the car body. When turning left or right, the control quantity of one steering wheel is θ+arctan(x/y), while the control quantity of the other steering wheel is -[θ+arctan(x/y)].

Another object of the present invention is that when the steering drive system completes the control steering of the two steering wheel angles, the automatic guidance device can calculate the error between the current position of the vehicle body, the radius of rotation and the target path, and use the error amount according to the error amount. PID control can obtain the corrected speed and radius of rotation 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 in a reverse way, so that the steering drive system can control the two steering wheels to correct and adjust The current position and rotation radius of the car body until the path guidance control of the car body is completed.

1: Unmanned self-propelled vehicle

11: car 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 self-propelled vehicle system of the present invention.

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

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

[Fig. 4] is a schematic diagram of the algorithm of the car body control steering wheel steering drive of the present invention.

[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 radius of gyration of the correction of the car body turning in the present invention.

[Fig. 7] is a schematic diagram (two) of the radius of gyration of the present invention's correction of the turning of the car body.

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

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

[Fig. 10] is the attitude of car body of the present invention is the steering control schematic diagram that translation advances.

[Fig. 11] is a schematic diagram showing that the attitude of the car body of the present invention is corrected to turn left.

[Fig. 12] is the schematic diagram that the posture correction of the car body of the present invention is turning right.

In order to achieve the above-mentioned purpose and its effect, the technical means and detailed structure adopted by the present invention, the drawing is hereby described in detail with respect to the preferred embodiment of the present invention.

Please refer to Figures 1 to 4, which are the schematic diagram of the unmanned self-propelled vehicle system of the present invention, the flow chart of the steps of the preferred embodiment, the schematic diagram of the coordinate system transformation of the position and attitude of the vehicle body, and the control steering wheel of the vehicle body Schematic diagram of the algorithm of steering drive. 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 a wheel module is placed under the vehicle body 11. It includes two steering wheels 111 arranged diagonally front and rear (i.e. driving and steering driving wheels), and the rotating shafts of the two steering wheels 111 are respectively connected with two runners 112 arranged diagonally (i.e. carrying or assisting steering). driven wheels), but not limited thereto, two or more runners 112 can also be separately provided at appropriate positions of the car body 11, and the automatic guidance 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 can run along the predetermined target path to form an automatic guided vehicle (AGV), autonomous mobile robot ( Automated Mobile Robot, AMR) or mobile vehicles, etc.

In the present embodiment, the unmanned self-propelled vehicle 1 uses the steering wheel 111 on the front left side of the vehicle body 11 as the left front wheel 111a, and the steering wheel 111 on the diagonal right side behind the vehicle body 11 is used as the right rear wheel 111b for driving and controlling steering. Not limited to this, also can come two rudders according to the actual design of car body 11 The position of wheel 111 is changed to right front wheel and left rear wheel diagonal setting, coordinates two runners 112 or the runners 112 of other appropriate positions that car body 11 is arranged at another angle again, is used for bearing or auxiliary steering, in order to make car The body 11 has better stability, and two runners 112 (such as fork wheels) can also be installed on the fork of the car body 11 to play a supporting role, so as to form a car body suitable for heavy-duty cargo handling or moving. Forklift-type AGVs, pallet-type or stacker-type AGVs.

In addition, the steering wheel 111 used by the body 11 of the unmanned self-propelled 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 gearbox, etc.) device, etc.), with driving and steering control, the linear movement and steering functions of the car body 11 can be realized 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 internal sensors (such as encoders, inertial measurement units, IMUs, etc.) mounted on the vehicle body 11 and external sensors (such as laser sensors, optical radar sensors, etc.) (Light Detection and Ranging, LiDAR) scanner, ultrasonic (Sonar) sensor or 3D vision sensor (3D Camera), etc.], and the position and attitude of the car body 11 are positioned by the internal sensor, so that the automatic On the basis of this positioning, the guidance device 12 can use the environmental information obtained by external sensors to correct the position or attitude, and the path planning unit 122 uses an algorithm to plan the path of the vehicle body movement according to the preset 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, 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 utilizes physical marks (such as electromagnetic tracks, tapes, reflective sheets, etc.) set up on the moving path as guidance, and is sensed by the automatic guidance device 12 The sensor module 121 detects the position and attitude of the car body 11 Positioning, to run along the target path planned 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 Guidance) Tape Guidance) or Optical Guidance (Optical Guidance), while the unmanned self-propelled vehicle 1 virtual path navigation/guidance control does not have a physical mark, and the configuration map data of the vehicle body 11 moving path is stored in the database or automatic guidance The route data in the map library in the device 12, and the sensor module 121 detects the position and attitude of the vehicle body 11, so that the path planning unit 122 can determine the planned target path by itself, including but not limited to inertial navigation (Inertial Navigation) Navigation), laser navigation (Laser Navigation) or ultrasonic navigation, visual navigation (Visual Navigation) or geographical navigation (such as Global Position System navigation (Global Position System)), but the unmanned self-propelled vehicle 1 path navigation/guidance control There are many ways, so I won't go into details here.

As shown in Figure 2, the guidance control method adopted by the above-mentioned unmanned self-propelled vehicle system of the present invention comprises the following implementation steps:

(S101) The automatic guidance device 12 of the unmanned self-propelled vehicle 1 first obtains the position of the center of the vehicle body 11 to establish a vehicle coordinate system in the global coordinate system, and performs coordinate system conversion to establish a local coordinate system.

(S102) Calculate the shortest distance from the center of the car body 11 to one of the target points on the planned target path, the angle between the current course of the car body 11 and the line connecting the center of the car body 11 to the target point, and the center of the car 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 angle 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.

(S104) Judging the turning direction of the car body 11, and obtaining the turning radius of the two steering wheels 111 and the required turning angle according to the law of cosines.

(S105) Obtain the speed of the two steering wheels 111 according to the relationship between the speed of the center of the car body 11, the radius of rotation and the angular speed.

(S106) The steering drive system 13 controls the two steering wheels 111 to steer 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 error between the current position of the vehicle body 11, the radius of rotation and the target path, and uses PID control to obtain the corrected speed and radius of rotation, and then calculates the vehicle body 11 using inverse kinematics. speed, so that the steering drive system 13 can control the two steering wheels 111 to correct and adjust the current position and radius of rotation of the vehicle body 11.

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

In this embodiment, the automatic guidance device 12 is utilized to establish a Global Coordinate System (Global Coordinate System) (such as the X _G Y _G coordinate plane in the 3rd figure) in the environment where the unmanned self-propelled 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 planned target path, and the The planned target path includes a straight line 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 get:

Wherein, θ is the current attitude angle of the vehicle body 11, which can be expressed as the angle of rotation of the X _GM or Y _GM axis of the vehicle coordinate system to the X _L or Y _L axis of the local coordinate system; X _C is the Y _G axis of the global coordinate system The distance between the Y _GM axis and 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; The 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, so as to locate the current position and posture of the vehicle body 11 .

According to the geometric relations of right triangles, we can get:

Wherein, D is the shortest path distance from the center point C (X _C , Y _C ) of the car body 11 in the local coordinate system to the target point P (X _L , Y _L ); θ _S is the Y _L axis of the local coordinate system The angle clockwise to the target point P can be expressed as the angle between the current course of the car body 11 and the line connecting the center of the car body 11 (ie, the center point C) to the target point P; The current course of the two steering wheels 111 (that is, the left front wheel 111a and the right rear wheel 111b) and the vehicle body 11 is consistent and parallel. When D and θ _S are known, according to the geometric relationship, R can be obtained as the vehicle body 11 The radius of rotation from the center to the target point P.

As shown in Figure 4, when the automatic guidance device 12 obtains the current position (such as x, y, θ) of the center of the car body 11, the distance D and the speed V of the shortest path, due to the center of the car 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 car body 11 to the geometric center of the steering wheel 111 is d, then it can be obtained:

Wherein this W is the fixed distance in the horizontal direction 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); 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 from the Y _L axis of the car body 11 in the local coordinate system to the center of any steering wheel 111 counterclockwise, which can be expressed as the current heading of the car body 11 and the center of the car body 11 to any steering wheel 111 The angle between the connecting lines between the centers; d is the fixed distance from the center of the car body 11 to the center of any steering wheel 111.

Then, the direction in which the car body 11 needs to turn can be judged according to the angle between the current heading of the car body 11 and the line connecting the center of the car body 11 to the target point P (that is, the turning angle or heading angle) θ _S . When θ _S is greater than 0 (that is, the positive sign is the direction of counterclockwise turning), according to the law of cosines, it can be obtained:

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

Wherein 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 surrounded by the center of the vehicle body 11 with the radius of rotation R; 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 vehicle body 11 The distance of the circle center O surrounded by the radius of rotation R; θ _lf is the required turning angle or heading angle when the front steering wheel 111 (i.e. the left front wheel 111a) turns along the radius of rotation R _lf ; θ _rr is the rear steering wheel 111 (i.e. the right rear wheel 111b) The required turning or heading angle when turning along the turning radius R _rr .

In addition, since the angular velocity of the center of the vehicle body 11 is equal to the angular velocity of the two front and rear steering wheels 111 (ie ω=ω _lf =ω _rr ), and the current velocity V of the center of the vehicle body 11 is known In this case, according to the relationship between the velocity (i.e. the average velocity) V of the center of the car body 11, the radius of rotation R and the angular velocity ω (i.e. V=R*ω), it can be obtained:

Wherein the V _f is the speed of the front steering wheel 111 (ie, the left front wheel 111a); V _r is the speed of the rear steering wheel 111 (ie, the right rear wheel 111b). Therefore, the automatic guidance device 12 can first obtain the rotation radii _Rlf and _Rrr required by the two steering wheels 111 (ie, the left front wheel 111a and the right rear wheel 111b) of the car body 11 to follow a straight path or a curved path, and return After deriving the required rotation angles θ _lf and θ _rr of the two steering wheels 111, and then obtaining the required speeds V _f and V _r of the two steering wheels 111 according to the relationship between the speed, the radius of rotation and the angular rate, it can be driven by steering The system 13 controls the two steering wheels 111 of the car body 11 to turn to the corresponding positions, and when the car body 11 turns, it can travel at different angles and speeds (ie speed difference), 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 following 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 complicated calculations or comparisons. The long calculation and processing cycle relatively reduces the calculation performance requirements of the on-board controller or processor, and can also effectively improve the overall navigation efficiency.

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

When the car body 11 is moving along the target path, the automatic guidance device 12 can coordinate transform the current position of the car body 11 and the radius of rotation, and calculate the error between the current position of the car body 11, the radius of rotation and the target path (error _d and error _R ), and then perform PID control according to the error amount to obtain the corrected speed V* and rotation radius R* of the car body 11, and then use inverse kinematics (Inverse Kinematics) to calculate the car body 11 The velocity V or acceleration A required to move to the target path, such as the velocity V _f of the steering wheel 111 in front of the vehicle body 11 (ie, the left front wheel 111a ), the velocity V _r of the rear steering wheel 111 (ie, the right rear wheel 111b ), and the front and rear The accelerations A _f and A _r of the two steering wheels 111 enable the steering drive system 13 to control the two steering wheels 111 to correct and adjust the current position and radius of rotation of the car body 11, so that the moving state of the car body 11 conforms to the desired target path through repeated corrections , until the path guidance control of the car body 11 is completed.

In this embodiment, the planned target path generated by the automatic guidance device 12 can guide the car body 11 to follow a straight path or a curved path, and give the car body 11 the current position and radius of rotation, and can continuously detect The error amount between the current position of the car body 11, the radius of rotation and the target path, where the error _d is the error amount of the straight-line distance between the current position of the car body 11 and the final position of the target path, and error _R = RR _false is the error amount of the car body 11 The error amount of the radius of rotation caused by the offset of the center rotation radius and the car body 11, and then performing PID control with different algorithms can be calculated to obtain the corrected speed V*=K _PR *(error _d ) of the car body 11, and the car body 11 Modified rotation radius R*=K _PR *(R-error _R ), where K _PR is the gain amount (proportional coefficient).

When the vehicle body 11 is traveling on a curved path, the turning radius of the vehicle body 11 can be changed by adjusting the radius of rotation. For example, when the radius of rotation at the center of the vehicle body 11 becomes larger, it means that the vehicle body 11 has shifted to the outside of the curved path, and the vehicle body 11 The turning range adjusted and changed by the body 11 should become smaller; in other words, when the turning radius of the car body 11 becomes smaller, it means that the car body 11 has shifted to the inner side of the curved path, and the turning range adjusted and changed by the car body 11 will become larger. The turning direction of the center of the body 11 can be judged in the algorithm, so the turning radius of the center of the car body 11 can be used as a variation to correct the steering of the car body 11, so that the car body 11 can quickly move when it deviates from the curved path. And it will accurately correct the deviation and keep it stably on the planned target path.

Please also refer to as shown in the 8th to 12th figures, which are respectively for the car body of the present invention. The schematic diagram of wire control, the schematic diagram of the attitude direction of the car body relative to the target path, the steering control diagram of the car body attitude as a translation forward, the schematic diagram of the car body attitude correction as a left turn, and the car body attitude correction as a right turn It can be clearly seen from the figure that the planned target path generated by the above-mentioned automatic guidance device 12 is divided into multiple line segments by using complex target points P0~P9, and the multiple line segments are connected to form a straight path trajectory , wherein the target point P0 can be expressed as the starting point of the straight line path, and the target point P9 can be expressed as the final point of the straight line path, and divided according to the 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 (as shown in Figure 9), and the angle between the center of the vehicle body 11 and the target point P1 is calculated [arctan(x/y) ] As the control of the two steering wheels 111 of the car body 11, the direction of turning of the car body 11 can also be judged by this. Generally, the direction of counterclockwise turning is taken as a positive sign, and the direction of clockwise turning is taken as a negative sign.

In this embodiment, the unmanned self-propelled vehicle 1 uses the two steering wheels 111 arranged diagonally 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 (i.e., the center point C) to the target point The distance of P1 is taken as the hypotenuse of the right triangle, and the point where the opposite side of the right triangle intersects with the adjacent side perpendicularly is taken as the starting point S (as shown in Figure 10), when the vehicle body 11 tracks the target point P1 translation on the straight path When moving forward, the system first rotates the Y and _L axes of the car body 11 counterclockwise in the local coordinate system 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 car body 11 rotating to The included angle θ is parallel to the target line, and after the two steering wheels 111 of the car 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 angle θ As a benchmark, the control quantities of the two steering wheels 111 of the car 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 car 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 car body 11 is turning, if the attitude deviation of the car body 11 is greater than the deviation of the set angle, then 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 car body 11 to be corrected. The control amount is multiplied by a negative sign, for example, when the car body 11 is turning 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 car body 11 is turning right (as shown in Figure 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 car body 11 can stably follow the target path.

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

To sum up, the guidance and control method for unmanned self-propelled vehicles of the present invention can indeed achieve its efficacy and purpose when used, so the present invention is an invention with excellent practicability, and is in accordance with the requirements for patent application. I have filed an application, hoping that the review committee will approve this case as soon as possible, so as to protect the hard work of the inventor. If the Junju review committee has any doubts, please feel free to send a letter to instruct. The inventor will do his best to cooperate, and I really appreciate it.

Claims (8)

A guidance control method for an unmanned self-propelled vehicle. The unmanned self-propelled vehicle includes a vehicle body, an automatic guidance device, and a steering drive system, and the vehicle body includes two steering wheels and two steering wheels arranged diagonally front and rear for driving and steering control. At least two auxiliary rotating wheels, the automatic guidance device is used to locate the position and attitude of the vehicle body, and generate a predetermined target path for the steering wheel of the vehicle body driven by the steering drive system to follow The target path runs, and the guidance control method includes the following steps: (A) The automatic guidance device obtains the position of the center of the vehicle body to establish a vehicle coordinate system in the global coordinate system, and obtains the current attitude angle of the vehicle body to perform coordinate system conversion to establish a local coordinate system; (B) Calculate the shortest distance D from the center of the car body to one of the target points on the target path, the rotation angle θ _S between the current heading of the car body and the line connecting the center of the car body to the target point, and the center of the car body The radius of rotation R to the target point; (C) Obtain the velocity V of the center of the vehicle body, and calculate the distance between the current course of the vehicle body and the line connecting the center of the vehicle body to the center of any steering wheel according to the distance d from the center of the vehicle body to the center of any steering wheel The included angle θ _a ; (D) Judging the turning direction of the car body according to the turning angle θ _S of the car body, assuming that θ _S is greater than 0, according to the law of cosines:

Assuming that the rotation angle θ _S of the car body is less than 0, according to the law of cosines:

Among them, R _lf is the radius of rotation of the front steering wheel; R _rr is the radius of rotation of the rear steering wheel; θ _lf is the required turning angle of the front steering wheel along the rotation radius R _lf ; θ _rr is the rotation radius of the rear steering wheel along the radius R _rr the required turning angle when making a turn; (E) Since the angular velocity of the center of the car body is equal to the angular rate of the two steering wheels with constant velocity circular motion, according to the relationship between the speed V of the center of the car body, the radius of rotation R and the angular rate:

Wherein the 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 controls the two steering wheels to steer to corresponding positions according to the calculated rotation angles θ _lf , θ _rr and rates V _f , V _r , so that the center of the vehicle body can follow the target path. The guidance control method for unmanned self-propelled vehicles as described in claim 1, wherein the automatic guidance device includes a sensor module for positioning the position and attitude of the vehicle body and a sensor module for generating the target path path planning unit. The guidance control method for unmanned self-propelled vehicles as described in claim 1, wherein the step (A) is to use the rotation matrix to transform the coordinate system:

Where θ is the current attitude angle of the car 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 axis of the vehicle coordinate system Distance between axes; X _G , Y _G are the coordinates of the target point in the global coordinate system; X _L , Y _L are the coordinates of the target point in the local coordinate system. The guidance and control method of the unmanned self-propelled vehicle as described in claim 1, wherein the step (B) is obtained according to the geometric relationship of a right triangle:

Among them, D is the shortest distance from the center of the car body to the target point; θ _S is the angle between the current heading of the car body and the line connecting the center of the car body to the target point; R is the center of the car body to the target point radius of rotation. The guidance and control method of the unmanned self-propelled vehicle as described 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, W is the horizontal distance from the center of the car body to the center of any steering wheel; L is the vertical distance from the center of the car body to the center of any steering wheel; θ _a is the current heading of the car body and the center of the car body The angle between the line 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. The guidance control method of unmanned self-propelled vehicle as described in claim item 1, wherein in this step (D) when the attitude of the two steering wheels of the vehicle body is moving forward in translation, first the vehicle body is in the local coordinate system Rotate the Y _L axis to an included angle θ parallel to a target line formed by a line connecting a starting point to the target point, and make the two steering wheels rotate to the included angle θ in a translational manner, assuming that the target line is divided into two segments , and taking the included angle θ as a benchmark, the control quantities 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 car body to the starting point , y is the distance from the starting point to the midpoint of the target line. As 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 turning, if the attitude deviation of the vehicle body is greater than the deviation of the set angle, the two According to the attitude direction of the car body to be corrected, the control amount of the two steering wheels is multiplied by a negative sign, so that when the car body turns left or right, the control amount of one of the steering wheels is θ+arctan(x/y ), and the control quantity of the other steering wheel is -[θ+arctan(x/y)]. The guidance control method of unmanned self-propelled vehicle as described in claim item 1, wherein this step (F) Complete the steering of the two steering wheels of the car body, and then perform the next step: (G) The automatic guidance device calculates the error amount between the current position of the vehicle body, the radius of rotation and the target path, and uses PID control to obtain the corrected speed and radius of rotation of the vehicle body according to the error amount, and then uses the inverse Kinematics calculates the speed or acceleration required for the car body to move to the target path in a reverse way, so that the steering drive system can control the two steering wheels of the car body to correct and adjust the current position and rotation radius of the car body , until the path guidance control of the car body is completed.

Images