Local obstacle avoidance navigation method for blind guiding stick based on multi-sensor fusion

Abstract

The invention discloses a blind guide stick local obstacle avoidance navigation method based on multi-sensor fusion, which comprises a telescopic blind guide stick, an RGB-D depth camera, a radar and other sensors and a high-performance processor, so as to realize accurate environmental perception and intelligent navigation functions and improve the travel safety and convenience of blind people and visually impaired people. The method comprises the following steps: acquiring depth information of a surrounding environment through an RGB-D depth camera, providing a three-dimensional perception capability of a scene, performing auxiliary correction on information acquired by the RGB-D depth camera through a plurality of sensors such as radar, transmitting the information acquired by the sensors to a high-performance processor, and performing SLAM (Simultaneous Localization and Map Construction) algorithm so as to obtain the three-dimensional perception capability of the scene. According to the method, information collected by multiple sensors is fused, the pose information of the current blind guiding stick is calculated, nonlinear optimization is carried out according to the calculated pose information and measured depth information, the result of nonlinear optimization is used for establishing an octree map of the environment, and a reasonable obstacle avoidance path is generated according to the octree map. And the obstacle avoidance path is fed back to the blind and visually impaired people in real time in a voice or vibration mode.

Description

Technical Field

[0001] The invention belongs to the application field of multi-sensor fusion SLAM algorithm, and mainly relates to a multi-sensor fusion local obstacle avoidance navigation method for a guide stick. Background Art

[0002] With the rapid development of intelligent navigation technology, guide aids are gradually evolving from traditional mechanical guide sticks to electronic and intelligent ones. However, existing intelligent guide devices are mostly limited to ultrasonic ranging sensors or pre-built maps. In outdoor scenarios, where environmental factors are complex and variable, single ultrasonic sensors are susceptible to interference. Furthermore, without pre-built maps, devices that rely on a single sensor and pre-built maps for navigation have significant limitations.

[0003] Therefore, this smart guide cane navigation method requires collecting depth information from multiple sensors, primarily from an RGB-D depth camera, with radar depth information used as a correction. This allows for more accurate and stable perception of the surrounding environment and the creation of an environmental model with strong interference resistance. This model is then used to detect the distance between the guide cane and obstacles, and alert the blind user through voice or vibration.

[0004] In summary, a local obstacle avoidance navigation method for a guide stick based on multi-sensor fusion is designed to improve the convenience and safety of blind users' travel. Summary of the Invention

[0005] To address these issues, the present invention provides a local obstacle avoidance navigation method for a guide stick based on multi-sensor fusion. This method uses an RGB-D depth camera and radar to acquire real-time environmental information, construct a 3D map, and update the map based on real-time depth information. This method allows for timely obstacle detection and safe navigation for the blind, improving their travel convenience and safety.

[0006] The present invention is mainly achieved through the following technical solutions: A local obstacle avoidance navigation method for a guide stick based on multi-sensor fusion, including a retractable guide stick equipped with an RGB-D depth camera, a radar, a high-performance processor, and a speaker, the method comprising: The system uses RGB-D depth cameras and radar to collect global depth information of the surrounding environment in real time, and generates an initial point cloud map based on the depth camera data, which is then used to estimate the position of the guide cane. Based on the depth information collected by the radar, the initial point cloud map is assisted in correction through BA graph optimization, and loop detection is used to check whether the position previously reached has been returned. Based on this, nonlinear optimization is performed on the feature point matching between the historical depth data, and the established map is corrected in real time to reduce the cumulative error and ensure the accuracy and robustness of the established point cloud map. According to the depth data of the point cloud map, the distance information of the obstacle is reminded to the blind user through voice and vibration.

