Dynamic Environment Navigation Method Based on Visual Information

Abstract

The present invention proposes a dynamic environment navigation method based on visual information, mainly solving the problems of poor processing of indoor dynamic targets and automatic navigation and positioning in the prior art. The solution is as follows: establish a human dynamic model and parameterize it; use the DeeplabV3+ neural network to perform region segmentation on indoor scene images to obtain a scene segmentation model; perform multi-view geometric operations based on the scene segmentation model trained by the neural network; regress the image after multi-view geometric operations to the ORBSLAM system, and use the visual odometry VIO module in the system to estimate the pose of the camera; use loop detection to optimize the pose and output a global pose, and reconstruct a 3D mesh according to the global pose; generate a 3D point cloud using the reconstructed 3D mesh, build a global map on the 3D point cloud, and the robot uses the global map to autonomously navigate in a crowded indoor dynamic environment. The present invention improves the navigation performance and can be used for indoor positioning.

Description

Technical Field

[0001] The present invention belongs to the field of computer vision, and particularly relates to a dynamic environment navigation method, which can be used for indoor positioning. Background Art

[0002] In recent years, related technologies and industries of location-based services have been developing towards indoor areas to provide ubiquitous location-based services. The main driving force is the huge application and commercial potential brought by indoor positioning. Continuously and reliably providing location information in indoor and outdoor environments can bring a better user experience to users. Outdoor positioning and location-based services have been mature. Currently, the Global Positioning System (GPS) is the most commonly used positioning and navigation system. However, since satellite signals cannot penetrate buildings, indoor positioning cannot be achieved using GPS. Many unique features of indoor environments limit many existing solutions. Practical indoor positioning solutions need to meet the following seven requirements: accuracy, coverage, reliability, cost, power consumption, scalability, and response time.

[0003] Existing indoor positioning technologies are mainly divided into the following six categories:

[0004] The first category is infrared positioning technology. Infrared technology has been very mature, and the positioning accuracy for indoor use is relatively high. However, since infrared can only propagate in a line-of-sight manner and has extremely poor penetration, it cannot work properly when the identifier is blocked and is also extremely susceptible to environmental factors such as lighting and smoke. In addition, the transmission distance of infrared is not long, so in terms of layout, no matter which method is used, receiving ends need to be installed behind each occlusion and even at corners. The layout is complex and the positioning effect is limited.

[0005] The second category is radio frequency identification (RFID) indoor positioning technology. This type of technology uses radio frequency to adjust the radio signal into an electromagnetic field by a fixed antenna, so that the tag attached to the item generates an induced current after passing through the magnetic field to transmit data, and exchanges data through multi-pair bidirectional communication to achieve the purpose of identification and triangulation positioning. However, it does not have communication capabilities, has poor anti-interference capabilities, and is not convenient to integrate into other systems.

[0006] The third category is Bluetooth indoor positioning technology. The biggest advantage of Bluetooth indoor positioning technology is that the device has a small volume, short distance, low power consumption, and is easy to integrate into mobile devices such as mobile phones. Although Bluetooth transmission is not affected by line-of-sight, for complex spatial environments, the stability of the Bluetooth system is slightly poor, it is greatly interfered by noise signals, and the price of Bluetooth devices and equipment is relatively expensive.

[0007] The fourth category is Wi-Fi indoor positioning technology. There are two types of Wi-Fi positioning technologies. One is to use the wireless signal strengths of a mobile device and three wireless network access points, and through differential algorithms, to perform relatively accurate triangulation positioning on people and vehicles. The other is to pre-record the signal strengths of a huge number of determined location points, and determine the location by comparing the signal strength of the newly added device with a database with a huge amount of data. However, the accuracy of this type of technology for indoor positioning can only reach about 2 meters, and precise positioning cannot be achieved.

[0008] The fifth category is ZigBee indoor positioning technology. ZigBee is a new short-range, low-rate wireless network technology. These sensors only require very little energy and relay data from one node to another via radio waves. As a low-power and low-cost communication system, ZigBee has very high working efficiency. However, the signal transmission of ZigBee is greatly affected by multipath effects and movement, and the positioning accuracy depends on the physical quality of the channel, the signal source density, the environment, and the accuracy of the algorithm, resulting in a relatively high cost of the positioning software, and there is still a large room for improvement.

[0009] The sixth category is UWB ultra-wideband indoor positioning technology. Ultra-wideband positioning technology is a brand-new technology that is very different from traditional communication positioning technologies. It uses pre-deployed anchor nodes and bridge nodes with known positions to communicate with newly added blind nodes, and uses triangulation positioning or "fingerprint" positioning methods to determine the position. However, this technology has a relatively high power consumption because the newly added blind nodes also need to actively communicate, and layout is also required in advance, making the cost unable to be reduced.

[0010] All of the above six technologies have problems such as poor noise resistance or high cost. Therefore, a new technology is needed to achieve navigation in indoor environments, especially dynamic environments.

[0011] With the popularity of robots, drones, driverless vehicles, VR / AR in recent years, the Simultaneous Localization and Mapping (SLAM) technology has also become well-known. This technology has gradually been introduced into indoor positioning due to its advantages such as high accuracy and low cost. The SLAM technology was first proposed in the field of robotics. It means that a robot starts from an unknown location in an unknown environment, locates its own position and attitude during movement by repeatedly observing environmental features, and then constructs an incremental map of the surrounding environment based on its own position, so as to achieve the purpose of simultaneous localization and mapping, as Figure 1 shown. Due to the important academic value and application value of SLAM, it has always been regarded as the key technology for realizing fully autonomous mobile robots.

[0012] As the core technology of robot positioning and navigation, SLAM is attracting increasing attention in the industry. However, in practical applications, SLAM technology is not exactly the same as robot autonomous positioning and navigation. The core process of SLAM mainly includes three major steps: preprocessing, matching, and map fusion. Among them, preprocessing is to obtain the environmental information of the location through lidar or other sensors, and then optimize the original lidar data, eliminate some problematic data, or perform filtering; matching is a very crucial step, mainly to find the corresponding position of the point cloud data of the current local environment on the established map. The quality of the matching has a direct impact on the accuracy of the map constructed by SLAM. During the process of positioning and mapping, it is necessary to match and splice the point cloud currently collected by the lidar into the original map; map fusion is to splice the new data from the lidar in this round into the original map, and finally complete the update of the map.

