JP3710451B2 - Method and apparatus for measuring position of moving object - Google Patents

Description

[0001]
BACKGROUND OF THE INVENTION
The present invention relates to, for example, a method for measuring the position and orientation of a moving body when the moving body is unmanned, and an apparatus suitable for implementing the method, and in particular, automatically at high speed, such as a traveling durability test for a multi-purpose lightweight traveling vehicle. The present invention relates to a method and apparatus for measuring the position and orientation of a moving object suitable for driving.
[0002]
[Prior art]
Conventionally, a small four-wheel work vehicle equipped with a large loading platform called a multi-purpose lightweight traveling vehicle at the rear has been put into practical use, and is active in fields such as golf courses, ranches, ski resorts, and sand beaches.
[0003]
As described above, a multi-purpose light-duty vehicle is usually used for traveling on rough terrain with a lot of slope and unevenness. Therefore, in the running endurance test in the development stage, it is performed on the same rough terrain. It is.
[0004]
In general, such a running durability test is performed continuously for a long time of several tens of hours. Also, in the durability test, driving around a certain traveling path specially provided in the mountains several times in consideration of surrounding areas also increases driver fatigue.
[0005]
Accordingly, it is desirable to automatically move a moving body such as a vehicle used for the running durability test. For example, a white line is provided along the traveling path, and a pair of optical sensors for detecting the white line is provided on the lower surface of the moving body in the left-right direction. And moving a moving body along the white line while maintaining the white line between the pair of optical sensors.
[0006]
In addition, for example, while magnetic markers or radio wave markers are intermittently embedded along the traveling path, a magnetic or radio wave sensor for detecting these markers is disposed on the lower surface of the moving body, and the marker is maintained directly below the sensor. There is also a technique for moving a moving body along a sensor.
[0007]
However, basically no object can be laid on the traveling road used for the durability test because the road surface is scraped off by repeated traveling of the moving body.
[0008]
Therefore, for example, reflectors are installed at appropriate intervals along the travel path, and the accurate position of the reflector is specified in advance by surveying or the like, while the moving body is provided with light beam irradiation / light receiving means to move the reflector. Various techniques have been developed to irradiate a light beam from a moving body while moving the body, and to measure the position, path, etc. of the moving body from the light receiving direction of the reflected light and the relative distance between the reflector and the moving body. (For example, refer to Patent Documents 1 and 2).
[0009]
[Patent Document 1]
Japanese Patent Laid-Open No. 11-183174 [Patent Document 2]
Japanese Patent Laid-Open No. 11-271043
[Problems to be solved by the invention]
However, in the prior art as described above, a moving body used in a factory such as an automatic guided vehicle is assumed, and all the reflectors are provided at a predetermined height. However, since the orientation of the moving body is not known, the light beam irradiation / light receiving means of the moving body is configured to scan the entire periphery.
[0011]
For this reason, since it takes time to measure a position, a course, etc. once, it is not suitable for an application of a running durability test that needs to run at a considerably high speed as compared with an automatic guided vehicle.
[0012]
The present invention has been made in view of such circumstances, and is suitable for measuring the position, path, etc. of a moving body that automatically travels a predetermined traveling path at a relatively high speed, and that scans the entire periphery. It is an object of the present invention to provide a method for measuring the position of a moving body that does not require beam irradiation / light receiving means and an apparatus suitable for the implementation.
[0013]
[Means for Solving the Problems]
In order to solve the above-described problem, a method for measuring a position of a moving body according to the present invention includes a moving direction position of the moving body in a direction along the traveling path of the moving body traveling on a predetermined traveling path, and a position of the traveling path. The position in the width direction of the moving body and the direction of the moving body are estimated by an inertial navigation device mounted on the moving body, and the traveling direction position generated by the inertial navigation device, the width direction A method for correcting an error in position and orientation, wherein the moving body passes through a side of a reflector installed at a predetermined position along the traveling path outside the predetermined traveling path. The distance measuring means provided in the moving body detects that the moving body passes through the reflector based on the output change of the distance measuring means using the reflector as a measurement reference, and the detected reflector. Progress from the position of A direction position is obtained, a first distance between the moving body and the reflector is obtained from an output of the distance measuring means, and the moving body and the reflector are obtained from the position in the width direction estimated by the inertial navigation device. The second distance is corrected, and based on the difference between the first distance and the second distance, the azimuth and the width direction position estimated by the inertial navigation device are corrected, and based on the traveling direction position. The correction of the position in the traveling direction estimated by the inertial navigation device is repeated every time the reflector passes.
[0014]
The moving body position measuring apparatus according to the present invention may be configured such that a moving body that travels along a traveling path in a predetermined traveling path travels along the traveling direction of the moving body and moves in the width direction of the traveling path. The position of the body in the width direction and the azimuth of the moving body are estimated by an inertial navigation device mounted on the moving body, and the traveling direction position generated by the inertial navigation device, the width direction position, and the direction An apparatus for correcting an error, comprising: a distance measuring unit provided in the moving body; and a measuring reference of the distance measuring unit at a predetermined position along the traveling path outside the predetermined traveling path. Passing detection means for detecting that the moving body passes through the reflector based on a change in the output of the distance measuring means when passing through the side of the reflector installed as much as possible, and detection by the passage detection means Said reflector Traveling direction position computing means for computing the traveling direction position from a position; first distance computing means for computing a first distance between the moving body and the reflector from the output of the distance measuring means; and the inertial navigation Based on a difference between the first distance and the second distance, a second distance calculating means for calculating a second distance between the movable body and the reflector from the position in the width direction estimated by the apparatus, the inertia An azimuth / width direction position correcting means for correcting the azimuth and width direction position estimated by the navigation device, and a traveling direction position estimated by the inertial navigation device based on the traveling direction position calculated by the direction position calculating means. And a traveling direction position correcting unit for correcting, the correction by the azimuth / width direction position correcting unit and the traveling direction position correcting unit are to be repeated every time it passes through the reflector.
[0015]
According to the present invention, since the position and orientation of the moving body can be obtained very easily, the processing time can be shortened. Accordingly, the position of the moving body that automatically travels at a relatively high speed on a predetermined traveling path, It is suitable for detecting the course and the like, and does not require a light beam irradiation / light receiving means for scanning the entire periphery as in the prior art. In addition, there is no need to process the road.
[0016]
In the present invention, the inertial navigation system includes a gyro and a general speed sensor used in an automobile or the like that detects a travel distance or the like using a speed pulse.
[0017]
Further, in the present invention, as an application to a moving body, it can be applied to, for example, an automobile, a snowplow, an automatic lawn mower, etc., in addition to the multipurpose lightweight traveling vehicle or the automatic guided vehicle as described above. .
[0018]
Further, the present invention is a configuration in which a GPS navigation device and an inertial navigation device are used in combination, and corrects measurement errors of the inertial navigation device when used in places or environments where radio waves from GPS satellites cannot be received, such as in the mountains. It is particularly applicable.
[0019]
DETAILED DESCRIPTION OF THE INVENTION
Hereinafter, the position measuring method according to the present invention will be described in detail with reference to the accompanying drawings.
[0020]
According to the position measurement method according to the present invention, the position (x ₁ , y ₁ ) and the azimuth φ ₁ of the moving body can be measured based on the following principle.
[0021]
As shown in FIG. 1, for example, a distance measuring means 2 such as a laser distance meter is provided on one side (right side in FIG. 1) of the moving body 1 traveling in the direction of the solid arrow. The distance measuring means may be any means that can measure the distance to the reflector, and may be a distance meter using a light beam, an ultrasonic wave, or the like.
[0022]
On the other hand, a plurality of reflectors 3, 3... Are installed along one side (a right side in FIG. 1) of a predetermined traveling path C, and the position of each reflector 3 is obtained as an absolute coordinate position in advance by surveying or the like. It has been. The distance measuring means 2 measures the distance in a direction perpendicular to the traveling direction of the moving body 1, and measures the relative distance between the moving body 1 and the reflector 3. It is assumed that there is no obstacle in the vicinity of the reflector 3 such that the distance measuring means 2 is mistaken for the reflector 3.
[0023]
At this time, if the output of the distance measuring means 2 is monitored during traveling, it is detected that the moving body 1 has come to a position substantially perpendicular to the reflector 3, and the reflector 3 and the moving body 1 The distance between them can be determined.
[0024]
In FIG. 1, two reflectors 3 are arranged. The reflector 3 on the front side in the traveling direction is B ₀ , the reflector 3 ahead thereof is B _1, and the reflector detection positions of B ₀ and B ₁ by the moving body 1 are P ₀ and P ₁ , respectively. Further, it is assumed that the position (x ₀ , y ₀ ) and direction (φ ₀ ) of the moving body 1 have already been obtained at the position P ₀ .
[0025]
Hereinafter, a method for correcting an error that occurs when the moving body 1 travels from the position P _{0 to} the position P ₁ will be described.
[0026]
The movement trajectory indicated by a broken line in FIG. 1 is a trajectory estimated by the inertial navigation device mounted on the moving body 1 for inertial navigation, and an inertial navigation device such as a drift of a gyro constituting the inertial navigation device is generated Due to the output error, the actual travel locus shown by the solid line in FIG. 1 deviates.
[0027]
Now, when the moving body 1 has reached the position P ₁ , the distance measurement means 2 determines the distance d between the moving body 1 and the reflector B ₁ . Here, the distance in the width direction with respect to the center line of the travel path C of the reflector B ₁ (indicated by the alternate long and short dash line in FIG. 1) is D ₁ (known) in FIG. 1, and the center line of the travel path C estimated by the inertial navigation system If the distance in the width direction of the moving body 1 with respect to is assumed to be e ₁ , the distance between the moving body 1 and the reflector B ₁ calculated from the estimated value of the inertial navigation device is D ₁ −e ₁ , and the inertial navigation device estimate An error from the actual position measured by the distance measuring means 2 is obtained by e ₂ = (D ₁ −e ₁ ) −d. Assuming that the travel distance of the moving body 1 moving from the position P _{0 to} the position P ₁ is L, the direction is shifted by approximately e ₂ / L, and the position of P ₁ estimated by the inertial navigation device By correcting the bearing at e ₂ / L, it is possible to prevent accumulation of errors in the inertial navigation system.
[0028]
At the same time, the estimated value of the traveling direction distance of the inertial navigation system is also assumed to be the position (x ₁ , y ₂ ) assuming that the moving body 1 is located at a distance d away from the position of the known reflector B _{1 on} the traveling path C side. Can be corrected.
[0029]
Hereinafter, the moving body to which the position measuring method according to the present invention is applied will be described.
[0030]
FIG. 2 is a block diagram showing the configuration of the moving body according to the embodiment of the present invention. The moving body 1 of the present embodiment is a multipurpose lightweight traveling vehicle having an automatic steering function for a traveling durability test.
[0031]
The moving body 1 according to the present embodiment irradiates a reflector 3 installed at a predetermined interval outside a predetermined traveling path (not shown) with a light beam such as a laser beam and reflects the reflected light by the reflector 3. One or a plurality of laser rangefinders 2 that receive the received light beam, and an autopilot device 4 that automatically steers the mobile body 1 by operating the steering 5, the brake 6, the accelerator 7, and the like of the mobile body 1. ing.
[0032]
The autopilot device 4 includes a control arithmetic device 40 with a built-in timer 40a, a GPS receiver 41, a gyro 42, a vehicle speed sensor 43, a drive device 44, and the like, and basically an autopilot based on GPS data. Thus, inertial navigation by an inertial navigation device 45 (see FIG. 4) composed of the gyro 42 and the vehicle speed sensor 43 is also possible.
[0033]
In the present embodiment, the automatic pilot device 4 is configured to include the vehicle speed sensor 43. In general, however, such a vehicle speed sensor is originally attached to a vehicle such as a multi-purpose lightweight traveling vehicle. Therefore, it is possible to use this.
[0034]
The GPS receiver 41 is, for example, a real time kinematic GPS receiver (RTK-GPS), and receives information (time information, phase information, etc.) received from the GPS satellite 8 and a ground reference station (not shown). The control arithmetic unit 40 is provided. In the RTK method, the reception position can be measured with an error of about several centimeters. The GPS receiver 41 may be a differential GPS receiver (DGPS), although the accuracy is somewhat reduced.
[0035]
The gyro 42 constituting the inertial navigation device 45 is a fiber optical type, a ring laser type, or the like, detects the triaxial acceleration of the moving body 1, and gives the detection result to the control arithmetic unit 40.
[0036]
The vehicle speed sensor 43 is an electric, magnetic, or optical sensor provided on the axle of the moving body 1 so as to generate a pulse signal for each rotation of the axle and to provide the control arithmetic unit 40 with the pulse signal. It has become.
[0037]
The driving device 44 includes various actuators that operate the steering 5, the brake 6, the accelerator 7, and the like of the moving body 1. The driving device 44 operates the actuator in accordance with a control instruction from the control arithmetic device 40, and determines the orientation and the moving body 1. Adjust the speed.
[0038]
Next, the configuration and operation of the control arithmetic unit 40 will be further described with reference to FIGS.
[0039]
The control arithmetic unit 40 includes a microprocessor, and basically calculates the position and direction (x, y, φ) of the moving body 1 based on information given from the GPS receiver 41 as shown in FIG. Then, based on this position and orientation, a control instruction is output to the drive device 44 to control the mobile body 1 to travel along the center line of a predetermined travel path (step S1). The course data of the traveling road is stored together with the position data of the reflector 3 in a memory (not shown) built in the control arithmetic device 40.
[0040]
However, if the GPS receiver 41 cannot properly receive information from the GPS satellite 8 and a ground reference station (not shown), it cannot depend on the information from the GPS receiver 41. Is switched to inertial navigation by an inertial navigation device 45 (see FIG. 4) including the gyro 42 and the vehicle speed sensor 43 (step S2).
[0041]
However, since errors occur in the angular velocity data given from the gyro 42 and the pulse signal given from the vehicle speed sensor due to drift or the like, it is necessary to correct them periodically. The correction is performed preferentially by the GPS data, but when it cannot depend on the GPS data, the laser distance fixed point position measurement (x ₁ , y ₁ , φ ₁ ) is performed by the laser distance meter 2, Based on the result, the position and orientation (x, y, φ) of the moving body 1 used for inertial navigation are corrected (step S3).
[0042]
More specifically, as shown in FIG. 4, the control arithmetic device 40 includes a GPS position detection unit 401, an inertial navigation position detection unit 402, a position / orientation correction processing unit 403, and the like. The unit 403 includes an azimuth correction calculation unit 404, a position correction calculation unit 405, and the like.
[0043]
The GPS data from the GPS receiver 41 is given to the GPS position detector 401, and the GPS position detector 401 calculates the position and orientation of the moving body 1 based on the GPS data.
[0044]
Inertial navigation data from the inertial navigation device 45 (such as the gyro 42 and the vehicle speed sensor 43) is given to the inertial navigation position detection unit 402, and the inertial navigation position detection unit 402 is calculated by the GPS position detection unit 401. The position and orientation of 1 are used as the initial position and initial orientation, and the position and orientation of the moving body 1 are calculated based on the inertial navigation data.
[0045]
The position / azimuth correction processing unit 403 calculates the azimuth of the moving body 1 by the azimuth correction calculation unit 404 using the principle of the first and / or second aspect of the present invention described above, and the position correction calculation unit 405 The position of the moving body 1 is calculated.
[0046]
Information such as the installation interval of the reflector 3 and its absolute coordinate position used in the calculation of the position and orientation of the moving body 1 in the position / orientation correction processing unit 403 is a memory (not shown) built in the control arithmetic unit 40 Is stored in advance.
[0047]
Further, information such as the travel distance and travel speed used in the calculation of the position and orientation of the moving body 1 in the position / orientation correction processing unit 403 includes a pulse signal from the vehicle speed sensor 43 (or an acceleration signal from the gyro 42). The wheel circumference information is specified based on the time signal from the timer 40a and the wheel circumference information of the moving body 1, and the wheel circumference information is stored in a memory (not shown) built in the control arithmetic unit 40. ing.
[0048]
【The invention's effect】
As described above, according to the method for measuring the position of the moving body and the apparatus suitable for the implementation according to the present invention, it is suitable for measuring the position, path, etc. of the moving body that automatically travels a predetermined traveling path at a relatively high speed. In addition, the present invention has an excellent effect, for example, that no light beam irradiating / receiving means for scanning the entire periphery is required.
[Brief description of the drawings]
FIG. 1 is a principle diagram for explaining a method of measuring a position of a moving body according to the present invention.
FIG. 2 is a block diagram showing a configuration of a multi-purpose lightweight traveling vehicle having an autopilot function for a traveling durability test as a moving body according to an embodiment of the present invention.
FIG. 3 is a flowchart showing a method for switching the position and direction detection of the automatic control apparatus for a moving body shown in FIG. 2;
4 is a block diagram showing a detailed configuration of the control arithmetic device for a moving body shown in FIG. 2;
[Explanation of symbols]
1 Mobile
2 Laser distance meter
3 Reflector
4 Autopilot
5 Steering
6 Brake
7 Accelerator
8 GPS satellite
40a timer
40 Control unit
41 GPS receiver
42 Gyro
43 Vehicle speed sensor
44 Drive unit
45 Inertial navigation system
401 GPS position detector
402 Inertial navigation position detector
403 Position / azimuth correction processing section
404 Azimuth correction calculator
405 Position correction calculator