[0007] In actual applications, although the depth information of RGB-D depth cameras can be directly used to measure the distance to obstacles, the discrete nature of point cloud data means that the depth information of some obstacles is far apart, making it difficult to achieve better distance recognition. Moreover, since the point cloud map only has logic for adding points but not for deleting points, it cannot effectively identify moving objects and has problems processing their distance information. In addition, the discrete nature of the point cloud map means that it does not have a topological structure. Therefore, the established point cloud map cannot be directly used for trajectory planning. To address practical issues that may arise during use, the point cloud map requires certain processing before it can be used for obstacle detection and trajectory planning. To do this, an octree map (OctoMap) is constructed. The raw point cloud data collected by the RGB-D depth camera and lidar is first preprocessed, including noise filtering and voxel downsampling, to reduce data redundancy and the impact of outliers. Subsequently, a voxelization method is used to divide the preprocessed point cloud data into voxel units of a certain size and store them. Each voxel records the occupancy of the environment, and the map constructed from all voxels is an octree map. This octree map not only smoothes the point cloud data but also uses a probabilistic model to continuously update and remove dynamic obstacles, thereby improving the effectiveness of obstacle detection and trajectory planning. It effectively addresses the issues caused by the discreteness of point cloud data and the inability to filter dynamic point cloud data.

[0008] Then, based on the established octree map, the distance information of obstacles in the current position is detected, and local obstacle avoidance navigation is performed based on this information, and reminders are given through voice or vibration. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] Figure 1 It is the overall design of the guide stick.

[0010] Figure 2 It is the process of collecting information from sensors on guide sticks and building point cloud maps.

[0011] Figure 3 It is a process of constructing an octree map through a point cloud map and performing local obstacle avoidance navigation based on the octree map. DETAILED DESCRIPTION

[0012] The present invention relates to a method for local obstacle avoidance navigation using a guide stick based on multi-sensor fusion. This method primarily targets blind users for real-time mapping and navigation in outdoor environments. It uses sensors such as RGB-D depth cameras and lidar to acquire environmental information, construct a 3D point cloud map, and construct an octotree (OctoMap) based on the point cloud map for map representation, enabling real-time obstacle avoidance navigation. The system can perform local path planning based on the guide stick's current position and sensor-collected data, and, combined with vibration feedback or voice broadcast, provides collision-free guidance for the blind. The present invention is further described below in conjunction with the accompanying drawings and specific embodiments: Figure 1 The above is a detailed description of the hardware architecture, data processing flow, map construction method, navigation obstacle avoidance strategy and human-computer interaction mode of the system from depth information acquisition to data processing, map construction and human-computer interaction of the present invention.

