Moving object, moving object control system, and moving object control method

Abstract

To provide a moving object, a moving object control system, and a moving object control method, which can maintain high position estimation accuracy and reduce an occurrence of a self-position loss.SOLUTION: A moving object control system comprises: a self-position estimation unit that performs self-position estimation on the basis of information of a camera image; an environmental map forming / updating unit that forms an environmental map on the basis of the information of the camera images; and an imaging control unit that controls position attitudes of the camera and lighting. The self-position estimation unit performs position estimation on the basis of detection positions of a plurality of feature points in the camera image, and motion assumption based on tracking of feature points. The environmental map forming / updating unit forms the environmental map by placing an acquired point group at the estimated position attitude. The imaging control unit controls the camera attitudes toward objects or feature areas and controls the lighting.SELECTED DRAWING: Figure 5

Description

[Technical field]

[0001] The present invention relates to a mobile object, a mobile object control system, and a mobile object control method. [Background technology]

[0002] Decommissioning work at nuclear power plants requires remotely operated survey robots and work robots for removing rubble, as the work is carried out in a high radiation environment. In order to operate such robots, environmental recognition technology equipped with sensors such as radiation-resistant cameras that can operate in high radiation environments is required. Radiation-resistant cameras generally produce low-quality images with noise and low resolution. Environmental recognition in low-quality images is prone to deterioration of position estimation accuracy and loss of self-position, so a robust method for low image quality is required. Operation in dark places and environments with external disturbances is also required.

[0003] In contrast, although it is not a radiation-resistant camera, Patent Document 1 proposes, with the aim of providing a vacuum cleaner that can capture images using a camera even in dark places, "an autonomous mobile body characterized by having a mobile main body, a camera mounted on the main body, detection means for detecting the brightness of the surroundings via the camera, and illumination means for relatively increasing the brightness in the image capturing direction of the camera at least when capturing an image with the camera when the brightness of the surroundings detected by the detection means is relatively low." [Prior art documents] [Patent documents]

[0004] [Patent Document 1] JP 2019-109854 A Summary of the Invention [Problem to be solved by the invention]

[0005] In dark places or environments with disturbances, feature-point-based environmental recognition using low-quality camera images such as those from radiation-resistant cameras deteriorates in feature-point matching performance, so it is necessary to apply an algorithm that improves the robustness of matching. If matching accuracy is poor, the position estimation accuracy of the remote-controlled robot may deteriorate or it may lose its own position.

[0006] In view of the above, an object of the present invention is to provide a mobile body, a mobile body control system, and a mobile body control method that can maintain high position estimation accuracy and reduce the occurrence of losing one's own position. [Means for solving the problem]

[0007] In view of the above, the present invention provides a mobile object control system having "a self-position estimation unit that estimates its own position based on camera image information, an environmental map creation and update unit that creates an environmental map based on camera image information, and a photography control unit that controls the position and attitude of the camera and lighting, wherein the self-position estimation unit estimates its position based on the detected positions of a plurality of feature points in the camera image and an assumption of camera movement made by tracking the feature points, the environmental map creation and update unit generates an environmental map by arranging the acquired point cloud in the estimated position and attitude, and the photography control unit controls the camera attitude in the direction of an object or a characteristic area, and controls the lighting."

[0008] In addition, the present invention defines a "mobile body equipped with a mobile body control system, the mobile body being equipped with a camera, lighting, and a moving means, characterized in that the positions and attitudes of the camera and lighting as well as the position and attitude of the moving means are controlled by the mobile body control system."

[0009] Furthermore, the present invention provides a mobile object control method in which, by means of a calculation unit of a computer, a self-location estimation process for estimating a self-location based on information from a camera image, an environmental map creation and update process for creating an environmental map based on information from the camera image, and a shooting control process for controlling the position and attitude of the camera and lighting, wherein the self-location estimation process estimates a position based on detected positions of a plurality of feature points in the camera image and motion assumptions based on feature point tracking, the environmental map creation and update process generates an environmental map by arranging the acquired point cloud in the estimated position and attitude, and the shooting control process controls the camera attitude in the direction of an object or a characteristic area and controls the lighting. Effect of the Invention

[0010] According to the present invention, it is possible to improve the robustness of self-location estimation in low-quality images in dark places and disturbance environments. [Brief description of the drawings]

