JP6880822B2 - Equipment, mobile equipment and methods - Google Patents

Description

The present invention relates to devices, mobile devices and methods.

Conventionally, there are known devices that acquire three-dimensional information around a moving body, generate an environmental map around the moving body, and measure information about the moving body such as the position, posture, and speed of the moving body (). For example, see Patent Document 1).

However, in the apparatus disclosed in Patent Document 1, there is room for improvement in generating an environmental map around the moving body and measuring information about the moving body in a stable and accurate manner.

The present invention is a device mounted on a moving body, and is a first acquisition means for acquiring three-dimensional information around the moving body and an area in which the first acquisition means acquires three-dimensional information. steric and second acquisition means for acquiring at least a portion of the 3-dimensional information of the first acquisition region, acquired by the first acquisition unit, a feature point definitive three-dimensional information of the first acquisition area A second acquisition area, which is an area in which the second acquisition means acquires three-dimensional information in the first acquisition area, is classified into a feature point of an object and a feature point of a non-three-dimensional object, and based on the classification result. The control means for controlling the above, the three-dimensional information of the first acquisition area acquired by the first acquisition means, and the three-dimensional information of the second acquisition area acquired by the second acquisition means. The control means includes means for generating an environmental map around the moving body and measuring information about the moving body based on the information, and the controlling means relative to the feature points of the non-three-dimensional object. It is an apparatus characterized in that a region containing a large amount of the above is the second acquisition region.

According to the present invention, it is possible to generate an environmental map around a moving body and measure information about the moving body in a stable and accurate manner.

1 (a) and 1 (b) are diagrams schematically showing the appearance of the mobile device according to the embodiment of the present invention, respectively. It is a figure which shows the shooting range of the stereo camera of FIG. 1 and the shooting range of the monocular camera in the initial posture. It is a block diagram which shows the structural example of the map generation / position measuring apparatus of FIG. It is a flowchart for demonstrating map generation and position measurement processing. It is a flowchart for demonstrating the feature point extraction process 1. It is a flowchart for demonstrating the feature point extraction process 2. It is a flowchart for demonstrating the feature point association processing. It is a figure for demonstrating the coordinate transformation from the coordinate system of a reference camera to the coordinate system of a monocular camera. It is a figure which shows the area where a three-dimensional object was detected by a map generation / position measuring apparatus. It is a figure which shows the feature point extracted by the map generation / position measuring apparatus. It is a figure which shows the feature point extracted by the map generation / position measuring apparatus separately into a three-dimensional object feature point and a non-three-dimensional object feature point. It is a flowchart for demonstrating the feature point classification process. It is a flowchart for demonstrating the position estimation process. It is a flowchart for demonstrating the environment map generation process. It is a flowchart for demonstrating the camera attitude control processing. It is a figure which shows the shooting range of a monocular camera after a camera attitude control process. 17 (a) and 17 (b) are diagrams schematically showing the appearance of the mobile device of the first modification, respectively. It is a figure which shows the light projection range of the lidar of the modification 1 and the photographing range of a monocular camera in an initial posture. It is a figure which shows the light projection range of the lidar of the modification 1 and the photographing range of a monocular camera after changing the posture. It is a figure for demonstrating the structure of the light emitting system and the light receiving system of the lidar (scanning type) of the modification 1. It is a figure for demonstrating the structure of the light emitting system and the light receiving system of the lidar (non-scanning type) of the modification 1 and the relationship between a light source or a light receiving element, and a detection area. It is a figure which shows the light projection range (initial state) of the lidar of the modification 2 and the photographing range of a monocular camera. It is a figure which shows the light projection range (after change) of the lidar of the modification 2 and the photographing range of a monocular camera. It is a figure which shows the light projection range (initial state) of the lidar of the modification 3 and the shooting range of a stereo camera. It is a figure which shows the light projection range (after change) of the lidar of the modification 3 and the shooting range of a stereo camera. It is a figure for demonstrating an example in which a stereo camera and a rider are integrally formed.

Hereinafter, an embodiment of the present invention will be described with reference to the drawings. FIG. 1A schematically shows a side view of the mobile device 1 of the embodiment. FIG. 1B schematically shows a front view of the mobile device 1.

"Device configuration"
As shown in FIG. 1, the mobile device 1 includes a vehicle 10 as a mobile body and a map generation / position measuring device 100 as an example of the device of the present invention mounted on the vehicle 10. .. The vehicle 10 is an automobile.

The map generation / position measuring device 100 is a device that measures its own position, that is, the position of the vehicle 10, and generates an environmental map around the vehicle 10, and includes a stereo camera 200 and a monocular camera 300.

The stereo camera 200 and the monocular camera 300 are provided in the vicinity of the rear-view mirror of the vehicle 10.

More specifically, the stereo camera 200 and the monocular camera 300 are suspended and supported on the ceiling of the vehicle 10 via the same support member so as to be vertically separated from each other.

The stereo camera 200 has an image pickup unit 200a which is a left side image pickup unit, an image pickup unit 200b which is a right side image pickup unit, and a parallax image generation unit 200c (see FIG. 3). The imaging units 200a and 200b include an image sensor (CMOS or CCD).

As shown in FIG. 2, the stereo camera 200 has a shooting range (a range in which the left-eye angle of view (the angle of view of the imaging unit 200a) and the right-eye angle of view (the angle of view of the imaging unit 200b) are combined) is in front of the vehicle 10. It is fixed to the support member so as to be.

The parallax image generation unit 200c generates a parallax image by using the parallel stereo method with respect to the images (luminance images) captured by the imaging units 200a and 200b.

The monocular camera 300 includes an image pickup unit 300a provided so that the posture can be changed, a camera posture control unit 300b (see FIG. 3) that controls the posture of the image pickup unit 300a, and a camera posture measurement unit that measures the posture of the image pickup unit 300a. It has 300c (see FIG. 3). In FIG. 2, the image pickup unit 300a is in the initial posture in which the shooting range substantially matches the shooting range of the stereo camera 200. Here, as the imaging unit 300a, one having an angle of view similar to that of the shooting range of the stereo camera 200 is used, but one having a large angle of view or one having a small angle of view may be used.

The imaging unit 300a can rotate in the pan direction (around the vertical axis) and the tilt direction (around the horizontal axis) so that the shooting range can be changed within a predetermined range including the shooting range of the stereo camera 200 (the posture can be changed). ) It is attached to the support member via a holding mechanism for holding. The image pickup unit 300a includes an image sensor (CMOS or CCD).

The camera posture control unit 300b has a drive unit that drives the holding mechanism to rotate the image pickup unit 300a in the pan direction and / or the tilt direction, and is based on the camera posture information 530b stored in the storage unit 530 described later. The posture of the imaging unit 300a is controlled. The drive unit includes a stepping motor for pan direction rotation and a stepping motor for tilt direction rotation.