[0013] like Figure 1 As shown, 110 is a sensor module, which includes 111 an RGB-D depth camera and 112 a laser radar as a surrounding depth information perception device; The depth camera 111 collects depth information of the surrounding environment. Based on this depth information, the high-performance processor 120 generates an initial 3D point cloud map through step 121 front-end visual odometry and step 122 back-end optimization. The laser radar 112 scans and collects surrounding environment data in real time through laser scanning, providing more accurate obstacle distance information and transmitting it to the back-end optimization 122 part of the high-performance processor; The visual odometry 121 needs to calculate the relative pose change of the guide stick based on the depth camera captured by the RGB-D depth camera to provide a preliminary pose estimate for SLAM; Loop detection 123 determines whether the current perspective is similar to the previous scene based on the data from the RGB-D depth camera. If similar, a loop is detected and the loop information is passed to the back-end optimization 122 for global optimization to correct the accumulated error. Backend optimization 122 performs BA nonlinear optimization based on the depth information from the RGB-D depth camera 111 and whether loops are detected 123, combined with the high-precision obstacle distance information collected by the lidar 112. This calculates pose changes that are more accurate than those from the visual odometry 121 and constructs a 3D point cloud map 124 with smaller errors. After the 3D point cloud map is built, if it is detected that the obstacle in front of the blind person is less than 0.5 meters away from the guide stick in the current posture, an emergency warning will be issued to prevent the blind person from hitting the obstacle; if the obstacle is more than 0.5 meters away, subsequent octree map construction and local obstacle avoidance navigation will be carried out; Since point cloud maps can only add points but not delete points, and there is no clear representation of spatial occupancy, it is necessary to construct a voxelized octree map 125 based on the created precise point cloud map 124. Each voxel stores information about whether the corresponding space is occupied by an object, and can be added or deleted in real time. Compared to the point cloud map 124, the octree map 125 deletes the shadow, color, and object texture information that we don’t care about. Therefore, it can not only express the spatial occupancy information of the surrounding environment, but also improve the calculation efficiency of the obstacle avoidance path and reduce the pressure on map storage. Obstacle avoidance navigation 126: Based on the guide cane position information calculated by back-end optimization, the obstacle distribution in the octree map corresponding to the current position is calculated, a local collision-free path in the current direction of movement is planned, and feedback is provided through the human-computer interaction module 130; To help the blind travel safely, the system provides multiple human-computer interaction 130 modes: Emergency warning vibration mode: When an obstacle is detected to be too close, the system will trigger the vibration motor to remind the user to slow down or avoid it; 1m - 1.5m: slight vibration reminder 0.5m - 1m: Moderate vibration warning 0 - 0.3m: Strong vibration warning and emergency avoidance Voice broadcast: Local obstacle avoidance navigation information can be output through speakers or Bluetooth devices, and the local obstacle avoidance path can be informed to blind users through voice, providing directional guidance, such as "move about 2 meters to the left" or "adjust slightly to the right"; Use key input or voice commands to adjust the navigation mode, such as "Turn on obstacle avoidance navigation" or "Turn off obstacle avoidance navigation."

[0014] Figure 2 It shows the process from collecting information from the sensor on the guide stick to building a point cloud map, which includes five steps 210, 220, 230, 240, and 250; 210 is the laser radar that scans the environment in real time to collect depth information and generate point cloud data; 220 is an RGB-D depth camera that collects data, obtains the color image and depth map of the current perspective, and combines historical data to construct preliminary depth information of the environment; Loop detection 230 uses DBoW2 (bag-of-words model) to detect whether the current frame matches the historical frame, determines whether the loop is closed, and feeds the loop information back to the backend for optimization; Pose Optimization 240 uses the BA nonlinear optimization method to calculate the precise pose changes of the guide stick and construct a high-precision point cloud map; 250 is an emergency warning based on the data of the point cloud map: the constructed point cloud map will perform preliminary obstacle detection. Combining the point cloud map and the posture information calculated in step 240, the relative position of the guide stick's current posture to the obstacle in front is calculated, and the warning intensity is selected according to the distance to the obstacle: Above 1.5m: No emergency warning is given, and the octree map is used for collision-free trajectory planning, achieving local obstacle avoidance navigation, and obstacle avoidance is achieved by waiting for voice announcements. Within 1.5m: Select slight, medium, or strong vibration based on the distance to remind the blind user to avoid obstacles urgently. For more information, see the explanation of the 130 human-computer interaction module.