[0011] [Figure 1] A diagram showing an example of a decommissioning work site inside a reactor building. [Figure 2A] FIG. 1 is a diagram showing a configuration example of a wheeled robot. [Figure 2B] FIG. 1 is a diagram showing a configuration example of an autonomous mobile robot. [Diagram 3] 1A and 1B are diagrams showing examples of images taken over time when the camera viewpoint is moved using a normal camera; [Figure 4] 11A and 11B are diagrams showing examples of images taken over time when the camera viewpoint is moved using a low-image-quality camera; [Diagram 5] FIG. 1 is a diagram showing an example of the configuration of a mobile object control system according to a first embodiment of the present invention. [Figure 6] FIG. 2 is a diagram showing the data processing content in the mobile object control system. [Figure 7] FIG. 2 is a diagram showing an example of the structure of data stored in a data storage unit M1; [Figure 8] FIG. 4 is a diagram showing an example of the data structure stored in an environmental map storage unit M2. [Figure 9]FIG. 2 is a diagram showing an example of a detailed configuration of a self-position estimation unit C2 according to the first embodiment of the present invention. [Figure 10] 4 is a diagram showing the processing contents of a movement assumption unit C22 and a feature point tracking unit C23 in a self-position estimation unit C2. FIG. [Figure 11] FIG. 2 is a diagram showing an example of the configuration of a mobile control system 13 that controls lighting 12 and a sensor 11. [Figure 12] 12 is a diagram showing an example of the configuration of a self-position estimation unit C2 in FIG. 11; [Figure 13] FIG. 12 is a diagram showing a specific example of the configuration of a feature amount extraction unit C1 in FIG. 11. [Figure 14] FIG. 12 is a diagram showing a specific example of the configuration of the feature amount extraction unit C1 in FIG. 11; [Figure 15] FIG. 2 is a diagram showing an overall processing flow executed by a calculation unit of a computer. [Figure 16] FIG. 16 is a diagram showing a detailed processing example configuration of processing step S13 in FIG. 15; [Figure 17] FIG. 16 is a diagram showing a detailed processing example configuration of processing step S13 in FIG. 15; [Figure 18] FIG. 16 is a diagram showing a detailed processing example configuration of processing step S12 in FIG. 15; [Figure 19] 11A to 11C are diagrams showing the effects of the present invention. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0012] Hereinafter, embodiments of the present invention will be described with reference to the drawings.

[0013] Before going into the description of the embodiments of the present invention, the premise of the present invention will be described with reference to FIGS.

[0014] Figure 1 shows an example of an investigation and work site. This site is a dark place with high radiation, where structure 1, rubble 2, and equipment 3 are located.

[0015] A mobile robot (hereinafter referred to as mobile unit 10) as shown in Fig. 2A and Fig. 2b is entered into a survey / work site to carry out environmental surveys and various tasks. Note that Fig. 2A employs a means of travel using wheels 15, while Fig. 2b employs a means of travel using independent walking 16, but various types of travel are applicable. In either case, mobile unit 10 is equipped with at least a sensor 11 that functions as a camera, lighting 12, a mobile unit control system 13, an actuator 14 for travel control (and various tasks), and a communication unit 17. Note that the power source may be built-in or may be externally powered via a cable.

[0016] FIG. 3 is a diagram showing an example of images in a time series when the camera viewpoint P is moved by a normal camera in a non-high radiation environment. The frame image F0-frame image F5 at the camera viewpoints P1-P5 shown by the arrows in FIG. 1 are shown. In contrast, FIG. 4 is a diagram showing an example of images in a time series when the camera viewpoint P is moved by a low-image-quality camera such as a radiation-resistant camera that can operate even in a high radiation environment. Compared to FIG. 3, the images have a lower resolution and contain noise-like information, making it difficult for the mobile control system 13 to perform feature-point-based self-location estimation processing. EXAMPLES

[0017] The first embodiment will be described with reference to Fig. 5 to Fig. 10. Based on the above premise, in the first embodiment of the present invention, a mobile object control system 13 is configured as shown in Fig. 5. The mobile object control system 13, which is the core of the mobile object 10, is configured by a computer having a calculation unit C and a memory unit M.

[0018] In terms of the processing contents, the calculation unit C can be said to include a feature extraction unit C1, a self-position estimation unit C2, a key frame detection unit C3, a key frame processing unit C4, an environmental map creation / update unit C5, and a mobile unit control unit C6. The memory unit M includes a data storage unit M1 that stores data (feature D2, movement amount D3, selected key frame D5) as intermediate products, an environmental map storage unit M2 that stores the environmental map created by the environmental map creation / update unit C5, and a feature dictionary (feature database) M3 that is referenced when determining feature amounts.

[0019] 6 is a diagram showing the data processing content in the mobile object control system 13. In the following figures, a parallelogram represents data D, and a rectangle represents a processing function in the calculation unit C.

[0020] The mobile object control system 13 acquires data D1 (hereinafter simply referred to as frame image) of frame image F in time series, such as those shown in Figures 3 and 4, from the sensor 11. For this reason, the sensor 11 has a camera function, but it is more preferable that the sensor 11 be a three-dimensional camera such as a stereo type camera in order to calculate the position and distance described later.

