JPS6316209A - Position recognizing device for moving body - Google Patents

Abstract

PURPOSE:To detect the run distance and direction of an unmanned moving body by providing rotation detecting means corresponding to a couple of wheels for run driving which have a differential mechanism. CONSTITUTION:The encoders 20a and 20b for detecting the rotating speeds of rear wheels 5a and 5b as the wheels for run driving which have the differential mechanism are provided. Then, detection signals of the encoders 20a and 20b are counted by pulse counters 24a and 24b, whose outputs are inputted to a CPU 22 through the I/O interface circuit in a controller 25. Then, the CPU 22 calculates the run distance from the mean values of the rotating speeds of the right and left wheels counted by the pulse counters 24 and 24b and their tire sizes. Further, when the difference in rotating speed between the right and left wheels is smaller than a set value, rotating speed data on it is disregarded and the last normal rotating speed is regarded as the current rotating speed to compute the run distance and direction.

Description

DETAILED DESCRIPTION OF THE INVENTION [Field of Industrial Application] This invention relates to a technology for recognizing the position of a moving object, and particularly to a technology that is effective when used in a position recognition device for an unmanned moving object that can move without a pilot. .

[Prior art] In recent years, various technologies have been researched to allow moving objects such as automobiles and guided vehicles to run unmanned. For example, some guidance methods for moving moving objects to run along a predetermined course have already been put into practical use. Some have been.

On the other hand, a method has been proposed in which the control system of an unmanned vehicle is given course information so that it can travel independently without requiring external guidance. In this type of non-guided unmanned moving object, the moving object itself must always be aware of its own position. For this purpose, information regarding the travel distance as well as the orientation (direction of movement) is required.

The method of using an encoder has traditionally been put into practical use as a means of knowing the distance traveled, but since the encoder alone cannot accurately determine the direction, it is necessary to use a gyro, a laser, an ultrasonic emitting device, or a TV camera as an aid. A device has been proposed that is provided as a means to know the direction and recognize one's own position.

[Problems to be solved by the invention] However, since emitting devices such as gyros and lasers, TV cameras, etc. are more complex than encoders,
There is a problem in that the position recognition device is large-scale and expensive.

Therefore, the present inventors provided encoders for each pair of left and right drive shafts having a differential mechanism, and detected the rotational speed of the left and right wheels separately, and detected not only the travel distance but also the running direction from the difference in rotational speed. We considered a method of doing so.

However, it has been found that the method of determining the bearing from the difference in the number of rotations of the left and right wheels has the disadvantage that an error occurs in the detected bearing when one of the wheels slips.

[Object of the Invention] An object of the present invention is to provide a position recognition technique for an unmanned moving object that can detect not only the traveling distance but also the direction using a simple means such as an encoder.

Another object of the present invention is to provide a method for correcting errors when detecting orientation using only an encoder.

The above and other objects and novel features of the present invention will become apparent from the description of this specification and the accompanying drawings.

[Means for Solving the Problems] In order to achieve the above object, the present invention attaches an encoder to each of the drive shafts having a differential mechanism, detects the rotation speed of the left and right drive shafts separately, and detects the rotation speed of the left and right drive shafts. I learned the direction from the difference in numbers. Additionally, if the detected rotation speed or difference in rotation speed is greater than the set value, the detected value is discarded, and the previous normal detection value is used to calculate the distance and direction as the rotation speed and rotation speed difference at that point. I tried to calculate it.

[Operation] According to the above means, it is possible to know the direction of a moving object only with the encoder without using complicated and expensive equipment such as a gyro or a laser or ultrasonic emitting device, and also to monitor the direction with the encoder. Even if an abnormality such as slip occurs in the wheel being used, the abnormality measurement data will not adversely affect the calculation of position and orientation.

[Embodiment] FIG. 1 shows an embodiment of a position recognition device for a moving body according to the present invention.

A rear wheel 5a as a running drive wheel having a differential mechanism 6;
Encoders 20a, 20b for detecting the respective rotational speeds are provided corresponding to the encoders 20a, 20b.
The detection signal (pulse) from 20b is sent to the pulse counter 2.
4a and 24b and input to the computer 22 via the I10 interface circuit in the controller 25. The computer 22 is
The average value of the rotation speeds of the left and right wheels counted by the pulse counters 24a and 24b is calculated, and the travel distance is calculated from the tire size.The azimuth, that is, the running direction of the vehicle is calculated from the difference in the rotation speeds. .

It is designed to recognize its own location.

