JPH0647378B2 - Automatic steering control device for traveling wheels for steering - Google Patents

Description

DETAILED DESCRIPTION OF THE INVENTION (Industrial field of use) The present invention relates to a steering traveling wheel for constantly traveling a traveling carriage having steering traveling wheels mounted therein in a pipe having a circular cross section. The present invention relates to an automatic steering control device.

(Prior art) At a thermal power plant, a nuclear power plant, etc., a steering vehicle mounted on a traveling carriage of a cleaning vehicle for cleaning and inspecting the circulating water pipe with a circular cross section for cooling the condenser. Conventionally, the steering operation of the wheels has been manually performed by an operator.

(Problems to be solved by the invention) However, in order to always stably drive the traveling carriage in the pipe having the circular cross section, since the traveling surface is curved, if the steering timing is bad, the traveling carriage will not be properly moved. Since it comes off from the traveling path, the worker constantly pays attention to the state of the traveling vehicle, for example, the inclination of the traveling vehicle, and when the traveling vehicle deviates from the regular traveling path, immediately operate the steering device manually with skill and intuition, I had to get the trolleys back on track. Therefore, the burden on the operator becomes excessive, the fatigue of the operator increases, and it takes a lot of time for education and training before the operator can properly operate the steering device manually. There was a problem that the efficiency decreased. Therefore, there has been a demand for the development of an automatic steering control device that can automatically detect the inclination angle of the traveling carriage with respect to the horizontal and can travel in a stable traveling posture without depending on the skill and intuition of the operator. .

The present invention has been made to solve these problems and realize the above-mentioned demand.

(Means for Solving Problems) Means for solving the above problems will be described with reference to FIGS. 1 and 2 corresponding to the embodiment of the present invention.

A traveling carriage 7 traveling in a pipe 6 having a circular cross section shown in FIG.
The steering traveling wheels 4a and 4b are attached to the vehicle.
The steering traveling wheels 4a are driven by the steering motor 2 via the speed reduction mechanism 3a. The steering traveling wheel 4b is driven by the steering motor 2 via the connecting shaft 5 and the speed reduction mechanism 3b. As shown in FIG. 2, an inclination angle detector 8 that detects an inclination angle of the traveling carriage 7 with respect to the horizontal and outputs a detected inclination angle θi; and an inclination angle to be given to the traveling carriage 7, that is, a command inclination angle θa. A tilt angle commander 14 for outputting; a steering angle detector 10 for detecting and outputting a steering angle θs of the steering traveling wheels 4a or 4b; a rotation for detecting and outputting a rotation speed ω of the steering motor 2. The speed detector 9 and the commanded tilt angle θa of the output of the tilt angle commander 14 and the detected tilt angle θi of the output of the tilt angle detector 8 are input, and the difference between the two is proportional to integral differential control (hereinafter abbreviated as PID control). And an output θb is output as a proportional-integral-derivative control circuit 15 (hereinafter abbreviated as PID control circuit 15); an output θb of the PID control circuit 15 and a steering angle θ of an output of the steering angle detector 10.
s is input and the position control circuit 12 that amplifies and outputs the difference between the two, and the steering motor so that the output of the position control circuit 12 and the output of the rotation speed detector 9 are input and they are equal The automatic steering control device 1 having the speed control circuit 13 for controlling the rotation speed ω of 2 is installed.

(Operation) In this case, if the command inclination angle θa to be given to the traveling vehicle 7 is set in advance in the inclination angle command device 14 and the traveling vehicle 7 travels in the pipe 6, the inclination angle command device 1
The commanded inclination angle θa of the output of 4 and the actual inclination angle of the traveling vehicle 7 detected by the inclination angle detector 8, that is, the detected inclination angle θi are input to the PID control circuit 15. P
The ID control circuit 15 receives the input command tilt angle θa.
The PID control is performed on the difference between the detected inclination angle θi and the detected inclination angle θi, θb is output to the position control circuit 12, and θa = θi is controlled. The position control circuit 12 is the PID control circuit 1
5 output θb and rudder angle detector 10 rudder angle θs
Enter (the value obtained by converting the steering angle to the tilt angle) and enter the difference (θ
b−θs) is amplified and output to the speed control circuit 13. The speed control circuit 13 inputs the output (θb−θs) of the position control circuit 12 and the output of the rotation speed detector 9, and controls the rotation speed ω of the steering motor 2 so that they are equal to each other. . Then, the changing rotation speed ω of the steering motor 2 is detected by the rotation speed detector 9 and fed back to the speed control circuit 13, and the power of the steering motor 2 is steered via the reduction mechanisms 3a and 3b. Traveling wheels 4a,
4b, the steering angle θs of the steering traveling wheels 4a, 4b
And the changed steering angle θs is detected by the steering angle detector 10, and the position control circuit 1 of the positioning control circuit 11
Feedback to 2. As a result, the speed control circuit 1
3, ω = 0, θb−θs = 0, the relationship of θb = θs is always established, and the steering angle θs is positioned by following the output θb of the PID control circuit 15. Further, when the traveling carriage 7 tilts during traveling, the tilt angle is detected by the tilt angle detector 8
The detected tilt angle θi is fed back to the PID control circuit 15 and the detected tilt angle θi is set to the command tilt angle θa set in the tilt angle command device 14 by this closed loop circuit (see FIG. 3). Controlled to be equal. Therefore, if the command tilt angle θa is set to zero now, the detected tilt angle θi can also be controlled to zero, that is, the traveling carriage 7 can always be held horizontally. Therefore, even if the traveling carriage 7 tilts due to disturbance during traveling, the traveling carriage 7 is always held horizontally, and the steering angle θs of the steering traveling wheels 4a and 4b is always equal to the output θb of the PID control circuit 15. Since they are controlled so as to be equal to each other, the traveling vehicle 7 can stably travel along the straight line portion and the curved portion in the pipe 6 while always maintaining the horizontal position.

