JP5361421B2 - Measuring device, laser position / orientation value correction method and laser position / orientation value correction program for measuring device - Google Patents

Abstract

<P>PROBLEM TO BE SOLVED: To regulate an attachment position and a posture value of a laser scanner attached to a measurement vehicle so as to generate a three-dimensional point group (aggregate of points on a three-dimensional coordinate) with high precision. <P>SOLUTION: The measurement vehicle having the laser scanner attached thereto reciprocally travels so as to obtain a distance azimuth point group 283. A position posture evaluation section 210 evaluates a position and a posture of the measurement vehicle on the basis of GPS data 282 or a posture angle velocity 284 obtained together with the distance azimuth point group 283. A three-dimensional point group generating section 211 generates a three-dimensional point group 292 on the basis of the position and posture of the measurement vehicle, the distance azimuth point group 283, and the attachment position and posture value of the laser scanner. A laser scanner regulating section 220 compares the three-dimensional point group 292 in an outward journey with the three-dimensional point group 292 in an inward journey so as to regulate the attachment position and posture value of the laser scanner on the basis of the deviation amount. The three-dimensional point group generating section 211 generates the accurate three-dimensional point group 292 on the basis of the attachment position and posture value of the regulated laser scanner. <P>COPYRIGHT: (C)2010,JPO&amp;INPIT

Description

  The present invention relates to, for example, a measurement apparatus and a laser position of a measurement apparatus that are related to the efficiency of manufacturing a post-processing type mobile unit self-position measurement apparatus that combines a GPS (Global Positioning System) and a gyro (IMU: Internal Measurement Unit). It is an attitude value correction method and a laser position and attitude value correction program.

  A technique is used in which a laser scanner or a camera is mounted on a vehicle to collect positional information around the vehicle. In addition, a laser point cloud and a camera image are superimposed, and position information is determined from the laser point cloud and an object is determined from the camera video.

International Publication No. 2008/099915 Pamphlet

In order to collect position information around the vehicle, it is necessary to accurately know the mounting position and attitude angle of the laser scanner on the vehicle in order to express the position information obtained from the laser scanner as an absolute position in the world coordinate system. Furthermore, in order to superimpose the laser point cloud on the camera image, it is necessary to accurately know the mounting position and the attitude angle of the camera on the vehicle.
Conventionally, mounting information (position, posture angle) of a laser scanner and a camera has been measured using a physical facility.

  However, this measurement method has a problem that a large-scale facility and equipment such as a total station are required, and it takes time and cost for measurement. Furthermore, with this measurement method, it is not known until the actual measurement is performed what the measurement accuracy will be. For this reason, in order to confirm the degree of accuracy, there has been a problem that a complicated procedure is required such as performing calibration several times and averaging the measurement results.

  An object of the present invention is, for example, to make it possible to easily and correctly adjust a mounting position / posture value of a laser scanner.

  The measuring device of the present invention is a distance indicating a distance from the specific body to the feature measured by a laser scanner attached to the specific body and an orientation from the specific body to the feature measured by the laser scanner. A feature coordinate calculation unit that calculates the coordinates of the feature using a CPU (Central Processing Unit) based on the orientation, the laser position and orientation value indicating the attachment position of the laser scanner and the attachment orientation of the laser scanner A first coordinate calculated by the feature coordinate calculation unit based on a first distance direction measured from a first direction for a specific feature by the laser scanner, and the laser scanner The feature coordinate calculation unit based on a second distance direction measured from the second direction for a specific feature A laser position / attitude value error specifying unit that calculates a difference from the second coordinate calculated by using a CPU and specifies an error included in the laser position / attitude value based on the calculated difference using the CPU; A laser position and orientation value correction unit that corrects the laser position and orientation value using a CPU based on the error specified by the laser position and orientation value error specifying unit.

  According to the present invention, for example, the mounting position / posture value of a laser scanner can be easily and correctly adjusted.

1 is a configuration diagram of a measurement vehicle 100 according to Embodiment 1. FIG. FIG. 3 is a configuration diagram of a measurement unit 110 in the first embodiment. [Fig. 3] Fig. 3 is a functional configuration diagram of a mobile body self-position measuring apparatus 200 according to Embodiment 1. FIG. 3 is a diagram illustrating an example of hardware resources of the mobile unit self-position measuring apparatus 200 according to the first embodiment. 3 is a flowchart showing a measurement method in the first embodiment. The figure which shows the measuring method for adjustment in each embodiment. 5 is a flowchart of laser scanner adjustment processing (S130) in the first embodiment. It shows a method of calculating the adjustment value [Delta] X _L of the laser scanner in the first embodiment. FIG. 6 is a diagram showing a method for calculating an adjustment value ΔY _L of the laser scanner in the first embodiment. FIG. 5 is a diagram showing a method for calculating an adjustment value Δφ _L of the laser scanner in the first embodiment. FIG. 6 is a diagram showing a method for calculating an adjustment value Δθ _L of the laser scanner in the first embodiment. FIG. 6 shows a method for calculating an adjustment value Δψ _L of the laser scanner in the first embodiment. FIG. 10 is a diagram illustrating a method for calculating an adjustment value Δψ of the laser scanner in the second embodiment. 9 is a flowchart of laser scanner adjustment processing (S130) in the second embodiment. The function block diagram of the mobile body self-position measuring apparatus 200 in Embodiment 3. FIG. 10 is a flowchart illustrating a measurement method according to Embodiment 3. 10 is a flowchart of camera adjustment processing (S160) in the third embodiment. FIG. 11 is a relationship diagram between a camera position shift, a camera image 281 and a neighboring point group in the third embodiment. FIG. 10 is a diagram illustrating the relationship between a camera posture shift, a camera image 281 and a far point group in the third embodiment. The function block diagram of the mobile body self-position measuring apparatus 200 in Embodiment 4. FIG. 10 is a flowchart illustrating a measurement method according to Embodiment 4. FIG. 10 is a diagram illustrating a method for calculating an adjustment time Δt of measurement time of the distance direction point group 283 in the fourth embodiment.

Embodiment 1 FIG.
A mobile mapping system (MMS) that generates a three-dimensional map of a traveling area based on laser scanner measurement data obtained by running a measurement vehicle equipped with a laser scanner and a camera and a camera image will be described.
Also, laser scanner calibration in MMS will be described.

FIG. 1 is a configuration diagram of a measurement vehicle 100 according to the first embodiment.
FIG. 2 is a configuration diagram of the measurement unit 110 according to the first embodiment.
The configuration of measurement vehicle 100 in the first embodiment will be described below based on FIG. 1 and FIG.

  The measurement vehicle 100 travels in the measurement target area, and acquires various data necessary for generating a three-dimensional map of the measurement target area.

In the measurement vehicle 100, a measurement unit 110 is attached to a top plate. In addition, the measurement vehicle 100 includes a unit storage unit 190 (storage device).
The measurement unit 110 includes a front camera 111, a road surface camera 112, a GPS 113, an upper surface laser scanner 114, a lower surface laser scanner 115, an IMU 116, and a wiring relay box 117.

The front camera 111 is attached horizontally and images the front of the measurement vehicle 100.
The road surface camera 112 is attached obliquely downward and images the road surface.

  GPS113 is installed in three places. Each GPS 113 is a device that receives a positioning signal (indicating satellite orbit information and the like) transmitted from a GPS (Global Positioning System) satellite and performs GPS positioning based on a reception result (GPS observation value).

The upper surface laser scanner 114 is mounted obliquely upward, and measures a distance direction for a feature (for example, a road sign) located at a location higher than the measurement unit 110.
The lower surface laser scanner 115 is mounted obliquely downward, and measures the distance direction for a feature (for example, a road surface) located at a place lower than the measurement unit 110.
The distance direction means the distance and direction from the upper surface laser scanner 114 or the lower surface laser scanner 115 to the feature.
In the measurement of the distance direction, the upper surface laser scanner 114 and the lower surface laser scanner 115 sequentially emit laser beams in the direction of 180 degrees from the left side to the front and from the front side to the right side while descending from side to side. Measure the time it takes for the object to reflect back and return. The upper surface laser scanner 114 and the lower surface laser scanner 115 calculate the distance from the feature reflecting the laser based on the measured time, and the calculated distance and the laser emission direction are set as the distance direction of the feature. .
The laser scanner is also referred to as LRF (Laser Range Finder).

  The IMU 116 (IMU: Inertial Navigation Device) measures angular velocities in three axial directions (x, y, z). For example, a gyro is used as the IMU 116.

  Each of the above devices outputs the acquired data to the wiring relay box 117 together with the acquisition time (measurement time).

  The wiring relay BOX 117 is a device that is connected to each device attached to the measurement unit 110, inputs data output from each device, and stores the input data in the unit storage unit 190.

