JPH0522924B2 - - Google Patents

Description

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

The present invention relates to a self-guidance control method for a traveling object that automatically travels following a scheduled travel course consisting of a plurality of continuous linear section courses.

[Prior art and its problems]

Steering methods that use an external guidance device to perform self-guidance control by following a preset planned travel course and transition control when changing courses are known, but it is difficult to avoid this when the traveling path of the vehicle is on the floor or on the road. Therefore, it is not yet suitable for application to off-road vehicles and involves various problems.

[Purpose of the invention]

This invention was created as a result of intensive research in view of the above circumstances, and its main purpose is to provide a self-guidance control method for an automatically traveling vehicle that can perform self-guidance of the vehicle regardless of whether it is on-road or off-road. There is something to do.

[Structure of the invention]

In order to achieve the above object, the present invention, as shown in the flowchart of FIG. Record this on the recording medium and perform the initial settings. Next, in step, the self-guidance control procedure is started, the traveling body starts traveling, and the speed is controlled in accordance with section course speed data preset for the section course to be followed. Then, in step, from the current position data of the traveling body obtained from the traveling position measuring means of the traveling body,
Measures the amount of deviation between the section course being followed and the traveling object. Based on this amount of deviation, the amount by which the steering wheel is turned in steps (steering angle) and the traveling speed at that time are calculated and determined. Next, in a step, the steering angle data and traveling speed data are converted into control commands and outputted to the control device, and the control device guides the traveling object onto the section course to be followed in accordance with the control commands. In this way, it is determined in each step whether or not the traveling body following the section course has reached the final target point of the scheduled traveling course from the traveling position (current position). Reached the final target point (that is, the coordinates indicated by the current position of the traveling object match the coordinates of the final point of the planned course)
If this happens, the running of the traveling body is stopped in the step, and this self-guidance control procedure is completed. If the traveling object has not reached the final target point, it is determined in step whether the current position of the traveling object is within the turning start area for changing the course. If it is determined that it is outside the area, return to the step,
If it is determined that it is within the range, the step performs turning control for transition, moves to the next section course, and returns to the step. As a result, course guidance and course transition of the vehicle that follows a scheduled travel course set as a plurality of continuous point coordinates can be smoothly performed.

【Example】