[0021] The feature extraction unit C1 extracts multiple features from one frame image D1 to obtain feature data D2 as a group of feature points. Here, feature points based on SLAM (Simultaneous Localization and Mapping) are assumed as the features, and it is preferable to extract feature points such as SIFT (scale invariant feature transform) or ORB (Oriented FAST and Rotated BRIEF). Many methods of extracting features are known, and an appropriate method can be applied in the present invention.

[0022] The feature amount data D2 obtained here is stored in the data storage unit M1. Fig. 7 shows an example of the data structure stored in the data storage unit M1. The feature amount data D2 stored in Fig. 7 includes an INDEX (D11) which is an ID in the order of data acquisition, an acquisition time D12 displayed in year, month, day, and second (yyyymmddhhmmss), an image (image data) D13, and data of a feature point group A (hereinafter referred to as p), which is a feature point group acquired from one frame image and is a vector group in which each feature point has multidimensional information. A It is composed of D14.

[0023] Returning to Fig. 6, the self-position estimation unit C2 tracks feature points at the same location in the environment for each successive image frame to determine the position of the sensor 11 (the position of the moving body 10) and the distance and angle to the feature points. At this time, by continuously processing a plurality of feature points per frame, the estimation accuracy can be improved. The present invention is characterized by the estimation method of the self-position estimation unit C2, and a specific processing configuration example is shown in Fig. 9, and a detailed description will be given later.

[0024] As a processing result in the self-position estimation unit C2, data D3 on the position and orientation of the sensor 11 of the moving object 10 is obtained. The data D3 on the position and orientation of the sensor 11 of the moving object 10 is also stored in the data storage unit M1. The data D3 on the position and orientation stored in the data storage unit M1 in Fig. 7 includes data D15 on the position t expressed by the vectors x, y, and z, linked to INDEX (D11), and data D16 on the orientation R expressed by a 3x3 rotation matrix.

[0025] Returning to FIG. 6, the data D3 on the position and orientation of the sensor 11 of the moving body 10 is subsequently processed by the key frame detection unit C3, and an image selected from a plurality of images consecutive in time series is obtained as a candidate key frame D4. The key frame detection unit C3 detects frame images to be used for correcting the environmental map and the position and orientation as key frame images, and secures frames detected under certain conditions as candidate key frames D4. The key frame processing unit C4 selects a frame image to be finally used for correcting the environmental map and the position and orientation from the candidate key frame data D4 as a selected key frame D5. As a method for selecting a key frame, it is preferable to select a frame as a key frame as follows. For example, it is preferable to use a method of acquiring key frames evenly according to the amount of movement, or a method of using one frame of a frame group in which a large number of environmental feature points are stably and continuously acquired as a key frame. It is also preferable to use a method of selecting a frame in which the viewpoint has changed significantly and the movement of the feature points has been large as a key frame. Furthermore, it is also useful to extract only images that are relatively clear and have little noise. Such key frame selection can contribute to highly accurate position and orientation detection and highly accurate map creation. The selected key frame D5 is also stored in the data storage unit M1.

[0026] The selected key frame data D5 stored in the data storage unit M1 in FIG. 7 is data D17 of the feature point group B. The data D17 of the feature point group B is p B These are feature points obtained from one frame image using a feature point tracking algorithm (feature point tracking unit C23 in FIG. 9, which will be described later) that is a feature of the present invention.

[0027] The environmental map creation / update unit C5 calculates the distance and angle between the sensor 11 and the group of feature points using the data (feature amount data D2, position and orientation data D3, and selected key frame data D5) stored in the data storage unit M1 in Fig. 7, and creates a new map of the field environment shown in Fig. 1, or adds the newly calculated distance data to an existing map to update the map. Note that there are many known methods for creating / updating an environmental map using the feature amount data D2, position and orientation data D3, and selected key frame data D5, and any appropriate method can be applied in the present invention.

[0028] The environment map created by the environment map creating / updating unit C5 is stored in the environment map storage unit M2, an example of which is shown in Fig. 8. The data constituting the environment map storage unit M2 are the acquisition time D22, the position D23, the attitude D24, and the feature point group A. However, since the environment map creating / updating unit C5 performs processing using the selected key frame data D5, these data are linked to the key frame number D21 of the selected key frame data D5, and further, a flag D26 that distinguishes between true and false values ​​is assigned to this data group linked to the key frame number D21. The flag D26 distinguishes whether the feature point group of this key frame is used in the environment map (True) or not (False).