The camera attitude measuring unit 300c has a counter that counts the number of steps of each stepping motor (the number of drive pulses applied to each stepping motor), and the rotation angle (rotation position) of the imaging unit 300a in the pan direction and tilt direction. ) That is, the posture of the imaging unit 300a is measured.

A servo motor may be used for the drive unit instead of the stepping motor. In this case, the camera posture measuring unit can obtain the rotation angle (rotation position) of the imaging unit 300a in the pan direction and the tilt direction from the encoder of the servomotor.

Further, the positional relationship between the stereo camera 200 and the monocular camera 300 is not limited to the positional relationship shown in FIG. 1, and the point is that the shooting range of the monocular camera 300 can be changed within a predetermined range including the shooting range of the stereo camera 200. Any relationship is fine.

The map generation / position measuring device 100 includes a control device 500 in addition to the stereo camera 200 and the monocular camera 300. The control device 500 is realized by, for example, a CPU or an FPGA.

The control device 500 includes an image processing unit 510 that extracts feature points in the image from the brightness image and the parallax image output from the stereo camera 200 and the brightness image output from the monocular camera 300, and the image processing unit 510. The position measurement unit 520 that measures the position of the vehicle 10 based on the extracted feature points and generates an environmental map around the vehicle 10, and the position information and image processing unit 510 of the vehicle 10 measured by the position measurement unit 520 are extracted. It includes a storage unit 530 that stores three-dimensional information and the like of the feature points, and an image pickup control unit 540 that controls the image pickup units 200a and 200b of the stereo camera 200 and the image pickup unit 300a of the monocular camera 300.

The image processing unit 510 is between a three-dimensional object detection unit 510a that detects a three-dimensional object in an image, a feature point extraction unit 510b that extracts a feature point in the image, and images taken at different times by the same camera. It includes a feature point tracking unit 510c that searches for the same feature point, and a feature point matching unit 510d that searches for the same feature point between images taken at the same time by different cameras.

In the three-dimensional object detection unit 510a, for example, the three-dimensional object extraction method disclosed in Japanese Patent Application Laid-Open No. 2015-207281 can be used.

In the feature point extraction unit 510b, known techniques such as the SHIFT method and the FAST method can be used.

In the feature point tracking unit 510c, a method using feature description such as the SHIFT method or an image template-based method such as block matching can be used.

The feature point matching unit 510d searches for feature points extracted by the stereo camera 200 in the image taken by the monocular camera 300.

The position measurement unit 520 estimates the position of the vehicle 10 and the feature point classification unit 520a that classifies the feature points based on the feature point information and the three-dimensional object information obtained from the image processing unit 510, and the estimation result is the vehicle control device. It includes a position estimation unit 520b that outputs to 700 and an environment map generation unit 520c. The position measurement unit 520 receives the reference data that is the base of the environmental map around the own vehicle from the GNSS (satellite positioning system) 600, generates the environmental map around the own vehicle in real time based on the reference data, and also generates the environmental map around the own vehicle in real time. The position information of the above is acquired, and the generated environment map data and the acquired position information of the own vehicle are output to the vehicle control device 700.

The vehicle control device 700 controls the own vehicle (for example, autobrake, autosteering, autoaccelerator, etc.) based on the received environmental map data and the position information of the own vehicle, and alerts the driver (for example, a warning sound). Occurrence, warning display on the car navigation screen, etc.).

The storage unit 530 contains feature point observation information 530a including information indicating which camera captured the feature point at which position on the image at what time, and a camera which is posture information of the monocular camera 300 at each time. Attitude information 530b, feature point 3D information 530c including 3D position information in a reference coordinate system with feature points, vehicle position information measured by position measurement unit 520, and position measurement unit 520 from GNSS600. The above-mentioned reference data transmitted to is stored in.

《Map generation / position measurement processing》
Hereinafter, an outline of the map generation / position measurement process performed by the map generation / position measurement device 100 will be described with reference to the flowchart of FIG. The map generation / position measurement process is started when the position measurement unit 520 receives the process start request from the vehicle control device 700. The vehicle control device 700 transmits a processing start request to the position measurement unit 520 when, for example, the electric system of the vehicle 10 on which the map generation / position measurement device 100 is mounted is turned on.

In the first step S1, the feature point extraction process 1 is performed. The details of the feature point extraction process 1 will be described later.

In the next step S2, the feature point extraction process 2 is performed. The details of the feature point extraction process 2 will be described later. Although the feature point extraction process 2 is after the feature point extraction process 1 in FIG. 4, the feature point extraction processes 1 and 2 are actually started at the same time, and some of the processes are performed in parallel. Therefore, in FIG. 4, the order of the feature point extraction processes 1 and 2 may be reversed.

In the next step S3, the feature point associating process is performed. The details of the feature point mapping process will be described later.

In the next step S4, the feature point tracking process is performed. The feature point tracking process is performed by the feature point tracking unit 510c by a method using a feature amount description such as the SHIFT method or an image template-based method such as block matching.

In the next step S5, the feature point classification process is performed. The details of the feature point classification process will be described later.

In the next step S6, the position estimation process is performed. The details of the position estimation process will be described later.

In the next step S7, the environment map generation process is performed. The details of the environmental map generation process will be described later.

In the next step S8, the camera attitude control process is performed. The details of the camera attitude control process will be described later.

In step S9, it is determined whether or not to end the process. The determination here is affirmed when the position measuring unit 520 receives the processing end request from the vehicle control device 700, and is denied when the position measuring unit 520 does not receive the processing end request. For example, the vehicle control device 700 sends a processing end request to the position measurement unit 520 when the electric system of the vehicle 10 on which the map generation / position measurement device 100 is mounted is turned off. If the determination in step S9 is affirmed, the flow ends, and if denied, the process returns to step S1.

<< Feature point extraction process 1 >>
Next, the feature point extraction process 1 (step S1 in FIG. 4) will be described in detail with reference to the flowchart of FIG.

In the first step S1.1, shooting is performed with the stereo camera 200. Specifically, the imaging units 200a and 200b of the stereo camera 200 photograph the front of the own vehicle, and the data of the captured images (brightness images) of the imaging units 200a and 200b are transmitted to the differential image generation unit 200c and the imaging unit 200b. The captured image (brightness image) is transmitted to the feature point extraction unit 510b. The stereo camera 200 starts shooting when it receives a shooting start signal from the image pickup control unit 540. The image pickup control unit 540 transmits the shooting start signal in synchronization with the stereo camera 200 and the monocular camera 300.

In the next step S1.2, a parallax image is generated. Specifically, the parallax image generation unit 200c generates a parallax image by performing remapping processing for parallelization based on the data of the two received luminance images, and the data of the parallax image is used as a stereoscopic object detection unit. It is transmitted to 510a.

In the next step S1.3, a three-dimensional object is detected. Specifically, the three-dimensional object detection unit 510a detects a three-dimensional object from the received parallax image data by using the above-mentioned three-dimensional object extraction method, and transmits the detection result to the feature point classification unit 520a.