[0013] Based on SLAM technology, many scholars have proposed many multi-threaded frameworks, applying SLAM technology to indoor positioning technology and achieving good results.

[0014] ORB-SLAM is a three-dimensional positioning and mapping algorithm based on ORB features. This algorithm was published by Raul Mur-Artal, J.M.M.Montiel, and Juan D. Tardos in IEEE Transactions on Robotics in 2015. ORB-SLAM was initially based on monocular sensors and later extended to binocular and RGB-D sensors. The system framework of ORB-SLAM is based on the PTAM architecture, adding functions of map initialization and loop closure detection, and optimizing the methods of key frame selection and map construction, achieving good results in processing speed, tracking effect, and map accuracy. The map constructed by ORB-SLAM is sparse. One of the major features of the ORB-SLAM algorithm is the unified use of ORB features of images in all steps. ORB features are a very fast feature extraction method, with rotational invariance, and scale invariance can be constructed using pyramids. The use of unified ORB features helps the SLAM algorithm to have endogenous consistency in steps such as feature extraction and tracking, key frame selection, three-dimensional reconstruction, and loop closure detection. However, the ORB-SLAM system only achieves good navigation effects in static indoor environments. In real-life scenarios, which are often dynamic, moving humans and other dynamic objects indoors are all non-negligible navigation influencing factors. Therefore, some better algorithms are needed to eliminate these influencing factors and make the SLAM system more robust.

[0015] Artificial neural network is a theory and technology that has developed rapidly in the field of computer intelligence research. It has the ability to learn knowledge from the environment and the ability to adapt to the environment in a biological-like interactive manner. Semantic segmentation is a fundamental technology for image understanding. It can give the accurate pixels corresponding to each type of object, but it cannot distinguish different individuals of the same type. Due to the powerful learning ability of deep neural networks, some non-rigid dynamic objects indoors, such as humans and pets, can be well recognized by deep models. If the good scene segmentation ability of neural networks can be utilized, the problem of dynamic noise interference in the environment can be well solved. Currently, semantic segmentation methods are basically based on deep learning methods, such as U-Net, SegNet, and PSPNet. The DeepLab network is a method that combines deep convolutional neural networks (DCNNs) and the probabilistic graphical model dense conditional random field.

[0016] Semantic SLAM is an algorithm that can simultaneously estimate the 3D geometry of a scene and attach semantic labels to objects and structures. Odest Chadwicke Jenkins, Karthik Desingh et al. published "Semantic Mapping with Simultaneous Object Detection and Localization" in 2018. In the paper, they proposed the technical framework of semantic SLAM, which is roughly divided into two parts: semantic extraction, and SLAM localization and mapping. The differences in different semantic SLAM problems lie in the methods of semantic extraction and the application of semantic information. Semantic extraction is mainly reflected in image processing, that is, identifying and segmenting images and adding semantic labels. Currently, the methods for identifying and segmenting images are mainly deep learning methods, and the results of network model training also determine the final effect of semantic SLAM. For the application of semantic information, it is mainly reflected in assisting localization and semantic maps. Among them, assisting localization can be further divided into aspects such as SLAM feature extraction, loop detection, and BA to improve the localization accuracy of SLAM itself; the semantic map is mainly to construct a map containing semantic information that can be used for navigation, providing rich information for the understanding at the robot level. However, semantic SLAM also has an obvious disadvantage, that is, different algorithm models rely strongly on the training and learning database. Often, separate segmentation models need to be trained in different scenarios, which leads to a huge increase in costs.

[0017] Nowadays, geometric methods have also gradually developed in the field of dynamic filtering. For example, using reprojection error to filter dynamic objects has been developed and matured, and it has low cost and good scene adaptability. However, the disadvantage of geometric methods is that the accuracy is not high and the evaluation criteria are not unified. For example, reprojection error is not an ideal evaluation criterion because the evaluation result is related to the pixels of the camera. Therefore, a small reprojection error does not necessarily mean that the judgment result is accurate.

[0018] The deficiencies of the above prior arts all affect the performance of the SLAM system in indoor navigation and cannot meet the actual application requirements. Summary of the Invention

[0019] The purpose of the present invention is to overcome the defects of the above prior arts and propose a dynamic environment navigation method based on visual information, so as to combine deep learning and geometric optimization algorithms into the three-dimensional positioning and mapping system ORBSLAM system based on ORB features. By basically reproducing the overall structure of the indoor environment through the indoor navigation model, reducing the dynamic element factor affecting the robot navigation, improving the performance of the SLAM system in indoor navigation, and meeting the actual application requirements.

[0020] The technical solution of the present invention is implemented as follows:

[0021] I. Technical Principle

[0022] The 3D environment and its spatial perception are key driving factors for robots to perform advanced tasks in the real world. To execute advanced instructions, such as "search for survivors on the second floor of a high-rise building", the robot first needs to use a deep neural network to learn the indoor environment and output relevant information about the structure, items, dynamic targets, etc. of the indoor environment. Then, it is necessary to combine the semantic information into a metric map to form a metric-semantic space representation. In addition, to link low-level obstacle avoidance and motion planning with high-level task planning, it is also necessary to build a world model to capture real-world scenarios at different abstraction levels. For example, a robot can effectively complete a series of actions such as reaching the building entrance, walking up the stairs, and entering each room according to motion planning. However, motion planning usually relies on fine-grained map representations, such as grid or volume models. This problem becomes more challenging when autonomous navigation systems are deployed in crowded indoor environments.

[0023] The present invention realizes the autonomous navigation of a robot in a crowded indoor environment and the real-time construction of an environmental map by establishing a dynamic object model based on semantic information and a SLAM framework based on deeplabV3+ semantic segmentation. Among them:

[0024] To establish a dynamic object model, dynamic objects are divided into two categories: movable objects such as chairs and cups and humans. For movable objects, semantic segmentation is performed on the input scene image, and then corresponding models are established according to the semantic features in different regions; for humans, first, an RGB image with panoramic segmentation labels is given using the method of the image convolutional neural network Graph-CNN, and then the human 3D SMPL model is constructed by combining with the person model in Unity;