[0029] The data stored in the data storage unit M1 and environmental map storage unit M2 formed as described above includes position and orientation information, but in the data storage unit M1, the position and orientation information D15, D16 is treated as relative information between the sensor 11 and the target object (1, 2, 3 in FIG. 1), while in the environmental map storage unit M2, the position and orientation information D23, D24 is treated as absolute information on the coordinate system indicating the on-site environment, which will be convenient for subsequent application scenarios.

[0030] By the above series of processes in Fig. 6, the self-position of the mobile object 10 is obtained in the data storage unit M1. Also, information on the distance between the mobile object and each position in the field environment (this corresponds to a map) is obtained in the environmental map storage unit M2. The mobile object control unit C6 in Fig. 6 autonomously determines a moving route according to the self-position stored in the data storage unit M1 and the distance stored in the environmental map storage unit M2, or determines an actuator control value D7 to move along a moving route instructed from the outside via a cable or the like, and the actuator 14 moves the mobile object 10 according to this.

[0031] 9 is a diagram showing a detailed configuration example of the self-location estimation unit C2 according to the first embodiment of the present invention. The self-location estimation unit C2 finally outputs data D3 on the amount of movement and data D6 on failure of self-location estimation based on the feature amount D2 and the on-site environment map information stored in the environment map storage unit M2.

[0032] The self-location estimation unit C2 in Fig. 9 is mainly composed of an initialization determination unit C21, a motion assumption unit C22, a feature point tracking unit C23, a feature amount matching unit C24, and a location estimation calculation unit C25. First, the initialization determination unit C21 clears and initializes the information stored in the environmental map storage unit M2 when the moving body 10 is first introduced into the field environment. Alternatively, if the current field environment map has been acquired from information from another moving body 10, it is set to the most recently acquired information. Note that both of these cases are referred to as initialization.

[0033] In the self-location estimation unit C2 of the basic configuration according to the first embodiment of the present invention, the following three-stage process is performed to robustly continue tracking. Note that Fig. 10 is a diagram showing the process contents of the movement assumption unit C22 and the feature point tracking unit C23 in the self-location estimation unit C2.

[0034] First, the motion assumption unit C22 performs position and orientation estimation based on a motion assumption. The motion assumption is, for example, assuming uniform linear motion and estimating the position of the current frame from the amount of movement (amount of change in position and orientation) in past consecutive frames. For example, the amount of change in position and orientation in the past two frames at time t-1 and time t-2, i.e., the speed per frame, is calculated, and it is assumed that the motion is at the same speed from time t-1 to the current frame (time t), and the positions of feature points observed in the past are estimated from the positions of the position and orientation based on this assumption. If the feature point exists in the current frame within a predetermined range from the position of the estimated feature point, it is deemed to have been estimated correctly.

[0035] The left part of FIG. 10 shows a schematic diagram of the processing contents of the motion assumption unit C22. Here, three consecutive moving object positions (sensor positions) P t-2 and P t-1 and P t , and the image frame F t-2 and F t-1 and F t and illustrate the positions of feature points in the actual environment. Points within an image frame F are the positions of multiple feature amounts extracted from one frame, and points outside frame F represent the positions of feature points in the actual environment. Note that here, the matrix that changes the relative position and orientation between each moving object position P is set as δ.

[0036] Image Frame F t-2 and F t-1 is the moving object position (sensor position) P t-2 and P t-1 The two frames on the left and center are taken from t-2 and F t-1 is past information, and the right frame F t In this case, if the position and orientation of the current frame are estimated from the movement of the past two frames on the left side, and the projected point is detected within a certain range, high-speed position estimation is possible.

[0037] That is, in the left side of FIG. 10, for example, the sensor position P t-2 Image frame F when the site position is captured fromt-2 The feature point positions in and the sensor position P t-1 Image frame F when the site position is captured from t-1 The feature point positions in and the sensor position P t-2 and the sensor position P t-1 When the change in the relative position and orientation between the frames (transformation matrix δ) is measured, the sensor position P t-1 The image frame F when the site position is captured from the location Pt which is further transformed by the transformation matrix δ t In this method, it is possible to estimate the position of a feature point in a frame, and when a feature can be observed within a certain width from the estimated position, the position can be estimated to the corresponding position for each point. In this case, since the moving body (sensor) does not only move in a straight line, the estimation must take into account changes in direction (changes in posture).

[0038] Based on the above-mentioned idea, the motion assumption unit C22 estimates the position and orientation of where the current frame should be based on the motion assumption of the feature point detection position from the past two frames. Note that when such an estimation is performed, there are cases where the estimated position of the next frame cannot be detected, and a flag D6a indicating whether the frame position can be estimated or not is set. The motion assumption unit C22 further estimates the frame F t The position information of the intra-frame feature observed at position Pt in is estimated.

