JPS60195620A - Route correcting device of moving body - Google Patents

Abstract

PURPOSE:To properly maintain the advancing direction of a moving body on sloping grounds, by detecting the advancing direction of the moving body through the output signal of an inclination sensor and correcting the advancing direction by operating a steering mechanism in accordance with the output signal. CONSTITUTION:When a moving body 1 runs on sloping grounds, its advancing direction deviates from an aimed direction shown by the arrow of broken line as shown by the arrows of solid line due to unbalance of revolving speed of right and left running motors 6 and 7, slippage of right and left crawlers 4 and 5, etc. Therefore, inclined angles in the front and rear directions of the moving body 1 obtained by means of an inclinometer 10 become larger than a threshold from a threshold setter 24 and a comparator 23 starts to operate and inputs inclined angles in the right and left directions from another inclinometer 11 in an arithmetic discriminating circuit 22. The circuit 22 makes positional correction and speed correction in accordance with the right and left inclined angles and outputs the correcting values to right and left position controlling systems and speed controlling systems. Therefore, the moving body 1 can climb up and climb down sloping grounds safely.

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Application of the Invention] The present invention relates to a moving mechanism, and more particularly to a course correction device suitable for correcting the traveling direction of a moving machine traveling on slopes and stairs.

[Background of the invention]

Conventionally, the direction of movement of a moving object during automatic travel on a slope is corrected by monitoring the direction of movement while looking at a television camera, and when the direction of movement deviates due to slippage of wheels or crawlers, etc., it is manually interrupted and corrected. It is carried out. Manual correction requires operator intervention, which reduces movement efficiency.

Correction using a gyro may also be considered, but the device would be large-scale, and when considering the loading of moving objects, space efficiency would be poor, power consumption would be large, and the weight would also be large.

[Purpose of the invention]

An object of the present invention is to provide a course correction device for obtaining a predetermined traveling direction of a moving body while traveling on a slope.

[Summary of the invention]

The present invention is characterized by detecting changes in the traveling direction of the moving object due to slipping of the crawler, axle, etc. of the moving object while traveling on a slope, or imbalance in running speed, etc. by using the output signal of a tilt sensor attached to the moving object. Based on this signal, the steering mechanism is operated to correct the direction of movement of the mobile object on a slope, allowing it to climb and descend safely.

[Embodiments of the invention]

Practical examples of the present invention will be described below with reference to the drawings.

A first embodiment of the present invention uses a crawler type steering mechanism and a continuous output signal type inclinometer as a sensor. FIG. 1 shows a situation in which a mobile body 1 loaded with an inspection television camera is running on a slope. On the slope 3, a solid line indicates a slope, and a dashed line indicates a staircase. As shown in FIG. 2, the moving body 1 performs forward and backward movement, steering, and turning operations by rotating crawlers 4 and 5 in forward and reverse directions using left and right running motors 6 and 7. Encoders 8 and 9 are for feedback of the speed of the travel motor for measuring the travel distance of the left and right crawlers.

On the main body of the moving body 1, there are two inclinometers 10. for detecting the inclination angle in the running direction and the inclination angle in the direction orthogonal to the running direction.
11 are installed.

If the inclinometer 10 is schematically represented as shown in FIG. 3, when the moving body 1 tilts forward in the traveling direction A, the needle of the inclinometer tilts in the direction F, and an output commensurate with the tilt is generated.

The inclinometer 11 in the direction perpendicular to the traveling direction generates an output corresponding to the inclination of the moving body 1 in the left-right direction, as shown in FIG.

The control circuit of the device of the invention is shown in FIG. In this figure, parts with the same reference numerals as in FIGS. 1 to 4 are the same parts. The travel correction control section includes an inclinometer 10 for the longitudinal direction, an inclinometer 11 for the horizontal direction of the moving body, a threshold value setter 24 for the inclination angle in the longitudinal direction of the moving body, a comparator 23, and an arithmetic determination circuit 22. Target value commands 12.13 for the left and right crawlers of the moving body 1 are normally given from a higher level control device. counter 20
．． 21 stores the current position of the moving body 1 obtained by counting output pulses from the encoders 8 and 9 of the motor 6.7. The converter 14.15 converts the motor rotation speed according to the deviation between the target position 12.13 and the current position into the servo amplifier 1.
6. Output to 17. The servo amplifiers 16.17 control the left and right travel motors 6. at the speed output from the converters 14.15.
In order to rotate the motor 6.7, the terminal voltage and current of the motor 6.7 are controlled. Further, the servo amplifiers 16 and 17 convert the output pulses of the encoders 8 and 9 into frequency and voltage, and apply speed feedback. In this manner, while traveling on flat ground, the moving body 1 is positioned by traveling to a target position instructed by a higher-level control device.