[0015] Since this point cloud map has many unnecessary details for blind guides, such as texture information of obstacles, even a point cloud map of a very small local space will occupy a large memory space. If you choose to reduce the resolution of the point cloud map, the map quality will be reduced. Because the point cloud map can only add point clouds but cannot delete them, and cannot express the occupancy of the surrounding environment, the point cloud map cannot meet the conditions for long-term and stable blind guide functions in outdoor environments. The point cloud map is compressed by constructing an octree map to improve the storage and computing efficiency of the 3D map. It can also update map data in real time and realize the detection of dynamic objects, providing certain guarantees for the travel safety of blind people in complex outdoor environments. Figure 3 The process is to construct an octree map through the already constructed high-precision 3D point cloud map, and perform local obstacle avoidance navigation based on the octree map, which includes four steps 241, 242, 243, and 244 respectively: Step 241 is the high-precision point cloud map that has been established. Due to the shortcomings mentioned above, it needs to be processed in some way. Step 242 is to perform preliminary image processing to filter out noise on the high-precision point cloud map, and then divide the processed point cloud map into voxels. Each voxel, as a node in the octree map, stores information about whether it is occupied in the surrounding environment. The probability of its occupation is calculated using the logit transformation based on the observation results, thereby realizing the addition and deletion of dynamic objects. The octree map constructed in step 242 selects a high resolution of 0.01m within 3m around the guide stick, and a low resolution of 0.05m for the surrounding environment map greater than 3m, so as to improve local positioning, navigation performance and accuracy, and reduce the memory space required for storing the map; After the octree map is established, step 243 will observe whether there are any obstacles ahead in the octree map and the distance of the obstacles under the current posture based on the current guide cane posture calculated in step 240 (usually the obstacle distance is more than 1.5m, and obstacles less than 1.5m are reminded to avoid them through emergency warnings); Adopting DWA dynamic window algorithm, it generates multiple forward paths based on the current movement state of the guide stick and selects the best path; If the path is detected to be blocked, vibration feedback or voice broadcast will be triggered to remind the user to adjust the direction.

Claims

1. A method for local obstacle avoidance navigation using a guide stick based on multi-sensor fusion, comprising a retractable guide stick, multiple sensors such as an RGB-D depth camera, a radar, and a high-performance processor, the method comprising: Processing data collected from multiple sensors. First, an RGB-D depth camera is used to obtain real-time depth information of the surrounding environment. Distance information collected by multiple sensors such as radar is then used to assist in correcting the data collected by the RGB-D depth camera. This improves the accuracy and stability of depth information, especially in environments with large lighting changes or heavy occlusion. The depth information collected by all sensors is transmitted to a high-performance processor, which runs the SLAM algorithm inside the processor. The data from multiple sensor sources is fused and processed using nonlinear optimization methods to calculate the current position information of the guide stick. The processor combines the calculated pose information and depth measurement data to construct a point cloud map of the surrounding environment. From this point cloud map, it generates an octree map, which is updated in real time based on sensor data. The octree map is then analyzed based on this model to determine the distribution of obstacles. Based on this, the system generates a reasonable obstacle avoidance path, ensuring safe navigation for blind users in complex environments. Finally, navigation information is fed back to the blind user through prompts such as vibration or voice, helping them avoid obstacles and improving travel safety and convenience.

2. The method for local obstacle avoidance navigation of a guide stick based on multi-sensor fusion according to claim 1, wherein the SLAM algorithm used is characterized in that it acquires environmental information through multiple sensors such as RGB-D depth cameras and radars, and performs mapping and trajectory planning on the environment where the guide stick is located, including: Acquire surrounding environment information through multiple sensors and transmit the acquired depth information to a high-performance processor; Depth information collected by all sensors is transmitted to a high-performance processor, which runs a SLAM algorithm. Through feature point matching, initial pose calculations, back-end (non-linear optimization), and loop closure detection, a real-time map of the surrounding environment is built. The processor provides real-time navigation information to users through vibration or voice prompts, ensuring safe obstacle avoidance.

3. The method for local obstacle avoidance navigation of a guide stick based on multi-sensor fusion according to claim 1, characterized in that: Based on the depth information obtained by the current RGB-D depth camera, radar and other sensors, and the surrounding environment map established by the SLAM algorithm, the guide stick is globally positioned to obtain the global positioning information of the guide stick relative to the environment map, including: Based on the depth information data of the RGB-D depth camera and radar, a point cloud map of the surrounding environment is created in real time in a high-speed processor, and an octree map is constructed based on this point cloud map; When the guide stick moves, the position change of the guide stick is calculated and recorded through the feature point matching in the visual odometry of the SLAM algorithm; The information collected by the RGB-D depth camera and radar is nonlinearly optimized through the back-end of the SLAM algorithm to modify inaccuracies in the map in real time, optimize the position changes of the guide stick to make it consistent with the actual situation, and mark the current position of the guide stick on the real-time corrected map.