[0039] The right part of Fig. 10 shows a schematic of the processing contents of the feature point tracking unit C23. Here, based on the characteristic that the images in the previous and current frames are similar, feature points between the previous and current frame images are tracked, and 3D measurement is performed by epipolar constraint from the correspondence of feature points between the two frames to obtain the position and orientation. This processing is generally called visual odometry.

[0040] As a specific circuit configuration for this purpose, the feature point extraction unit C231 extracts the previous frame F t-1 The feature points are extracted from the frame F estimated by the motion assumption unit C22. tFor each of these past and current feature sets, the feature point tracking unit C232 performs a feature point tracking process on each of the past feature points in the current frame F. t Track which of the feature points on the image correspond to each other. A good method for tracking this is to use the Lucas-Kanade optical flow method, which generates all combinations of the old and new feature points and eliminates combinations with low likelihood of orientation.

[0041] Using multiple feature point pairs (combinations of feature points that correspond to each other in previous and next frames) after feature point tracking, a visual odometry estimation unit C233 then estimates the position (distance, direction) and orientation between the sensor 11 and the feature points in the field environment using the Ebipolar constraint equation, the five-point algorithm, and RANSAC. The position and orientation estimation results are obtained as information E=(R, t) of position t and orientation R in a fundamental matrix calculation unit C234. As a result of processing in the feature point tracking unit C23, a flag D6b indicating whether or not estimation is possible is set for information that could not be tracked.

[0042] The information E=(R, t) on the position t and orientation R is stored in the data storage unit M1, and then stored in the environmental map storage unit M2 via the environment / map creation unit C5. As a result, the processing result of the feature point tracking unit C23 is reflected in the processing of the movement assumption unit C22, thereby making it possible to complement and integrate the position / orientation estimation function.

[0043] In the process of FIG. 10, at least the image frame F t-2 and F t-1 This information is obtained from the environmental map storage unit M2, which stores information on key frames. By using the information on the feature amounts in the environmental map storage unit M2, various estimation processes can be performed based on highly reliable information.

[0044] The feature matching unit uses a common method called Bag-of-visual words (BoVW) to match feature points. It finds the correspondence between feature points observed in a frame and feature points observed in a key frame, and obtains the correspondence information between the feature points of the frame and 3D points based on that. If feature matching fails, it sets a flag D6 indicating that self-location estimation has failed.

[0045] In this process, it is advisable to utilize the feature dictionary storage unit M3. It is sufficient to select an appropriate location within the frame as the feature, but in this case, it is possible to create a database of locations that are known to be advantageous for performing various calculations with high accuracy, as well as their shapes and characteristics, and create a dictionary, which can be used as reference information when comparing new and old feature values, thereby improving accuracy.

[0046] 9, as described above, the tracking results in the feature point tracking unit C23 are reflected in the processing in the movement assumption unit C22, thereby improving accuracy. Using the processing results of the feature point tracking unit C23, the movement assumption unit C22, and the feature amount matching unit C24, data D10 of the amount of change in one posture between the past and the present is found, and data D3 of the amount of movement is provided to the position estimation calculation unit C25.

[0047] In addition, when the self-location estimation unit C2 sets the flag D6a or D6b indicating whether or not self-location estimation is possible in the processing of Figure 9, or when it issues a self-location estimation failure D6, the re-self-location estimation unit C2R extracts a new past keyframe image from the environmental map storage unit M2, selects a new image D1 based on the new past keyframe image, and re-executes the processing from the feature extraction unit C1 to the self-location estimation unit C2, thereby preventing the subsequent processing from being performed based on unstable data and the resulting unstable, unreliable results.

[0048] In the first embodiment of the present invention, by using position estimation based on a motion assumption by tracking feature points between consecutive frame images in this way, it is possible to reduce the possibility of being lost due to a failure in feature point matching. EXAMPLES

[0049] In the first embodiment, a description is given of the moving object 10 and the moving object control system 13 mounted on the moving object 10. In contrast, in the second embodiment, a description is given of a moving object control method. The second embodiment will be described with reference to Figs. 11 to 14.

[0050] In the mobile object control method of the second embodiment, the lighting 12 and the sensor 11 provided in the mobile object 10 in Fig. 1 are controlled. Fig. 11 is a diagram showing a configuration example of a mobile object control system 13 that controls the lighting 12 and the sensor 11. Fig. 11 basically adopts the configuration in Fig. 6, and since the part indicated by the bold line is newly added to the configuration in Fig. 6, the part indicated by the bold line will be mainly described.