In the final step S1.4, feature points in the image are extracted. Specifically, the feature point extraction unit 510b extracts feature points of a three-dimensional object and a non-three-dimensional object from the received luminance image data (three-dimensional information) of the imaging unit 200b.

<< Feature point extraction process 2 >>
Next, the feature point extraction process 2 (step S2 in FIG. 4) will be described in detail with reference to the flowchart of FIG.

In the first step S2.1, a picture is taken with the monocular camera 300 in the kth posture (k ≧ 1). Specifically, the imaging unit 300a of the monocular camera 300 photographs the front of the own vehicle, and the obtained luminance image data (three-dimensional information) is transmitted to the feature point extraction unit 510b. The monocular camera 300 starts shooting when it receives a shooting start signal from the image pickup control unit 540. That is, step S2.1 is started at the same time as shooting with the stereo camera 200 (step S1.1).

Here, the "kth posture" is any one of the known plurality of types (K types) of the monocular camera 300, and is in the kth loop of the map generation / position measurement process of FIG. Means posture. The posture of the monocular camera 300 when the feature point extraction process 2 is performed in the first (first) loop of the map generation / position measurement process is the initial posture.

In the next step S2.2, the posture of the monocular camera 300 is measured. Specifically, the camera posture measuring unit 300c counts the number of steps of each stepping motor to determine which of the K types of postures the current posture of the monocular camera 300 is. At this time, the counted number of steps is stored in the storage unit 530 as the camera posture information 530b. The number of steps of each stepping motor when the feature point extraction process 2 is executed in the first loop of the map generation / position measurement process of FIG. 4 is 0, and the posture of the monocular camera 300 is the initial posture.

In the final step S2.3, feature points in the image are extracted. Specifically, the feature point extraction unit 510b extracts feature points of a three-dimensional object and a non-three-dimensional object from the data (three-dimensional information) of the received image (luminance image) of the monocular camera 300.

<< Feature point mapping process >>
Next, the feature point associating process (step S3 in FIG. 4) will be described in detail with reference to the flowchart of FIG. 7. The feature point associating process is performed by the feature point matching unit 510d.

The feature point associating process is a process of searching for feature points extracted from the luminance image of the stereo camera 200 on the coordinate system of the monocular camera 300. That is, in the feature point associating process, the location of the feature points obtained by the stereo camera 200 on the coordinate system of the monocular camera 300 is obtained from the observed coordinates in the brightness image of the stereo camera 200 and the parallax value thereof. It is a process.

In the first step S3.1, n is set to 1.

In the next step S3.2, the initial position of the nth feature point extracted from the luminance image of the stereo camera 200 (more specifically, the luminance image of the imaging unit 200b) is calculated. The calculation method will be described later.

In the next step S3.3, it is determined whether or not there is a feature point extracted from the luminance image of the monocular camera 300 in the pixel block of i pixels near the initial position of the extracted nth feature point. If the judgment here is affirmed, the process proceeds to step S3.4, and if denied, the process proceeds to step S3.7. By matching the feature points around the initial position of the nth feature point in this way, it is possible to robustly match the feature points in an environment in which similar patterns continue.

In the next step S3.4, the cost value is calculated. Here, the "cost value" is an evaluation value representing the degree of coincidence of the feature points extracted from the luminance image of the monocular camera 300 with respect to the feature points extracted from the luminance image of the stereo camera 200.

In the next step S3.5, it is determined whether or not the cost value is equal to or less than the threshold value. If the judgment here is affirmed, the process proceeds to step S3.6, and if denied, the process proceeds to step S3.7.

In the next step S3.6, the position information of the nth feature point on the luminance image of the monocular camera 300 (hereinafter, also referred to as “observation information of the monocular camera 300”) is added to the feature point observation information 530a. That is, if the cost value of the feature point in the i-pixel pixel block near the initial position of the nth feature point is equal to or less than the threshold value, the observation information of the monocular camera 300 is added to the storage unit 530 as the feature point observation information 530a. If there is no feature point in the pixel block of i-pixel near the initial position of the nth feature point, or if there is no feature point with a cost value below the threshold, the observation of the monocular camera 300 in the feature point observation information 530a. Do not add information.

In the next step S3.7, it is determined whether or not n <N. N is the total number of feature points extracted from the luminance image of the stereo camera 200. If the judgment here is affirmed, the process proceeds to step S3.8, and if it is denied, the flow ends.

In step S3.8, n is incremented. When step S3.8 is executed, the process returns to step S3.2.

Next, a method of calculating the initial position of the nth feature point in step S3.2 will be described with reference to FIG.

但し、bはステレオカメラ２００の基線長、f_sはステレオカメラ２００の焦点距離を示す値、cx_s,cy_sはステレオカメラ２００の中心を示す値である。これらの値は事前に校正されており、このシステムが動作する時点では既知のものとする。 Here, the right image pickup unit 200b of the two left and right image pickup units 200a and 200b of the stereo camera 200 is used as a reference camera. The three-dimensional coordinates P (Xi, Yi, Zi) in the reference camera coordinate system (coordinate system of the reference camera) of the feature point taken by the reference camera are the sensor coordinate system (coordinates of the image sensor) of the reference camera of the feature point. It can be expressed by the following equation (1) using the two-dimensional coordinates p (xi, yi) in the system) and its disparity value di.

However, b is the baseline length of the stereo camera 200, f _s is a value indicating the focal length of the stereo camera 200, and cx _s and cy _s are values indicating the center of the stereo camera 200. These values have been pre-calibrated and should be known at the time the system operates.

Coordinates P (Xi, Yi, Zi) in the reference camera coordinate system, coordinates in the coordinate system of the monocular camera 300 (Xi ^p ,
Yi ^p, conversion to Zi ^p) may be carried out by the following equation (2) (see FIG. 8).

pRr is a rotation component that transforms the coordinates from the reference camera coordinate system to the coordinate system of the monocular camera 300. pRr can be measured by the camera posture measuring unit 300c.
pTr is a translation component that transforms the coordinates from the reference camera coordinate system to the coordinate system of the monocular camera 300. If the position of the monocular camera 300 in the reference camera coordinates is known, it can be obtained as pTr = pRr ^-1 pT.

但し、fpは単眼カメラ３００の焦点距離を示す値、cxp,cypは単眼カメラ３００のセンサ座標系（イメージセンサの座標系）の中心を示す値である。これらの値は事前に校正されており、このシステムが動作する時点では既知のものとする。 The position of the feature point observed by the stereo camera 200 on the image taken by the monocular camera 300 can be obtained by the following equation (3).

However, fp is a value indicating the focal length of the monocular camera 300, and cxp and cyp are values indicating the center of the sensor coordinate system (coordinate system of the image sensor) of the monocular camera 300. These values have been pre-calibrated and should be known at the time the system operates.