[0025] The SLAM framework based on DeeplabV3+ semantic segmentation is built on the basis of ORB-SLAM2, adding the ability of object detection and background drawing to detect moving objects through multi-view geometry, deep learning or a combination of both, and allowing the background of frames occluded by these dynamic objects not to be drawn for the static map of the scene, and the map of the static part of the scene can be estimated to meet the needs of long-term applications in the real environment.

[0026] II. Implementation Scheme

[0027] According to the above technical principle, the implementation steps of the present invention are as follows:

[0028] (1) Establish a dynamic object model:

[0029] 1a) First, match the 3D mesh to the 2D image key points based on an optimized method, and then match the semantic image key points with the 2D image key points based on a deep learning method to establish the connection between the semantic image pixel points and the 3D mesh;

[0030] 1b) According to the connection between the semantic image pixel points and the 3D mesh, parameterize the skin multi-person linear model SMPL, and obtain a 3D human mesh model with 6890 vertices and 23 joints through its compact pose and shape description;

[0031] (2) Use the images captured by the binocular camera and the depth images captured by the RGB-D camera as inputs, and use the DeeplabV3+ neural network to perform region segmentation on the indoor scene images to obtain a scene segmentation model;

[0032] (3) Based on the scene segmentation model of the DeeplabV3+ neural network, segment the picture frames, remove the pixels inside and around the potential dynamic regions based on the segmentation results, project the feature points of the static regions into the picture frames, and minimize the reprojection error to achieve multi-view geometry operations;

[0033] (4) Based on the existing ORBSLAM system, input single / binocular images and RGB-D depth images, fuse the DeeplabV3+ neural segmentation network and the human 3D SMPL model, perform multi-view geometry operations on the picture frames of the scene using the initial results of the region segmentation in step (2), and then return the geometrically operated images to the ORBSLAM system for vision-based camera pose estimation;

[0034] (5) Obtain the global pose of the camera relative to the indoor environment:

[0035] (5a) Based on the Visual Inertial Odometry (VIO) module in the ORBSLAM system, use the Inertial Measurement Unit (IMU) data to calculate the local camera pose estimation value, and use the existing non-linear optimization method to integrate the local camera pose estimation value and the visual pose estimation value in step (4) to obtain the optimized local camera pose estimation value;

[0036] (5b) Use the loop detection method to eliminate the cumulative drift of the optimized local camera pose estimation value caused by multiple tracking, and output the global pose of the camera relative to the indoor environment;

[0037] (6) Reconstruct the 3D mesh:

[0038] (6a) Use the obtained global pose to continuously adjust the tracking trajectory of the robot, obtain the 2D feature points on the image at the key frames during the camera tracking process, and perform Delaunay triangulation on these feature points to form a 2D triangular mesh on the frame image;

[0039] (6b) Perform a back-projection operation on the 2D triangular mesh to generate a 3D mesh;

[0040] (7) Semantic mapping:

[0041] (7a) Use the voxel-based Truncated Signed Distance Function (TSDF) algorithm to remove the noise of the reconstructed 3D mesh and extract the global 3D mesh, and then add the 3D human model mesh in step (1) to the global 3D mesh;

[0042] (7b) Use the dense stereo algorithm on the global 3D mesh after adding the 3D human model mesh to obtain the 3D point cloud corresponding to the current stereo images;

[0043] (7c) Use the bundle adjustment ray casting method on the obtained 3D point cloud map containing the human model to construct a global map, and the robot navigates in the dynamic indoor environment according to the global map.

[0044] Compared with the prior art, the present invention has the following advantages:

[0045] 1. The present invention uses the RGB images segmented by CNN and the person model in Unity to predict the human 3D model, and parameterizes the Skin Multi-Person Linear model (SMPL), which can meet the requirement of giving the vertex information of the time-varying 3D model surface shape at any time, providing indispensable dynamic information for pose acquisition and 3D map construction.

[0046] 2. The present invention uses both multi-view geometry and deep learning to detect moving objects, and can detect semi-dynamic objects such as chairs without prior information in indoor scenes, providing more reliable static feature points for camera tracking and reducing the interference of dynamic objects on the robot's autonomous navigation indoors.

[0047] 3. The new loop detection method PCM proposed by the present invention, that is, the online operation of detection implemented by using the adjacency matrix of PCM, ensures the real-time performance of the system operation. At the same time, due to the addition of the Gauss-Newton optimization method in the consistency observation, the robustness of the loop is ensured, enabling the long-term autonomy of the robot's navigation.

[0048] 4. Since the present invention generates a 3D point cloud map for the binocular image fusion 3D mesh corresponding to the completed perspective matching, the robot can more accurately achieve self-localization and subsequent navigation in the 3D real world according to the binocular image, improving the performance of the SLAM system in indoor navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 is the implementation flowchart of the present invention:

[0050] Figure 2 is an instance diagram of the linear human body model SMPL constructed in the present invention;

[0051] Figure 3 is a schematic diagram of the 3D human body model pose tracking in the present invention;

[0052] Figure 4 is the network structure diagram of DeeplabV3+ in the present invention;

[0053] Figure 5 is the schematic diagram of the principle of dilated convolution in the present invention;

[0054] Figure 6 is the schematic diagram of depthwise separable convolution in the present invention;

[0055] Figure 7 is the semantic instance diagram of segmenting the indoor scene based on the DeeplabV3+ network in the present invention;

[0056] Figure 8 is the schematic diagram of calculating the frame coincidence degree by using the translation and rotation between the new frame and the key frame in the present invention;

[0057] Figure 9 is the instance diagram of dynamic region segmentation generated based on multi-view geometry operations in the present invention;

[0058] Figure 10 is the schematic diagram of the positioning and mapping framework built in the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0059] The embodiments and effects of the present invention will be further described in detail below with reference to the accompanying drawings.

[0060] Refer to Figure 1 , the implementation steps of this example are as follows:

[0061] Step 1, establish a dynamic object model.

[0062] In this step, based on an optimized method, the 3D mesh is matched to the 2D image key points, and then based on a deep learning method, the mesh is directly inferred from the pixel information. Finally, a dynamic model is constructed according to the mesh and the panoramic segmentation map. The specific implementation is as follows:

[0063] 1.1) Select all the meshes of the preset human objects from the 3D mesh with built-in semantic tags, and use Euclidean clustering for instance segmentation. Then project the segmented human clusters into the 2D image, and calculate the centroid and direction of the corresponding object in the 2D image from the mesh vertices of the human clusters;