[0051] 11, the feature extraction unit C1 determines a lighting control value D8 to control the lighting amount by the lighting 12, and the feature extraction unit C1 also determines an actuator control value D7 to control the position and attitude of the moving body 10 by the actuator 14. The position and attitude control value D9 actually detected at this time is used for position estimation calculation in the self-position estimation unit C2.

[0052] In the second embodiment, by adopting the above configuration, in short, when the feature extraction unit C1 determines that lighting is necessary, the lighting 12 is controlled, and the feature extraction unit C1 controls the actuator 14 (assuming position and orientation control of the image sensor 11) in the direction with the most features.

[0053] The details of the additional functions written in bold in FIG. 11 are illustrated in FIG. 12 for the self-location estimation unit C2, and in FIG. 13 and FIG. 14 for the feature amount extraction unit C1.

[0054] First, regarding the processing of the self-position estimation unit C2 in Fig. 12, only the bolded parts here are different from Fig. 9. In Figs. 9 and 12, the position and orientation change amount D10 obtained by image processing from the image sensor 11 includes a bias, but by using actually detected position and orientation information D9, it is possible to offset the bias included in the current position and orientation information D10 of the image sensor 11. The position estimation calculation unit C25 executes offset processing. In the offset processing, for example, the difference between the position and orientation from the image processing and the detected position and orientation is obtained, and the difference is excluded from the position and orientation from the image processing.

[0055] Fig. 13 is a diagram showing a specific configuration example of the feature amount extraction unit C1 in Fig. 11. The main function of the feature amount extraction unit C1 is to calculate a feature amount (group of feature points) D2 for an image input D1 by a feature point extraction unit C11. Therefore, in Fig. 13, the parts other than the feature point extraction unit C11 represent the parts related to the second embodiment. Specifically, the following processing functions have been added.

[0056] First, based on the number of feature points extracted by the feature point extraction unit C11, the additional processing determination unit C12 determines whether additional processing is necessary. For example, when 10 or more feature points should be obtained for each frame, but it is not possible to secure enough feature points to be used as features due to measurement circumstances such as image blurring, additional processing is required.

[0057] In this case, the present invention attempts to change the lighting and position and orientation to create an environment in which 10 or more feature points can be acquired. Specifically, when the additional processing determination unit C12 determines that additional processing is necessary, the image analysis unit C13 calculates the number of feature points for each specified image region, the edge amount D12, and the brightness and contrast D13 of the entire image.

[0058] An actuator control value calculation unit C14 calculates an actuator control value D7 for controlling the position and orientation of the image sensor 11 in a direction that makes it easy to capture features, based on the number of feature points and edge amount D12 for each image region. The actuator control value D7 here assumes pan-tilt of the location where the image sensor 11 is attached. Also, an illumination adjustment amount calculation unit C15 calculates an illumination control value D8 based on the brightness value and contrast D13 of the image.

[0059] Figure 13 shows a mobile control system that can point the camera in the direction of a characteristic area. In this case, if the characteristic area is known, the position and orientation are controlled so that the camera faces in that direction, and if the characteristic area is not known, the characteristic area is recognized and the position and orientation are controlled so that the camera faces in that direction, thereby ensuring the number of feature points.

[0060] For example, by shining light on a wall or ceiling and providing a camera and lighting attitude control function that recognizes characteristic areas and tracks their direction, it is possible to determine when the light is hitting the wall well and the characteristic wall is clearly visible, when there is not enough light in the direction of the back and there are no characteristics, and by pointing the light towards the ceiling, it is possible to determine when the ceiling's features are easily captured, making it easier to identify the areas that should be used as characteristic quantities.

[0061] The function of Fig. 13 can be said to be equipped with the function of a shooting control unit that controls the camera attitude and lighting attitude. The shooting control unit has the function of recognizing objects or characteristic areas at a specified distance and controlling the camera attitude in the characteristic direction for the camera installed in the upward and horizontal directions and the lighting that can be controlled to a position linked to the camera's shooting direction.

[0062] Fig. 14 is an example in which an image processing function has been added to Fig. 13, and this function is written in bold. In Fig. 14, an image processing unit C16 performs image processing to reduce the influence based on the number of feature points for each predetermined image region obtained by the image analysis unit C13, the edge amount D12, and the brightness and contrast D12 of the entire image. The image processing includes removal of haze, gamma correction, smoothing, sharpening, masking of illumination and unnecessary information regions, etc., and these can be realized by using a rule-based method or a machine learning method such as a neural network.

[0063] By extracting features again from an image that has been corrected by image processing, it becomes easier to obtain the required number of features. EXAMPLES

[0064] In the third embodiment, a mobile object control method for enabling various processes and controls to be executed using a computer will be described. The third embodiment will be described with reference to Figs.