Claims (2)

A traveling direction position of the moving body in a direction along the traveling path of a moving body traveling on a predetermined traveling path, a width direction position of the moving body in a width direction of the traveling path, and an orientation of the moving body A method for correcting an error in the traveling direction position, the width direction position, and the azimuth generated by the inertial navigation apparatus, as estimated by the inertial navigation apparatus mounted on the moving body,
When the moving body passes by a side of a reflector installed at a predetermined position along the traveling path outside the predetermined traveling path, the distance measuring means included in the moving body causes the reflector to Based on the change in the output of the distance measuring means based on the measurement reference, the moving body is detected to pass through the reflector, and the traveling direction position is determined from the detected position of the reflector,
Obtaining a first distance between the movable body and the reflector from the output of the distance measuring means;
Obtaining a second distance between the movable body and the reflector from the position in the width direction estimated by the inertial navigation device;
Based on the difference between the first distance and the second distance, the azimuth and the width direction position estimated by the inertial navigation apparatus are corrected, and the inertial navigation apparatus estimates based on the traveling direction position. The moving body position measuring method is characterized in that correcting the position in the traveling direction is repeated every time the reflector passes. A traveling direction position of the moving body in a direction along the traveling path of a moving body traveling on a predetermined traveling path, a width direction position of the moving body in a width direction of the traveling path, and an orientation of the moving body An apparatus that estimates the inertial navigation device mounted on the moving body and corrects the error in the traveling direction position, the width direction position, and the azimuth generated by the inertial navigation device,
A distance measuring means provided in the moving body;
When the moving body passes by a side of a reflector installed as a measurement reference of the distance measuring means at a predetermined position along the traveling path outside the predetermined traveling path, the distance measuring means Passage detecting means for detecting that the moving body passes through the reflector based on an output change;
Traveling direction position calculating means for calculating the traveling direction position from the position of the reflector detected by the passage detecting means;
First distance calculating means for calculating a first distance between the movable body and the reflector from the output of the distance measuring means;
Second distance calculation means for calculating a second distance between the movable body and the reflector from the position in the width direction estimated by the inertial navigation device;
An azimuth / width direction position correcting means for correcting an azimuth and a width direction position estimated by the inertial navigation device based on a difference between the first distance and the second distance;
Traveling direction position correcting means for correcting the traveling direction position estimated by the inertial navigation device based on the traveling direction position calculated by the direction position calculating means;
An apparatus for measuring a position of a moving body, wherein the correction by the azimuth / width direction position correcting means and the traveling direction position correcting means are to be repeated every time the light passes through the reflector.

Images