《Detection of three-dimensional objects》
Next, the detection of the three-dimensional object in step S1.3 will be specifically described.
In FIG. 9, the area surrounded by the rectangular broken line is the area in which the three-dimensional object is detected in the image (hereinafter, also referred to as “three-dimensional object detection area”).
For example, by using a known method such as Japanese Patent Application Laid-Open No. 2015-207281, it is possible to detect a three-dimensional object on a road surface such as a person or a car in an image. As shown by the rectangular broken line in FIG. 9, the three-dimensional object detection region can be defined by, for example, a convex quadrangle. As the information of the three-dimensional object detection region, for example, the coordinates on the image of the four vertices of the rectangular broken line are output.

In FIG. 10, white circles indicate feature points extracted from the image. The three-dimensional object detection area is indicated by a rectangular broken line. Therefore, by the feature point classification process in step S5, as shown in FIG. 11, the feature points are divided into a three-dimensional object feature point (three-dimensional object feature point) and a non-three-dimensional object from the coordinates of the feature point and the information of the three-dimensional object detection area. It can be classified into object feature points (feature points other than three-dimensional objects).

<< Feature point classification processing >>
Next, the feature point classification process (step S5 in FIG. 4) will be described in detail with reference to the flowchart of FIG. The feature point classification process is carried out by the feature point classification unit 520a.

In the first step S5.1, n is set to 1.

In the next step S5.2, it is determined whether or not the nth feature point extracted from the luminance image of the stereo camera 200 (more specifically, the luminance image of the imaging unit 200b) is within the stereoscopic object detection region. Specifically, it is calculated whether or not a feature point exists inside the rectangular three-dimensional object detection region. For this purpose, the three-dimensional object detection region is divided into two triangles, and it is determined which of the two triangles the feature points are included in. The process of determining whether a certain point is inside or outside a triangle is generally called an inside / outside determination process of a triangle, and a method using an outer product is known, and it may be used. If the judgment in step S5.2 is affirmed, the process proceeds to step S5.3, and if denied, the process proceeds to step S5.4.

In step S5.3, the nth feature point is registered in the feature point three-dimensional information 530c of the storage unit 530 as a three-dimensional object feature point.

In step S5.4, the nth feature point is registered in the feature point three-dimensional information 530c of the storage unit 530 as a non-three-dimensional object feature point.

In step S5.5, it is determined whether or not n <N. N is the total number of feature points extracted from the luminance image of the stereo camera 200. If the judgment here is affirmed, the process proceeds to step S5.6, and if denied, the flow ends.

In step S5.6, n is incremented. When step S5.6 is executed, the process returns to step S5.2.

<< Position estimation processing >>
Next, the position estimation process (step S6 in FIG. 4) will be described with reference to FIG. The position estimation process is performed by the position estimation unit 520b.

In the first step S6.1, in-line selection by RANSAC is performed. That is, the static feature points are extracted from the non-three-dimensional feature points by the RANSAC method. Since erroneous matching in the parallax calculation by the stereo camera 200 and erroneous matching in the feature point tracking unit 510c may occur among the feature points, the RANSAC method is used to reduce the influence of the erroneous matching.

ここで、ex_i ^r及びey_i ^rは、それぞれ基準カメラ（右側の撮像部２００ｂ）でのx方向及びy方向の再投影誤差である。ex_i ^lは、左カメラ（左側の撮像部２００ａ）でのx方向の再投影誤差である。ex_i ^p及びey_i ^pは単眼カメラ３００でのx座標とy座標の再投影誤差である。W_i ^rは、基準カメラでの重みであり、基準カメラで追跡成功した特徴点には1が、それ以外には0がセットされる。W_i ^pは、単眼カメラ３００での重みであり、単眼カメラ３００で検出された特徴点には1がセットされ、それ以外には0がセットされる。 Here, the cost function with the position and orientation (R, t) of the camera as a parameter can be obtained for a certain feature point by the following equation (4).

Here, ex _i ^r and _{e i} ^r are the reprojection errors in the x direction and the y direction in the reference camera (imaging unit 200b on the right side), respectively. ex _i ^l is the reprojection error in the x direction with the left camera (imaging unit 200a on the left side). ex _i ^p and ey _i ^p are the reprojection errors of the x and y coordinates of the monocular camera 300. W _i ^r is the weight of the reference camera, and 1 is set for the feature points that are successfully tracked by the reference camera, and 0 is set for the other feature points. W _i ^p is the weight of the monocular camera 300, and 1 is set for the feature points detected by the monocular camera 300, and 0 is set for the other feature points.

Here, the position of the point in the reference camera coordinate system is [X _i ^r (R, t), Y _i ^r (R, t), Z _i ^r (R, t)], and the position of the point in the map coordinate system. ^Is [X _i w, Y _i ^w , Z _i ^w ], and the position of the point in the coordinate system of the monocular camera 300 is [X _i ^p (R, t), Y _i ^p (R, t), Z _i ^p ( R, t)] can be converted by the following equations (5) and (6), respectively.

In the next step S6.2, the position of the vehicle 10 is estimated at the feature point of the inlay. Specifically, after the feature points of the inlyre are extracted by the RANSAC method, the position of the vehicle 10 is estimated only by the feature points of the inlyre. The position estimation can be formulated as a problem of minimizing the cost of the reprojection error for each feature point described above, and the optimum position can be estimated by using a non-linear minimization method such as the Levemberg-Marquart method.

In the next step S6.3, the stationary feature points are extracted from the three-dimensional object feature points. Specifically, the reprojection error of the three-dimensional object feature point is calculated using the position information estimated in step S6.2, and whether or not it is a stationary feature point is determined by the threshold calculation. As a result, it is possible to suppress the influence when the stationary feature point starts to move, such as when the stationary vehicle starts to move.

In the next step S6.4, the position of the vehicle 10 is estimated at the stationary feature point. Specifically, it is optimal to re-incorporate the feature points determined to be stationary among the three-dimensional object feature points into the cost function of position estimation (Equation (4) above) and solve the nonlinear minimization problem. Find the position.

《Environmental map generation process》
Next, the environment map generation process (step S7 in FIG. 4) will be described with reference to FIG. The environmental map generation process is a process of generating a highly accurate environmental map by adding feature point information to the reference data that is the base of the environmental map transmitted from the GNSS 600. The environmental map generation process is carried out by the environmental map generation unit 520c.

In the first step S7.1, n is set to 1.

In the next step S7.2, the nth feature point extracted from the luminance image of the stereo camera 200 (more specifically, the luminance image of the imaging unit 200b) is on the environmental map (however, when n = 1, it is on the reference data). Determine if it exists in. If the judgment here is denied, the process proceeds to step S7.3, and if the judgment is affirmed, the process proceeds to step S7.4.

In step S7.3, it is determined whether or not the nth feature point is a non-three-dimensional object feature point. If the judgment here is affirmed, the process proceeds to step S7.5, and if denied, the process proceeds to step S7.8.