25 is a changeover switch.

Now, while the moving body 1 is traveling on a slope 3, due to an imbalance in the rotational speed of the left and right traveling motors 6, 7, slippage of the left and right crawlers 4, 5, etc. In this case, the moving direction of the moving body 1 moves away from the target direction indicated by the broken line. Therefore, the loaded inclinometer 10.11
The operation of correcting the running direction of the moving object 1 using the following will be described below.

When the moving object 1 starts traveling on a slope, the inclination angle of the moving object 1 in the longitudinal direction obtained by the inclinometer 10 becomes larger than the threshold value from the threshold value setting device 24, and the comparator 23 is activated to perform calculation. The horizontal inclination angle from the inclinometer 11 is input to the discrimination circuit 22 . The calculation discrimination circuit 22 performs position correction and speed correction according to the horizontal tilt angle, and controls the left and right position control system,
Outputs the correction value to the speed control system. This operation determination circuit 22
The operation will be explained using the flowchart shown in FIG.

If the left and right inclination angle from the inclinometer 11 is smaller than the correction start threshold, the correction value is 0; if not, it is determined whether the vehicle tilted to the right while ascending or to the left while descending. do. If so.

Outputs position and speed correction values to the travel motor drive system.

The speed correction value is a value depending on the magnitude of the left and right inclination angles, and by subtracting the speed correction value from the input speed to the servo amplifier, the speed of the left travel motor is lowered and the moving direction of the moving object 1 is adjusted. Make corrections. Perform position correction at the same time as speed correction to reduce errors when stopping. The position correction value is the product of the speed correction value, A and t, and is added to the target position of the right crawler to correct the position. Δt is the time width of a fixed cycle for activating this processing circuit. This position correction may not be necessary if the device is not operated on a slope or depending on the configuration of the control system.

In addition, in the present invention, even if the 0N-OFF servo type control system is configured with an 0N-OFF type tilt angle sensor read using a reed relay instead of the continuous output type inclinometer, the same idea will apply. , the course of the moving body can be corrected.

〔Effect of the invention〕

As described above, according to the present invention, it is possible to appropriately maintain the traveling direction of a mobile object on a slope, and it is possible to improve adaptive stress to the external world during autonomous movement.

[Brief explanation of the drawing]

FIG. 1 is a side view showing a running state of an example of a moving body equipped with an embodiment of the device of the present invention, FIG. 2 is a plan view thereof, and FIGS. 3 and 4 are front views of an inclinometer used in the present invention. 5 is a control circuit diagram configuring the apparatus of the present invention, and FIG. 6 is a flowchart showing the control flow. DESCRIPTION OF SYMBOLS 1... Moving object, 3... Inclined surface, 6, 7... Traveling motor, 10.11... Inclinometer, 22... Arithmetic determination circuit.

Claims (1)

[Claims] 1. In a mobile body equipped with a steering mechanism, a tilt sensor for detecting the tilt direction of the mobile body is attached, and the amount of deviation of the mobile body on a slope is determined based on the output signal of the sensor. 1. A course correction device for a moving object, comprising: control means for determining the direction of movement of the moving object, controlling the direction of movement of the moving object, and causing the moving object to proceed in the instructed direction of movement. 2. The course correction device for a moving object according to claim 1, wherein the inclination sensor uses a reed relay. 3. The course correction device for a moving body according to claim 1, wherein the inclination sensor uses a pendulum-shaped sensor. 4. The moving direction of the moving object is controlled by reducing the input speed of the speed servo system of the steering mechanism on the side opposite to the direction of movement to correct the moving direction. A course correction device for a moving body according to any one of items 1 to 3.