JPH0522927B2 - - Google Patents

Description

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

The present invention relates to a course guidance method for an automatic traveling body for automatically guiding a traveling body that automatically travels by following a scheduled traveling course onto the scheduled traveling course.

[Prior art and its problems]

Conventionally, the following theory is known as a steering method for self-guiding a traveling object that automatically travels by following a preset course. That is, to explain based on Fig. 4, the nth segment of the course (nth point C1 (Xn, Yn)
If the traveling object is traveling following a straight section course connecting the n+1st point C2 (Xn+1, Yn+1), then the perpendicular line drawn from the traveling object P (X, Y) to the course Place your foot Q and point R on the course or its extension. Now, let PQ=l1, QR=l2, and 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 as to be parallel to the straight line PR. The farther the traveling body P is from the course, the shorter l2 becomes, 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 Steering by controlling this steering angle As shown in FIG. 5, the result of controlling the course of the traveling object using the method is a trajectory that draws a constant asymptote. According to this steering method, the traveling object theoretically never travels on the course, so when the traveling object approaches the course, it runs substantially parallel to the course.

[Problems to be solved by the invention]

An object of the present invention is to bring the traveling object closer to the course in a short time and to bring the traveling object as close to the planned course as possible when such a traveling trajectory draws an asymptote.

[Means to solve the problem]

In order to solve the above-mentioned problem, the present invention includes (A) a planned traveling course that is a factor in determining the steering angle of a traveling object, (i) a main course in which the traveling object is scheduled to travel, and (ii) a left-right direction of the main course. (iii) a virtual course set parallel to the main course at a predetermined interval on at least one side of the main course; and (iii) a virtual course set parallel to the main course at a closer interval on the opposite side of the virtual course across the main course. A designated switching area or switching line shall be provided. (B) When the vehicle is traveling outside the switching area or switching line (away from the main course), calculate the steering angle to follow the virtual course. (C) When the vehicle enters the inside of the switching area or switching line (main course approach side), calculate the steering angle to follow the main course. We are taking technical measures.

[Effect]

The operation of this invention will be explained based on FIG. Here, MC is a main course in which the travel schedule of the traveling object is set in advance, IC is a virtual course that is set parallel to and spaced apart from the main course MC, and SE is a switching area or a switching line. If the traveling object is now located at a point P1 outside the switching area (or switching line, the same applies hereinafter), the amount of deviation L1 between the traveling object and the virtual course IC is calculated,
The steering angle is determined based on this. When the vehicle is controlled and moves according to the determined steering angle, the vehicle enters the switching area SE (P2). When the traveling object enters the switching area SE, the amount of deviation L1' from the main course MC is calculated, and the steering angle is determined based on the deviation amount L1'. By proceeding according to the determined steering angle, the traveling object can approach the main course MC as much as possible, and can accurately follow the planned course. The trajectory when switching between the virtual course IC and the main course MC is shown by the dashed line, and the steering angle of the traveling object was calculated and controlled using the same steering angle calculation means as described above based only on the main course MC. The trajectory in this case is shown by a dotted line. As a result, according to the course guidance control method of the present invention, when the traveling object follows the virtual course IC, the amount of deviation increases, and the angle of approach increases accordingly. Once inside the switching area, it follows the main course MC, so it is similar to conventional control, so the main course MC
can be approached quickly and accurately.

【Example】

FIG. 3 is a book diagram showing a self-guidance control system using the course guidance method for a traveling object according to the present invention. In this embodiment, the course guidance control system for the traveling object uses a microcomputer (hereinafter referred to as CPU) that performs calculation processing based on data input from various sensors equipped on the traveling object or installed on the traveling route. ) and a control means 15 that controls the steering mechanism section S and the power mechanism section D of the automatic traveling body in accordance with control signals from the CPU. In addition, in this invention, in order to display the course data of the planned running course, the position of the running object, and other position data, an
This is done by converting to point coordinates based on the Y axis. Here, reference numeral 1 denotes a planned driving course storage section provided in the CPU, in which data of the planned driving course expressed in point coordinates is stored. This scheduled running course is set in advance by an appropriate means depending on the running purpose of the traveling object, for example, by setting the scheduled course based on a map, or actually using manual or radio control. It is determined by an appropriate means, such as by setting a scheduled travel course based on travel locus data of a test vehicle. When using actual travel trajectory data, the course setting for this planned driving course is corrected to a planned driving course with consecutive straight courses, and each intersection point where the straight courses intersect is read as a continuous point coordinate to plan the driving. This is stored in the course storage unit 1. The data file for this planned course includes data regarding the original main course MC set as described above, data regarding the virtual course IC set parallel to the main course MC at predetermined intervals, and data regarding the virtual course IC. It consists of data regarding the switching area (or switching line) SE set parallel to the main course MC at intervals closer than the course IC. In this embodiment, the main course MC is made up of a set of continuous point coordinates, and the section course is determined from the point coordinates of two successive points. Now, if the section course established by two points on the main course is represented by y = ax + b, the virtual course set on the + side with respect to the main course is y = ax +
It is expressed as b+L. The boundary line or switching line of the switching area for this is represented by y=ax+b-S. (Here, L>S) In this way, the virtual courses are set parallel to each other at predetermined intervals based on the point coordinates of the main course. The predetermined distance L to be separated from the main course is such that even if the vehicle travels a predetermined distance on an asymptote-like trajectory that is generated when the steering angle is determined by a steering angle determining means (to be described later) based on the main course and the vehicle is advanced. It is preferable to appropriately select and determine the optimum numerical value experimentally based on the interval at which the distances do not approach each other, depending on the model of the traveling object, the conditions of the traveling road surface, and the like. The switching area may be set in the shape of a strip parallel to the main course, and may be set as a shift amount of a predetermined width of +S or -S with respect to the main course. Also, the predetermined interval S of the switching area or switching line
The steering angle is determined based on the virtual course, and the steering angle is determined based on the virtual course. As for the interval, it is preferable to appropriately select and determine the optimum numerical value experimentally depending on the model of the traveling object, the conditions of the traveling road surface, etc., as described above. Here, in the case of a switching area, whether or not the value of the amount of deviation between the current position of the traveling object and the main course measured by the relative position measuring means described later is within the predetermined deviation amount set as the switching area. Then, it is determined whether the traveling object is within the range of the switching area. On the other hand, in the case of a switching line, it is determined whether the point coordinates indicating the current position of the traveling object is the + direction or - direction of the straight line (linear equation) set by the two point coordinates of the switching line, and then the direction is determined. If the main course is included in the main course, it is further determined whether the current position of the traveling object exists between the straight line (linear equation) set by the point coordinates of the two corresponding points of the main course, and if it exists, It is determined that the switch is inside the switching line. Further, if there is no main course in that method, it is determined that the switching line is outside. Note that in the switching area or switching line, even when online, it is determined that the switching area or switching line is within the range. Next, the data of this virtual course and switching area or switching line may be calculated separately from the data of the main course and input into the planned driving course storage section 1, or the data of the main course input into the planned driving course storage section 1. Based on the data, the predetermined interval required for setting the virtual course and switching area or switching line is input, and a microcomputer calculates the virtual course data and switching area or switching line data, which is stored in the planned running course. It may also be something that is stored in the section. In this invention, the virtual course and the switching area or the switching line may each be located on either side of the main course, but since it is usually unclear whether the traveling object will deviate to the left or right with respect to the main course, this It is preferable to set them on both the left and right sides as in the embodiment. When a virtual course and switching areas or switching lines are provided on both sides in this way, it is necessary to determine which side the traveling object is currently deviating from with respect to the main course, as described later, and determine the virtual course to be followed and the switching line. It is necessary to provide means for selecting the area or switching line. The main course, virtual course, and
Data of a planned travel course consisting of switching lines is set as a plurality of consecutive point coordinates (or deviation amounts when a switching area is provided instead of a switching line) and is stored in the planned travel course storage unit 1. In addition, in this planned running course storage unit 1, the section course running speed and the section course minimum running speed when traveling on the linear section course represented by the point coordinates are set in advance, and the section course running speed and section course minimum running speed are set in advance to correspond to each section course. and store it. Reference numeral 2 denotes a position detecting means for the traveling body, which detects the current position of the traveling body 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 pole (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 traveling body position data P outputted from the position detecting means 2 and the point coordinate data C1 and C2 of two points called from the course data recording means 1 and representing the section course that the traveling body is following are relative to each other. It is input to the position measuring means 3. As the course data input here, either main course data or virtual course data is selected according to the flowchart shown in FIG. First, it is determined whether the current position of the traveling object detected by the position detection means in the step is within the switching area SE (inside or outside in the case of a switching line).
is determined by the switching necessity determining means 21. If it is determined that it is outside the area, then in the next step, the follow-up course determining means 22 determines whether it deviates in the + direction or in the - direction with respect to the main course MC. This determination result is input to the follow-up course selection means 23. If it is determined that the deviation is to the + side, in step the azimuth of the virtual course IC set to the - side with respect to the main course MC is called from the travel planned course storage unit 1, and the relative position measuring means Enter 3. If it is determined that the deviation is to the - side, in step ' the data of the virtual course IC set to the + side with respect to the main course MC is called from the travel planned course storage section 1, and the relative position measuring means Enter 3. In the data of the virtual course IC input in this way, the amount of deviation is calculated by the relative position measuring means 3 in a step, and based on this, the steering angle determining means 4 calculates the deviation amount to determine the steering angle. In this way, the steering angles are sequentially determined and the steering angle of the vehicle is controlled accordingly.
The traveling object enters the switching area SE. In step, the switching necessity determining means 21 determines that the traveling object has entered the switching area SE, and the determination result is transmitted to the following course selecting means 23.
, the data of the planned travel course inputted to the relative position measuring means 3 in step is switched to the data of the main course. Then, in step, the amount of deviation is calculated based on the data of this main course, and based on this, the steering angle is sequentially calculated by the steering angle determining means 4. It should be noted that if a stop instruction signal is input to the control device of the traveling object in step, the course selection ends, but if not, the process returns to step and repeats the above procedure. In this way, the relative position measuring means 3 detects the straight line and Based on the position data P (X, Y) of the traveling object, the amount of deviation between the planned traveling course and the traveling object, that is, the length L1 ( (corresponding to l1 in Figure 4), Calculated by The relative position L1 calculated in this way is based on the vehicle azimuth data θp obtained from the position detecting means 2 and CD1 and CD2 representing the section course being followed that was read from the planned travel course storage unit 1. It is input to the steering angle determining means 4 together with the course azimuth angle data θi calculated based on the point coordinate data. 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 scheduled traveling course storage section 1, each data of the set traveling speed Vc and the minimum traveling speed Vmin of the section course currently being followed in the scheduled traveling course is called up and input into the traveling 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 6, start point (1,0)
When you set a planned travel course that starts from 1 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 (or inside). ). 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 meandering, the speed control described above cannot fully control the vehicle. Therefore, the approach angle for the section course is the angle PRQ shown in Figure 4, that is, tan ^-1 (L1 ₂ /Co) (radian), but the limit of the approach angle is α (radian).
Then, if the course approach angle PRQ>α, the steering angle V(φ) is corrected to V(φ)=α+(θi+θp) by the approach angle limiting means 9, and based on this, the same procedure as above is performed. Take control. 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. 7 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. 6. This shows that deviations from the planned course could be kept to a minimum. In addition, in the above example, the scheduled driving course is
Although the explanation was based on continuous point coordinates, this invention
It has excellent versatility and can be applied to methods for guiding and controlling guiding cables and other running objects. Further, it goes without saying that the virtual course and the switching line may be a guidance cable or other known guidance control structure.

【Effect of the invention】

This invention uses a virtual course and a switching area or a switching line to control the steering angle, which according to self-guidance theory, approaches the planned course asymptotically but does not accurately follow the planned course. By doing this, you can get as close as possible. The virtual course and switching area or switching line of the present invention can be provided regardless of whether it is all or a part of the main course, so for example, as shown in FIG. When traveling on a scheduled course repeatedly, if the course guidance method of the present invention is used for the terminal point side section course, traveling can always be started at approximately the same position, and the amount of deviation due to repeated traveling does not increase, resulting in optimal control. can be carried out.

[Brief explanation of the drawing]

Fig. 1 is an explanatory diagram when the course guidance method of the present invention is used, Fig. 2 is a flowchart showing selection between the main course and the virtual course, Fig. 3 is a functional block diagram of the invention, and Fig. 4 is An explanatory diagram of the course guidance control theory. Figure 5 is a diagram showing the traveling trajectory when using only conventional theory control. Figure 6 is a diagram showing the traveling trajectory when no approach angle limiting means is used. Figure 7 is a diagram showing the traveling trajectory when the approach angle limiter is not used. FIG. 8 is a diagram illustrating a travel trajectory when angle limiting means is used, and is a diagram in which a virtual course is provided at the terminal portion of the main course. DESCRIPTION OF SYMBOLS 1...Travel planned course storage unit, 2...Position detecting means, 3...Relative position measuring means, 4...Steering angle determining means, 21...Switching necessity determining means, 22...Following course determining means, 23 ... Follow-up course selection means, MC ... Main course, IC ... Virtual course, SE
...Switching area or switching line.

Claims (1)

[Scope of Claims] 1. A planned traveling course that is a factor in determining the steering angle of the traveling body includes a main course in which the traveling body is scheduled to travel, and a predetermined interval from the main course in at least one of the left and right directions of the main course. a virtual course set parallel to the main course, and a switching area or a switching line set parallel to the main course at closer intervals on the opposite side of the virtual course via the main course; However, when the vehicle is traveling outside the switching area or switching line (away from the main course), the steering angle is calculated to follow the virtual course, and the vehicle is running inside the switching area or switching line (approaching the main course). A course guidance method for an automated traveling object, characterized by calculating a steering angle to follow a main course when the object enters a main course.