In step S7.4, it is determined whether or not the nth feature point is determined as an inlier. If the judgment here is affirmed, the process proceeds to step S7.6, and if denied, the process proceeds to step S7.7.

In step S7.5, the three-dimensional information of the nth feature point (being a non-three-dimensional object feature point) is added as the feature point three-dimensional information 530c to the environment map (however, when n = 1, the reference data). The observation information of the nth feature point (position information of the nth feature point on the brightness image of the monocular camera 300) is added as the feature point observation information 530a. That is, when the nth feature point is not on the environmental map, if the nth feature point is a non-three-dimensional object feature point, three-dimensional reconstruction is performed using the parallax value obtained from the stereo camera 200, and the three-dimensional object feature point can be used. For example, it will not be added to the environmental map. When step S7.5 is executed, the process proceeds to step S7.8.

In step S7.6, the observation information of the nth feature point (position information of the nth feature point on the luminance image of the monocular camera 300) is added to the environment map (however, reference data when n = 1). Add as. When step S7.6 is executed, the process proceeds to step S7.8.

In step S7.7, the nth feature point is excluded from the environmental map (however, when n = 1, the reference data). When step S7.7 is executed, the process proceeds to step S7.8.

In step S7.8, bundle adjustment is performed. That is, after the feature point information is added to the environmental map, the overall reprojection error is minimized by using the bundle adjustment method, and the position information of the vehicle 10 and the three-dimensional information of the feature points are optimized.

In step S7.9, it is determined whether or not n <N. N is the total number of feature points extracted from the luminance image of the stereo camera 200. If the judgment here is affirmed, the process proceeds to step S7.95, and if it is denied, the flow ends.

In step S7.95, n is incremented. When step S7.95 is executed, the process returns to step S7.2.

<< Camera attitude control processing >>
Next, the camera attitude control process in step S8 of FIG. 4 will be described with reference to the flowchart of FIG. The camera attitude control process is performed by the camera attitude control unit 300b. It is assumed that a plurality of types (K types) of known postures that the monocular camera 300 can take are prepared in advance. The posture of the monocular camera 300 is preferably a posture in which as many stationary feature points as possible enter the angle of view. Among the feature points in the image, the non-three-dimensional object feature points are usually stationary feature points, so that the posture of the monocular camera 300 is such that as many non-three-dimensional object feature points enter the angle of view as much as possible. desirable.

In the first step S8.1, k is set to 1.

In the next step S8.2, the number Na of feature points that can be photographed by the monocular camera 300 in the k-th posture is calculated. Na can be calculated by using the equations for coordinate transformation and projection onto the sensor coordinate plane (the above equations (2) and (3)).

In the next step S8.3, the number Ns of the three-dimensional object feature points that can be photographed by the monocular camera 300 in the k-th posture is calculated. Ns can be calculated by using the equations for coordinate transformation and projection on the sensor coordinate plane (the above equations (2) and (3)).

In the next step S8.4, the posture score S is calculated. Here, it is defined as S = Na—Ns.

In the next step S8.5, it is determined whether or not k <K. If the judgment here is affirmed, the process returns to step S8.2, and if denied, the process proceeds to step S8.6.

In step S8.6, the posture having the maximum posture score S is selected as the target posture among the K types of postures of the monocular camera 300 prepared in advance.

In step S8.7, the allowable angular velocity is calculated. That is, the maximum angular velocity in the attitude control of the monocular camera 300 is calculated. This maximum angular velocity is the upper limit of the angular velocity of the monocular camera 300 in which motion blur does not occur during the rotational movement of the monocular camera 300.

但し、xは、単眼カメラ３００から見た特徴点の位置[x,y,z]である。fx,fyは、単眼カメラ３００の内部パラメータとしての、ピクセル単位で表される焦点距離である。 _{The movement [v x} , v _y ] of the feature points between the frames of the monocular camera 300 can be calculated by the following equation (7) when the rotational movement R = [r ₁ , r ₂ , r _{3] between the frames is known.} Can be calculated.

However, x is the position [x, y, z] of the feature point as seen from the monocular camera 300. fx and fy are focal lengths expressed in pixel units as internal parameters of the monocular camera 300.

但し、cはcosθ,sはsinθである。 In the case of the monocular camera 300, the rotation is around the x-axis (tilt direction) and around the Y-axis (pan direction), so the movement of the feature points [v _x , v _y ] when rotated by θ around the X-axis is It is expressed by the following equation (8).

However, c is cosθ and s is sinθ.

但し、cはcosφ,sはsinφである。 _{Similarly, the movement [v x} , v _y ] of the feature point when φ-rotated around the Y-axis is expressed by the following equation (9).

However, c is cosφ and s is sinφ.

In order to prevent motion blur, each [v _x , v _y ] should be a value of 1 or less.

For the range of θ and φ where both [v _x , v _y ] are 1 or less, for example, the values of cos and sin are changed in 0.1 degree increments to calculate the value of _{[v x} , v _y]. Can be obtained by.

In the next step S8.8, the posture of the monocular camera 300 is shifted to the posture selected in step S8.6. Specifically, the camera attitude control unit 300b uses the PID (Proportional-Integral-Differential) method to target rotation around the X-axis and rotation around the Y-axis within a range that does not exceed the allowable angular velocity (maximum angular velocity). The attitude of the monocular camera 300 is controlled so as to approach the attitude. At this time, the camera attitude control unit 300b is selected from the attitude information (tilt direction, pan direction rotation angle) of the monocular camera 300 measured in step S2.2 and stored in the storage unit 530 as the camera attitude information 530b. The posture of the monocular camera 300 is controlled by calculating the number of steps of each stepping motor for shifting to the posture.

FIG. 16 shows an example of the photographing range of the monocular camera 300 after the attitude control. In FIG. 16, it can be seen that the preceding vehicle, which is a three-dimensional object and a moving object, is out of the shooting range (angle of view) of the monocular camera 300.

When the map generation / position measurement process is continued after the camera posture control process (step S8) is performed as described above (NO in step S9), the optimum posture after the camera posture control process is obtained. Step S2 (feature point extraction process 2) in the next loop is performed on a monocular camera 300. At this time, it is preferable to exclude the three-dimensional information of the region within the shooting range of the monocular camera 300 and outside the shooting range of the stereo camera 200 from the target of feature point extraction. This is because in this region, the properties of the feature points (whether or not they are stationary points or whether or not they are three-dimensional objects) cannot be accurately determined. The camera attitude control process (step S8) reduces the load of the processes (steps S3 to S5) after step S2 in the next loop, and step S6 (position estimation process) obtains highly accurate position information, and step S7. (Environmental map generation process) can generate a highly accurate environmental map.