However, when recognizing the position and orientation of the vehicle only from the number of rotations of the wheels, if the wheels slip, there is a risk that position recognition will be incorrect. Therefore, in this embodiment, when the difference between the rotation speeds of the left and right wheels exceeds a preset value, that rotation speed data is discarded, and the most recently obtained rotation speed with high validity is regarded as the rotation speed at that point. The distance traveled and direction are calculated.

The computer 22 also includes an encoder 20a.

While recognizing its own position by the signal from 20b, I
According to the course information read into the internal main memory from a storage device 26 such as a C cassette, the travel drive motor 7 as a rotation drive means and the steering motor 11 as a turning drive means are controlled to move along a predetermined course. The vehicle can now be driven.

Furthermore, in this embodiment, course information such as the permissible range of rotation speed, driving course, and prohibited driving area is input.
An input operation device 27 such as a keyboard is also provided.

Further, although not particularly limited, in order to check the position of a traveling vehicle under automatic control, laser projectors and receivers 23a and 23b are provided, for example, on both sides between the vehicles. These laser projector/receivers 23a and 23b emit a laser beam in a direction perpendicular to the center line of each vehicle, that is, directly to the side between the vehicles, and scan the laser beam vertically.

On the other hand, as shown in FIG.
1. A checkpoint is constructed by arranging the cc and cc so as to face each other. Since the corner cube has a characteristic of reflecting incident light in the same direction as the incident direction, the incident laser beam is reflected toward the laser projector/receiver 23a, 23b that emitted it and is detected. Depending on the direction of this reflected light, the vehicle can correct its orientation.

In other words, the incident position of the reflected light from the corner cube CC and CC2 is as shown in Fig. 2 (B) < P t
When the distance between -P z, P, and -P is set to 0, the coordinates of Pl and P2 are determined by self-position recognition based on the encoder output as Pz (Xzt yi) t Pi (Xzt y
It is recognized as a). Therefore, if the angle between the preset course line A and the line (cube line) B that connects the cubes is set to α, and the distance between the cubes is set to L, then the reflected light from the cube and the cube line B are The angle θ formed can be calculated using the following formula (% formula %).

From this, if the angle between the actual inter-vehicle travel line and the set course line A is β, then β=θ+90″, so the deviation β-α in the direction with respect to the course can be determined as θ+90@-α.

Further, the pair of laser projector/receivers 23a facing right sideways
, 23b, by installing a diagonally forward facing laser projector/receiver on either the left or right side of the car body, the distance from the corner cube at the end of the course can be detected by a type of triangulation method and the position can be corrected. You can also do this.

Next, the position recognition method for the unmanned moving object configured as described above will be specifically explained using FIG. 3.

Specifically, this invention includes left and right encoders 20a, 20
It recognizes its own position and orientation by performing calculations once periodically based only on the signal from b. Third
In the X-Y coordinate system shown in the figure, the current position of the vehicle is IP, and the position of the vehicle at the previous cycle is ΦP.
, and the amount of movement from point IP to ΦP is DLx, DLy
, the amount of change in direction is set as Δθ, it is expressed as follows: IPx=ΦPx+DLx 1py=ΦPy+DLy IPθ=ΦPθ+Δθ. Therefore, D L x HD L y t
By determining Δθ, the current position and orientation can be determined.

Therefore, the amount of movement of the left and right wheels per cycle is
It can be determined by calculation from each rotation speed obtained from the signal from the encoder and the diameter of the wheel. Further, if the moving amounts of the left and right wheels are LLL and LLR, the moving amount of the center of the vehicle, that is, the length of the arc connecting the points ΦP and IP is expressed as (LL L + LLR)/2.

Here, since the amount of inter-vehicle movement per cycle is minute, the length of the circular arc connecting points ΦP and IP can be considered to be approximately equal to the length of the straight line. Therefore, the distance ΦP between points ΦP and 12
・If the IP is set as LL, LL= (LLL+LL R)
/2.

