JPH075922A - Steering control method for unmanned work vehicle - Google Patents

Abstract

PURPOSE:To always drive an unmanned work vehicle with desired distances secured to the right and left obstacles of the vehicle by setting the offset value used for the steering amount based on the feedback value due to the deviation of the value detected by a sensor from the relevant traveling course. CONSTITUTION:The unmanned work vehicle 1 calculates its position on a traveling course and then calculates the feedback value of signals to perform the steering control in accordance with the calculated position of the vehicle. Then, the vehicle 1 compares the detected distance (d) between the vehicle and an obstacle 2 with the desired space value D and then calculates the offset value of the feedback value based on the result of comparison. Then, the vehicle 1 calculates a steered variable from the feedback value acquired by the hitherto calculations and the offset value. Then, the steering control of the vehicle 1 is carried out. Thus, it is possible to set a steering amount to secure the space D when the deviation of the vehicle 1 from a prescribed traveling course (x axis) is generated in an optional form. Therefore, a stable distance D is kept in particular to an obstacle in a oblique direction.

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to a steering control method for an unmanned work vehicle for cleaning a floor or the like.

[0002]

2. Description of the Related Art In a steering control method for an unmanned work vehicle of this type, particularly with respect to obstacles existing on the left and right of the vehicle body, at least the distance between the vehicle body and the obstacle can be changed at any time. Steering control is desired to keep it. In order to achieve this, non-contact distance sensors such as ultrasonic distance sensors and optical sensors are installed toward the left and right sides of the vehicle body, and the distance between the left and right obstacles is detected. A steering method has been proposed in which the traveling path of the vehicle body is moved in parallel according to the difference between the two to maintain the distance from the obstacle.

This steering method will be described with reference to FIGS. 10 and 11. As shown in FIG. 11, when the vehicle body 1 changes direction from the traveling direction A to the traveling direction B, since the locus of the corner of the vehicle body 1 draws an arc, a predetermined distance D is provided between the vehicle body 1 and the obstacle 2. Will be needed. In addition, 3 is a distance sensor and 4 is a wheel. FIG. 10 shows a steering situation when the obstacle 2 exists on the side of the predetermined traveling route 5. When the sensor detection value d1 of the distance between the vehicle body 1 and the obstacle 2 is smaller than the predetermined distance D, (D
-D1) move the travel route. When the detected value d2 of the interval between the new travel route and the obstacle 2 becomes smaller than the predetermined interval D again, the travel route is further moved. If the detected value d3 is equal to or greater than the predetermined interval D, the vehicle travels as it is. Hereinafter, the same operation is repeated. Obstacle 2 disappears and detection value d4
Becomes larger than (D + α) (α is set to a value slightly larger than the amount of lateral displacement generated), the measurement of the traveling distance L is started,
When L becomes larger than La (approximately the vehicle body length), the traveling route is returned by the previous lateral movement amount (D-d1) + (D-d2), and the vehicle body 1 is returned to the predetermined traveling route 5.

[0004]

However, in this steering method, since the traveling path is moved in parallel, the oblique obstacle 2
On the other hand, as shown in FIG. 12, an obstacle was immediately approached, an unstable operation was performed, and there was no effect of traveling at an interval capable of changing the direction. The present invention is
SUMMARY OF THE INVENTION It is an object of the present invention to solve the above problems and to provide a steering control method for an unmanned work vehicle that can always travel at desired intervals with respect to obstacles on the left and right of the vehicle body.

[0005]

In order to achieve the above object, the present invention detects the distance to an obstacle existing on the left and right of the body of an unmanned work vehicle and sets the distance between the body and the obstacle to a desired value. A steering control method of steering so as to maintain, a step of calculating a vehicle body position on a travel route, and a signal feedback amount in a steering mechanism for performing steering control according to the vehicle body position obtained above. The step of comparing the magnitude of the detected value of the distance between the obstacle and the vehicle body and the desired distance value, the step of calculating the offset value of the feedback amount according to the comparison result, and the steps of And a step of calculating the steering amount using the feedback amount and the offset value obtained by the calculation.

[0006]