Preferred embodiments of the invention will be described below. The self-guidance control method of the present invention includes a self-guidance system including a course guidance system and a course transition guidance system, and a self-guidance control program for controlling the self-guidance system. The self-guidance system uses a microcomputer (hereinafter referred to as
CPU), and the steering mechanism section S and the power mechanism section D of the traveling body according to the control signals of the CPU.
and a control means 15 for controlling. In addition, in this invention, the course data ((position)) of the planned course, the position of the traveling object, and other position data are based on the X-axis and Y-axis assumed at the desired position of the traveling route, etc.
This is done by converting into point coordinates (x, y) with the axis as a reference. FIG. 2 shows a system block diagram of the invention. Reference numeral 1 denotes a planned driving course storage unit in which the planned driving course is input into the storage unit of the CPU or the storage body, and stores course data of the planned driving course expressed in point coordinates. This planned running course is set in advance by an appropriate means, for example, by setting the planned course based on a map, or by using data on the running trajectory of the running object that has actually been tested by manual or radio control. It is determined by appropriate means such as setting a scheduled driving course based on. When using actual travel locus data to set the course for this planned driving course, the planned driving course should be modified to have consecutive straight courses, and each intersection point where the straight courses intersect with each other should be set to a plurality of continuous point coordinates (x , y), and the planned driving course storage unit 1
It is to be stored in . Explaining based on FIG. 4 showing the running trajectory, the planned running course PC starts from the starting point PC1 (5,0)→
PC2 (0,0) → PC3 (0,5) → PC4 (4,7)
→ PC6 (4, 2) → end point PC6 (0, 0) is set as a plurality of consecutive point coordinates, and is input and stored in the planned travel course storage unit 1. In addition, the scheduled travel course storage unit 1 stores preset travel speeds and minimum travel speeds for a linear section course represented by the coordinates of two consecutive points. 2 is a means for measuring the traveling position (current position) of the traveling object, which measures the current position of the traveling object following the planned course (represented by coordinate values of the same standard as the point coordinates of the course data). In this embodiment, an encoder and a gyroscope (not shown) mounted on the vehicle are used to measure the travel distance from the base point (start point) of the planned course and the travel distance of the vehicle. direction (azimuth)
and correct the relatively calculated travel data using an absolute position detection means such as a gateball (not shown) that is installed on the ground to absolutely measure the position of the traveling object. Position data, that is, vehicle position data expressed in point coordinates and vehicle azimuth data are continuously output. Now, the various parameters for the initial settings are
When the information is input to the CPU memory and the switch is turned on, the self-guidance control program stored in the recording medium starts. As a result, the traveling object is controlled by the traveling speed and azimuth of the section course that have been preset in the section course data, and travels from the base point ((starting point)) to follow the first section course of the scheduled course. At the same time, the position measuring means 2 operates, and the traveling body position data (hereinafter referred to as P(X, Y)) outputted from the position measuring means 2 and called from the course data recording means 1 are started. The point coordinates C1 ((X1,
Y1) and C2 (X2, Y2) are input to the relative position measuring means 3. This relative position measuring means 3 determines the planned traveling course and the traveling body from the position data P (X, Y) of the traveling body and the point coordinate data C1 (X1, Y1) and C2 (X2, Y2) representing the divided course. In other words, it measures the length l1 of a perpendicular line drawn from the position of the traveling body to the linear course that the traveling body is following, Calculated by The relative position l1 calculated in this way is based on the traveling body azimuth data θP obtained from the position detection means and the coordinate point data (C1,
C2) is input to the steering angle determining means 4 together with the course azimuth angle data θi calculated based on C2). The steering angle determining means 4 uses the steering angle calculation formula: P((φ)=tan ^-1 (l1 ² /Co) + (θi−θp) (Co...This is a control constant that determines the steering angle and has been experimentally determined. The final numerical value is obtained.) The steering angle P(φ) is calculated using the following.Next, the steering angle P(φ) calculated in this way is
is converted into a steering angle command CMD (φ) and input to the traveling speed determining means 5. Further, from the course data recording means 1, each data of the set speed Vc and the minimum running speed Vmin for the section course currently being followed in the planned running course is called up and input into the running speed determining means 5. Here, the minimum running speed Vmin is set by experimentally determining the optimum speed at which the vehicle is less likely to deviate from the course when the steering wheel is at its maximum turning angle. Next, based on the input data, the traveling speed determining means 5 determines the output traveling speed V according to the following formula.
(φ) is determined. That is, V(φ)=Vmin+((Vc-Vmin)×f(φ) f((φ)=1-(CMD((φ)/R) R...Maximum turning angle of the steering As a result of the above, the steering angle is The traveling speed decreases as the steering angle increases.When the steering angle is 0, that is, when traveling in a straight line, the travel speed becomes the section setting travel speed Vc, and when the steering angle is maximum, the travel speed becomes the minimum travel speed Vmin, and Vc changes depending on the size of the steering angle. The running speed changes between Vmin and Vmin. As described above, the steering angle and running speed obtained by the steering angle determining means 4 and the running speed determining means 5 are converted into a steering angle command signal and a running speed command signal, respectively. These signals are respectively input to the steering control means 5 and the traveling speed control means 6.The steering control means 5 operates the steering mechanism section S via the actuator based on the steering angle command signal, and controls the steering according to the steering angle determining means 4. At the same time, the traveling speed control means 6 operates the power mechanism section D via the actuator based on the traveling speed command signal, and the traveling speed is determined by the traveling speed determining means 6. The vehicle speed is controlled by varying the travel speed just before the steering is controlled.As a result, the steering angle, that is, the steering angle, and the travel speed are controlled in a correlated manner, so that the vehicle follows the planned course. The vehicle can be followed correctly. When controlling the speed of the vehicle in response to the steering angle in this way, as the steering angle increases, the traveling speed slows down. Therefore, the steering angle suddenly increases. In such cases, the traveling speed may suddenly decrease and a slip phenomenon may occur.Therefore, if the amount of steering angle turning increases beyond the rapid increase reference value, the steering angle is increased in stages. On the other hand, when the steering angle decreases by exceeding a sharp reduction reference value, such as when turning from a turn to straight running, the steering angle control means 6 is provided with a steering angle control means 8 that decreases the steering angle in stages. If the steering angle control means 8 is used for control, it is possible to smoothly reduce the traveling speed, thereby avoiding the slip phenomenon, which is preferable. The body can follow the planned course, but if the running speed is high and the approach angle is large, the actual trajectory tends to swell outward (or inward). Since the area where the vehicle intersects with the steering wheel is close to a state where the steering is not turned (the direction of travel and the direction of the vehicle such as tires are the same), the traveling speed is controlled as a function of the steering angle (steering angle). 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 meandering, the speed control described above cannot fully control the vehicle. Therefore, the approach angle for the section course is angle PRQ
That is, tan ^-1 (l1 ² /Co) radiance, but the approach angle limit determined experimentally is α radiance, and the course approach angle is determined by the approach angle limiting means 9, and if the angle PRQ>α, the steering angle is P((φ) is corrected to P(φ)=α+((θi−θp), and the same control as above is performed based on this. As a result, the deviation from the section course can be minimized. In this way, the traveling object that has left the section course is guided to the section course to be followed, and when the position data of the traveling object matches the point coordinates of the final point of the planned course, it is determined that the course guidance has ended, and the traveling object is guided to the following section course. stops, and the self-guidance control program ends. If it does not match the final point, the program proceeds further and determines whether or not the position data of the traveling object is within the turning start area for changing the course. If yes. If it is determined, the course transition guidance control is performed as follows. That is, 10 is the minimum turning radius setting means,
The travel speed data detected by the travel speed detection sensor 11 is input, and the minimum turning radius corresponding to the travel speed data is set. Based on the travel speed data, the optimum ( (minimum) turning radius r'(t). Reference numeral 12 denotes an understeer correction value setting means, which determines the correction value Cl(t) experimentally or as a function of the running speed based on the steering response characteristics of the vehicle and the running speed data during running. be. Data obtained by each of the above means, namely,
Minimum turning radius r'(t) data, azimuth angle θn data of the section course being followed, azimuth angle θn+1 data of the section course to be transferred, traveling object azimuth angle θp data, relative position (difference between the traveling object and the section course) The amount) ξ data and the understeer correction value Cl(t) data are each input to a course transition position calculating means 13 that measures a turning start area for a course transition. The course transition position calculation means 13 calculates the course transition start distance a(t) according to the following formula. a(t)=r′(t)×tan {(φn+1−θp)/2} +ξ×tan (θn+1−θp−π/2)+Cl(t) Note that θn+1 and θn are not equal when creating the course. If θn + 1 = θp, and if the value of transition start distance a(t) becomes larger than the total length of course n, a(t) takes the total length of course n as the maximum value, and the next course is Let's move on to n+1. As a result, since the azimuth angle θp of the vehicle is used in place of the azimuth angle θn of the vehicle being followed in the first term, an error may occur even when the vehicle is not parallel to the course it is following but faces in a different direction. do not have. In addition, the minimum turning radius r′(t) changes depending on the traveling speed immediately before the transition turn, so when the traveling speed is high, the turning radius becomes larger than when the speed is slow. The optimum minimum turning radius for turning at the above-mentioned speed was also given to suppress the influence of traveling speed. Next, in the second term, the influence of the departure distance ξ from the section course being followed is corrected. In addition, in the third section, the influence of understeer tendency on speed is offset, assuming a case where sufficient deceleration cannot be achieved by speed control, which will be described later. The course transition start distance a(t) data thus calculated is then input to the course transition determination means 14. This course transition determination means 14 determines the section course n+1 to be transitioned from the position data of the running vehicle.
Measure the distance η from the point coordinates of C2,
It is compared whether this distance is within the range of a(t). If it is within the range, a course transition control signal is output to the control means 15. The control means 15 turns the steering by a predetermined amount via the steering control means based on the course change control signal to perform a course change turn. Here, as described above, the traveling speed changes between Vc and Vmin according to the steering angle by speed control. Further, the steering angle adjusting means 8 increases the steering angle in stages when the amount of steering angle turning is increasing, and conversely, the amount of steering angle turning from turning to straight running is decreasing. If the vehicle is headed towards the target, the steering angle is gradually reduced. Further, when the approach angle to the section course is angle PRQ>α, the approach angle limiting means 9 replaces the angle PRQ with α to calculate the steering angle. When the traveling object moves to the next section course by such course transition control, the self-guidance control program returns to the course guidance procedure when following a straight course, and the position data of the traveling object matches the end point of the planned course. Continue controlling until It is preferable that this self-guidance control system is provided with an emergency processing section 21 that gives priority to the control signal from the obstacle detection means 20 over other guidance control signals. That is, when it is determined by the obstacle detection means 20 that there is an obstacle on the course to be traveled,
Obstacle detection data is input to the emergency processing section 21. Then, the emergency processing unit 21 determines whether the distance to the obstacle is within the set value, and if it is within the set value, sends an emergency control signal to the emergency control device 2.
Output to 2. This emergency control signal may be, for example, a control signal to stop the operation of a traveling object, or it may be a control signal that recognizes the pattern of an obstacle and determines whether to go around or stop, and if it is to go around the obstacle, it can be used to stop the vehicle. An appropriate control method can be adopted, such as a control method in which the vehicle learns an obstacle detour course that returns to the planned travel course, and performs self-guidance control along the planned travel course that includes the obstacle detour course. In this way, it is advantageous and safe to provide a step in which when the detection signal of the obstacle detection means is input to the self-guidance control program, the emergency control is performed with priority over any other guidance control signal. .

[Brief explanation of the drawing]

Fig. 1 is a flowchart showing the procedure of the self-guidance control method of the present invention, Fig. 2 is a functional block diagram of the self-guidance control system, Fig. 3a is an explanatory diagram for explaining course guidance, and Fig. 3b is a course diagram. FIG. 4, which is an explanatory diagram for explaining the transition, is a diagram showing a traveling locus when traveling according to this guidance control method.

Claims (1)

[Scope of Claims] 1. The traveling position of the traveling body is measured, and the steering angle and running speed of the traveling body are controlled to form a plurality of consecutive straight section courses, each section course being A self-guidance control method for an automatic running object, characterized by comprising the following steps, for self-guiding the running object by following a planned travel course set as a straight line connecting starting point coordinates and ending point coordinates. A planned running course is set, the set course data and control constants are recorded in the storage section, and a self-guidance control procedure is started. The traveling body starts running, and its speed is controlled according to section course speed data preset for the section course to be followed. The amount of deviation between the following section course and the traveling body is measured from the current position data of the traveling body obtained from the running position measuring means of the traveling body. Based on the amount of deviation measured in the step, the amount by which the steering wheel is turned (steering angle) and the traveling speed at that time are calculated and determined. The steering angle data and traveling speed data obtained in the step are converted into control commands, and the steering angle is controlled via the control device to guide the vehicle along the section course to be followed. It is determined whether the running object has reached the final target point of the planned running course from the running position (current position). When the final target point is reached, the traveling object is stopped and this self-guidance control procedure is completed. In order to control the traveling body to turn at a predetermined turning radius when moving from an arbitrary section course that the traveling body follows to the next section course,
When the vehicle is on its planned course,
It is determined whether the current position of the traveling object is within the turning start area for changing the course, and if it is not within the area, the process returns to step. If the vehicle has entered the turning start area, the turning control described above for transition is performed, and the process moves to the next section course and returns to the step. 2. When an obstacle is detected by the obstacle detection means, no matter what step the self-guidance control procedure is at, the emergency processing means will perform emergency control such as stopping the vehicle or driving around the obstacle. A self-guidance control method for an autonomous vehicle as claimed in claim 1.