(Example) Below, one Example of the automatic steering control apparatus of the steering traveling wheel of this invention is described about drawing.

FIG. 1 is a front view of an embodiment of the present invention, FIG. 2 is a block diagram thereof, FIG. 3 is a concrete circuit diagram of a proportional-plus-integral-derivative control circuit, and FIG. 4 is FIG. 2 and FIG. FIG. 6 is a block diagram showing a transfer function of FIG.

1 to 3, the same reference numerals indicate the same members.

1 to 3, an automatic steering control device 1 for a traveling wheel for a steering angle uses a steering motor 2 to drive a reduction mechanism 3a in a direction in which an inclination angle θi of a traveling carriage 7 approaches a command inclination angle θa. The steering traveling wheel 4a driven to rotate by the steering angle θs and the steering traveling wheel 4a in the same direction as the steering traveling wheel 4a via the connecting shaft 5 and the reduction mechanism 3b by the steering motor 2. It is installed on a traveling carriage 7 that travels in a pipe 6 having a circular cross section by attaching a steering traveling wheel 4b that is driven to rotate by an angle θs, and detects the inclination angle of the traveling carriage 7 with respect to the horizontal. An inclination angle detector 8 for outputting the detected inclination angle θi; an inclination angle to be given to the traveling vehicle 7,
That is, the tilt angle command device 1 that outputs the command tilt angle θa
4; a steering angle detector 10 that detects and outputs the steering angle θs of the steering traveling wheel 4a or 4b; and a rotation speed detector 9 that detects and outputs the rotation speed ω of the steering motor 2. A proportional-integral-derivative control circuit 16 (hereinafter referred to as PD control circuit 16) which receives the detected inclination angle θi of the output of the inclination angle detector 8 and first-order differentiates the input detected inclination angle θi to prevent overshoot (Abbreviated), and the command inclination angle θa of the output of the inclination angle command device 14, and an operational amplifier that amplifies the difference between the command inclination angle θa and the detected inclination angle θi of the output of the PD circuit 16 and outputs it as θb. 17 and a proportional-integral-derivative control circuit 19 (hereinafter referred to as PI control circuit 1) that limits the output θb of the operational amplifier 17 by a limiter circuit 18, performs proportional integration and feeds back to the operational amplifier 17.
9), resistors 20, 21, 22, 23, and capacitors 24, 25.
5 (hereinafter abbreviated as PID control circuit 15);
The position control circuit 12 that inputs the output θb of the ID control circuit 15 and the steering angle θs of the output of the steering angle detector 10 and amplifies and outputs the difference (θb−θs) between them, and the position control circuit 1
2 and the output of the rotation speed detector 9 are input, and the speed control circuit 13 controls the rotation speed ω of the steering motor 2 so that they become equal.