According to the above method, the position of the vehicle body on the travel route is calculated, and the feedback amount for steering control is calculated according to the position of the vehicle body obtained above to calculate the distance between the obstacle and the vehicle body. The detected value of the above is compared with the desired interval value, the offset value of the feedback amount is calculated according to the above comparison result, and steering is performed using the feedback amount and the offset value obtained by each of the above calculations. The amount is calculated, and the steering control is performed by this.

[0007]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS An embodiment of the present invention will be described below with reference to the drawings. FIG. 1 shows a steering configuration of an unmanned work vehicle in which steering is performed by steering wheels. The vehicle body 1 includes drive and steering wheels and driven wheels in (a), and steering wheels and drive wheels in (b). Prepare FIG. 2 shows the steering configuration of an unmanned work vehicle when steering is performed by the left and right drive wheels, and includes caster wheels and drive wheels. FIG. 3 shows the procedure of the steering method for avoiding the left and right obstacles according to the present embodiment. Various calculations of this procedure will be described below in order. First, the calculation (# 1) of the vehicle body position using the following equation (1) will be described. FIG. 4 shows the vehicle body position on the coordinate axis xy when the predetermined traveling route is the coordinate axis x. In FIG. 4, the position calculation is the following formula (1).
Is obtained by

[0008]

ΔL _L ⁱ = (p _L ⁱ −p _L ⁱ⁻¹ ) · D _L / P _L Δl _R ⁱ = (p _R ⁱ −p _R ⁱ⁻¹ ) · D _R / P _R Δθ ⁱ = ( Δl _R ⁱ −Δl _L ⁱ ) / W Δl ⁱ = (Δl _L ⁱ + Δl _R ⁱ ) / 2 x ⁱ = x ^i-1 + Δl ⁱ · cos (θ ^i-1 + Δθ ⁱ / 2) y ⁱ = y ^{i- 1} + Δl ⁱ · sin (θ ^i-1 + Δθ ⁱ / 2) θ ⁱ = θ ^i-1 + Δθ ⁱ (1)

In the above equation (1), i is a subscript indicating a value for each operation period, P _L and P _R are the number of pulses per rotation of the left and right encoders, and p _L ⁱ and p _R ^{i are} left and right Encoder pulse count integrated value (addition when moving forward, subtraction when moving backward), D _L , D _R = rolling distance (mm) per left / right encoder rotation of each wheel, Δl _L ⁱ , Δl _R ⁱ = left / right between calculation cycles Rolling distance (mm) of each wheel, W = tread (mm) of left and right wheels, Δθ ⁱ = direction change amount (rad) of vehicle body during calculation cycle, Δl ⁱ = moving distance (mm) of vehicle body during calculation cycle, x ⁱ , y ⁱ = vehicle body position (mm), θ ⁱ = vehicle body direction (rad) −π ≦ θ ⁱ ≦ π. All the angle values are positive in the counterclockwise direction.

Next, the feedback amount calculation (# 2) in the steering control using the following equation (2) will be described.
FIG. 5 shows the coordinate axis x- when the predetermined travel route is the coordinate axis x.
The vehicle body position on y is shown, and the feedback amount calculation during traveling on a straight route is obtained by the following equation (2).

[0011]

[Expression 2] l ⁱ = y ⁱ ψ ⁱ = θ ⁱ · 180 / π (when moving forward) = −θ ⁱ · 180 / π (when moving backward) φ ⁱ = bar φ ⁱ −φ ₀ (steering with steering wheels is performed Only in the case) ω ⁱ = (Δθ ⁱ / Δt ⁱ ) · (180 / π) (when moving forward) = − (Δθ ⁱ / Δt ⁱ ) · (180 / π) (when moving backward) (2)

In the above equation (2), i = subscript indicating a value for each calculation cycle, y ⁱ = vehicle body position (mm), θ ⁱ = vehicle body direction (rad), Δθ ⁱ = vehicle body during calculation cycle Change amount (rad), Δt ⁱ = calculation period (sec), l ⁱ = lateral displacement feedback amount (mm), ψ ⁱ = posture angle feedback amount (deg), φ ⁱ = steering angle feedback amount (deg), Bar φ ⁱ = steering angle detection value (deg), φ ₀ = steering angle detection offset value (deg), and ω ⁱ = angular velocity feedback amount (deg / sec). All the angle values are positive in the counterclockwise direction.

Next, the sensor detection value d and the desired interval D are compared (# 3), and the offset value S is determined according to the comparison result.
Decide ₀ (# 4, # 7, # 11), and then (3) below
Alternatively, the steering command value is calculated using the equation (4) (# 5), and the steering output is performed based on the obtained steering command value (# 6). The procedure for determining the offset value S ₀ will be described later, and the calculation of the steering command value will be described first. First, a case where the steering wheel is used for steering will be described. FIG. 6 shows a signal feedback circuit configuration in the steering mechanism for performing steering control according to the vehicle body position in this case.

[0014]

(3) Bar S ⁱ = G _S · (G _l·l ⁱ + G _ψ · ψ ⁱ + G _φ · φ ⁱ + G _ω · ω ⁱ −S ₀ ) S ⁱ = bar S ⁱ (of bar S ⁱ <S _lim Case) S ⁱ = (bar S ⁱ / | bar S ⁱ |) · S _lim (when bar S ⁱ ≧ S _lim ) (3)

In the above equation (3), i = subscript indicating a value for each calculation cycle, Gs = total feedback gain, _Gl = lateral displacement feedback gain, G _ψ = attitude angle feedback gain, G _φ = steering angle Feedback gain, _Gω = angular velocity feedback gain, l ⁱ = lateral displacement feedback amount (mm), ψ ⁱ = attitude angle feedback amount (deg), φ ⁱ = steering angle feedback amount (deg), ω ⁱ = angular velocity feedback amount ( deg / sec), S _o = feedback amount offset value, S ⁱ = steering command value (steering speed command), and S _lim = steering command limit value.

The case where steering is performed by the left and right driving wheels will be described below. FIG. 7 shows a signal feedback circuit configuration in the steering mechanism for performing steering control according to the vehicle body position in this case.

[0017]

[Number 4] ^{1 / R i = (1 /} G R) · (G l · l i + G ψ · ψ i + G ω · ω i -S o) ΔT i = Cv · (W / 2) · (1 / R ⁱ ) · (Δl ⁱ / Δt ⁱ ) _TL ⁱ = T ⁱ + ΔT ⁱ T _R ⁱ = T ⁱ −ΔT ⁱ (4)

In the above equation (4), i = subscript indicating a value for each operation cycle, G _R = reciprocal feedback gain of turning radius, G _l = lateral displacement feedback gain, G _ψ = posture angle feedback gain, G _ω = angular velocity feedback gain, l ⁱ = lateral displacement feedback amount (mm), ψ ⁱ = attitude angle feedback amount (deg), ω ⁱ = angular velocity feedback amount (deg / sec), S _o = feedback amount offset value, 1 / R = target value of reciprocal of turning radius (1 / mm), W = tread of right and left wheels (mm), Δl ⁱ = amount of movement of vehicle body during calculation cycle (mm) ... Calculated value of formula (1), Δt ⁱ = Calculation cycle (sec), traveling speed by Δl ⁱ / Δt ⁱ , Cv = conversion constant to motor rotation speed command value, ΔT ⁱ = steering command value (increase / decrease value of drive motor rotation speed), _TL ^i, T _R ⁱ = left, a right drive motor speed command value.

Steering is output based on the steering amount obtained by the above equation (3) or (4). Here, the feedback offset value S _{0 in} the equation (3) or (4) is determined (# 3). , # 4, # 7 to # 11) will be described. First, if the sensor detection value d is d <D + α (α is an appropriate value that is slightly larger than the amount of lateral displacement generated when traveling on a traveling route), the following processing is performed.

[0020]

[Number 5] determine the ^{_{F = G l · l i +}} G ψ · ψ i. Regarding the sensor detection value on the left side of the vehicle body, if F ≧ G _l · (D−d), then S ₀ = 0 F <G ₁ · (D−d), then S ₀ = F−G _l · ( D
-D). Regarding the sensor detection value on the right side of the vehicle body, if F ≦ −G _l · (D−d), then S ₀ = 0 F> −G _l · (D−d), then S ₀ = F + G _l · ( D-d). … (5)

During the above repetitive processing, the sensor detection value d
However, if d ≧ D + α, the offset value S ₀ is d ≧ D +
The value before becoming α is maintained (# 7). Also, d ≧ D + α
If so, the measurement of the traveling distance L is started from that time (#
9), if the traveling distance while the sensor detection value d maintains d ≧ D + α is L> La (YES in # 10),
S ₀ = 0 (# 11). If d ≧ D + α, the travel distance L is cleared (# 8).

According to the steering method of this embodiment, the traveling route is
No need to move. Due to deviations from the travel route,
Amount of feedback (lⁱ , Ψⁱ ) Is zero, then
The steering amount required to obtain the distance D is
Lateral displacement gain G_l Multiply by G_l ・ (D
-D). However, in reality, deviation from the travel route
Since the amount of feedback is generated due to differences, etc.,
In the formula (5), in order to obtain the necessary interval D in consideration of this.
The steering amount of is determined. Offset value S in equation (5) ₀ =
In some cases, the fee from the travel route may be 0.
When the required steering amount can be generated only by the feedback amount.
It

8 and 9 show the situation when the above steering control is carried out. Regardless of the form of the deviation of the vehicle body 1 from the predetermined travel route (x-axis), the steering amount for obtaining the desired desired distance D is determined, so as shown in FIG. The distance D can be stably maintained even with respect to the oblique obstacle 2.

[0024]

As described above, according to the present invention, the offset value used when determining the steering amount in the feedback steering control is set from the sensor detection value and the feedback amount due to the deviation from the traveling route at that time. Therefore, even with diagonal obstacles, the distance between the obstacles on the left and right of the vehicle body can be maintained at a desired value so that the direction can be changed at any time. It is also possible to stabilize the running.

[Brief description of drawings]

FIG. 1 is a diagram showing a steering configuration of an unmanned work vehicle when steering is performed by steered wheels according to an embodiment of the present invention.

FIG. 2 is a diagram showing a steering configuration of an unmanned work vehicle when steering is performed by left and right drive wheels.

FIG. 3 is a flowchart showing a procedure of a steering method for avoiding an obstacle according to the present embodiment.

FIG. 4 is a diagram showing a vehicle body position when a predetermined traveling route is used as a coordinate axis.

FIG. 5 is a diagram showing a vehicle body position when a predetermined traveling route is used as a coordinate axis.

FIG. 6 is a signal feedback circuit configuration diagram of a steering mechanism in the case of performing steering with steered wheels.

FIG. 7 is a signal feedback circuit configuration diagram of a steering mechanism when steering is performed by the left and right driving wheels.

FIG. 8 is a diagram showing a state of steering control according to the present embodiment.

FIG. 9 is a diagram showing a situation of steering control according to the present embodiment.

FIG. 10 is a diagram showing a situation of steering control by a conventional method.

FIG. 11 is a diagram showing a situation when the vehicle body changes direction.

FIG. 12 is a diagram showing a situation when steering control is performed on a tilted obstacle by a conventional method.

[Explanation of symbols]

 1 vehicle body 2 obstacle 3 sensor

Claims (1)

[Claims] 1. A steering control method for detecting a distance from an obstacle existing on the left and right of a vehicle body of an unmanned work vehicle and steering the vehicle so as to maintain a distance between the vehicle body and the obstacle at a desired value. Of calculating the vehicle body position in the vehicle, calculating the feedback amount of the signal in the steering mechanism for performing steering control according to the vehicle body position obtained by the above calculation, and detecting the distance between the obstacle and the vehicle body. Using the feedback amount and its offset value obtained by each of the above calculations, the step of comparing the magnitude of the value and the desired interval value, the step of calculating the offset value of the feedback amount according to the above comparison result, And a step of calculating a steering amount by means of a steering control method for an unmanned work vehicle.