The map generation / position measuring device 100 of the above embodiment uses a combination of a stereo camera 200 and a monocular camera 300 whose posture can be changed, but the present invention is not limited to this. For example, as in the map generation / position measuring device 100'of Modification 1 shown in FIG. 17, a combination of a lidar (Light detecting and ranking) 200 and a posture-changeable monocular camera 300 may be adopted. FIG. 17A is a schematic view of the vehicle 10 on which the map generation / position measuring device 100'is mounted, as viewed from the side. FIG. 17B is a front view of the vehicle 10.

As shown in FIG. 17, the rider 400 is attached near the license plate on the front side of the vehicle 10 so that the light projection range faces the front of the vehicle 10.

The rider 400 is a distance measuring device that measures a distance by emitting laser light and detecting reflected light, and is also called a laser radar. From the measurement result of the rider 400, three-dimensional information in front of the vehicle 10 can be obtained.

The monocular camera 300 is mounted in the vicinity of the rear-view mirror of the vehicle 10 so as to be able to change its posture (rotate in the pan direction and tilt direction).

In the map generation / position measuring device 100', as shown in FIG. 18, for example, the projection range of the lidar 400 and the angle of view of the monocular camera 300 are made equal, and the posture control of the monocular camera 300 is performed in the same manner as in the above embodiment. (See FIG. 19), the environment map is generated, and the position of the vehicle 10 is measured. It should be noted that the extraction of the feature points in the three-dimensional information which is the measurement result of the rider 400 and the classification of the extracted feature points can be performed in the same manner as in the above embodiment. The detection of a three-dimensional object can be performed using the measurement result of the rider 400.

As shown in FIG. 20 as an example, the configuration of the rider 400 is such that a light projecting system including a semiconductor laser (LD) and a coupling lens and a light receiving system including a reflection mirror, an imaging optical system and a photodetector scan. It may be a scanning type that shares a mirror.

Further, as shown in FIG. 21 as an example, the configuration of the rider 400 is a non-scanning type including a light projecting system including a light source (for example, a semiconductor laser) and a light receiving system including an imaging optical system and a light receiving element. You may. In this case, by making at least one of the light emitting point of the light source and the light receiving area of the light receiving element a plurality, the detection area can be divided and detected.

Further, a combination of the rider 400 and the fixed (non-movable) monocular camera 300 ″ may be adopted as in the map generation / position measuring device 100 ″ of the second modification shown in FIG. 22. In this case, the detection of the three-dimensional object can be performed using the measurement result of the rider 400.

In the map generation / position measuring device 100 ″, an area in which the rider 400 acquires three-dimensional information by controlling the rider 400, for example, a control means realized by an FPGA or a CPU changes the projection range of the rider 400. Can be controlled (see FIG. 23). When the lidar 400 is a scanning type, the projection range can be changed by controlling the lighting / extinguishing of the light source for each scanning position, and when the lidar 400 is a non-scanning type, the light emitted by the light source is turned on. This can be done by selecting a point.

Further, a combination of the rider 400 and the stereo camera 200 may be adopted as in the map generation / position measuring device 100 ″ ″ of the modification 3 shown in FIG. 24. In this case, the detection of the three-dimensional object can be performed by using the parallax image generated by the stereo camera 200 or the measurement result of the rider 400.

In the map generation / position measuring device 100 ″, the rider 400 acquires three-dimensional information by controlling the rider 400, for example, a control means realized by an FPGA or a CPU changes the projection range of the rider 400. The area can be controlled (see FIG. 25).

Further, as a configuration of the map generation / position measuring device, a configuration in which a stereo camera 200 and a rider 400 are integrally provided (see FIG. 26), or a configuration in which a movable or non-movable monocular camera and a rider are integrally provided. May be adopted. The two acquisition means integrally provided in this way can be attached, for example, to the vicinity of the rear-view mirror of the vehicle or the vicinity of the license plate on the front side of the vehicle.

As can be seen from the above description, in the map generation / position measuring device which is an example of the "device" of the present invention, various combinations can be adopted as the combination of the acquisition means for acquiring the three-dimensional information around the vehicle. It is possible. Of course, combinations other than the combinations of the above-described embodiment and each modification can also be adopted. In particular, from the viewpoint of accurately detecting a three-dimensional object, it is preferable that at least one of the two combined acquisition means can accurately detect the distance information (for example, a stereo camera, a rider, etc.). It is also possible to acquire the distance information from the images of a plurality of frames taken in time series by the monocular camera. In this case, a combination of two monocular cameras can be adopted as a combination of acquisition means.

From the first viewpoint, the map generation / position measuring device of the above-described embodiment and each modification is a device mounted on a moving body (for example, a vehicle), and three-dimensional information around the moving body (for example, a vehicle). More specifically, the first acquisition means for acquiring (three-dimensional position information of an object existing around the moving body) and the first acquisition area in which the first acquisition means acquires three-dimensional information. The second acquisition means is the first acquisition based on the second acquisition means for acquiring at least a part of the three-dimensional information and the three-dimensional information of the first acquisition area acquired by the first acquisition means. The control means for controlling the second acquisition area, which is the area for acquiring the three-dimensional information in the area, the three-dimensional information of the first acquisition area acquired by the first acquisition means, and the second acquisition means. It is a device including a generation means for generating an environment map around a moving body based on the acquired three-dimensional information of the second acquisition area.

Further, the map generation / position measuring device of the above-described embodiment and each modification is a device mounted on a moving body (for example, a vehicle) from the second viewpoint, and three-dimensional information around the moving body (more than that). In detail, at least a first acquisition means for acquiring (three-dimensional position information of an object existing around a moving body) and a first acquisition area in which the first acquisition means acquires three-dimensional information. Based on the second acquisition means for acquiring a part of the three-dimensional information and the three-dimensional information of the first acquisition area acquired by the first acquisition means, the second acquisition means is the first acquisition area. Control means for controlling the second acquisition area, which is an area for acquiring the three-dimensional information, the three-dimensional information of the first acquisition area acquired by the first acquisition means, and the acquisition by the second acquisition means. It is a device including an estimation means for estimating the position of a moving body based on the obtained three-dimensional information of the second acquisition area. The map generation / position measuring device may measure the posture (for example, roll angle, pitch angle, yaw angle) of the moving body and the speed of the moving body in addition to or instead of the position of the moving body. By measuring the posture and speed of the moving body in addition to the position of the moving body, vehicle control can be performed more accurately. In the present specification, information on a moving body such as the position, posture, and speed of the moving body is also referred to as "moving body information".

In the above embodiment and each modification, a map generation / position measuring device that generates both an environmental map and a moving object information has been described as an example, but the "device" of the present invention is an environmental map generation. It may be a device that performs only, or a device that only measures moving object information.

In summary, the device of the present invention is a device mounted on a moving body, and acquires three-dimensional information around the moving body (more specifically, three-dimensional position information of an object existing around the moving body). The first acquisition means, the second acquisition means for acquiring at least a part of the three-dimensional information in the first acquisition area, which is the area where the first acquisition means acquires the three-dimensional information, and the first acquisition means. With the control means for controlling the second acquisition area, which is the area in which the second acquisition means acquires the three-dimensional information in the first acquisition area, based on the three-dimensional information of the first acquisition area acquired in , Around the moving body based on the three-dimensional information of the first acquisition area acquired by the first acquisition means and the three-dimensional information of the second acquisition area acquired by the second acquisition means. It is a device provided with a means for generating at least one of the generation of an environmental map of the above and the measurement of information about a moving body.