4. The method for local obstacle avoidance navigation of a guide stick based on multi-sensor fusion according to claim 1, characterized in that: Based on the point cloud data collected by the RGB-D depth camera and radar, a 3D point cloud map of the indoor scene is constructed, including: Acquire RGB-D depth camera point cloud data and radar point cloud data of the surrounding environment, generate an initial point cloud based on the RGB-D depth camera data, and use the radar point cloud data for correction to improve the point cloud accuracy; Through statistical filtering, the noise in the point cloud data acquired by the sensor is eliminated, isolated points and abnormal points are removed, and the quality of the point cloud is improved; The point cloud registration algorithm ICP (Iterative Closest Point) is used to perform inter-frame point cloud fusion to reduce cumulative errors. The position change of the guide stick starting from the initial position is estimated based on the feature point matching results between adjacent frames. The inter-frame fusion is performed using a graph optimization method to improve the accuracy of the guide stick's pose estimation. The SLAM backend uses a nonlinear optimization method. Based on the real-time data of the RGB-D depth camera and radar, combined with the established map model, and through loop detection to identify previously observed objects, the SLAM backend algorithm based on BA graph optimization is used to optimize the entire 3D point cloud map and guide cane posture, reduce cumulative errors, and generate a complete and accurate 3D point cloud map. This map is used to detect nearby obstacles and provide emergency obstacle avoidance warnings.

5. The method for local obstacle avoidance navigation of a guide stick based on multi-sensor fusion according to claim 4, characterized in that: Based on the point cloud data collected by the RGB-D depth camera and radar, an octree map of the indoor scene is constructed, including: Based on the constructed 3D point cloud map, multiple small voxels are constructed to store information about whether a location in the space is occupied. All the small voxels are spliced ​​together to construct an octree map of the surrounding environment. In order to realize the dynamic addition and deletion of spatial objects, the observation results are used to calculate the probability of their occupancy using logit transformation, and thresholds of 0.1 and 0.9 are set. If the threshold is exceeded, the occupancy status is changed, and local obstacle avoidance navigation is performed based on the established octree map.

6. The method for local obstacle avoidance navigation of a guide stick based on multi-sensor fusion according to claim 5, characterized in that: Optimizing the 3D point cloud map includes: Global loop closure detection: In the SLAM backend, loop closure detection is performed on the point cloud data collected by the RGB-D depth camera and radar to identify previously visited locations and calculate the transformation relationship between the current frame and the historical frames; Constraint calculation and error adjustment: Based on the loop detection results, the relative pose constraints of the current frame and the historical frames are calculated, the pose graph is constructed, and incorrect matches are eliminated to reduce errors; Based on the global consistency adjustment of BA graph optimization, the global point cloud map is optimized, the map accuracy is improved and the optimized map is generated: based on the optimized point cloud data, the constructed octree map is updated for use by the path planning module.

7. A local obstacle avoidance navigation method for a guide stick based on multi-sensor fusion, characterized by: Sensor module: Consists of an RGB-D depth camera and radar. The RGB-D depth camera is used to collect depth information of the surrounding environment and generate initial point cloud data. The radar is used to provide additional environmental point cloud data to assist in correcting the RGB-D depth camera point cloud and improve mapping and positioning accuracy. A computing processing unit uses a high-performance embedded processor to perform noise filtering, point cloud fusion, map construction, loop detection, and path planning; Data storage unit, used to store historical point cloud data, optimized map information, etc.; An interactive module, including a speaker for voice output and a vibration motor for tactile feedback, to help the blind perceive obstacles ahead and facilitate guidance for movement; The power management module includes a rechargeable lithium battery and energy-saving management circuit to ensure the endurance of the device and add a backup power supply to deal with emergencies.

Images