Hereinafter, the traveling direction of the measurement vehicle 100 is referred to as “x axis”, the width direction of the measurement vehicle 100 is referred to as “y axis”, and the height direction of the measurement vehicle 100 is referred to as “z axis”.
The rotation angle around the x axis is “roll φ (rotation angle)”, the rotation angle around the y axis is “pitch θ (elevation angle)”, and the rotation angle around the z axis is “yaw ψ (azimuth angle)”. .
The origin of the xyz coordinate system is “O”.

FIG. 3 is a functional configuration diagram of the mobile unit self-position measuring apparatus 200 according to the first embodiment.
A functional configuration of mobile body self-position measuring apparatus 200 in Embodiment 1 will be described below with reference to FIG.

  The mobile body self-position measuring device 200 measures the three-dimensional coordinates of the feature existing in the measurement target area based on various data acquired by the measurement vehicle 100. The three-dimensional coordinates of the feature are used for generating a three-dimensional map, for example.

  The mobile body self-position measuring apparatus 200 includes a position and orientation determination unit 210, a three-dimensional point group generation unit 211 (an example of a feature coordinate calculation unit), a three-dimensional point group projection unit 212 (an example of a laser point projection unit), and a feature. A position measurement unit 213, a laser scanner adjustment unit 220 (an example of a laser position / orientation value error identification unit and a laser position / orientation value correction unit), an acquisition data storage unit 280, and a measurement data storage unit 290 are provided.

The acquired data storage unit 280 is a storage device that stores each data acquired by the measurement vehicle 100 using a storage medium.
Hereinafter, image data captured by the front camera 111 and the road surface camera 112 of the measurement vehicle 100 is referred to as “camera image 281”.
Various data (information indicated by the positioning signal, positioning result, etc.) observed by the GPS 113 of the measuring vehicle 100 are referred to as “GPS data 282”.
A plurality of pieces of data indicating distance directions measured by the upper surface laser scanner 114 and the lower surface laser scanner 115 of the measurement vehicle 100 are referred to as “distance direction points”, and a set of distance direction points is referred to as a “distance direction point group 283”. "
Data indicating the angular velocity measured by the IMU 116 of the measurement vehicle 100 is referred to as “angular velocity 284”.

The position and orientation determination unit 210 calculates the position (three-dimensional coordinates) and attitude (roll, pitch, and yaw) of the measurement vehicle 100 at each measurement time using the CPU based on the GPS data 282 and the angular velocity 284.
Hereinafter, the position and orientation of the measurement vehicle 100 at each measurement time calculated by the position and orientation determination unit 210 are referred to as “vehicle position and orientation 291”.

The three-dimensional point group generation unit 211 calculates the three-dimensional coordinates of each feature existing in the measurement target area using the CPU based on the distance / azimuth point group 283 and the position / orientation 291. The three-dimensional point group generation unit 211 calculates three-dimensional coordinates for each distance direction point.
Hereinafter, three-dimensional coordinates corresponding to one distance direction point calculated by the three-dimensional point group generation unit 211 are referred to as “three-dimensional points”, and a set of three-dimensional points is referred to as a “three-dimensional point group 292”.

The three-dimensional point group projection unit 212 projects the three-dimensional point group 292 onto the camera image 281 using the CPU.
Here, the term “projection” means that a mark (for example, a black circle) is superimposed and displayed on one or a plurality of pixels in which a point indicated by a three-dimensional point among the pixels in the camera image 281 is reflected, or It is to associate three-dimensional points.

The feature position measurement unit 213 calculates the three-dimensional coordinates of the feature (the feature located in the measurement target area) reflected in the camera image 281 based on the three-dimensional point group 292 projected on the camera image 281. Calculate using the CPU.
Hereinafter, the three-dimensional coordinates of the feature calculated by the feature position measuring unit 213 are referred to as “feature position 293”.

Based on the three-dimensional point group 292, the laser scanner adjustment unit 220 uses the CPU to determine the amount of deviation between the position and orientation values of the upper surface laser scanner 114 and the lower surface laser scanner 115 attached to the measurement unit 110 of the measurement vehicle 100. To identify. Position and orientation values (for example, design values) of the upper surface laser scanner 114 and the lower surface laser scanner 115 attached to the measurement unit 110 of the measurement vehicle 100 are stored in advance in an arbitrary storage unit (storage device). The laser scanner adjustment unit 220 identifies the difference between the stored position / orientation value and the actual position / orientation value as a deviation amount.
Further, the laser scanner adjustment unit 220 corrects the stored position / orientation value using the CPU based on the calculated deviation amount. Hereinafter, the correction of the position and orientation value is referred to as “adjustment (calibration)”.

  The measurement data storage unit 290 is a storage device that stores the vehicle position and orientation 291, the three-dimensional point group 292, and the feature position 293 using a storage medium.

FIG. 4 is a diagram illustrating an example of hardware resources of the mobile unit self-position measuring apparatus 200 according to the first embodiment.
In FIG. 4, the mobile body self-position measuring device 200 includes a CPU 911 (Central Processing Unit) (also called a central processing unit, a microprocessor, or a microcomputer) that executes a program. The CPU 911 includes a ROM 913, a RAM 914, a communication board 915, a display device 901, a keyboard 902, a mouse 903, an FDD 904 (Flexible Disk Drive), a CDD 905 (compact disk device), a printer device 906, a scanner device 907, via a bus 912. It is connected to a microphone 908, a speaker 909, and a magnetic disk device 920, and controls these hardware devices.

  The communication board 915 is wired or wirelessly connected to a communication network such as a LAN (local area network), the Internet, a telephone line network or the like.

  The magnetic disk device 920 stores an OS 921 (operating system), a window system 922, a program group 923, and a file group 924. The programs in the program group 923 are executed by the CPU 911, the OS 921, and the window system 922.

  The program group 923 stores a program for executing a function described as “˜unit” in the embodiment. The program is read and executed by the CPU 911.

In the file group 924, in the embodiment, result data such as “determination result”, “calculation result of”, “processing result of” when executing the function of “to part”, “to part” The data to be passed between programs that execute the function “,” other information, data, signal values, variable values, and parameters are stored as items “˜file” and “˜database”.
The “˜file” and “˜database” are stored in a recording medium such as a disk or a memory. Information, data, signal values, variable values, and parameters stored in a storage medium such as a disk or memory are read out to the main memory or cache memory by the CPU 911 via a read / write circuit, and extracted, searched, referenced, compared, and calculated. Used for CPU operations such as calculation, processing, output, printing, and display. During these CPU operations, information, data, signal values, variable values, and parameters are temporarily stored in the main memory, cache memory, and buffer memory.
In addition, arrows in the flowcharts described in the embodiments mainly indicate input / output of data and signals. The data and signal values are the RAM 914 memory, the FDD 904 flexible disk, the CDD 905 compact disk, and the magnetic disk device 920 magnetic field. It is recorded on a recording medium such as a disc, other optical discs, mini discs, DVD (Digital Versatile Disc). Data and signal values are transmitted online via a bus 912, signal lines, cables, or other transmission media.

  In addition, what is described as “˜unit” in the embodiment may be “˜circuit”, “˜device”, “˜device”, and “˜step”, “˜procedure”, “˜”. Processing ". That is, what is described as “˜unit” may be realized by firmware stored in the ROM 913. Alternatively, it may be implemented only by software, or only by hardware such as elements, devices, substrates, and wirings, by a combination of software and hardware, or by a combination of firmware. Firmware and software are stored as programs in a recording medium such as a magnetic disk, a flexible disk, an optical disk, a compact disk, a mini disk, and a DVD. The program is read by the CPU 911 and executed by the CPU 911. That is, the program causes the computer to function as “to part”. Alternatively, the procedure or method of “to part” is executed by a computer.

FIG. 5 is a flowchart showing the measurement method in the first embodiment.
The measurement method in Embodiment 1 is demonstrated below based on FIG.
The measuring vehicle 100 and the moving body self-position measuring device 200 execute processing described below using a CPU.