[0065] Fig. 15 is a diagram showing the overall processing flow executed by the calculation unit of the computer. In processing step S11, a sensor image is acquired, and in processing step S12, features are extracted in the feature extraction unit C1. In processing step S13, the self-location estimation process is performed in the self-location estimation unit C2. Note that a detailed processing example of processing step S12 is shown in Fig. 18, and a detailed processing example of processing step S13 is shown in Fig. 16 and Fig. 17, and will be described in detail later.

[0066] In process step S14, it is determined whether or not the self-location estimation is successful. If it is unsuccessful (process step S14 No), the estimation success / failure flag D6a, D6b or failure flag D6 is output. If it is unsuccessful, the process of the self-location re-estimation unit C2R is performed in process step S20, and the process returns to the top of the process without executing the subsequent processes, and the process is restarted from acquiring the sensor image in process step S11.

[0067] If self-location estimation is successful (Yes in process step S14), the following processes are sequentially executed: key frame extraction (process step S15), key frame determination and selection process (process step S15), data storage process in data storage unit M1 (process step S17), environmental map creation and update process (process step S18), and moving body control value output (process step S19).

[0068] A detailed processing example of processing step S12 is shown in Fig. 18. First, feature points are extracted in processing step S121, and it is confirmed whether the number of feature points meets a predetermined number in processing step S122. If the number meets the predetermined number (Yes in processing step S122), there is no need to execute the subsequent processing in processing step S12, and the processing is skipped.

[0069] If the predetermined number is not met (No in process step S122), the processes following process step S123 are executed, and processes such as lighting and movement are performed so that the necessary number is ensured. Specifically, for example, the number of feature points and edge amount D12 for each predetermined image region are calculated (process step S123), a direction in which features are likely to be found (a direction with many features) is estimated based on the number of feature points and edge amount D12 for each image region (process step S124), brightness and contrast D13 for the entire image are obtained (process step S125), and a lighting control value D8 is calculated based on the brightness value and contrast D13 of the image (process step S126).

[0070] Furthermore, it is determined whether or not it is advisable to execute noise reduction processing by image processing (processing step S127), and if necessary (processing step S127 Yes), noise reduction processing by image processing is executed (processing step S128).

[0071] Fig. 16 shows a detailed processing example configuration example of processing step S13 in Fig. 15. This part corresponds to the processing of the self-position estimation unit C2. First, in processing step S131, it is determined whether the environmental map is initialized, and in processing step S132, it is determined whether it has been initialized. If it has not been initialized, the process proceeds to processing step S140 where initialization is performed. After the initialization is performed, the process proceeds to processing step S133.

[0072] In the initialized state, estimation of the position and orientation is performed based on a motion assumption in process step S133. In process step S134, it is determined whether or not estimation of the position and orientation is possible, and if estimation of the position and orientation is possible (Yes in process step S134), the process proceeds to process step S135 where calculation of position and orientation estimation is performed.

[0073] If position and orientation estimation is not possible (No in process step S134), the process proceeds to process step S136 where position and orientation estimation is performed by tracking feature points. This estimation result is used to determine again in process step S137 whether position and orientation estimation is possible, and if position and orientation estimation is possible (Yes in process step S137), the process proceeds to process step S135 where calculation of position and orientation estimation is performed.

[0074] Furthermore, if it is determined in step S137 that position and orientation estimation is not possible (No in step S137), the process proceeds to step S138 where position and orientation estimation is performed by feature matching. From the estimation result, it is determined again in step S138 whether position and orientation estimation is possible, and if position and orientation estimation is possible (Yes in step S138), the process proceeds to step S135 where calculation of position and orientation estimation is performed.

[0075] According to the above flow, in the processing stage of the motion assumption unit C22 on the left side of Fig. 10, a new frame position at the same distance is estimated from the past two frames, and the feature point positions within the frame that will be grasped at that position are estimated. If there is no difference between the estimation result of this new frame and the obtained frame at the current time (the positions of all feature points within the frame almost match), then position estimation from this information is possible, and there is no need to perform feature point tracking or even feature point matching.

[0076] If it is not possible to estimate the position and orientation using the information on the assumed motion, the feature point tracking unit C23 performs processing, and if it is possible to estimate the position from the information obtained as a result, there is no need to perform feature point matching.

[0077] In this way, in the present invention, position and orientation are estimated in three stages. As a result, if an estimation is possible at an early stage, this can contribute to speeding up the judgment process, and as a result of performing confirmation processing up to the later stages, high accuracy can be ensured.

[0078] Fig. 17 is a diagram in which an offset function in the position estimation calculation unit C25 in Fig. 12 is added to Fig. 16. In process step S141, it is confirmed that a position and orientation value is input from the actuator, and in process step S142, an offset addition process for the position and orientation is performed.