Therefore, when the command inclination angle θa to be given to the traveling vehicle 7 is preset in the inclination angle command device 14 and the traveling vehicle 7 travels in the pipe 6, the command inclination angle θa of the output of the inclination angle command device 14 and The actual inclination angle of the traveling carriage 7 detected by the inclination angle detector 8, that is, the detected inclination angle θi is both input to the PID control circuit. The PID control circuit 15 amplifies the difference between the command tilt angle θa and the detected tilt angle θi that is first-order differentiated by the PD control circuit 16 by the operational amplifier 17 and outputs it as θb. The output θb of the operational amplifier 17 is limited by the limiter circuit 18, proportionally integrated by the PI control circuit 19 and fed back to the operational amplifier 17, and the position control circuit 1 is also provided.
It is output to 2. The position control circuit 12 inputs the output θb of the PID control circuit 15 and the steering angle θs of the steering traveling wheel 4b detected by the steering angle detector 10, and amplifies the difference (θb−θs) between them to increase the speed. Output to the control circuit 13. The speed control circuit 13 uses the position control circuit 12
(Θb-θs) and the output of the rotation speed detector 9 are input, and the rotation speed ω of the steering motor 2 is controlled so that they become equal. Then, the changing rotation speed ω of the steering motor 2 is detected by the rotation speed detector 9 and fed back to the speed control circuit 13, and the power of the steering motor 2 is used for steering via the speed reduction mechanism 3a. The steering wheel 4b is attached to the traveling wheel 4a via the connecting shaft 5 and the reduction mechanism 3b.
Are transmitted to the steering wheels 4a and 4b, and the steering angles θs of the steering wheels 4a and 4b are changed.
It is detected by 0 and fed back to the position control circuit 12. As a result, ω = 0, θb−θs = 0, and PI
Output θb of D control circuit 15 and steering traveling wheels 4a, 4b
The relationship of θb = θs is always established between the steering angles of
The steering traveling wheel 4 follows the output θb of the control circuit 15.
The steering angles θs of a and 4b are positioned. When the traveling carriage 7 tilts while traveling in the pipe 6, the tilt angle is detected by the tilt angle detector 8, and the detected tilt angle θi.
Is fed back to the PID control circuit 15, and by this closed loop circuit (see FIG. 3), the detected tilt angle θi of the traveling carriage 7 is controlled to be equal to the command tilt angle θa set in the tilt angle command device 14. To be done. Therefore,
If the command inclination angle θa is set to zero now, the detected inclination angle θi can be controlled to zero, that is, the traveling carriage 7 can be controlled to be always held horizontally, so that the traveling carriage 7 is not disturbed during traveling. Even if the vehicle is tilted by, for example, the traveling vehicle 7 is always held horizontally, and the steering angle θs of the steering traveling wheels 4a and 4b is always controlled to be equal to the output θb of the PID control circuit 15. While the carriage 7 is always kept horizontal, the carriage 7 can travel stably in the straight line portion and the curved portion in the pipe 6.

Now, in FIG. 3, the resistance values of the resistors 20, 22, and 23 are R, the proportional gain of the PI control circuit 19 is K _P , and the resistance value of the resistor 21 is K _P , R when the integral gain is K _I. The volumes of the capacitors 24 and 25 are set to 1 / K _IR and K _P , respectively.
/ K _I R and the block diagram of FIG. 2 is represented by the block diagram of the transfer function as shown in FIG. In FIG. 4, 1
5a is a transfer function of the PID control circuit 15 shown in FIG.
Is a transfer function when the positioning control circuit 11 is approximated to a first-order lag element and its time constant is T _S, and 26 is approximated to an integral element with the steering angle θs as an input and the detected inclination angle θi as an output, and the time constant is Tθ. Is the transfer function, and ε is the disturbance. Note that Tθ can be considered as a proportional constant that is inversely proportional to the traveling speed of the traveling carriage 7.

In that case, if K _P / Tθ = K _I / K _P, and for the sake of simplicity, if Tθ / K _P >> T _S , the delay of the steering angle positioning control circuit 11 can be ignored, so 1 / ( 1 + T
_It can be approximated as _S ) ≈1. Therefore, in this case, the block diagram of the transfer function of FIG. 4 is combined with the commanded inclination angle θa as the input, the detected inclination angle θi as the output, and the disturbance ε = 0, and the combined transfer function G (S) is , G (S) = {(K _P + K _I / S) (1 / TθS)} / {1+ (2K _P + K _I /
S) (1 / TθS)} = 1 / (1 + TθS / K _P ) ... (1) Expression (1) shows a first-order lag response, and the control system has an optimal response without overshoot and offset. Understand.

Further, when the disturbance ε acts, the transfer function G (S) after the coupling in FIG. 4 has a response of a second-order delay, and the disturbance ε =
Similar to the case of 0, the optimum response can be easily derived from FIG. It can be considered that the disturbance ε has an inclination angle proportional to the curvature of the curved portion when the traveling carriage 7 travels along the curved portion in the pipe 6, so that the traveling carriage 7 is horizontally moved at the curved portion. In order to hold and run the vehicle, the steering angle θs must be θs = −ε, but this operation is continuously performed by the PID control circuit 15 as described above. That is, the PID control circuit 15
Is the command tilt angle θa set in the tilt angle command device 14.
When the command inclination angle θa is set to zero, the steady integration is performed so that the difference between the actual inclination angle of the traveling vehicle 7 detected by the inclination angle detector 8, that is, the detected inclination angle θi becomes zero. In this state, control is performed so that θa = θi = 0 and θs = −ε. Therefore, by performing the PID control, the detected inclination angle θi of the traveling carriage 7 can be made zero at all times, so that the traveling carriage 7 is kept horizontal without being inclined even in the curved portion in the pipe 6. It is possible to drive stably.