[0064] 1.2) Obtain the semantic map with the 2D scene map from the convolutional neural network CNNs, and use the Brute-Force matching function provided by OpenCV to match the semantic image with the corresponding key points of the 2D image to establish the connection between the semantic image pixel points and the 3D mesh;

[0065] 1.3) According to the connection between the semantic image pixel points and the 3D mesh, first crop the left original camera image to the bounding box near each detected person in the semantic map, then regress the 3D position of the vertices of an SMPL mesh model from the bounding box, and then obtain a 3D human model SMPL according to the specific scene 3D position, as Figure 2 shown, where Figure 2 (a) is the 3D human object in the dynamic scene, Figure 2 (b) is the 3D human model SMPL constructed for the object;

[0066] 1.4) Use the Perspective-n-Point algorithm PNP to extract the complete pose of the human in the original camera view image frame, as Figure 3 shown.

[0067] Step 2, perform regional segmentation on the indoor scene.

[0068] In this step, the images captured by the binocular camera and the depth images captured by the RGB-D camera are used as inputs, and the DeeplabV3+ neural network is used to perform regional segmentation on the indoor scene images to obtain a scene segmentation model. The specific implementation is as follows:

[0069] Refer to Figure 4, The DeeplabV3+ neural network includes a downsampling module Encoder and an upsampling module Decoder. The Encoder is responsible for feature extraction, and the Decoder is responsible for restoring the size of the feature map through interpolation and transposed convolution. These two modules, Encoder and Decoder, are symmetric in structure, and high-resolution features are obtained through the Encoder-Decoder structure. The specific implementation is as follows:

[0070] 2.1) Feature extraction is performed by the downsampling module Encoder:

[0071] 2.1.1) The downsampling module Encoder first uses parallel atrous convolution on the initially effective feature layer compressed four times to expand the receptive field and capture the environmental information of multi-scale images:

[0072] Refer to Figure 5 , Atrous convolution is to add some spaces between the elements of the convolution kernel to perform convolution expansion. The receptive field size after convolution of the original convolution kernel is 3, and the receptive field size after convolution of the expanded convolution kernel is 5. Since the larger the value of the neuron receptive field, the larger the range of the original image it can contact, which also means that it may contain more comprehensive and higher-level semantic features. Therefore, it is necessary to use the expanded receptive field to capture the environmental information of multi-scale images. And due to the existence of spaces in the convolution kernel, as long as the size of the final convolution output is the same as that of the convolution output before expansion, there will be no cost waste;

[0073] 2.1.2) Feature extraction is performed using Atrous Spatial Pyramid Pooling structures ASPP with different sampling rates respectively; then the feature extraction results are concatenated and merged to obtain a merged feature layer;

[0074] 2.1.3) The merged feature layer is subjected to 1x1 pointwise convolution to obtain a green feature map, completing the preliminary extraction of high-resolution features of the scene graph;

[0075] 2.2) The upsampling module Decoder restores the size of the feature map:

[0076] 2.2.1) The upsampling module Decoder adjusts the number of channels of the initially effective feature layer compressed twice using 1x1 pointwise convolution, and then stacks the adjusted effective feature layer and the effective feature layer processed by ASPP in the Encoder;

[0077] 2.2.2) The stacked feature layer is subjected to two depthwise separable convolutions to obtain a final effective feature layer, and a 1x1 pointwise convolution is used to adjust the channels of this final effective feature layer and adjust it to the total number of categories:

[0078] Depthwise separable convolution is mainly divided into two processes: depthwise convolution and pointwise convolution. Conventional neural networks utilize depthwise convolution. However, the number of feature maps after depthwise convolution is the same as the number of channels in the input layer, making it impossible to expand for subsequent feature map expansion. Moreover, this operation performs convolution independently on each channel of the input layer without effectively utilizing the feature information of different channels at the same spatial position. Therefore, pointwise convolution is needed to recombine these feature maps to obtain more feature information. Figure 6 Three convolution forms of depthwise separable convolution are given, where Figure 6 (a) is the existing depthwise convolution, Figure 6 (b) is the existing pointwise convolution, Figure 6 (c) is the depthwise convolution of the present invention that incorporates dilated convolution;

[0079] 2.2.3) After the adjusted effective feature layer outputs the predicted feature map, use image resizing to restore the output prediction map to the original image size;

[0080] 2.3) Segment the picture frame based on the scene segmentation model of the DeeplabV3+ neural network:

[0081] Use this network to first perform model iterative training on the scene picture dataset until the scene segmentation model is obtained: then use the segmentation model to perform segmentation prediction on the scene picture, and output a semantic segmentation result map with the same size as the original scene picture, as Figure 7 shown, where Figure 7 (a) is the original grayscale image of the indoor 3D scene, Figure 7 (b) is the semantic segmentation result map corresponding to the indoor scene.

[0082] Step 3, multi-view geometry operation.

[0083] 3.1) Based on the result of segmentation by the scene segmentation model of the DeeplabV3+ neural network, remove the pixels inside and around the dynamic area of the picture frame, and then project the feature points of the static area into the picture frame;

[0084] 3.2) Set the coincidence degree evaluation criterion:

[0085] 3.2.1) Set the number of maximum overlapping key frames to 5 as a compromise between the computational cost and accuracy in dynamic object detection;

[0086] 3.2.2) Refer to Figure 8 , calculate the rotation view error and depth error between the newly input picture frame and each projected frame:

[0087] 3.2.2.1) Calculate the rotation perspective error: According to Figure 8 As shown in (a), the projection frame after projecting the static area is set as the key frame CF. When a new input picture frame KF arrives, calculate the projection X of each feature point x in it to the key frame, and use the camera parameter matrix to calculate the new feature point x′, as well as the projection depth z from the feature point x to the projection point X and the projection depth z from the new feature point x′ to the projection point X proj :

[0088] That is, use the Euclidean distances from x and x’ to the projection point X to represent the depths z and z of the two proj ;

[0089] Use the projection depths z and z proj Calculate the parallax angle α between x and x′ as the rotation perspective error. Set the error threshold to 30°. If α is greater than 30°, then this point will be marked as dynamic and will be deleted from this moment on.

