JPH0522925B2 - - Google Patents

Description

[Detailed description of the invention] [Industrial application field]

The present invention relates to a course guidance control system for a traveling body that automatically travels by following a scheduled travel course.
In particular, the present invention relates to a course guidance control system for an automatic traveling body for self-guiding the traveling body onto a linear planned course.

[Prior art and its problems]

BACKGROUND ART Conventionally, the following theory has been known as a steering method for self-guiding a traveling object that automatically travels by following a preset scheduled travel course. In other words, the nth segment of the planned driving course (the nth point Cn (Xn, Yn) and the n+1st point
If the moving object is traveling following a straight section course connecting Cn+1 (Xn+1, Yn+1), then from Fig. 4, the current position point of the moving object P (X, Y)
Take the leg of the perpendicular line drawn down to the course from Q, and point R on the course or its extension. Also, set the steering angle to φ
shall be. Now, with PQ=l1 and QR=l2, determine a constant Co such that l1×l2=Co. If Co is constant, l1 and l2 are inversely proportional, so that the vehicle P is steered so that it is parallel to the straight line PR. As the traveling body P moves away from the course, l2 becomes shorter and the steering angle φ takes a value closer to 0. Therefore, Co is a control constant that determines the steering angle. Therefore, the steering angle φ can be expressed by the following equation. φ = Angle PRQ + (θi - θp) = tan ^-1 (l1 ² /Co) + (θi - θp) Here, θi... Azimuth angle of the n-th segment θp... Azimuth angle of the vehicle Only for this steering angle Using controlled maneuvers,
FIG. 5 shows the results of the course guidance control of the traveling body. Here, PC is a planned running course set using point coordinates (PC1 to PC6). LL is a travel trajectory when the vehicle is traveling at a low speed, and LF is a travel trajectory when the vehicle is traveling at a high speed. From this result, with the conventional steering method, when the vehicle deviates from the planned course, if the vehicle is traveling at low speed, it will show a trajectory that almost follows the planned course, slightly inside the planned course; It has been found that as the speed becomes larger (faster), there is a stronger tendency to deviate to the outside of the planned course. From this, it is understood that the speed of the traveling object has a large influence on course guidance control. If the deviation that occurs when changing the course is large, the distance it takes to approach the planned course becomes longer. This is because if the steering response characteristics are poor, even if an instruction to significantly reduce the steering angle (steering angle command) is issued, the steering wheel cannot be suddenly displaced to the instructed angle. In other words, the error occurs because the vehicle is traveling without the traveling means such as tires being completely displaced in the originally intended direction. The inventor of the present invention believed that such course deviations could be resolved by changing the traveling speed according to the magnitude of the steering angle, and as a result of intensive research, the present invention was completed.

[Purpose of the invention]

This invention introduces a speed control method in which the traveling speed is a function of the steering angle in a course guidance control method for a traveling object that automatically travels by following a planned traveling course, and controls the steering angle and the corresponding traveling speed. The purpose is to

[Structure of the invention]

In order to achieve the above-mentioned object, the present invention (a) detects a traveling body based on course data of a planned running course and position data of the traveling body obtained from a position detection means for detecting the current position of the traveling body; Relative position measuring means is provided for measuring the relative position of the vehicle with respect to the planned travel course. (b) A steering angle determining means is provided for inputting the measured relative position data, the course data, and the position data of the traveling object to determine a steering angle for self-guiding to the planned course. (c) A traveling speed determining means is provided which inputs the determined steering angle data and determines the traveling speed at the time of steering together with the course data. (d) A control means is provided for controlling the steering angle of the traveling vehicle and the traveling speed during steering based on the traveling speed and steering angle determined by the traveling speed determining means and the steering angle determining means. We are taking technical measures. Since the traveling speed is changed according to the steering angle for following the planned course, course guidance control can be performed with less error from the planned course even when the steering response characteristics are slow.

【Example】

FIG. 1 is a block diagram showing a course guidance control system for a traveling object according to the present invention (guidance theory will be explained based on FIG. 4). In this embodiment, this self-guidance control system for a traveling object is a microcomputer (hereinafter referred to as
The control means 15 controls the steering mechanism section S and the power mechanism section D of the automatic traveling body using control signals from the CPU. In addition, in this invention, in order to display the course data (position) of the planned course, the position of the traveling object, and other position data, point coordinates are used based on the X-axis and Y-axis assumed at the desired position of the traveling route, etc. This is done by converting to (x, y). Here, reference numeral 1 denotes a planned driving course storage section provided in the CPU, in which planned driving course data expressed in point coordinates is stored. This planned running course is set in advance by an appropriate means depending on the purpose of the running object, for example, by setting the planned course based on a map, or by actually conducting a test run using manual or radio control. The planned travel course is determined by an appropriate means such as a method that sets a scheduled travel course based on travel locus data of the traveling object. When using actual travel locus data, the course setting of this planned driving course is modified to a planned driving course with consecutive straight courses, and each intersection point where the straight courses intersect with each other is set to a continuous point coordinate (x,
y) and is stored in the planned running course storage unit 1. To explain based on Figure 5, the planned driving course
PC starts from PC1 (5, 0) → PC2 (0,
0) → C3 (0,5) → PC4 (4,7) → PC5
It is set as a plurality of consecutive point coordinates from (4, 2) to the end point PC6 (0, 0), and is input and stored in the planned running course storage unit 1. Further, in this planned running course storage unit 1, the linear section course represented by the point coordinates, that is, the section course running speed and the section course minimum running speed when traveling between two point coordinates are preset. Store it aside. Reference numeral 2 denotes a position detecting means for the traveling body, which detects the current position of the traveling body P that is running following the planned travel course. The running distance from the base point (start point) and the direction (azimuth) of the running object are calculated using an encoder and gyroscope (not shown), and position correction means such as a gate ball (not shown) installed on the ground. The system corrects the traveling position data and continuously outputs the obtained traveling position data, that is, the traveling body position data and the traveling body azimuth data expressed in point coordinates. The point P (X, Y) of the current position of the traveling object outputted from the position detection means 2, and two points called from the course data storage section 1 representing the section course that the traveling object is following. Coordinate data C1 (X1,
Y1) and C2 (X2, Y2) are relative position measuring means 3
is input. This relative position measuring means 3 determines the planned traveling course from the point P((X, Y) of the current position of the traveling body and point coordinate data C1 (X1, Y1) and C2 (X2, Y2) representing the divided course. This is to measure the amount of deviation from the traveling body, that is, the length of a perpendicular drawn from the position of the traveling body to the linear section course that the traveling body is following (corresponding to l1 in Fig. 4). Calculated by The relative position l1 calculated in this way is based on the traveling body azimuth angle data θp obtained from the position detecting means 2, and C1 and C2 representing the section course being followed that was read from the planned travel course storage unit 1. Course azimuth angle data θi calculated based on coordinate point data
It is also input to the steering angle determining means 4. The steering angle determining means 4 uses the same steering angle calculation formula as described above, namely, P (φ) = tan ^-1 (l1 ² /Co) + (θi - θp) (where Co is a control constant that determines the steering angle. The steering angle P(φ) is calculated using the optimum value determined experimentally. Next, the steering angle P(φ) calculated in this way
is converted into a steering angle command CMD (φ) and input to the traveling speed determining means 5. Further, from the course data storage unit 1, each data of the set travel speed Vc and the minimum travel speed Vmin of the section course currently being followed in the planned travel course is called up and input into the travel speed determining means 5. Here, the minimum running speed Vmin is set by experimentally determining the optimum speed at which the vehicle leaves the course at the maximum turning angle of the steering wheel. Next, based on the input data, the traveling speed determining means 5 determines the output traveling speed V(φ) according to the following formula. In other words, V(φ)=Vmin+(Vc-Vmin)×f(φ) f(φ)=1-(CMD(φ)/R) R...Maximum turning angle of steering From the above, the larger the steering angle, the faster the vehicle will travel. The speed decreases, and when the steering angle is 0, that is, when traveling in a straight line, it becomes the section set traveling speed Vc, when the steering angle is maximum, it becomes the minimum traveling speed Vmin, and from Vc to Vmin depending on the size of the steering angle. The running speed changes between. As described above, the steering angle and traveling speed obtained by the steering angle determining means 4 and the traveling speed determining means 5 are converted into a steering angle command signal and a traveling speed command signal, respectively, and are converted into a steering angle command signal and a traveling speed command signal, respectively, and are converted into a steering angle command signal and a traveling speed command signal, respectively. is input. This steering control means 6 controls the steering mechanism section S, turns the steering via an actuator based on the steering angle command signal, and displaces the steering to the steering angle determined by the steering angle determining means 4. At the same time, the traveling speed control means 7 controls the power mechanism section D based on the traveling speed command signal, and changes the traveling speed to the traveling speed determined by the traveling speed determining means 6 via the actuator, thereby adjusting the steering. Controls the traveling speed when controlled. As a result, the amount of steering, that is, the steering angle, and the traveling speed are controlled in a correlated manner, so that the traveling object can be correctly self-guided to the planned course. When controlling the speed of a traveling object in accordance with the steering angle in this way, the traveling speed becomes slower as the steering angle increases. Therefore, if the amount of steering wheel turning suddenly increases, the traveling speed may drop suddenly and a slip phenomenon may occur. Therefore, when it is determined that the steering angle turning amount determined by the steering angle determining means 4 increases beyond a preset rapid increase reference value, the steering angle is increased stepwise, and vice versa. The steering angle determining means 4 is provided with a steering angle adjustment means 8 that gradually decreases the steering angle when it is determined that the amount of steering angle is decreased by a sharp decrease reference value, such as when traveling in a straight line. It is preferable to provide one. The steering angle adjusting means 8 makes the above determination based on the data input from the steering angle determining means, and sends a steering angle command signal adjusted to increase or decrease stepwise at predetermined intervals (time) to the steering mechanism. S
Output to. If the adjustment control by the steering angle adjustment means 8 is used, the traveling speed can be smoothly decelerated, and the slip phenomenon can be avoided, which is preferable. Next, as shown in Figure 2, start point (1,
If you set a planned travel course that starts from 0) and follows along the Y axis, if you look at the travel trajectory of the vehicle, if the travel speed is high and the approach angle is large, the actual travel trajectory will be on the outside ( It can be seen that there is a tendency to bulge inward (or inward). This is because near the point where the vehicle intersects the planned course, the steering is not turned (the direction of travel and the direction of the vehicle such as tires are the same), so the speed of the vehicle is determined by the steering angle (steering angle). ), but the speed control described above does not reduce the speed. Therefore, it is considered that the vehicle deviates in the approach extension direction because the vehicle continues to travel at the set travel speed without deceleration, similar to when the vehicle is directly traveling. In other words, when the steering angle reaches 0 degrees in the middle of steering, the speed control described above cannot fully control the steering angle. Therefore, the approach angle for the section course is the angle PRQ shown in Figure 4, that is, tan ^-1 (l1 ² /Co) (radian).
However, if the approach angle limit is set to α (radian) and the course approach angle angle PRQ>α by the approach angle limiting means 9, the steering angle V(φ) is set as V(φ)=α+(θi −θp), and the same control as above is performed based on this. That is, the course approach angle used in calculating the steering angle is called from the steering angle calculated by the steering angle determining means 4, and it is determined whether the course approach angle exceeds the approach angle upper limit setting value (α). Only when the course approach angle exceeds the set value (α), the steering angle determining means 4 uses the set value (α) instead of the approach angle used to calculate the steering angle to determine the steering angle V (φ) again. Based on the revised steering angle, a steering angle command signal is output to the steering mechanism section S to perform steering control. FIG. 3 shows a case in which the approach angle limiting means 9 is used to self-guide the vehicle along a course set in the same manner as in FIG. 2. This shows that deviations from the planned course could be kept to a minimum.

【Effect of the invention】

The present invention is advantageous because it changes the running speed according to the amount of steering, so it can improve the ability of the running object to follow the planned course. In addition, it is highly versatile as it can be used as a self-guidance control method for all types of vehicles, regardless of the purpose, type, or running surface conditions of the vehicle to be controlled, such as floor, on-road, or off-road. ing.

[Brief explanation of the drawing]

FIG. 1 is a functional block diagram of the present invention, FIG. 2 is a diagram showing the traveling trajectory when the approach angle limiting means is not used, FIG. 3 is a diagram showing the traveling trajectory when the approach angle limiting means is used, and FIG. FIG. 4 is an explanatory diagram for determining the steering angle for returning when the traveling body deviates from the section course to be followed, and FIG. 5 is a diagram showing the travel locus when controlled by the conventional steering method.

Claims (1)

[Scope of Claims] 1. A course guidance control system for a traveling object that automatically travels by following a planned traveling course, comprising: course data of the scheduled traveling course and a position detection means for detecting the current position of the traveling object; a relative position measuring means for measuring the relative position (deviation amount) l1 of the traveling body with respect to the above-mentioned planned travel course based on the traveling body position data, and the measured relative position data l1 and the azimuth angle of the traveling body. By inputting θp, the control constant Co, and the azimuth angle θi of the course the vehicle is following, the steering angle P(φ) for self-guiding to the planned course is calculated as P(φ)=tan ^-1 ( a steering angle determining means that calculates l1 ₂ /Co) + (θi - θp), a travel speed (Vc) and a minimum travel speed (Vmin) preset for the planned course, and the steering angle P (φ). Steering angle command CMD (φ) based on
and the maximum turning angle R of the steering wheel to calculate the traveling speed V(φ), V(φ)=Vmin+(Vc−Vmin)×f(φ) f(φ)=1−(CMD(φ)/ R) A steering angle of the traveling object is determined based on the traveling speed V (φ) and the steering angle P (φ) determined by the traveling speed determining means and the steering angle determining means. A course guidance control system for an automatic running body, comprising: a control means for controlling a running speed; 2. The steering angle determining means increases the steering angle in stages when the amount of turning of the steering angle determined by the steering angle determining means exceeds a rapid increase reference value; claim 1, further comprising a steering angle adjusting means that reduces the steering angle in stages when the steering angle exceeds a rapid reduction reference value.
A course guidance control system for an automatic traveling body as described in . 3. The steering angle determining means includes approach angle limiting means for calculating the steering angle using the set value as the course approach angle when the course approach angle of the traveling object exceeds the approach angle upper limit set value. A course guidance control system for an automatic traveling body according to claim 1 or 2.