(Effects of the Invention) As described above, the automatic steering control device for a steering traveling wheel according to the present invention detects and outputs the inclination angle θi of the traveling carriage traveling in the pipe having a circular cross section with respect to the horizontal inclination. An angle detector; an inclination angle commander that outputs a tilt angle to be given to the traveling vehicle, that is, a command inclination angle θa; a steering angle detector that detects the steering angle θs of the steering wheel for steering; a steering motor A rotational speed detector for detecting the rotational speed ω of the output angle; and the detected inclination angle θi of the output of the inclination angle detector and the commanded inclination angle θa of the output of the inclination angle commander, and PID control the difference between them to obtain θ.
a PID control circuit that outputs as b; a position control circuit that inputs the output θb of the PID control circuit and the steering angle θs of the output of the steering angle detector, and amplifies and outputs the difference (θb−θs) between them; A positioning control circuit including a speed control circuit that inputs the output (θb-θs) of the control circuit and the output of the rotation speed detector and controls the rotation speed ω of the steering motor so that they are equal to each other. ing. Therefore, when the inclination angle commander sets the instruction inclination angle θa to be given to the traveling carriage in advance and the traveling carriage travels in the pipe, the PID control circuit detects the instruction inclination angle θa and the inclination angle detector. The actual inclination angle of the traveling carriage, that is, the detected inclination angle θi is input, and the difference between the two is PID-controlled,
θb is output to the positioning control circuit and θ
Control is performed so that a = θi. Further, the positioning control circuit inputs the output θb of the PID control circuit, the steering angle θs of the output of the steering angle detector, and the output of the rotation speed detector, and outputs θb.
The rotation speed of the steering motor is controlled so that = θs. Therefore, if the command inclination angle θa is set to zero, the traveling carriage can be controlled to be kept horizontally during traveling, and even if the traveling carriage tilts due to the disturbance ε during traveling, the traveling carriage becomes horizontal. After returning to the position, the steering angle θs is −ε
Since the traveling carriage is controlled so as to be equal to, it is possible to perform stable traveling while always maintaining the horizontal level not only in the straight section in the pipe having a circular cross section but also in the curved section. Therefore, the operator does not have to constantly pay attention to the state of the traveling carriage as in the past, the burden and fatigue of the operator can be reduced, and the time required for the education and training of the operator can be shortened, and the stability is never before achieved. Since an automatic steering control device having a highly accurate control function can be realized, there is an effect that work efficiency is improved.

[Brief description of drawings]

FIG. 1 is a front view of an embodiment of the present invention, FIG. 2 is a block diagram thereof, and FIG. 3 is a specific circuit diagram of a PID control circuit.
FIG. 4 is a block diagram showing the transfer function of FIGS. 2 and 3. 1 ... Automatic steering control device, 2 ... Steering motor, 4a,
4b ... steering wheel for steering, 6 ... piping, 7 ... traveling carriage, 8 ... tilt angle detector, 9 ... rotation angle detector, 1
0 ... Rudder angle detector, 11 ... Positioning control circuit, 12 ...
... Position control circuit, 13 ... Speed control circuit, 14 ... Inclination angle commander, 15 ... Proportional-integral-derivative control circuit.

Claims (1)

[Claims] 1. A tilt angle detector for detecting and outputting a tilt angle of a traveling carriage traveling in a pipe having a circular cross section with respect to the horizontal; and an inclination angle commanding device for outputting an inclination angle to be given to the traveling carriage; A steering angle detector that detects and outputs a steering angle of a steering traveling wheel installed on the traveling carriage; and a rotation that detects and outputs a rotation speed of a steering motor that drives the steering traveling wheel. A speed detector; a proportional-integral-derivative control circuit for inputting the outputs of the tilt angle detector and the tilt angle commander, and performing proportional-plus-integral-derivative control of the deviation between the two, and outputting the result.
A position control circuit that inputs the output of the proportional-plus-integral-derivative control circuit and the output of the steering angle detector and amplifies and outputs the difference between the two; and the output of this position control circuit and the output of the rotational speed detector. And a speed control circuit for controlling the rotation speed of the steering motor so that they are equal to each other.