[0090] 3.2.2.2) Calculate the depth error: According to Figure 8 As shown in the depth error calculation principle of (b), through experimental data analysis, for static objects with a parallax angle α greater than 30°, it is very likely that they will be considered dynamic due to their viewpoint differences. Therefore, in this step, the translation distance between the new feature point x′ and the projection point X will be measured using the rigid translation matrix as the depth z′, and the projection depth z calculated by the camera parameter matrix will be compared proj , to obtain the error function:

[0091] Δz = z proj - z′, where Δz represents the difference between the two depths

[0092] Set three thresholds τ to 0.4m, 0.5m, and 0.6m respectively by manually marking the dynamic objects in 30 images in the dataset, and establish an evaluation function based on the precision and recall obtained from these thresholds to confirm the optimal threshold:

[0093] F = 0.7 * Precision + 0.3 * Recall

[0094] Where F is the evaluation function, Precision is the precision obtained from the three thresholds, and Recall is the recall obtained from the three thresholds.

[0095] In this example, the optimal threshold τ is confirmed to be 0.4m by maximizing the evaluation function F. If Δz is less than 0.4m, the point marked as dynamic will be restored to static.

[0096] 3.2.3) Combine the rotation perspective error and the depth error as a coincidence degree judgment criterion;

[0097] 3.3) Use this evaluation criterion to remove the dynamic regions without prior information to obtain a dynamic filtering result map, and complete the multi-view geometry operation. The segmentation effect is as shown in Figure 9 , where Figure 9 (a) is the RGB map of the original dynamic scene, Figure 9 (b) is the segmentation result map after the multi-view geometry operation. Since the chair has no prior semantics in the deep learning model, it is also segmented after the multi-view geometry operation.

[0098] Step 4, build a tracking and positioning framework.

[0099] This step is based on the existing ORBSLAM system. Input monocular / binocular images and RGB-D depth images, fuse the DeeplabV3+ neural segmentation network, and perform multi-view geometry operations on the picture frames of the scene using the initial results of semantic segmentation. Then, the images after the geometry operation are regressed to the ORBSLAM system to form a complete positioning and mapping system framework.

[0100] Reference Figure 10 , and the specific implementation is as follows:

[0101] 4.1) In the monocular and binocular image pipelines, use the scene segmentation model of the DeeplabV3+ neural network to segment the picture frames, remove the dynamic regions with prior information in the picture frames and the pixels inside and around the potential dynamic regions. The feature points belonging to dynamic objects are neither tracked nor used for mapping;

[0102] 4.2) In the RGB-D image pipeline, add multi-view geometry operations to achieve more accurate motion segmentation, and remove the dynamic region contour feature points based on the results of the multi-view geometry operations;

[0103] 4.3) Add the results of removing dynamic regions in the monocular and binocular image pipelines and the results of more accurate motion segmentation in the RGB-D image pipeline to the Tracking and Mapping module based on the ORBSLAM system, and then regress to the ORBSLAM system.

[0104] Step 5, obtain the relative local pose of the camera and the indoor environment.

[0105] This step is based on the Visual Inertial Odometry (VIO) module in the ORBSLAM system. Use Inertial Measurement Unit (IMU) data, non-linear optimization methods, and loop detection to obtain the relative global pose of the camera and the indoor environment. The specific implementation is as follows:

[0106] 5.1) For the obtained IMU data, use the IMU manifold pre-integration method to obtain the relative pose between two consecutive key frames of the camera:

[0107] Taking the one-dimensional spatial position estimation as an example, its IMU pre-integration assumes that in the global coordinate system where the camera or lidar is located, each IMU data is integrated from the previous moment t1 to the next moment t2 of the key frame. That is, given that the measured values of the IMU are acceleration, velocity, and displacement, assuming we want to optimize the displacements and velocities at these two moments, we can obtain:

[0108] v k = v k-1 +(a k-1 - b a )Δt

[0109]

[0110] where v k-1 is the measured velocity at the previous moment, and v k is the measured velocity at the current moment; x k-1 is the measured displacement at the previous moment, and x k is the measured displacement at the current moment; a k-1 is the measured acceleration at the previous moment; b a is the IMU measurement error; Δt is the time difference from t1 to t2;

[0111] If we directly integrate the above equation in the global coordinate system, we can obtain:

[0112]

[0113]

[0114] where a k is the measured acceleration at the current moment; v t1 is the optimized velocity at t1, and v t2 is the optimized velocity at t2; x t1 is the optimized displacement at t1, and x t2 is the optimized displacement at t2;

[0115] In the three-dimensional space, since velocity has a direction, each time we optimize, updating v t1 in the global coordinate system requires recalculating the result inside the summation symbol. However, if we first use:

[0116]

[0117] as the velocity increment from the previous moment t1 to the next moment t2, we can quantitatively represent the result inside the summation symbol for velocity calculation. Therefore, using the measured velocity v k at the current moment to replace v t2 , and then substituting it into xt2 In the integral summation symbol, the result γ of the pre-integration is obtained:

[0118]

[0119] Then substitute the expression of the optimized velocity v at the current moment t2 into the calculation formula of x t2 to obtain the new expression of the optimized displacement x at the current moment t2 :

[0120]

[0121] Each time the v in the global coordinate system is updated t1 , only need to substitute the measured velocity increment between the previous and the next moments into the expression of the optimized velocity v at the current moment t2 , then substitute the pre-integration result γ and the new expression of v t2 into the expression of the optimized displacement x at the current moment t2 to obtain the new integration result:

[0122] v t2 = v t1 + β

[0123] x t2 = x t1 + v t1 (t2 - t1)+ γ

[0124] where β is the quantitative representation of the measured velocity increment between the previous and the next moments, γ is the new pre-integration result of the optimized displacement in the local coordinate, v t2 is the expression of the optimized velocity after quantitative representation at the current moment, and x t2 is the expression of the optimized displacement after adding the new pre-integration at the current moment;

[0125] Use the above new integration result to obtain the relative pose between two consecutive key frames;

[0126] 5.2) Use the Shi-Tomasi corner points as feature points and use the Lukas-kanade tracker to track the feature points, and then perform cross-frame tracking on the feature points based on the relative pose between the two frames that have been obtained to obtain continuous pose estimates;