The measurement vehicle 100 performs measurement for adjusting the attachment position / posture value of the laser scanner (S110).
The mobile body self-position measuring apparatus 200 generates a three-dimensional point group 292 based on the measurement result (S120), and adjusts the attachment position / posture value of the laser scanner based on the three-dimensional point group 292 (S130).
The measurement vehicle 100 performs measurement for measurement of the feature position (S140), and the mobile body self-position measurement device 200 measures the feature position 293 based on the measurement result (S150).
Below, the detail of each process (S110-S150) is demonstrated.

<S110>
The measurement vehicle 100 performs measurement for adjusting the attachment position and orientation value of the laser scanner.

FIG. 6 is a diagram illustrating a measuring method for adjustment in each embodiment.
The measurement for adjusting the attachment position and orientation value of the laser scanner in S110 will be described below with reference to FIG.

  The user prepares two arbitrary features A and B, and arranges the two prepared features at different positions. The two features to be prepared are preferably small so that the number of distance azimuth points to be measured does not increase and at least one distance azimuth point is measured. The place where the two features are arranged is preferably a plaza (open space) where other features are not measured.

The user reciprocates the measurement vehicle 100 on the same route at the place where the two features are arranged (FIGS. 6 (1) and (2)).
Hereinafter, a feature located closer to the measurement vehicle 100 is referred to as “neighboring feature B”, and a feature located farther from the measurement vehicle 100 is referred to as “far feature A”.

  Each device attached to the measurement unit 110 of the measurement vehicle 100 acquires data in each of the forward path (hereinafter referred to as “forward direction”) and the return path (hereinafter referred to as “reverse direction”). The acquired various data (camera image 281, GPS data 282, distance direction point group 283 and angular velocity 284) are stored in the unit storage unit 190 of the measurement vehicle 100.

  The user inputs various data stored in the unit storage unit 190 to the mobile unit self-position measuring apparatus 200. The acquired data storage unit 280 stores various data input to the mobile unit self-position measuring apparatus 200.

Returning to FIG. 5, the description of the measurement method will be continued.
After S110, the process proceeds to S120.

<S120>
The mobile body self-position measuring device 200 generates the three-dimensional point group 292 based on various data stored in the acquired data storage unit 280 as follows.

The position / orientation locating unit 210 calculates the three-dimensional coordinates and orientation of the measurement vehicle 100 at each measurement time as the vehicle position / orientation 291 based on the GPS data 282 and the angular velocity 284, and uses the calculated vehicle position / orientation 291 as the measurement data storage unit 290. To remember.
For example, the position / orientation location determination unit 210 sets the positioning result of the GPS 113 included in the GPS data 282 as the three-dimensional coordinates of the measurement vehicle 100.
In addition, for example, the position / orientation locating unit 210 calculates the vehicle position / orientation 291 by inertial navigation (also referred to as dead reckoning) based on the angular velocity 284.
The coordinate system of the vehicle position and orientation 291 calculated by the position and orientation determination unit 210 is a specific coordinate system (for example, the ENU coordinate system). Hereinafter, the coordinate system of the vehicle position and orientation 291 is referred to as an ENU coordinate system.

The three-dimensional point group generation unit 211 generates a three-dimensional point group 292 based on the vehicle position / posture 291 and the distance direction point group 283, and stores the generated three-dimensional point group 292 in the measurement data storage unit 290.
At this time, the three-dimensional point group generation unit 211 acquires the vehicle position and orientation 291 at the time of measurement of the distance azimuth point for each distance azimuth point included in the distance azimuth point group 283 from the measurement data storage unit 290 as follows. 3D points are calculated for each distance and orientation point. A plurality of calculated three-dimensional points are a three-dimensional point group 292.

The attachment position / posture value of the laser scanner indicates a value in the local coordinate system (xyz coordinate system shown in FIG. 1), and is stored in advance in an arbitrary storage unit. The orientation of the feature indicated by the distance orientation point is a relative orientation with respect to the mounting posture of the laser scanner.
The three-dimensional point cloud generation unit 211 calculates the feature orientation in the local coordinate system based on the attachment position and orientation value of the laser scanner indicated in the local coordinate system and the relative feature orientation based on the laser scanner. To do. Further, the three-dimensional point cloud generation unit 211 calculates the feature direction in the ENU coordinate system based on the vehicle position / posture 291 (posture) indicated in the ENU coordinate system and the feature direction in the local coordinate system.
Based on the vehicle position and orientation 291 shown in the ENU coordinate system, the feature azimuth in the ENU coordinate system, and the distance azimuth point, the three-dimensional point cloud generation unit 211 moves the distance azimuth point from the measurement vehicle 100 to the feature azimuth. The three-dimensional coordinates of the points separated by the distance indicated by are calculated as three-dimensional points. The three-dimensional point indicates the coordinates of the ENU coordinate system.

  After S120, the process proceeds to S130.

<S130>
The mobile body self-position measuring apparatus 200 adjusts the attachment position and orientation value of the laser scanner based on the three-dimensional point group 292 generated in S120.
Details of S130 will be described later.
After S130, the process proceeds to S140.

<S140>
The user travels in the measurement target area with the measurement vehicle 100. Each device attached to the measurement unit 110 of the measurement vehicle 100 acquires data, and the acquired various data is stored in the unit storage unit 190 of the measurement vehicle 100.
The user inputs various data stored in the unit storage unit 190 to the mobile unit self-position measuring apparatus 200. The acquired data storage unit 280 stores various data input to the mobile unit self-position measuring apparatus 200.
After S140, the process proceeds to S150.

<S150>
The mobile body self-position measuring device 200 measures the feature position 293 based on various data stored in the acquired data storage unit 280 as follows, for example.

First, the position and orientation determination unit 210 calculates the vehicle position and orientation 291 and stores the calculated vehicle position and orientation 291 in the measurement data storage unit 290, as in S120.
Next, the 3D point group generation unit 211 generates a 3D point group 292 based on the vehicle position and orientation 291 and stores the generated 3D point group 292 in the measurement data storage unit 290, as in S120.
Next, the three-dimensional point group projection unit 212 projects the three-dimensional point group 292 onto the camera image 281 based on the focus of the camera and the attachment position / posture value of the camera.
The feature position measuring unit 213 measures the feature position 293 based on the three-dimensional point group 292 projected on the camera video 281 and stores the measured feature position 293 in the measurement data storage unit 290. For example, the 3D point group projection unit 212 displays the camera image 281 and the 3D point group 292 projected on the camera image 281 on the display device. Based on the display, the user designates a feature for which the feature position 293 is to be measured to the mobile body self-position measuring device 200 and designates a three-dimensional point displayed superimposed on the feature. The feature position measurement unit 213 sets the three-dimensional coordinates indicated by the designated three-dimensional point as the feature position 293 of the feature.
After S150, the measurement method process ends.

The feature position 293 indicates three-dimensional coordinates of each feature (a road sign, a power pole, a road surface, a curb, a kilopost, etc.) located in the measurement target area. Thereby, a three-dimensional map can be generated using the feature position 293.
For example, the technique disclosed in Patent Document 1 can be used as a three-dimensional map generation method and the above-described measurement method (excluding S130).

As described in S120, since the three-dimensional point group 292 is generated based on the attachment position / posture value of the laser scanner, an accurate three-dimensional point group 292 is generated if the attachment position / posture value of the laser scanner is not accurate. The feature position 293 cannot be measured accurately.
The mobile body self-position measuring apparatus 200 can accurately measure the feature position 293 because the mounting position and orientation value of the laser scanner is adjusted to an accurate value in S130.
Next, the details of the laser scanner adjustment process (S130) will be described.

FIG. 7 is a flowchart of the laser scanner adjustment process (S130) in the first embodiment.
The laser scanner adjustment process (S130) in the first embodiment will be described below with reference to FIG.
Each part of the mobile body self-position measuring apparatus 200 performs each process (S131-S139) demonstrated below using CPU.

In each process (S131 to S139) described below, the laser scanner adjusting unit 220 attaches the laser scanner (X _L , Y _L ) based on the three-dimensional point group 292 of the nearby feature B (see FIG. 6). After that, the mounting posture (φ _L , θ _L , ψ _L ) of the laser scanner is adjusted based on the three-dimensional point group 292 of the distant feature A (see FIG. 6).
The closer the distance between the laser scanner and the feature, the smaller the amount of coordinate shift due to the influence of the posture of the laser scanner, and the farther the distance between the laser scanner and the feature, the greater the amount of coordinate deviation due to the influence of the posture of the laser scanner. That is, the main cause of the deviation of the three-dimensional point group 292 of the nearby feature B is the error of the laser scanner mounting position (X _L , Y _L ). In addition, after adjusting the mounting position (X _L , Y _L ) of the laser scanner, the main cause of the deviation of the three-dimensional point group 292 of the distant feature A is the mounting posture of the laser scanner (φ _L , θ _L , ψ _L ). Error.