[0079] Fig. 19 is a diagram showing the effect of the present invention. As shown in the example on the left for the conventional case and on the right for the present invention, when a moving body changes its position and orientation at a steep angle, there are cases where the feature amount in the frame is lost and the movement assumption unit is unable to determine its own position, but the present invention can significantly reduce the possibility of losing position by further performing a downstream determination processing function and further performing illumination and position and orientation control.

[0080] Through the above multiple examples, according to the present invention, the possibility of being lost due to failure in feature point matching is reduced by using position estimation based on a motion assumption by tracking feature points between consecutive frame images. Furthermore, by irradiating the walls and ceiling with light, recognizing a characteristic area, and providing a camera and light attitude control function that tracks its direction, it is possible to stably recognize the environment even in a dark place and in an environment with external disturbance. These embodiments are merely examples and do not limit the technical scope of the present invention. The present invention can take various other embodiments, and further, various modifications such as omissions and substitutions can be made within the scope of the present invention. These embodiments and their modifications are included in the scope and gist of the invention described in this specification, etc., and are included in the scope of the invention described in the claims and their equivalents. [Explanation of symbols]

[0081] 10: Mobile 13: Mobile control system C: Arithmetic section M:Memory part C1: Feature extraction unit C2: Self-position estimation part C3: Keyframe detection section C4: Keyframe processing section C5: Environmental Mapping and Update Division C6: Mobile control unit M1: Data storage M2: Environmental map storage section M3: Feature dictionary

Claims

1. The system includes a self-location estimation unit that estimates a self-location based on information from a camera image, an environmental map creation / update unit that creates an environmental map based on information from the camera image, and a photography control unit that controls the position and orientation of the camera and lighting, the self-position estimation unit performs position estimation based on detected positions of a plurality of feature points in a camera image and a motion assumption based on feature point tracking; the environmental map creation / update unit generates an environmental map by arranging the acquired point cloud in the estimated position and orientation; A mobile control system, characterized in that the photography control unit controls the camera attitude in the direction of an object or a characteristic area and controls lighting.

2. The mobile object control system according to claim 1, The mobile object control system, wherein the self-position estimation unit comprises: a motion estimation unit that estimates a next frame and its feature point positions from two past frames and their feature point positions for position estimation; a feature point tracking unit that tracks feature points of the next frame from the feature points of the past frames; and a feature amount matching unit that matches and identifies the feature points of the past frame and the feature points of the next frame.

3. The mobile object control system according to claim 2, The self-location estimation unit sequentially executes the processing of the motion estimation unit, the feature point tracking unit, and the feature matching unit, and performs self-location estimation processing when the feature point positions can be estimated, and executes the processing of the next stage when the feature point cannot be estimated.

4. A moving body equipped with the moving body control system according to any one of claims 1 to 3, A mobile body comprising a camera, lighting, and a moving means, the positions and orientations of the camera and lighting, and the position and orientation of the moving means being controlled by the mobile body control system.

5. The moving body according to claim 4, A moving body is characterized in that the moving body is provided with a means for detecting its own position and corrects its own position determined from information on a camera image.

6. The moving body according to claim 4, A moving body capable of controlling lighting to a posture linked to the shooting direction of a camera, recognizing an object or a characteristic area at a specified distance, and controlling the camera posture in the characteristic direction.

7. A mobile object control method, comprising: a calculation unit of a computer that executes a self-location estimation process for estimating a self-location based on camera image information; an environmental map creation / update process for creating an environmental map based on the camera image information; and a photographing control process for controlling the positions and orientations of a camera and lighting, the method comprising the steps of: In the self-location estimation process, a location is estimated based on detected positions of a plurality of feature points in a camera image and a motion assumption based on tracking of the feature points; In the environmental map creation and update process, an environmental map is generated by arranging the acquired point cloud in the estimated position and orientation; A mobile object control method, characterized in that the shooting control process controls the camera attitude in the direction of an object or a characteristic area and controls lighting.

8. The moving object control method according to claim 7, The mobile object control method is characterized in that the self-position estimation process includes a motion estimation process that estimates the next frame and its feature point positions from two past frames and their feature point positions for position estimation, a feature point tracking process that tracks feature points of the next frame from the feature points of the past frames, and a feature amount matching process that matches and identifies the feature points of the past frame and the feature points of the next frame.

9. The moving object control method according to claim 8, The method for controlling a mobile object is characterized in that, in the self-position estimation process, the motion estimation process, the feature point tracking process, and the feature matching process are executed in sequence, and when the feature point positions can be estimated, the self-position estimation process is performed, and when the feature point cannot be estimated, the next stage of processing is executed.

Images