[0127] 5.3) Use the Random Sample Consensus algorithm RANSAC to geometrically verify the pose estimation results and output the final local camera pose estimation value;

[0128] 5.4) Use direct linear transformation (DLT) to estimate the 3D positions of the feature points in the dynamic region from the visual results segmented by the neural network, and eliminate the corresponding 3D points from the camera tracking state to obtain an unstructured visual model;

[0129] 5.5) Add the unstructured visual model and the IMU data results after front-end manifold pre-integration to a fixed-delay smoother in each key frame, so as to form a factor graph structurally;

[0130] 5.6) Use the joint tree iSAM2 in the factor graph solver GTSAM to solve the factor graph:

[0131] Combine the factor graph with Bayes' theorem, and denote each fused data in the fixed-delay smoother as factor X i , describe the problem as a maximum a posteriori probability problem, and obtain the objective function X * :

[0132]

[0133] For the objective function X * Take the negative logarithm of the right side, transform the problem of maximizing the product of factors into a non-linear least squares problem, that is, transform the objective function X * into the following form:

[0134]

[0135] Solve this non-linear least squares problem to obtain the optimal solution of the factor graph, where φ i (X i ) is the posterior probability function, -log is the negative logarithm taken on the posterior probability function, argmaxΠi is the maximization of the product of posterior probabilities, and f i (X i ) is the error function;

[0136] 5.7) Every time a key frame is obtained during the movement of the camera, iSAM2 enters an iteration. According to the optimal solution of the factor graph, delete the points behind the camera in the key frame, the points without sufficient parallax for triangulation, and the points with large reprojection errors, and then track the remaining feature points in the key frame to obtain a pose estimation value;

[0137] 5.8) Use the covariance matrix calculated from the pose estimation value to smoothly reduce the camera pose with large changes between adjacent frames to obtain the final pose estimation value.

[0138] Step 6, loop detection.

[0139] This step uses the loop detection method to eliminate the cumulative drift of the optimized local camera pose estimation values caused by multiple trackings. The specific implementation is as follows:

[0140] 6.1) Set multiple loops according to the camera tracking trajectory, and use the bag-of-words with ORB feature descriptors in the bag-of-words file library DBoW2 to quickly detect these loops to obtain the preliminarily screened loops;

[0141] 6.2) Use the Random Sample Consensus algorithm RANSAC to perform geometric verification on the screened loops to reject the loops with more outliers;

[0142] 6.3) Pass the remaining loops to the outlier rejector PCM to select the optimal continuous loops to complete the elimination of the cumulative drift:

[0143] 6.3.1) Add an odometry consistency check to the loop. The odometry check is to ensure that each loop is always consistent with the odometry, that is, mark the loops with inconsistent accumulated errors along the loop and the observed errors obtained by the chi-square test as outlier tags. If a loop detected at the current moment passes the odometry check, it will be tested whether it is pairwise consistent with the previous loops;

[0144] 6.3.2) Construct a loop adjacency identity matrix A(1X1) to enable online operation, where "1" represents the number of loops. Whenever a new loop is detected, only add a row and a column to this matrix;

[0145] 6.3.3) Use the fast maximum subgraph method for the final loop matrix to obtain a continuous maximum consistent loop set.

[0146] Thus, the cumulative drift of the camera pose estimation values caused by multiple trackings is eliminated, and a robust and long-term global pose is output.

[0147] Step 7, reconstruct the 3D mesh.

[0148] This step uses the obtained optimized global pose to continuously adjust the robot's tracking trajectory, obtain 2D feature points on the image at a single key frame during the camera tracking process, generate corresponding meshes for each feature point to obtain a single-frame 3D mesh, then organize and fuse the single-frame meshes into a mesh that can span multiple frames, and finally regularize the mesh to output a multi-frame global mesh. The specific implementation is as follows:

[0149] 7.1) Continuously adjust the tracking trajectory of the robot using the optimized global pose obtained, and then, in real-time, obtain the static 2D feature points on the key frames again according to the corrected trajectory, and perform Delaunay triangulation on these feature points to form a 2D triangular mesh on the frame image; then perform a back-projection operation on this 2D mesh to generate a 3D mesh, and merge the 3D meshes corresponding to all the static feature points of each key frame into a single-frame mesh;

[0150] 7.2) Use the spatial distance limit of the signed distance function TSDF to remove the spatial voxel outliers in the 3D mesh, thereby removing the noise in the single-frame mesh;

[0151] 7.2) Use the multi-frame mesh generator to collect the denoised single-frame meshes and fuse them into a multi-frame mesh;

[0152] 7.3) Encode the single-frame mesh and the multi-frame mesh respectively to generate a single-frame mesh triangle list and a multi-frame mesh triangle list with node IDs, view and add all the nodes that are only in the single-frame mesh triangle list but not in the multi-frame mesh triangle list. At the same time, update the 3D mesh positions corresponding to the nodes in real-time to regularize the multi-frame mesh, and finally output a multi-frame global 3D mesh.

[0153] Step 8, semantic mapping.

[0154] 8.1) Generate 3D point cloud using the multi-frame global 3D mesh and binocular images:

[0155] 8.1.1) Use the known distortion parameters of the camera to perform monocular correction on the image distortion, that is, correct the imaging distortion curve at the edge of the image;

[0156] 8.1.2) According to the internal and external parameters of the camera, call the UndistortRectifyMap function and remap function in the existing visual opencv to perform stereo distortion correction on the binocular image to make the left and right viewing angles match;

[0157] 8.1.3) Calculate the disparity of the binocular image that has completed the viewing angle matching using the depth point cloud library LIBELAS, and based on the disparity result, fuse and correct the global 3D mesh, substitute the correction result into the 3D point cloud library ELAS for processing, and finally obtain a 3D point cloud map corresponding to the binocular image;

[0158] 8.2) Use the bundle ray casting method for the 3D point cloud map to construct a global map:

[0159] 8.2.1) Mark semantic labels corresponding to the scene semantic segmentation map on the 3D point cloud map;

[0160] 8.2.2) Use the bundle ray casting method to perform ray casting on the 3D point cloud to form a point cloud bundle ray beam. Calculate the label probability using the frequency of static semantic labels observable in the beam set. Construct a vector storing the label probability using each ray in the point cloud bundle ray beam set;