Among the laser scanner mounting positions, the height Z _L is the difference between the true value and the three-dimensional point group 292 obtained by measuring a feature whose true height value (survey value) is known with the laser scanner. Adjusted based on. The mounting height Z _L of the laser scanner is adjusted in advance.

<S131>
The laser scanner adjustment unit 220 generates a three-dimensional point group 292 (hereinafter referred to as “three-dimensional point group (positive direction)”) generated based on the distance direction point group 283 acquired by the measurement vehicle 100 in the forward path (positive direction). ) To extract the three-dimensional point group of the neighboring feature B as the first neighboring point group. Further, the laser scanner adjustment unit 220 generates a three-dimensional point group 292 (hereinafter referred to as “three-dimensional point group (reverse direction)” generated based on the distance direction point group 283 acquired by the measurement vehicle 100 on the return path (reverse direction). The 3D point group of the nearby feature B is extracted as the second nearby point group.

For example, the laser scanner adjustment unit 220 extracts a three-dimensional point group whose distance from the coordinates of the nearby feature B measured in advance is less than a predetermined value as a first nearby point group and a second nearby point group.
For example, the laser scanner adjustment unit 220 may display the three-dimensional point group 292 on the display device and allow the user to designate the first neighboring point group and the second neighboring point group.
The first neighboring point group and the second neighboring point group may each be one point.

Then, the laser scanner adjustment unit 220 adjusts (ΔX _L , ΔY _L ) the adjustment values (ΔX _L , ΔY _L ) of the laser scanner mounting position (X _L , Y _L ) based on the deviation amount between the first neighboring point group and the second neighboring point group. Is calculated.
Hereinafter, a method for calculating the adjustment values (ΔX _L , ΔY _L ) of the attachment positions (X _L , Y _L ) of the laser scanner will be described.

Figure 8 is a diagram showing a method of calculating the adjustment value [Delta] X _L of the laser scanner in the first embodiment.
FIG. 9 is a diagram showing a method for calculating the adjustment value ΔY _L of the laser scanner in the first embodiment.
8 and 9, a location corresponding to the attachment position and orientation value of the lower surface laser scanner 115 is indicated by a dotted square, and a location where the lower surface laser scanner 115 is actually attached is indicated by a solid square.
The point where the near feature B is actually located is indicated by a black circle (coordinate value P _B ), and the first and second neighboring points are indicated by dotted white circles (coordinate values P _B ′, P _B ″). Show.

As shown in FIG. 8, when the laser scanner (for example, the lower surface laser scanner 115) is shifted by “ΔX _L ” in the x-axis direction (the traveling direction of the measurement vehicle 100), the first neighboring point P _B ′ and the second neighboring point P _B ′ Each of the neighboring points P _B ″ indicates a coordinate value shifted by “ΔX _L ” from the actual coordinate value P _B. For this reason, the amount of deviation between the first neighboring point P _B ′ and the second neighboring point P _B ″ is twice that of “ΔX _L ”.
The laser scanner adjustment unit 220 calculates, as an adjustment value, “ΔX _L ”, which is half the amount of deviation between the first neighboring point P _B ′ and the second neighboring point P _B ″.

Similarly, as shown in FIG. 9, when the laser scanner is shifted by “ΔY _L ” in the y-axis direction (the width direction of the measurement vehicle 100), the first neighboring point P _B ′ and the second neighboring point P _B ″ Is twice as large as “ΔY _L ”.
The laser scanner adjustment unit 220 calculates half the amount of deviation “ΔY _L ” between the first neighboring point P _B ′ and the second neighboring point P _B ″ as an adjustment value.

Returning to FIG. 7, the description of the laser scanner adjustment process (S130) will be continued.
After S131, the process proceeds to S132.

<S132>
The laser scanner adjustment unit 220 adjusts the attachment position (X _L , Y _L ) of the laser scanner by adding (or subtracting) the adjustment values (ΔX _L , ΔY _L ) calculated in S131.
After S132, the process proceeds to S133.

<S133>
Three-dimensional point group generation unit 211, the mounting position in S132 _{(X L, Y} L) based on the mounting position and orientation values of the adjusted laser scanner, also to generate a three-dimensional point group 292 and S120.
After S133, the process proceeds to S134.

<S134>
The laser scanner adjustment unit 220 extracts the first neighboring point group and the second neighboring point group from the three-dimensional point group 292 generated in S133, and coordinates of the extracted first neighboring point group and second neighboring point group. Determine whether the values match. Here, “match” may mean either “complete match of values” or “difference is equal to or less than a predetermined value”.
If the coordinate values of the first neighboring point group and the second neighboring point group match (YES), the process proceeds to S135.
If the coordinate values of the first neighboring point group and the second neighboring point group do not match (NO), the process returns to S131 and S131 to S131 until the coordinate values of the first neighboring point group and the second neighboring point group match. S134 is repeated.

<S135>
Similarly to S131, the laser scanner adjustment unit 220 extracts the three-dimensional point group of the distant feature A (see FIG. 6) from the three-dimensional point group (positive direction) as the first far point group, and also the three-dimensional point group. The three-dimensional point group of the distant feature A is extracted as the second distant point group from (reverse direction). Each of the first far point group and the second far point group may be one point.
The laser scanner adjustment unit 220 adjusts (Δφ _L , Δθ _L ) the adjustment values (Δφ _L , Δθ _L ) of the laser scanner mounting posture (φ _L , θ _L , ψ _L ) based on the shift amount between the first far point group and the second far point group. , Δψ _L ).
Hereinafter, a method for calculating the adjustment values (Δφ _L , Δθ _L , Δψ _L ) of the mounting posture (φ _L , θ _L , ψ _L ) of the laser scanner will be described.