According to the map generation / position measuring device of the above embodiment and each modification, the second acquisition area, which is the area where the second acquisition means acquires the three-dimensional information, is, for example, a stationary point in the first acquisition area. Can be set in a relatively large area.

As a result, it is possible to generate an environmental map around the moving body and measure information about the moving body (moving body information) in a stable and accurate manner.

Further, it is preferable that the control means classifies the feature points in the three-dimensional information of the first acquisition region into the feature points of the three-dimensional object and the feature points of the non-three-dimensional object, and controls the acquisition region based on the classification result.

Further, the control means may use a region containing a relatively large number of feature points of the non-three-dimensional object as the second acquisition region. This is because a non-three-dimensional object is extremely unlikely to be a moving object as compared with a three-dimensional object, and is suitable for use in generating an environmental map and measuring moving object information.

Further, the control means has a region containing a relatively large number of feature points of a non-three-dimensional object as one second acquisition region, and includes a relatively large number of feature points having a small reprojection error among the feature points of the three-dimensional object. The area may be set as another second acquisition area, and the second acquisition means may acquire the three-dimensional information of the first and other second acquisition areas at different timings.

In this case, in addition to the feature points of the non-three-dimensional object, the feature points of the three-dimensional object that are unlikely to be moving objects (feature points with a small reprojection error) may be included in the second acquisition region. As a result, the number of feature points used for generating an environmental map and measuring moving object information increases, and it is possible to generate an environmental map and measure moving object information with higher accuracy.

Further, the control means uses only the object having a height of 3.0 m or less from the reference surface (for example, the road surface on which the vehicle travels or the road surface of the sidewalk) as a three-dimensional object from the three-dimensional information of the first acquisition area. It is preferable to extract and perform the above classification. The upper limit of the height of the object to be extracted as a three-dimensional object is not limited to 3.0 m, and may be, for example, 2.5 m, 2.0 m, 1.5 m, or the like.

In this case, while eliminating the feature points of moving objects such as pedestrians and vehicles in front, the feature points of relatively tall buildings and installations are used as the feature points of stationary objects as well as the feature points of non-three-dimensional objects. It can be handled, the number of feature points used for generating an environmental map and measuring moving object information increases, and it is possible to generate an environmental map and measure moving object information more accurately.

Further, the second acquisition means is an image pickup device (for example, a monocular camera) whose posture can be changed, and the control means may control the posture of the image pickup device.

Further, the control means is the number obtained by subtracting the number of the feature points of the three-dimensional object from the number of feature points in the three-dimensional information of the second acquisition region among the plurality of known postures of the image pickup apparatus. It is preferable to control the posture of the imaging device with the most posture as the target posture.

In this case, since many feature points of non-three-dimensional objects are included in the shooting range (angle of view) of the image pickup apparatus, it is possible to generate an environmental map and measure moving object information with higher accuracy.

Further, the control means can control the posture of the image pickup device at an angular velocity at which the movement prediction value calculated from the position of the feature point in the three-dimensional information of the second acquisition region and the internal parameter of the image pickup device is 1 or less. preferable.

In this case, motion blur is less likely to occur, and it becomes easy to associate feature points in the three-dimensional information acquired by the first and second acquisition means, respectively.

Further, the second acquisition means is a rider, and the control means may control the light projection range of the rider. In this case, in order to control the second acquisition region, it is sufficient to control the light emission timing of the light source and the number of light emission points, so that a mechanism (for example, a hinge or the like) or a drive unit (for example, a hinge) used when changing the posture of the image pickup apparatus is required. For example, it is not necessary to provide a motor).

Further, according to the mobile device including the map generation / position measurement device of the above embodiment or each modification and the moving body (for example, a vehicle) on which the map generation / position measurement device is mounted, the control of the moving body is provided. Can be performed stably and with high accuracy.

Further, in the map generation / position measurement method of the above-described embodiment and each modification, three-dimensional information around a moving body (for example, a vehicle) (more specifically, three-dimensional position information of an object existing around the moving body) is obtained. The first acquisition step to acquire, the second acquisition step to acquire at least a part of the three-dimensional information of the first acquisition area, which is the area where the three-dimensional information is acquired in the first acquisition step, and the first Based on the three-dimensional information of the first acquisition area acquired in the acquisition process of, the second acquisition area, which is the area in which the three-dimensional information is acquired in the first acquisition area in the second acquisition process, is controlled. The movement is based on the three-dimensional information of the first acquisition area acquired in the first acquisition step and the three-dimensional information of the second acquisition area acquired in the second acquisition step. It is a method including a step of generating an environmental map around the body and measuring information about the moving body.

According to the map generation / position measurement method of the above embodiment and each modification, the second acquisition area, which is the area where the three-dimensional information is acquired in the second acquisition step, is, for example, stationary in the first acquisition area. It can be set in an area with a relatively large number of points.

As a result, it is possible to generate an environmental map and measure information about a moving body (moving body information) in a stable and accurate manner.

In the above embodiment and each modification, both the generation of the environmental map around the moving body and the measurement of the information about the moving body are performed based on the three-dimensional information of the acquisition area acquired in the second acquisition step. It is done, but it is also possible to do only one.

That is, the "method" of the present invention includes a first acquisition step of acquiring three-dimensional information around a moving body (more specifically, three-dimensional position information of an object existing around the moving body), and the first acquisition step. A second acquisition step of acquiring at least a part of the three-dimensional information of the first acquisition area, which is an area in which the three-dimensional information is acquired in the acquisition step of the above, and the first acquisition step acquired in the first acquisition step. A step of controlling a second acquisition area, which is an area in which the three-dimensional information is acquired in the first acquisition area in the second acquisition step, based on the three-dimensional information of the first acquisition area, and the first acquisition step. Based on the three-dimensional information of the first acquisition area acquired in the acquisition step of the above and the three-dimensional information of the second acquisition area acquired in the second acquisition step of the moving body. It is a method including a step of generating an ambient map and at least one of measuring information about the moving body.

Further, in the above-described embodiment and each modification, the case where the map generation / position measuring device is mounted on an automobile has been described as an example, but the present invention is not limited to this. For example, the map generation / position measuring device of the above embodiment or each modification may be mounted on a moving body such as a robot that can move autonomously, a vehicle other than an automobile, a ship, or an aircraft. In this case as well, the same effects as those of the above-described embodiment and each modification can be obtained.

In addition, the numerical values, shapes, and the like used in the above-described embodiment and the description of each modification can be appropriately changed without departing from the spirit of the present invention.