[0161] 8.2.3) Only traverse the entire spatial voxel along the ray with the vector within the truncation distance of the signed distance function TSDF, and assign the corresponding label probability to these spatial voxels;

[0162] 8.2.4) Use the Bayesian method to update the probability of each voxel, and extract the voxel corresponding to the label with the maximum probability;

[0163] 8.2.5) Arrange all spatial voxels using the known coordinate file to form a global map. The robot uses this global map to autonomously navigate in a crowded dynamic indoor environment, and the global map can also provide relocalization assistance and mapping background information in the construction of future navigation systems.

[0164] The above description is only a specific example of the present invention and does not constitute any limitation to the present invention. Obviously, for professionals in the field, after understanding the content and principle of the present invention, various modifications and changes in form and details may be made without departing from the principle and structure of the present invention. However, these modifications and changes based on the idea of the present invention are still within the protection scope of the claims of the present invention.

Claims

1. A dynamic environment navigation method based on visual information, characterized by the following steps: (1) Establish a dynamic object model: 1a) First, match the 3D mesh to the 2D image key points based on an optimization method, and then match the semantic image key points with the 2D image key points based on a deep learning method to establish the connection between the semantic image pixel points and the 3D mesh; 1b) According to the connection between the semantic image pixel points and the 3D mesh, parameterize the Skin Multi-Person Linear model (SMPL), and through its compact pose and shape description, obtain a 3D human mesh model with 6890 vertices and 23 joints; (2) Use the images captured by the binocular camera and the depth images captured by the RGB-D camera as inputs, and use the DeeplabV3+ neural network to perform regional segmentation on the indoor scene images to obtain a scene segmentation model; (3) Based on the scene segmentation model of the DeeplabV3+ neural network, segment the picture frame, remove the pixels inside and around the potential dynamic area based on the segmentation result, project the feature points of the static area into the picture frame, and minimize the reprojection error to achieve multi-view geometric operations; (4) In the existing ORBSLAM system, input single / binocular images and RGB-D depth images, fuse the DeeplabV3+ neural segmentation network and the human 3D SMPL model, and use the initial result of the regional segmentation in step (2) to perform multi-view geometric operations on the picture frame of the scene, and then return the geometrically operated image to the ORBSLAM system for visual camera pose estimation; (5) Obtain the global pose of the camera relative to the indoor environment: (5a) In the Visual-Inertial Odometry (VIO) module of the ORBSLAM system, use the Inertial Measurement Unit (IMU) data to calculate the local camera pose estimation value, and use the existing non-linear optimization method to synthesize the local camera pose estimation value and the visual pose estimation value in step (4) to obtain an optimized local camera pose estimation value; (5b) Use the loop detection method to eliminate the cumulative drift of the optimized local camera pose estimation value caused by multiple trackings, and output the global pose of the camera relative to the indoor environment; (6) Reconstruct the 3D mesh: (6a) Use the obtained global pose to continuously adjust the tracking trajectory of the robot, obtain the 2D feature points on the image at the key frames during the camera tracking process, and perform Delaunay triangulation on these feature points to form a 2D triangular mesh on the frame image; (6b) Perform a back-projection operation on the 2D triangular mesh to generate a 3D mesh; (7) Semantic mapping: (7a) Use the voxel-based Truncated Signed Distance Function (TSDF) algorithm to remove the noise of the reconstructed 3D mesh and extract the global 3D mesh, and then add the 3D mesh of the human model in step (1) to the global 3D mesh; (7b) Use the dense binocular stereo algorithm on the global 3D mesh after adding the 3D mesh of the human model to obtain the 3D point cloud corresponding to the current binocular image; (7c) Use the bundle ray casting method on the already obtained 3D point cloud map containing the human model to construct a global map, and the robot navigates in the indoor dynamic environment according to the global map.

2. The method according to claim 1, characterized in that, In step 1a), based on the optimization method, first match the 3D mesh to the 2D image key points, and then based on the deep learning method, match the semantic image key points with the 2D image key points, as follows: 1a1) Select all the meshes of the preset human objects from the 3D mesh with built-in semantic labels, use Euclidean clustering for instance segmentation, then project the segmented human clusters into the 2D image, and calculate the centroid and direction of the corresponding object in the 2D image from the mesh vertices of the human clusters; 1a2) Obtain the semantic map with the 2D scene map from the convolutional neural network CNNs, use the Brute-Force matching function provided by OpenCV to match the corresponding key points of the semantic image and the 2D image, and establish the connection from the semantic image pixel points to the 3D mesh.

3. The method according to claim 1, characterized in that, In step 1b), parameterize the skin multi-person linear model SMPL according to the connection from the semantic image pixel points to the 3D mesh, as follows: According to the connection from the semantic image pixel points to the 3D mesh, first crop the left original camera image to the bounding box near each detected person in the semantic map, and then regress the 3D position of the vertices of the SMPL mesh model from the bounding box; Use the image convolutional neural network method to obtain a 3D SMPL; Use the Perspective-n-Point algorithm PNP to extract the complete pose of the human in the original camera view image frame.

4. The method according to claim 1, wherein In step (3), segment the picture frame based on the scene segmentation model of the DeeplabV3+ neural network, as follows: The scene segmentation model of the DeeplabV3+ neural network model includes a downsampling module and an upsampling module, where: This downsampling module is used for feature extraction. First, use parallel atrous convolution on the initially effective feature layer compressed four times to expand the receptive field and capture the environmental information of the multi-scale picture; then use the Atrous Spatial Pyramid Pooling structure ASPP with different sampling rates for feature extraction respectively; then concat and merge the feature extraction results, and perform 1x1 pointwise convolution on the merged feature layer to obtain the green feature map, completing the preliminary extraction of the high-resolution features of the scene map; This upsampling module is used to restore the size of the feature map. First, use 1x1 pointwise convolution to adjust the number of channels for the initially effective feature layer compressed twice through interpolation and transposed convolution, and then stack the adjusted effective feature layer and the effective feature layer processed by ASPP in the Encoder; then use Pytorch to perform two depthwise separable convolutions on the stacked feature layer to obtain a final effective feature layer, and use a 1x1 pointwise convolution to adjust the channels of this final effective feature layer and adjust it to the total number of categories; finally, use resize for upsampling to restore the output prediction picture to the original picture size; The scene segmentation model based on the DeeplabV3+ neural network segments the picture frame. First, the network is used to perform iterative training on the scene picture dataset until the scene segmentation model is obtained. Then, the segmentation model is used to segment and predict the scene picture and output a semantic segmentation result map with the same size as the original scene picture.