Figure 10 is a diagram showing a method of calculating the adjustment value [Delta] [phi _L of the laser scanner in the first embodiment.
FIG. 11 is a diagram illustrating a method of calculating the adjustment value Δθ _L of the laser scanner in the first embodiment.
FIG. 12 is a diagram illustrating a method of calculating the adjustment value Δψ _L of the laser scanner in the first embodiment.
10 to 12, a location corresponding to the attachment position and orientation value of the lower surface laser scanner 115 is indicated by a dotted square, and a location where the lower surface laser scanner 115 is actually attached is indicated by a solid square.
The point where the distant feature A is actually located is indicated by a black circle (coordinate value P _A ), and the first far point and the second far point are indicated by dotted white circles (coordinate values P _A ′, P _A ″). Show.

As shown in FIG. 10, when the laser scanner (for example, the lower surface laser scanner 115) is rotated by “Δφ _L ” around the x axis, the first far point P _A ′ and the second far point P _A ′ are rotated. 'indicates a coordinate value obtained by "[Delta] [phi _L" rotate actual coordinate values P _a as the center point of each laser scanner with. For this reason, the amount of shift in the y-axis direction between the first far point P _A ′ and the second far point P _A ″ is twice the movement amount “y (Δφ _L )” for “Δφ _L ” rotation. Further, the shift amount in the z-axis direction between the first far point P _A ′ and the second far point P _A ″ is twice the movement amount “z (Δφ _L )” of “Δφ _L ” rotation.
The laser scanner adjustment unit 220 adjusts the amount of adjustment “Δφ” based on “y (Δφ _L )” and “z (Δφ _L )” which are half of the deviation between the first far point P _A ′ and the second far point P _A ″. _L "is calculated.

Similarly, as shown in FIG. 11, when the laser scanner rotates by “Δθ _L ” around the y axis, the amount of deviation between the first far point P _A ′ and the second far point P _A ″ is This is twice the value of “x (Δθ _L )”.
The laser scanner adjustment unit 220 calculates an adjustment amount “Δθ _L ” based on a half “x (Δθ _L )” of the shift amount between the first far point P _A ′ and the second far point P _A ″.

Further, as shown in FIG. 12, when the laser scanner is rotated by “Δψ _L ” around the z axis, the xy coordinates of the first far point P _A ′ and the second far point P _A ″ are “ [Delta] [phi] _L ".
The laser scanner adjustment unit 220 calculates an adjustment amount “Δψ _L ” based on the amount of deviation between the first far point P _A ′ and the second far point P _A ″.

Returning to FIG. 7, the description of the laser scanner adjustment process (S130) will be continued.
After S135, the process proceeds to S136.

<S136>
The laser scanner adjustment unit 220 adds (or subtracts) the adjustment values (Δφ _L , Δθ _L , Δψ _L ) calculated in S135 to adjust the mounting position (φ _L , θ _L , ψ _L ) of the laser scanner.
After S136, the process proceeds to S137.

<S137>
The three-dimensional point group generation unit 211 generates a three-dimensional point group 292 in the same manner as in S120, based on the attachment position / posture value of the laser scanner whose attachment posture (φ _L , θ _L , ψ _L ) has been adjusted in S136.
After S137, the process proceeds to S138.

<S138>
The laser scanner adjustment unit 220 extracts the first far point group and the second far point group from the three-dimensional point group 292 generated in S137, and the coordinates of the extracted first far point group and second far point group. Determine whether the values match. Here, the term “match” may be “complete match of values” or “difference is equal to or less than a predetermined value”.
If the coordinate values of the first far point group and the second far point group match (YES), the process proceeds to S139.
If the coordinate values of the first far point group and the second far point group do not match (NO), the process returns to S135, and S135 to S135 until the coordinate values of the first far point group and the second far point group match. S138 is repeated.

<S139>
The laser scanner adjustment unit 220 determines whether the coordinate values of the 3D point group (forward direction) and the 3D point group (reverse direction) generated in S137 match. Here, the term “match” may be “complete match of values” or “difference is equal to or less than a predetermined value”.
If the coordinate values of the three-dimensional point group (forward direction) and the three-dimensional point group (reverse direction) match (YES), the laser scanner adjustment process (S130) ends.
If the coordinate values of the three-dimensional point group (forward direction) and the three-dimensional point group (reverse direction) do not match (NO), the process returns to S131, and the three-dimensional point group (forward direction) and the three-dimensional point group (reverse) S131 to S139 are repeated until the coordinate value matches (direction).

As described above, the mobile unit self-position measuring device 200 does not require any special facilities or equipment, and can easily adjust the laser scanner mounting position and orientation value easily based on the acquired data of the measuring vehicle 100. .
Thereby, the mobile body self-position measuring apparatus 200 can generate an accurate three-dimensional point group 292 and calculate an accurate feature position 293.

Embodiment 2. FIG.
A mode of adjusting the mounting yaw angle ψ of the laser scanner by a method different from that of the first embodiment will be described.
Hereinafter, items different from the first embodiment will be mainly described. Matters whose description is omitted are the same as those in the first embodiment.

FIG. 13 is a diagram illustrating a method of calculating the adjustment value Δψ of the laser scanner in the second embodiment.
A method of calculating the adjustment value Δψ of the laser scanner in the second embodiment will be described below based on FIG.
The notation of FIG. 13 is the same as that of FIG. However, the measurement vehicle 100 traveling in the reverse direction (return) is indicated by a dotted line, and a third far point described later is indicated by a dotted white circle (coordinate value P _A ″).

As described in FIG. 1, the laser scanner is attached obliquely (for example, the lower surface laser scanner 115 is obliquely downward) to measure features having different heights (road surface, road sign, etc.), and the neck is placed on the left and right. While getting off, measure by emitting laser.
When the feature is located at the same height as the laser scanner, the feature can be measured by the laser scanner when the measurement vehicle 100 moves and the feature comes to the side of the laser scanner. is there.

As shown in FIG. 13, when a feature (for example, a distant feature A) (coordinate P _A ) is measured directly beside a laser scanner (for example, the bottom surface laser scanner 115), the laser scanner moves “Δψ around the z axis. Even if the rotation is shifted, the three-dimensional point (forward direction) and the three-dimensional point (reverse direction) indicate the same coordinate P ′, and no shift occurs.
However, when measuring from two directions (positive direction, second direction) across the feature (coordinate P _A ), the first far point (coordinate P _A ′) based on the measured value of one (positive direction) and the other (coordinate P _A ′) There is a deviation from the third far point (coordinate P _A ″) based on the measured value in the second direction.
The laser scanner adjustment unit 220 calculates an adjustment value Δψ _L of the attachment yaw angle ψ _L of the laser scanner based on the amount of deviation between the coordinates P _A ′ and the coordinates P _A ″.

The flow of the measurement method is the same as that of the first embodiment (see FIG. 5).
However, in S110 of the measurement method, the user reciprocates the measurement vehicle 100 to acquire various data (FIGS. 6 (1) (2)), and the measurement vehicle on the opposite side across the distant feature A. 100 is run (FIG. 6 (3)). The direction of travel on the opposite side across the distant feature A (second direction) may be the forward direction or the reverse direction. Further, the distance between the measurement vehicle 100 and the distant feature A is preferably equal in the positive direction and the second direction.

FIG. 14 is a flowchart of the laser scanner adjustment process (S130) in the second embodiment.
The laser scanner adjustment process (S130) in the second embodiment will be described below with reference to FIG.
The flowchart shown in FIG. 14 is obtained by changing S135 to S137 and S139 of Embodiment 1 (see FIG. 7) to S135a to S137a and S139 ′, and adding S135b to S137b.
Hereinafter, S135a to S137a, S135b to S137b, and S139 ′ will be mainly described.

<S131 to S134>
As in the first embodiment, the mounting position of the laser scanner is adjusted.
After S131 to S134, the process proceeds to S135a.

<S135a to S136a>
The laser scanner adjustment unit 220 calculates an adjustment value based on the shift amount between the first far point group and the second far point group (S135a), similarly to S135 to S136 of the first embodiment, and attaches the laser scanner. The posture (φ _L , θ _L ) is adjusted (S135a).
In S135a to S136a, the yaw ψ is not adjusted in the mounting posture of the laser scanner.
After S135a to S136a, the process proceeds to S137a.

<S137a>
The three-dimensional point group generation unit 211 generates a three-dimensional point group 292, similar to S120, based on the mounting position / posture value of the laser scanner whose mounting posture (φ _L , θ _L ) has been adjusted in S136a.
After S137a, the process proceeds to S138.

<S138>
As in the first embodiment, the laser scanner adjustment unit 220 determines whether the coordinate values of the first far point group and the second far point group match.
If the coordinate values of the first far point group and the second far point group match (YES), the process proceeds to S135b.
When the coordinate values of the first far point group and the second far point group do not match (NO), the process returns to S135a, and until the coordinate values of the first far point group and the second far point group match, S135a˜ S138 is repeated.

<S135b>
Similarly to S135 of the first embodiment, the laser scanner adjustment unit 220 extracts the three-dimensional point group of the distant feature A from the three-dimensional point group (positive direction) as the first far point group, and also the three-dimensional point group. The three-dimensional point group of the distant feature A from (second direction) is extracted as the third distant point group. The three-dimensional point group (second direction) is generated based on the distance direction point group 283 measured by the measurement vehicle 100 from the second direction on the opposite side across the distant feature A with respect to the forward direction and the reverse direction. A three-dimensional point group 292 is shown (see FIGS. 6 and 13). Each of the first far point group and the third far point group may be one point.
The laser scanner adjustment unit 220 calculates an adjustment value (Δψ _L ) of the laser scanner attachment yaw ψ _L based on the amount of deviation between the first far point group and the third far point group.

As shown in FIG. 13, the laser scanner (for example, the lower surface laser scanner 115) rotates with a shift of “Δψ _L ” around the z axis, and the distance between the measurement vehicle 100 and the distant feature A is the positive direction and the second direction. Are equal to each other, the amount of deviation between the first far point P _A ′ and the third far point P _A ″ is twice that of “x (Δψ _L )” and “y (Δψ _L )”.
The laser scanner adjustment unit 220 adjusts the amount of adjustment “Δψ” based on the half “x (Δψ _L )” and “y (Δψ _L )” of the amount of deviation between the first far point P _A ′ and the second far point P _A ″. _L "is calculated.

  After S135b, the process proceeds to S136b.

<S136b>
Laser scanner adjustment unit 220 adds an adjustment value [Delta] [phi] _L calculated in S135b (or subtracted) to adjust the mounting yaw [psi _L of the laser scanner.
After S136b, the process proceeds to S137b.

<S137b>
Three-dimensional point group generation unit 211, based on the mounting position and orientation values of the mounting yaw [psi _L laser scanner which is adjusted in S136b, similarly to generate a three-dimensional point group 292 and S120.
After S137b, the process proceeds to S139 ′.

<S139 '>
The laser scanner adjustment unit 220 determines whether the coordinate values of the three-dimensional point group (forward direction), the three-dimensional point group (reverse direction), and the three-dimensional point group (second direction) generated in S137b match. Here, the term “match” may be “complete match of values” or “difference is equal to or less than a predetermined value”.
When the coordinate values of the three-dimensional point group (forward direction), the three-dimensional point group (reverse direction), and the three-dimensional point group (second direction) match (YES), the laser scanner adjustment process (S130) ends.
When the coordinate values of the three-dimensional point group (forward direction), the three-dimensional point group (reverse direction), and the three-dimensional point group (second direction) do not match (NO), the process returns to S131, and the three-dimensional point group ( S131 to S139 ′ are repeated until the coordinate values of the forward direction), the 3D point group (reverse direction), and the 3D point group (second direction) match.

  According to the second embodiment, even when the feature is located at the same height as the laser scanner, the attachment position / posture value of the laser scanner can be accurately adjusted.

Embodiment 3 FIG.
A mode of adjusting the attachment position / posture value of the camera together with the laser scanner will be described.
Hereinafter, items different from the first and second embodiments will be mainly described. Matters whose description is omitted are the same as in the first and second embodiments.

FIG. 15 is a functional configuration diagram of the moving body self-position measuring apparatus 200 according to the third embodiment.
A functional configuration of mobile body self-position measuring apparatus 200 according to Embodiment 3 will be described below with reference to FIG.

  In addition to the functional configuration of the first and second embodiments (see FIG. 3), the moving body self-position measuring apparatus 200 includes a camera adjustment unit 230 (an example of a camera position / posture value error specifying unit and a camera position / posture value correction unit). Prepare.

Based on the three-dimensional point group 292 projected on the camera image 281, the camera adjustment unit 230 shifts the position and orientation values of the front camera 111 and the road surface camera 112 attached to the measurement unit 110 of the measurement vehicle 100. Is specified using the CPU. Position and orientation values (for example, design values) of the front camera 111 and the road surface camera 112 attached to the measurement unit 110 of the measurement vehicle 100 are stored in advance in an arbitrary storage unit (storage device). The camera adjustment unit 230 identifies the difference between the stored position / orientation value and the actual position / orientation value as a deviation amount.
Furthermore, the camera adjustment unit 230 corrects (adjusts) the stored position / orientation value using the CPU based on the specified shift amount.

FIG. 16 is a flowchart illustrating a measurement method according to the third embodiment.
The measurement method in Embodiment 3 is demonstrated below based on FIG.
The flowchart shown in FIG. 16 is obtained by adding S160 to the first embodiment (see FIG. 5).
Hereinafter, S160 will be described, and description of other processes (S110 to S150) will be omitted.

<S160>
The mobile body self-position measuring apparatus 200 adjusts the camera attachment position / posture value based on the camera image 281 and the three-dimensional point group 292.

FIG. 17 is a flowchart of camera adjustment processing (S160) in the third embodiment.
The laser scanner adjustment process (S160) in the third embodiment will be described below with reference to FIG.
Each part of the mobile body self-position measuring apparatus 200 performs each process (S161-S169b) demonstrated below using CPU.

<S161>
The three-dimensional point group projection unit 212 projects the three-dimensional point group 292 onto the camera image 281 based on the camera attachment position and orientation values, as in S150.
After S161, the process proceeds to S162.

<S162>
The camera adjustment unit 230 extracts the 3D point group of the nearby feature B as the nearby point group from the 3D point group 292 projected on the camera image 281 in S161.
For example, the camera adjustment unit 230 extracts a three-dimensional point group whose distance from the coordinates of the nearby feature B measured in advance is less than a predetermined value as the nearby point group. Further, for example, the camera adjustment unit 230 may display the three-dimensional point group 292 on the display device and allow the user to specify the neighboring point group. The neighboring point group may be one point.
Furthermore, the camera adjustment unit 230 identifies the pixel in which the nearby feature B is captured from the camera video 281. For example, the camera adjustment unit 230 determines the pixel position (two-dimensional coordinates on the camera image 281) of the neighboring feature B based on the attachment position and orientation value of the camera and the coordinates of the neighboring feature B measured in advance. calculate. For example, the user may specify a pixel.
The camera adjustment unit 230 adjusts the camera attachment position (X _C , Y _C , Z _C ) based on the amount of deviation between the pixel on which the neighboring point group is projected and the pixel on which the neighboring feature B is reflected ( ΔX _C , ΔY _C , ΔZ _C ) are calculated.

FIG. 18 is a diagram illustrating the relationship between the camera position shift, the camera image 281, and the neighboring point group in the third embodiment.
FIG. 18 shows a camera image 281 in which the gate (downward U-shaped) located in front of the measurement vehicle 100 is copied, and a neighboring point group (black circle) projected on the camera image 281 (measurement of the gate). It shows.
Hereinafter, the coordinate system of the camera image 281 representing the pixel position is referred to as a uv coordinate system.

(1) When the camera is displaced in the z-axis direction (the height direction of the measurement vehicle 100), the neighboring point group is projected with a shift of “Δv” up and down with respect to the gate shown in the camera image 281.
(2) When the camera is deviated in the x-axis direction (the traveling direction of the measurement vehicle 100), the neighboring point group is projected with different sizes (Δu, Δv) on the gate shown in the camera image 281.
(3) When the camera is shifted in the y-axis direction (the width direction of the measurement vehicle 100), the neighboring point group is projected with a shift of “Δu” to the left and right with respect to the gate shown in the camera image 281.

In S162 (FIG. 17), the camera adjustment unit 230 determines whether the camera attachment position (Xu, Δv) is based on the shift amount (Δu, Δv) between the pixel on which the neighboring point group is projected and the pixel on which the neighboring feature B is reflected. calculated _{C, Y} C, the adjustment value of _{Z C) (ΔX C, ΔY} C, a [Delta] Z _C).
After S162, the process proceeds to S163.

<S163>
The camera adjustment unit 230 adds (or subtracts) the adjustment values (ΔX _C , ΔY _C , ΔZ _C ) calculated in S162 to adjust the camera attachment position (X _C , Y _C , Z _C ).
After S163, the process proceeds to S164.

<S164>
The three-dimensional point group projection unit 213 converts the three-dimensional point group 292 into the camera image 281 based on the attachment position / posture value of the camera whose attachment position (X _C , Y _C , Z _C ) has been adjusted in S163. Project.
After S164, the process proceeds to S165.

<S165>
In step S164, the camera adjustment unit 230 determines whether the pixel on which the neighboring point group is projected matches the pixel in which the neighboring feature B is reflected. Here, the term “match” may mean either “the same pixel” or “a pixel separated by a predetermined amount or less”.
When the pixel on which the neighboring point group is projected matches the pixel in which the neighboring feature B is reflected (YES), the process proceeds to S166.
If the pixel on which the neighboring point cloud is projected does not match the pixel on which the neighboring feature B appears (NO), the process returns to S161, and the pixel on which the neighboring point cloud is projected and the neighboring feature B are reflected. S161 to S165 are repeated until the pixel matches.

<S166>
Similarly to S162, the camera adjustment unit 230 extracts the three-dimensional point group of the distant feature A from the three-dimensional point group 292 projected on the camera image 281 in S164 as a far point group.
Further, the camera adjustment unit 230 identifies pixels in which the distant feature A is captured from the three-dimensional point group 292, similarly to S162.
The camera adjustment unit 230 adjusts the camera mounting posture (φ _C , θ _C , ψ _C ) based on the shift amount between the pixel on which the far point group is projected and the pixel on which the far object A is reflected ( Δφ _C , Δθ _C , Δψ _C ) are calculated.

FIG. 19 is a relationship diagram between the camera posture deviation, the camera image 281, and the far point group in the third embodiment.
The way of viewing FIG. 19 is the same as FIG.

(1) When the camera rotates around the x axis (roll), the far point group is projected at a position rotated (Δu, Δv) with respect to the gate shown in the camera image 281.
(2) When the camera rotates around the y-axis (pitch), the far point group is projected with a deviation (Δu, Δv) with respect to the gate shown in the camera image 281.
(3) When the camera rotates around the z-axis (yaw), the far point group is projected with a deviation (Δu, Δv) with respect to the gate shown in the camera image 281.

In S166 (FIG. 17), the camera adjustment unit 230 determines the camera mounting posture (φ) based on the shift amount (Δu, Δv) between the pixel on which the distant point cloud is projected and the pixel on which the distant feature A is reflected. _{C, θ C,} the adjustment value of _{ψ C) (Δφ C, Δθ} C, calculates the [Delta] [phi] _C).
After S166, the process proceeds to S167.

<S167>
The camera adjustment unit 230 adds (or subtracts) the adjustment values (Δφ _C , Δθ _C , Δψ _C ) calculated in S166 to adjust the camera mounting posture (φ _C , θ _C , ψ _C ).
After S167, the process proceeds to S168.

<S168>
The three-dimensional point group projection unit 213 converts the three-dimensional point group 292 into the camera image 281 based on the camera attachment position / posture value whose attachment position (φ _C , θ _C , ψ _C ) has been adjusted in S167. Project.
After S168, the process proceeds to S169a.

<S169a>
In step S168, the camera adjustment unit 230 determines whether the pixel on which the far point group is projected matches the pixel on which the far feature A is reflected. Here, the term “match” may mean either “the same pixel” or “a pixel separated by a predetermined amount or less”.
When the pixel on which the distant point cloud is projected matches the pixel on which the distant feature A is reflected (YES), the process proceeds to S169b.
If the pixel on which the far point cloud is projected does not match the pixel on which the far feature A appears (NO), the process returns to S166, and the pixel on which the far point cloud is projected and the far feature A appear. S166 to S169a are repeated until the pixel matches.

<S169b>
In step S168, the camera adjustment unit 230 determines whether the pixel on which the three-dimensional point group 292 is projected matches the pixel in which each feature is reflected. Here, the term “match” may mean either “the same pixel” or “a pixel separated by a predetermined amount or less”.
When the pixel on which the three-dimensional point group 292 is projected matches the pixel in which each feature is reflected (YES), the camera adjustment process (S160) ends.
If the pixel on which the 3D point group 292 is projected and the pixel on which the feature is shown do not match (NO), the process returns to S161, and the pixel on which the 3D point group 292 is projected and the feature are shown. S161 to S169b are repeated until the pixel matches.

As described above, the mobile body self-position measuring device 200 does not require any special facility or equipment, and can easily adjust the camera mounting position / posture value accurately based on the acquired data of the measurement vehicle 100.
Thereby, the mobile body self-position measuring apparatus 200 can accurately project the three-dimensional point group 292 onto the camera image 281 and calculate the accurate feature position 293.

Embodiment 4 FIG.
An embodiment using a laser scanner that does not output the measurement time of the distance direction point will be described.
Hereinafter, items different from the first to third embodiments will be mainly described. Matters whose description is omitted are the same as those in the first to third embodiments.

The configuration of measurement vehicle 100 in the fourth embodiment is the same as the configuration of the first to third embodiments (see FIGS. 1 and 2).
However, the laser scanners (the upper surface laser scanner 114 and the lower surface laser scanner 115) may not output the measurement time of the distance direction. That is, the price of the laser scanner can be reduced, and the measurement vehicle 100 can be configured at low cost.
The wiring relay box 117 outputs the measurement time of the distance direction instead of the laser scanner. The measurement time that can be output by the wiring relay BOX 117 is not the actual measurement time but the time when the distance direction is output from the laser scanner. That is, at the measurement time output by the wiring relay BOX 117 and the actual measurement time, the distance azimuth is calculated after the laser scanner observes the laser reflected by the feature, and the calculated distance azimuth is output to the wiring relay BOX 117. Deviation occurs only by the time required until.
The mobile body self-position measuring device 200 has a function of adjusting the deviation of the measurement time.

FIG. 20 is a functional configuration diagram of the mobile unit self-position measuring apparatus 200 according to the fourth embodiment.
A functional configuration of the mobile unit self-position measuring apparatus 200 according to Embodiment 4 will be described below with reference to FIG.

  In addition to the functional configuration of the third embodiment (see FIG. 15), the moving body self-position measuring apparatus 200 includes a laser measurement time adjusting unit 240 (an example of a measurement time error specifying unit and a measurement time correcting unit).

Based on the three-dimensional point group 292, the laser measurement time adjustment unit 240 identifies an error in the measurement time set in the distance / azimuth point group 283 using the CPU.
Further, the laser measurement time adjustment unit 240 corrects (adjusts) the measurement time set in the distance and azimuth point group 283 based on the identified error using the CPU.

FIG. 21 is a flowchart showing a measurement method according to the fourth embodiment.
A measurement method according to Embodiment 4 will be described below with reference to FIG.
The flowchart shown in FIG. 21 is obtained by adding S170 to S190 to the third embodiment (see FIG. 16).
Hereinafter, S170 to S190 will be described, and description of other processes (S110 to S160) will be omitted.

<S170>
The user travels in the same area at different speeds in the measurement vehicle 100 to acquire various data, and stores the acquired various data in the acquired data storage unit 280 of the mobile body self-position measuring device 200.
After S170, the process proceeds to S180.

<S180>
The mobile body self-position measuring apparatus 200 generates the three-dimensional point group 292 based on the various data acquired in S170 as in S120.
After S180, the process proceeds to S190.

<S190>
The mobile body self-position measuring device 200 adjusts the measurement time of the distance azimuth point group 283 based on the three-dimensional point group 292 generated in S180.
Hereinafter, among the distance azimuth points 283 acquired at different travel speeds in S170, the distance azimuth points 283 having the slower travel speed are referred to as “first distance azimuth points”, and the distance azimuth points having the higher travel speed. 283 is referred to as a “second distance azimuth point group”.
In S190, the laser measurement time adjustment unit 240 sets the three-dimensional point group 292 (hereinafter referred to as “first three-dimensional point group”) generated based on the first distance direction point group and the second distance direction point group. The adjustment time of the measurement time of the distance direction point group 283 is calculated based on the amount of deviation from the three-dimensional point group 292 (hereinafter referred to as “second 3D point group”) generated based on the calculated adjustment time. The measurement time of the distance direction point group 283 is adjusted based on the above.

FIG. 22 is a diagram illustrating a method of calculating the adjustment time Δt of the measurement time of the distance direction point group 283 in the fourth embodiment.
In FIG. 22, the measurement vehicle 100 that performs measurement at the time of stopping is indicated by a solid line, and the measurement vehicle 100 that performs measurement at the time of travel is indicated by a dotted line. In addition, the first three-dimensional point is “P”, the second three-dimensional point is “P ′”, and the measurement time error of the distance azimuth point group 283 is “Δt”.

As shown in FIG. 22, the measuring vehicle 100 traveling at the speed v moves by “d (Δt)” (= v × Δt) during the measurement time error Δt.
Therefore, the x coordinate between the first three-dimensional point P based on the distance azimuth point measured at the time of stopping and the second three-dimensional point P ′ based on the distance azimuth point measured at the time of traveling at the speed v is “d (Δt ) ”.

In S190 (see FIG. 21), the laser measurement time adjustment unit 240 divides the deviation “d (Δt)” between the first three-dimensional point P and the second three-dimensional point P ′ by the travel speed difference “v”. The calculated value “Δt” is calculated as the adjustment time.
Then, the laser measurement time adjustment unit 240 changes each measurement time set in the distance azimuth point group 283 to a time before the adjustment time Δt.
After S190, S110 to S160 described in the first to third embodiments are executed.

As described above, the mobile body self-position measuring apparatus 200 correctly adjusts the measurement time of the distance azimuth point group 283 even if the measurement time of the distance azimuth point group 283 is inaccurate because the laser scanner cannot output the measurement time. can do.
Thereby, the mobile body self-position measuring apparatus 200 can generate an accurate three-dimensional point group 292 and calculate an accurate feature position 293.

In the above embodiments, the following calibration method has been described.
The laser scanner calibration value (adjustment value) is calculated so that the point cloud of the same target overlaps as a result. The basis of this idea is that if the position and orientation are set correctly, the same position is calculated as a result of irradiating the same target even if the traveling direction or position changes. Conversely, the fact that the same position is not calculated as a result of irradiating the same target means that the information is incorrect. Further, it is a method of focusing so that the deviation of data differs when the attachment position is collapsed and when the attachment posture is collapsed, and the deviation is minimized.

When comparing the data (three-dimensional points of the target) between the forward path and the return path, the following characteristics occur.
・ If the position, orientation, and measurement time of the laser scanner are calibrated perfectly, the same 3D point of the target will show the same position regardless of the measurement direction and speed.
• If the laser scanner is misaligned, the 3D point of the target will be shifted by the same distance whether it is near or far from the vehicle.
・ When the position of the laser scanner is collapsed, the 3D point of the target is less displaced near, but the displacement becomes larger as it is further away.
• If the laser scanner is misaligned, the 3D point of the target will shift in the same direction on the left and right of the vehicle.
・ When the posture of the laser scanner is broken, the 3D point of the target is shifted in the opposite direction on the left and right of the vehicle.
-When the measurement time of the laser scanner is shifted, the measurement speed of the three-dimensional point of the target cannot be reduced, but the shift amount increases as the measurement speed increases.
A value that is converged so that a plurality of measured point clouds overlap most using the above characteristics is set as a calibration value.

  However, in the case of a laser scanner, the yaw angle cannot be estimated on the forward path and the return path on the same path. This is because when the yaw angle is incorrect, the point cloud is shifted in the same direction. It is possible to estimate the yaw angle by using data traveling in the same direction (possible in the reverse direction) across the target (see Embodiment 2).

This technique can be used not only for laser scanner calibration but also for camera calibration.
The basic idea is the same: X, Y, and Z are corrected if the distance is the same in both the vicinity and the distance, and the posture angle is corrected if the distance is the same but there is a shift in the distance. To do.

Every time each parameter (position, orientation, time) is corrected, the three-dimensional point group is recalculated, and it is checked whether it is more consistent or conversely the amount of deviation is increased, and the parameters are driven. Complete when the degree of coincidence is sufficient.
If the parameters are individually changed, how the shift amount changes is checked in advance, and if the parameters are changed based on the result of checking in advance, it becomes easier to drive the parameters.

This technique has the following effects, for example.
-The number of calibration steps can be greatly reduced without the need for special facilities and equipment.
-To what extent the resulting measurement data is accurate can be confirmed during calibration.
Since the calibration method is simple, in some cases, this calibration is performed for each measurement, so that individual measurement accuracy can be improved.
-Since it is a software method, automatic calibration can be performed by building dedicated software.

  DESCRIPTION OF SYMBOLS 100 Measuring vehicle, 110 Measuring unit, 111 Front camera, 112 Road surface camera, 113 GPS, 114 Upper surface laser scanner, 115 Lower surface laser scanner, 116 IMU, 117 Wiring relay BOX, 190 Unit memory | storage part, 200 Mobile body self Position measurement device, 210 Position and orientation determination unit, 211 3D point group generation unit, 212 3D point group projection unit, 213 Feature position measurement unit, 220 Laser scanner adjustment unit, 230 Camera adjustment unit, 240 Laser measurement time adjustment unit 280 acquisition data storage unit, 281 camera image, 282 GPS data, 283 distance azimuth point group, 284 angular velocity, 290 measurement data storage unit, 291 vehicle position and orientation, 292 three-dimensional point group, 293 feature position, 901 display device, 902 Keyboard, 903 904 FDD, 905 CDD, 906 printer device, 907 scanner device, 908 microphone, 909 speaker, 911 CPU, 912 bus, 913 ROM, 914 RAM, 915 communication board, 920 magnetic disk device, 921 OS, 922 window system, 923 programs, 924 files.

Claims (13)

A distance azimuth indicating a distance from the specific body to the feature measured by the laser scanner attached to the specific body and an orientation from the specific body to the feature measured by the laser scanner; and A feature coordinate calculation unit that calculates the coordinates of the feature using a CPU (Central Processing Unit) based on a laser position and orientation value indicating the attachment position and the attachment orientation of the laser scanner;
First coordinates calculated by the feature coordinate calculation unit based on a first distance direction measured from a first direction for a specific feature by the laser scanner, and the specific feature by the laser scanner. Using the CPU, the difference from the second coordinate calculated by the feature coordinate calculation unit is calculated based on the second distance direction measured from the second direction for the feature, and the calculated difference A laser position / orientation value error identification unit that identifies an error included in the laser position / orientation value based on the CPU,
A measurement apparatus comprising: a laser position and orientation value correction unit that corrects the laser position and orientation value using a CPU based on an error specified by the laser position and orientation value error specifying unit. The laser position / orientation value error specifying unit uses a feature located far away from a plurality of features at different positions as the specific feature, and the laser scanner out of errors included in the laser position / orientation value The measuring apparatus according to claim 1, wherein an error included in the mounting posture is specified. The laser position / orientation value error specifying unit has a roll opposite to the first direction as the second direction and represents a part of the mounting posture of the laser scanner among errors included in the laser position / orientation value. 3. The measuring apparatus according to claim 1, wherein at least one of an error included in a corner and an error included in a pitch angle representing a part of the mounting posture of the laser scanner is specified. . The laser position / orientation value error specifying unit sets the laser position / orientation value with one of the directions from two locations sandwiching the specific feature as the first direction and the other as the second direction. 4. The measurement apparatus according to claim 1, wherein an error included in a yaw angle representing a part of the mounting posture of the laser scanner is specified among errors included in the laser scanner. 5. The laser position and orientation value error specifying unit further includes a feature located in the vicinity among a plurality of features at different positions as the specific feature, and among the errors included in the laser position and orientation value, The measurement apparatus according to claim 2, wherein an error included in a mounting position of the laser scanner is specified. 6. The laser position and orientation value error specifying unit specifies an error included in an attachment position of the laser scanner, with a direction opposite to the first direction as the second direction. Measuring device. The laser position / posture value error specifying unit specifies an error included in the mounting posture of the laser scanner after specifying an error included in the mounting position of the laser scanner. The measuring device according to any one of the above. The feature coordinate calculation unit calculates the coordinates of the feature based on the distance azimuth, the laser position and orientation value, and the measurement time of the distance azimuth,
The measuring device further includes:
The coordinates calculated by the feature coordinate calculation unit based on the distance direction measured for the second specific feature by the laser scanner when the specific body is traveling at a specific speed; and Calculated by the feature coordinate calculation unit based on the distance direction measured for the second specific feature by the laser scanner when the specific object is traveling at a speed faster than the specific speed. A measurement time error specifying unit that calculates a difference from the coordinates using a CPU and specifies an error included in the measurement time of the distance direction based on the calculated difference using the CPU;
8. A measurement time correction unit that corrects the measurement time of the distance direction using a CPU based on the error specified by the measurement time error specifying unit. The measuring device described. The measuring device further includes:
A camera position / posture value indicating a mounting position of a camera attached to the specific body and a mounting posture of the camera, corresponding to the coordinates of the third specific feature calculated by the feature coordinate calculation unit. Based on the above, a laser point projecting unit that projects using a CPU onto an image obtained by copying the third specific feature by the camera;
A deviation amount between the laser point projected on the image by the laser point projection unit and the third specific feature reflected in the image is specified using a CPU, and the deviation amount is determined based on the specified deviation amount. A camera position / orientation value error identifying unit that identifies an error included in the camera position / orientation value using a CPU;
9. A camera position / orientation value correcting unit that corrects the camera position / orientation value using a CPU based on an error specified by the camera position / orientation value error specifying unit. The measuring device according to any one of the above. The camera position / orientation value error specifying unit uses a feature located far away from a plurality of features at different positions as the third specific feature, and includes an error included in the camera position / orientation value. The measuring apparatus according to claim 9, wherein an error included in the mounting posture of the camera is specified. The camera position / orientation value error specifying unit sets a feature located in the vicinity among a plurality of features at different positions as the third specific feature, and includes an error included in the camera position / orientation value. The measurement apparatus according to claim 9, wherein an error included in a mounting position of the camera is specified. The feature coordinate calculation unit indicates a distance from the specific body to the feature measured by the laser scanner attached to the specific body and a direction from the specific body to the feature measured by the laser scanner. Feature coordinate calculation processing for calculating the coordinates of the feature using a CPU (Central Processing Unit) based on the orientation, the laser position and orientation value indicating the attachment position of the laser scanner and the attachment orientation of the laser scanner And
The first coordinate calculated by the feature coordinate calculation unit based on the first distance direction measured from the first direction for the specific feature by the laser scanner. And a difference between the second coordinate calculated by the feature coordinate calculation unit based on a second distance direction measured from the second direction with respect to the specific feature by the laser scanner. Performing a laser position and orientation value error specifying process for specifying an error included in the laser position and orientation value based on the calculated difference using a CPU,
The laser position and orientation value correction unit performs a laser position and orientation value correction process for correcting the laser position and orientation value using a CPU based on the error specified by the laser position and orientation value error specifying unit. Laser position and orientation value correction method for the device.   A laser position and orientation value correction program for causing a measuring apparatus to execute the laser position and orientation value correction method according to claim 12.