Hereinafter, the thinking process that led to the inventor's idea of the above embodiment and each modification will be described.

Conventionally, in the automatic operation of a moving body such as a vehicle, it is necessary to measure the moving body information such as the position, posture, and speed of the moving body itself. There is a technique called Visual-SLAM that detects information. Visual-SLAM is a method of simultaneously generating an environmental map including three-dimensional position information of surrounding stationary objects called an environmental map from a time-series image and estimating moving object information.

A general Visual-SLAM is performed using image feature points, and the environment map includes three-dimensional position information of the image feature points measured three-dimensionally by a stereo camera or a multi-view stereo. A method of estimating moving object information based on the three-dimensional coordinates of the image feature point and the coordinates in the image in which the image feature point is observed is already known.

However, in the conventional Visual-SLAM, since it is premised that the image feature points included in the environment map are absolutely stationary, there are many moving objects such as people and vehicles in the actual environment. In such an environment, there is a concern that the premise will be broken and the estimation accuracy of mobile information will decrease.

In particular, when measuring information about a vehicle (for example, the position, attitude, speed, etc. of the vehicle) or when generating an environmental map around the vehicle, there may be a vehicle in front of the vehicle or a vehicle crossing the front of the vehicle. Then, most of the image may be filled with image feature points with speed, so many image feature points with uncertain three-dimensional positions are mixed in the acquired image feature points, and an environmental map is generated. There is concern that the accuracy of measuring information about the vehicle will be significantly reduced. That is, when the environmental map is generated and the moving object information is measured using the feature points of the moving object, the accuracy of the environmental map generation and the measurement of the moving object information is lowered because the three-dimensional position of the moving object is uncertain. There is concern about doing so.

Therefore, it is conceivable to use a wide-angle lens for the camera and make the area of the moving object relatively small in the image. In this case, the spatial resolution is lowered and the position, orientation, and velocity are estimated. There is concern that the resolution will decrease.

Further, Patent Document 1 (Japanese Unexamined Patent Publication No. 2016-157197) describes a self-position estimation device, a self-position estimation method, and a self-position estimation device capable of generating an initial environmental map used for self-position estimation and estimating the self-position with high accuracy. The purpose is to provide a program. In Patent Document 1, in order to achieve this object, feature points of an object are extracted from a time-series image input in order from an image input unit (camera) 11, and the extracted feature points are tracked to calculate a movement vector. To do. Then, based on the calculated length and direction of the movement vector of the feature point, it is determined whether or not the feature point is a moving object, and when it is determined that the feature point is a moving object, it corresponds to the feature point. The object is removed from the time-series image, map information including the coordinates of the object of the feature point not removed in the world coordinate system is generated, and the self-position is estimated based on the generated map information. That is, in Patent Document 1, a moving object is excluded from the feature point extraction region for generating an environmental map and measuring moving object information.

However, even in Patent Document 1, in a situation where a moving object occupies most of the image, it is not possible to extract stationary feature points, and there is a concern that the accuracy of map information and self-position estimation will decrease.

Therefore, in Visual-SLAM, the inventor efficiently generates a highly accurate environment map even in an environment where there is a moving object around the moving body, and in order to realize highly accurate position estimation, the above-described embodiment and each modification Came to the idea.

For example, the map generation / position measuring device 100 of the above embodiment includes a stereo camera 200 and a monocular camera 300 that captures the inside of the field of view of the stereo camera 200, and stationary image feature points captured by the stereo camera 200. It is classified into a feature point that is present and a feature point that is moving, and the orientation of the monocular camera 300 is controlled according to the number of feature points that are stationary in the area in the image, and the image taken by the monocular camera 300 is stereo. The processing for detecting the feature points observed by the camera 200 is performed, and the environment map is generated and the self-position is detected by using the observation information of the stereo camera 200 and the observation information of the monocular camera 300.

1 ... Mobile device, 10 ... Vehicle (moving body), 100 ... Map generation / position measurement device (device), 200 ... Stereo camera, 300, 300'... Monocular camera, 300b ... Camera attitude control unit (control means), 400 ... Rider, position measurement unit 520 (means).

Japanese Unexamined Patent Publication No. 2016-157197

Claims (7)

A device mounted on a moving body
A first acquisition means for acquiring three-dimensional information around the moving body, and
A second acquisition means for acquiring at least a part of the first acquisition area, which is an area in which the first acquisition means acquires three-dimensional information, and a second acquisition means.
The acquired by the first acquisition means, and classifying the feature points definitive three-dimensional information of the first acquisition region to the feature points of the feature point and the non-solid objects of the three-dimensional object, on the basis of the classification result, A control means for controlling the second acquisition area, which is an area in which the second acquisition means acquires three-dimensional information in the first acquisition area,
Based on the three-dimensional information of the first acquisition area acquired by the first acquisition means and the three-dimensional information of the second acquisition area acquired by the second acquisition means, the said A means for generating an environmental map around the moving body and measuring at least one of the information about the moving body .
The control means is a device characterized in that a region containing a relatively large number of feature points of the non-three-dimensional object is set as the second acquisition region . The control means
Among the feature points of the three-dimensional object, a region containing a relatively large number of feature points having a small reprojection error is defined as the other second acquisition region.
Apparatus according to claim 1, characterized in that to get into the second acquisition means at different timings three-dimensional information of said one and other second acquisition region. The control means is characterized in that only an object having a height of 3.0 m or less from a reference plane is extracted as a three-dimensional object from the three-dimensional information of the first acquisition region and the classification is performed. The device according to claim 1 or 2. The second acquisition means is an image pickup device whose posture can be changed.
The device according to any one of claims 1 to 3, wherein the control means controls the posture of the image pickup device. The control means subtracts the number of feature points of the three-dimensional object from the feature points in the three-dimensional information of the second acquisition region from the plurality of known postures of the image pickup device. The device according to claim 4 , wherein the posture of the imaging device is controlled with the posture having the largest number as a target posture. The device according to any one of claims 1 to 5.
A mobile device including a mobile body on which the device is mounted. The first acquisition process to acquire the three-dimensional information around the moving body, and
A second acquisition step of acquiring at least a part of the three-dimensional information of the first acquisition area, which is an area in which the three-dimensional information is acquired in the first acquisition step,
The feature points in the three-dimensional information of the first acquisition region acquired in the first acquisition step are classified into the feature points of the three-dimensional object and the feature points of the non-three-dimensional object, and based on the classification result, the first In the second acquisition step, a step of controlling a second acquisition area, which is an area in which three-dimensional information is acquired in the first acquisition area, and a step of controlling the second acquisition area.
Based on the three-dimensional information of the first acquisition region acquired in the first acquisition step and the three-dimensional information of the second acquisition region acquired in the second acquisition step, the said and performing at least one of measurement information on the generation and the moving body environment map around the moving body, only including,
The control step is a method characterized in that a region containing a relatively large number of feature points of the non-three-dimensional object is set as the second acquisition region.

Images