Further, if the distance between the tread of the vehicle, that is, the wheels 5a and 5b, is T, then the amount of movement of each wheel is represented by the product of the distance from the center of rotation 0 and the rotation angle Δθ in FIG. That is, LR: (o・(RIPT/2)XΔθLL=(
0・IP+T/2)xΔθ. From this, Δθ is found as Δθ=(LLR−LLL)/T.

Now, if we focus on the triangle whose vertices are the points Q'l, ΦP, and IP, we can see that this triangle is an isosceles triangle, and the exterior angle of the apex angle is 80 degrees. Therefore, ZO′・ΦP
・IP=Δθ/2, and the line ΦP connecting point ΦP and IP
The angle α that IP makes with the X-axis is expressed as α=ΦPθ−Δθ/2.

Therefore, DLx=LL-cosa, DLy=LL-sina
Therefore, DLx and DLy are found, and the coordinates of point IP (IPx
, IPy) and the orientation IPθ.

Note that cosa is required in actual driving control.

sin a, cosΔθ, sinΔθ and even cos
(I Pθ).

Calculating the value of sin (I P O) takes a very long time if performed sequentially by a computer. However, at the time when the travel distances LLL and LLR of the left and right wheels are calculated as Δθ=(LLR-LLL)/T based on the signals from the left and right encoders, cos (Δθ/2) and s
It has been found that by calculating the value of in (Δθ/2), the subsequent calculation of sun and cos becomes unnecessary.

That is, C08 (Δθ/2) and sin (Δθ/
Let the values of 2) be ALc and ALs, respectively. In addition, the data regarding the orientation ΦPθ in the previous cycle is changed to C05(Φp
O) = ΦPθL, 5in (Φcos) = ΦPθS
so that it is retained as As a result, cosa,
Since α=ΦPΦ−Δθ/2, sina is cos
a = cos(ΦPθ)Xcosa-sin(ΦPθ
) X sin a=ΦPθc X A L c−ΦP
θs X A L 5sin a = sin(ΦP O
) X cos a +cos(ΦPO)Xsina=
It is determined by the product and sum formula: ΦPθ5XALc−ΦPθcXALc.

Also, cos (ΔθL 5in(Δθ) is obtained from the double angle theorem, cos(Δ0)=2cos”(Δ0/2)−1=
2ALc2-1 sin (Δθ)=2sin(Δθ/2) X c
It is determined by the product and sum formula: os(Δθ/2)=2ALsXALc.

Furthermore, cos(IPθ), 5in(IPθ) are
Since IPθ=ΦPθ+Δθ, the above cos (Δθ
) by setting A c and A s respectively, cos
(IPθ)=cos(ΦPθ)xcos(Δθ)−si
n(ΦPθ)Xsin(Δθ) =ΦPθc X A c−ΦPθs X A 5sin
(I Pθ)=sin(ΦPθ)Xcos(Δθ)+c
os (ΦPθ) X5in (Δθ)=ΦPθs X
It is determined by the product and sum formula A c + ΦPθcXAs.

Therefore, sin and cos calculations may be repeated many times, and sin and cos calculations may be performed to omit such calculations.
There is no need to hold the value of s in a storage device as a data table. As a result, the calculation speed required for travel control can be increased or the storage capacity can be reduced.

Furthermore, this embodiment employs a travel control system that decelerates the vehicle in accordance with the steering angle when the vehicle changes direction, that is, travels along a curve. This is because when a vehicle changes direction, a centrifugal force proportional to the square of the vehicle speed acts, and if this centrifugal force exceeds the tire's grip force (frictional force with the road surface), the tire may slip. This is because the method of the present invention in which the recognized position is determined based on the number of revolutions is likely to have a large effect on the error in the recognized position.

Therefore, in this embodiment, the centrifugal force is calculated based on the desired turning radius, that is, the target steering angle, and the vehicle speed at that time.
The vehicle speed is determined so that the centrifugal force does not exceed the grip force of the tires, and the vehicle is decelerated when turning.

However, the relationship between the steering angle and the limit vehicle speed at this time is not linear, but is as shown in FIG. 4.

Therefore, in order to obtain the vehicle speed in real time while the vehicle is running, it is necessary to perform complex calculations at high speed.

Therefore, a table showing the relationship with the target steering angle as shown in FIG. 4 may be provided, and the vehicle speed may be determined by drawing the table without calculating the vehicle speed while the vehicle is running.

Furthermore, in the above case, if the number of steering angle control pulses for the steering pulse motor is large (for example, 500 pulses on one side), the table information will become enormous, so the table should be configured in two stages as follows. It is also possible to significantly compress table information.

That is, first of all, the vehicle speed data given from the computer to the steering motor is binary data;
Discretize the steering angle according to the data resolution. for example,
If the steering angle data is 4 bits, the steering angle can be divided into 16 stages. Therefore, with "8" as the center of the steering angle, the steering angle initial number i is 7, 6, 5...1 for the clockwise steering angle, and 9° 10... Given a steering angle initial number i of 11...15, a steering angle vehicle speed table as shown in Table 1 is created.

In Table 1, j is the vehicle speed initial number for drawing the vehicle speed definition table, for example, the steering angle initial number 5.6 and 10.
．． 11 means that the same vehicle speed initial number "3" is given, and the same vehicle speed is set for the steering angle in that range.

Table 2 shows a vehicle speed definition table showing the correspondence between the vehicle speed initial j and the vehicle speed υ.

To determine the target steering angle using the data tables in Table 11 and Table 2 above, first, calculate the steering angle initial number i using the following formula. In the above formula, A
P is the number of steering angle pulses, and H is the steering angle resolution (number of pulses). For example, if the maximum number of steering angle pulses is 500, and the number of data bits is 4 bits as described above, H is 500 x 2 ÷
2'=62.5.

Then, Table 1 shows the steering angle initial number i obtained from the above formula.
After obtaining the vehicle speed initial number j by drawing the table of Table 2, the vehicle speed υ is obtained by drawing the table of Table 2 using this vehicle speed initial number j. This makes it possible to control the vehicle speed in relation to the steering angle, roughly following the trend shown in Figure 4. As a result, when the vehicle changes direction, the grip force of the tires is overcome by the centrifugal force and slippage occurs. You can prevent turtles from occurring.

[Effects of the Invention] As explained above, the present invention has an encoder attached to each of the drive shafts having a differential mechanism to detect the number of revolutions of the left and right drive wheels separately, and to determine the direction from the difference in the number of revolutions. Therefore, the orientation of the moving body can be determined using only the encoder without using complicated and expensive equipment such as a gyro or a laser or ultrasonic emitting device.

Also, if the detected rotation speed or rotation speed difference is greater than the set value, the detected value is discarded and the distance and direction are calculated using the previous normal detection value as the rotation speed and rotation speed difference at that point. Therefore, even if the wheels monitored by the encoder have an abnormality such as slipping, the abnormal measurement data will not have a negative effect on the calculation of the position and orientation, so the position and orientation of the vehicle can be accurately determined. This has the effect of making it possible to know the

In addition, when an unmanned moving object is equipped with a laser emitter/receiver and corner cubes are provided at appropriate locations on the driving course, it is possible to check whether the self's position has deviated from the predetermined course while driving. As mentioned above, by discarding the data when the rotation speed of the left and right wheels exceeds the set value, it is possible to reduce positional deviations, so it is possible to extend the setting interval of checkpoints using corner cubes. .

Furthermore, in vehicles with conventional distance measuring means,
The measurement system and drive system were clearly separated from the viewpoint of preventing interference between the mechanisms.

However, in this invention, a pair of encoders is provided in the drive system to perform measurements, so there is no need to separate the measurement system and drive system, and moreover, data such as distance and direction can be transmitted using a simple means called an encoder. can be obtained from both at the same time.

[Brief explanation of the drawing]

FIG. 1 is a block configuration diagram showing an embodiment of a moving body position recognition device according to the present invention and a travel control system to which the same is applied. A schematic plan view showing the configuration of the checkpoint and an explanatory diagram of the orientation correction method at the checkpoint. Figure 3 is an explanatory diagram to explain the method of calculating the position and orientation based on the encoder output. It is a graph showing the relationship between steering angle and ideal vehicle speed. 2...Front wheels, 5a, 5b...Traveling drive wheels (
Rear wheel), 6...Differential mechanism, 7...Rotation drive means (traveling drive motor), 11...Turning drive means (steering motor), 20a, 20b...Rotation detection Means (encoder), 22...control device (Figure 2, Figure 3, Figure 4, Figure 4)

Claims (2)

[Claims] (1) A pair of travel drive wheels having a differential mechanism and their rotational drive means, a wheel coupled to a steering shaft and their turning drive means, and a control device that performs drive control of the rotational drive means and the turning drive means. A moving object is provided with a rotation detecting means corresponding to each of the pair of driving wheels having the differential mechanism, and the travel distance and the direction are determined based on the signals from these rotation detecting means. 1. A position recognition device for a moving object, characterized in that it is configured to determine the position of a mobile object. (2) The control device has a setting means that can set a desired value from the outside, and when the rotation speed or the difference in rotation speed based on the detection signals from the pair of rotation detection means exceeds a preset value. 2. The position recognition device for a moving object according to claim 1, wherein the device is configured to discard the measured value when the measured value is discarded.