5. The method according to claim 1, wherein Step (4) returns the geometrically operated image to the ORBSLAM system for vision-based camera pose estimation, which is implemented as follows: (4a) Remove the pixels inside and around the dynamic region of the picture frame based on the result of segmentation by the scene segmentation model, and then project the feature points of the static region into the picture frame. (4b) Calculate the translation and rotation distances between the newly input picture frame and each projected frame, and use this translation and rotation distance as a criterion for judging the coincidence degree. Use this criterion to remove the dynamic regions without prior information and complete the multi-view geometric operation. (4c) First, use the result of the multi-view geometric operation to remove the feature points of the dynamic region contour, and then use the track function in the ORBB feature point SLAM system to track the remaining static feature points in the scene picture after removing the feature points of the dynamic region contour to complete the preliminary camera pose estimation.

6. The method according to claim 1, characterized in that, In step (5a), based on the visual odometry VIO module in the ORBSLAM system, the local camera pose estimation value is calculated using the inertial measurement IMU data, which is implemented as follows: (5a1) For the acquired inertial measurement IMU data, use the IMU manifold pre-integration method to obtain the relative pose between two consecutive key frames of the camera. (5a2) Use the Shi-Tomasi corner points as feature points and use the Lukas-kanade tracker to track the feature points. Then, based on the relative pose between the two frames that has been obtained, perform cross-frame tracking on the feature points to obtain continuous pose estimations. (5a3) Use the random sample consensus algorithm RANSAC to perform geometric verification on the pose estimation result and output the final local camera pose estimation value.

7. The method according to claim 1, wherein Step (5a) uses a non-linear optimization method to integrate the local camera pose estimation value of the camera and the visual pose estimation value in step (4), which is implemented as follows: (5a4) Use the direct linear transformation DLT to estimate the 3D positions of the feature points in the dynamic region for the visual result after neural network segmentation, and eliminate the corresponding 3D points from the camera tracking state to obtain an unstructured visual model. (5a5) Add the unstructured visual model and the result of the IMU data after front-end manifold pre-integration to a fixed-delay smoother in each key frame, so that they form a factor graph structurally. (5a6) Use the joint tree iSAM2 in the factor graph solver library GTSAM to solve the factor graph. Combining the factor graph with Bayes' theorem, each fused data in the fixed-delay smoother is denoted as factor X i , and the problem is described as a maximum a posteriori probability problem to obtain the objective function X * : Take the negative logarithm of the right side of the objective function X * to transform the problem of maximizing the product of factors into a non - linear least - squares problem, that is, transform the objective function X * into the following form: Among them, is the posterior probability function, -log is the negative logarithm taken on the posterior probability function, argmaxΠi is the maximization of the product of posterior probabilities, and f i (X i ) is the error function; Solve this non-linear least squares problem to obtain the optimal solution of the factor graph. (5a7) Every time a key frame is acquired during the movement of the camera, iSAM2 enters an iteration. According to the optimal solution of the factor graph, delete the points behind the camera in the key frame, the points that do not have enough parallax for triangulation, and the points with large reprojection errors, and then track the remaining feature points in the key frame to obtain a pose estimation value. (5a8) The covariance matrix calculated using the pose estimation value is used to smooth and reduce the camera pose with large changes between adjacent frames, and the final pose estimation value is obtained.

8. The method according to claim 1, wherein In step (5b), the loop detection method is used to eliminate the cumulative drift of the optimized local camera pose estimation value caused by multiple trackings, and the implementation is as follows: (5b1) Set multiple loops according to the camera tracking trajectory, and use the bag-of-words with ORB feature descriptors in the bag-of-words file library DBoW2 to quickly detect these loops, and obtain the preliminarily screened loops; (5b2) Use the Random Sample Consensus algorithm RANSAC to perform geometric verification on the screened loops to reject the loops with more outliers; (5b3) Pass the remaining loops to the outlier rejector PCM to select the optimal continuous loops to complete the elimination of the cumulative drift.

9. The method according to claim 1, wherein In step (7a), the voxel-based TSDF algorithm is used to remove the noise of the reconstructed 3D mesh. First, the Delaunay triangulation method is used to extract the surface of the scene 3D model to obtain the voxel values of the 3D mesh; then, the spatial distance outliers in the 3D mesh are removed using the spatial distance limit of the signed distance function TSDF.

10. The method according to claim 1, characterized in that, In step (7b), the dense binocular stereo algorithm is used for the global 3D mesh after adding the 3D human model mesh to obtain the 3D point cloud corresponding to the current binocular image, and the implementation is as follows: (7b1) Use the known distortion parameters of the camera to perform monocular correction on the image distortion, that is, correct the imaging distortion curve at the edge of the image; (7b2) According to the internal and external parameters of the camera, call the UndistortRectifyMap function and remap function in the existing vision opencv to perform stereo distortion correction on the binocular image to make the left and right views match; (7b3) Use the depth point cloud library LIBELAS to calculate the disparity of the binocular image after the view matching is completed, and fuse and correct the global 3D mesh based on the disparity result; (7b4) Substitute the correction result into the 3D point cloud library ELAS for processing, and finally obtain the 3D point cloud map corresponding to the binocular image.

11. The method according to claim 1, characterized in that, In step (7c), the bundle light projection method is used for the obtained 3D point cloud map containing the human model to construct a global map, and the implementation is as follows: (7c1) Attach semantic labels corresponding to the scene semantic segmentation map to the 3D point cloud map; (7c2) Use the bundle light projection method to project light rays on the 3D point cloud to form a point cloud bundle of light rays, and calculate the label probability using the frequency of the static semantic labels that can be observed in the light beam set; (7c3) Use each light ray in the point cloud bundle of light rays to construct a vector storing the label probability; (7c4) Only traverse the entire voxel space along the light rays with vectors within the truncation distance of the signed distance function TSDF, and assign the corresponding label probability to these voxels; (7c5) Use the Bayesian method to update the probability of each voxel, and extract the voxels corresponding to the label with the maximum probability; (7c6) Arrange all voxels using the known coordinate file to form a global map.

Images