Multimodal door detection and position correction method and system based on dynamic Voronoi skeleton constraints

Abstract

The application discloses a dynamic Voronoi skeleton constrained multi-modal door detection and position correction method and system, and the method comprises the following steps: acquiring sensor data through an RGBD camera and a multi-line laser radar; processing an RGB image by using an improved YOLO deep learning network, realizing coarse positioning of a door body, and outputting three-dimensional initial coordinates of a door frame; combining point cloud data fusion to perform three-dimensional geometric verification, including ground point cloud elimination, door frame plane fitting and point cloud density analysis to determine the opening and closing state of the door body; constructing a dynamic Voronoi skeleton model, and correcting the environmental topological structure in real time through an incremental distance field updating algorithm; adopting a curvature constraint projection algorithm to map the detection result to the skeleton center line; and outputting the coordinates of the door body center on a two-dimensional grid map. The application significantly improves the robustness and navigation reliability of door body positioning in a complex dynamic environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of mobile robot environment perception and navigation, and in particular relates to a multi-modal door detection and position correction method and system with dynamic Voronoi skeleton constraints. Background Art

[0002] In recent years, with the continuous development of computer technology, robotics, and electronic information technology, mobile robots have been widely used in various fields such as logistics, industry, agriculture, and services. Mobile robots such as indoor service robots, logistics robots, and security robots often need to identify and locate doors in their environment during autonomous exploration and navigation to better complete their tasks. However, current door detection and positioning methods still have the following technical shortcomings:

[0003] Current methods often use single-modal detection, resulting in unreliable results. Traditional methods rely on a single sensor (such as lidar or RGB cameras), resulting in high false positive rates in scenes with varying lighting, occlusion, or complex geometry.

[0004] Current methods do not fully integrate semantic and geometric information. Existing methods do not deeply couple visual semantic detection (door frame recognition) with point cloud geometric verification (door open and closed status, precise location).

[0005] Current methods lack a method to correct the door position to further improve detection accuracy. This patent proposes using the environment's Voronoi skeleton to further correct the door position. At the same time, to address the problem of insufficient static skeleton constraints and inability to adapt to dynamic environmental changes (such as moving obstacles and temporarily stacked items), which leads to deviations in door position mapping, this patent proposes constructing a dynamic Voronoi skeleton environment representation model and designing a curvature-constrained projection algorithm to accurately map the detection results to the skeleton centerline, thereby correcting the detected door position and improving adaptability to dynamic environments. Summary of the Invention

[0006] To solve the above technical problems, the present invention proposes a multimodal door detection and position correction method and system with dynamic Voronoi skeleton constraints, which significantly improves the robustness of door positioning and navigation reliability in complex dynamic environments.

[0007] On the one hand, to achieve the above-mentioned purpose, the present invention provides a multimodal door detection and position correction method based on dynamic Voronoi skeleton constraints, comprising:

[0008] Acquire sensor data through RGBD cameras and multi-line lidar;

[0009] The improved YOLO deep learning network is used to process the RGB image to achieve rough door positioning and output the initial 3D coordinates of the door frame.

[0010] The three-dimensional geometric verification is combined with point cloud data fusion, including ground point cloud elimination, door frame plane fitting and point cloud density analysis to determine the opening and closing state of the door body;

[0011] A dynamic Voronoi skeleton model is constructed, and the environment topological structure is corrected in real time through an incremental distance field updating algorithm;

[0012] A curvature constraint projection algorithm is used to map the detection result to the skeleton center line;

[0013] The coordinates of the door body center on the two-dimensional grid map are output.

[0014] Optionally, the process of acquiring the sensor data comprises:

[0015] Time synchronization is performed, a weighted least squares model is established to align the time stamps of the RGBD camera and the laser radar, and the model is:

[0016] ;

[0017] wherein, is an optimal synchronization time offset; is time; is a sensor weight coefficient of the RGBD camera; is a sensor weight coefficient of the laser radar; , respectively are the time stamp deviations of the RGBD camera and the laser radar;

[0018] Spatial calibration is performed, a homogeneous transformation matrix is solved through a calibration board, and the sensor coordinate system is unified, and the matrix is:

[0019] ;

[0020] wherein, is a homogeneous transformation matrix of the laser radar to the camera; is a 3D rotation matrix; is a translation vector; is a more accurate transformation matrix calculated by an optimization method; , respectively are the corresponding point coordinates of the laser radar and the camera; is a transformation matrix.

[0021] Optionally, the process of coarse positioning of the door body comprises:

[0022] A YOLO network is used to identify the door frame bounding box in the RGB image, and confidence filtering and non-maximum suppression processing are performed;

[0023] Combined with the depth map or lidar point cloud data, the two-dimensional pixel coordinates are converted into three-dimensional space coordinates. The conversion formula is:

[0024] ;

[0025] in, is the X coordinate in the camera coordinate system; is the pixel coordinate; is the column coordinate of the principal point of the image; is the focal length in the X-axis direction; is the depth value; is the Y coordinate in the camera coordinate system; is the focal length in the Y-axis direction; is the row coordinate of the principal point of the image.

[0026] Optionally, the three-dimensional geometry verification process includes:

[0027] Perform ground point cloud removal, use the RANSAC algorithm to fit the ground plane and filter out interference points;

[0028] Perform door frame plane fitting and use a robust fitting algorithm to construct a door frame plane model. The door frame plane model is:

[0029] ;

[0030] in, is the unit normal vector of the door frame plane; is the offset from the plane to the origin; A three-dimensional point in space ; is a point on the door frame plane; is the German-McClure robust kernel parameter;

[0031] The door opening and closing status is analyzed based on the point cloud density. The point cloud density inside the door frame is calculated and the status is determined. The determination formula is:

[0032] ;

[0033] in, represents the set of door states; The door is in the closed state; The door is in the open state; is the point cloud density inside the door frame; is the point cloud density; for Other situations besides.

[0034] Optionally, the process of constructing the dynamic Voronoi skeleton model includes:

[0035] establishing a two-dimensional grid map based on laser radar data;

[0036] The environment topology skeleton network is dynamically generated through an incremental distance field updating algorithm, and skeleton points of the environment topology skeleton network satisfy a condition that:

[0037] ;

[0038] wherein, is any point on the grid map; is a Voronoi skeleton point set; is a set of all obstacle coordinates in the environment; is any point on the grid map; is any point on the grid map; is a resolution of the grid map.

[0039] Optionally, the process of the curvature constraint projection algorithm comprises:

[0040] projecting an initial door coordinate to a Voronoi skeleton;

[0041] finding an optimal projection point of a low-curvature region in a skeleton search radius in combination with a curvature optimization objective function, and the objective function is:

[0042] ;

[0043] wherein, is a corrected door center coordinate position; is any point in a Voronoi skeleton point set; is a Voronoi skeleton point set; is an initial detected door center coordinate; is a weight coefficient; is a curvature.

[0044] Optionally, the process of outputting the door center coordinate comprises: mapping the corrected coordinate to a two-dimensional grid map and marking.

[0045] In another aspect to achieve the above object, the present application further provides a dynamic Voronoi skeleton constrained multi-modal door detection and position correction system, comprising:

[0046] a multi-sensor space-time alignment module, configured to synchronize space-time data of an RGBD camera and a multi-line laser radar;

[0047] a door body rough detection module, configured to identify a door frame in an RGB image through an improved YOLO network and output three-dimensional initial coordinates;

[0048] The point cloud fine processing module is used for fusing point cloud data to perform ground elimination, door frame plane fitting and open / close state determination.

[0049] The dynamic Voronoi skeleton generation module is used for constructing an environment topology skeleton in real time through an incremental distance field updating algorithm.

[0050] The position correction module is used for mapping the detection result to the skeleton center line through a curvature constraint projection algorithm.

[0051] The coordinate output module is used for outputting the coordinates of the door body center on a two-dimensional grid map.

[0052] Technical effects of the present application:

[0053] (1) Multi-modal sensor fusion and space-time alignment technology: Through a weighted time synchronization model and a joint calibration algorithm, the space-time data of an RGBD camera and a laser radar are accurately aligned, the time sequence misalignment and coordinate deviation problems in cross-modal data fusion are solved, and the reliability of detection and the consistency of environment perception are improved.

[0054] (2) Dynamic Voronoi skeleton generation and real-time updating mechanism: An incremental distance field updating method is adopted, and the skeleton topology structure is dynamically adjusted in combination with the motion speed of an obstacle, so that the environment representation can reflect the latest state (such as a moving obstacle and a temporary pile) in real time, and the defects of a traditional static skeleton that cannot adapt to a dynamic scene are overcome.

[0055] (3) Topology projection optimization algorithm with curvature constraint: A curvature penalty term is introduced in skeleton mapping, the projection ambiguity of high-curvature regions such as corners and bifurcation points is avoided, and the door center position is always mapped to the safe center line of the passable region.

[0056] (4) Door open / close state verification mechanism: The door open / close state is effectively determined through the sparsity of point clouds inside the door frame (low density when closed). BRIEF DESCRIPTION OF DRAWINGS

[0057] The accompanying drawings, which form a part of this application, are included to provide a further understanding of the application and are incorporated in and constitute a part of this application. The embodiments of this application and their description, together with the drawings, serve to explain the application. The accompanying drawings are included as a part of this application and serve to provide a further understanding of the application. The embodiments of the present application and their description serve to explain the present application, and do not constitute an improper limitation on the present application. In the drawings:

[0058] Figure 1 The flowchart of the dynamic Voronoi skeleton constrained multi-modal door detection and position correction method of the embodiment of the present application;

[0059] Figure 2 The structural diagram of the dynamic Voronoi skeleton constrained multi-modal door detection and position correction system of the embodiment of the present application;

[0060] Figure 3A diagram for recognizing a door in a robot autonomous exploration process of an embodiment of the present application is shown.

[0061] Figure 4 A diagram for recognizing a door when a robot explores an actual environment of an embodiment of the present application is shown. DETAILED DESCRIPTION

[0062] It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict. The present application will be described in detail below with reference to the accompanying drawings and in combination with embodiments.

[0063] It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a group of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described herein can be executed in an order different from that shown herein.

[0064] As shown in the accompanying drawings, Figure 1 a dynamic Voronoi skeleton constrained multi-modal door detection and position correction method is provided in the present embodiment, which includes:

[0065] Sensor data is acquired by an RGBD camera and a multi-line laser radar;

[0066] An improved YOLO deep learning network is used to process an RGB image, to realize coarse positioning of a door body and output three-dimensional initial coordinates of a door frame;

[0067] Three-dimensional geometric verification is performed in combination with point cloud data fusion, including ground point cloud elimination, door frame plane fitting, and point cloud density analysis to determine the open or closed state of the door body;

[0068] A dynamic Voronoi skeleton model is constructed, and an incremental distance field updating algorithm is used to correct the environment topology in real time;

[0069] A curvature constraint projection algorithm is used to map the detection result to the skeleton center line;

[0070] The coordinates of the door body center on a two-dimensional grid map are output.

[0071] Figure 1The patent flowchart shows that it starts from the multi-sensor data synchronization module, unifies the spatiotemporal reference of the RGBD camera and lidar through timestamp alignment and spatial calibration, and inputs the synchronized RGB image into the door body coarse detection module. The door frame bounding box and confidence level are output using the YOLO algorithm. At the same time, the point cloud data enters the refined processing module for ground removal, plane modeling and density analysis, extracts the three-dimensional parameters of the door frame and determines the open and closed status; the dynamic Voronoi skeleton generation module builds an environmental topology network based on the real-time point cloud, and adapts to obstacle changes through incremental distance field updates. The position correction module combines the initial detection coordinates with the skeleton topology for curvature optimization projection, avoids high curvature areas, and finally outputs the coordinates of the door center on the two-dimensional grid map, forming a complete closed-loop process from data acquisition, feature extraction, environmental modeling to error suppression.

[0072] Furthermore, RGBD cameras and LiDARs have different hardware characteristics. The former captures color images and depth information at high frequency, while the latter scans the environment's geometry in the form of a point cloud. First, a timestamp-weighted calibration is performed to ensure that both sensors collect data at the same moment. Then, a spatial coordinate transformation is performed to align the camera's image coordinate system and the LiDAR's 3D point cloud coordinate system to the robot's coordinate system. This process eliminates data bias caused by hardware differences, providing accurate and consistent input for subsequent processing.

[0073] In the time synchronization phase, a weighted least squares synchronization model was established to achieve spatiotemporal alignment of multi-sensor data. This model eliminates timing deviations between the RGBD camera and the LiDAR due to hardware clock discrepancies. The data acquisition times of the two sensors were aligned through weighted least squares optimization.

[0074] ;

[0075] in, is the optimal synchronization time offset (unit: seconds), Value , Value Is the sensor weight coefficient, the weight coefficient can be set according to the sensor sampling rate, , It is the timestamp deviation between the RGBD camera and the lidar.

[0076] In the spatial calibration phase, the coordinate transformation matrix between sensors is solved through joint calibration with a calibration plate, thereby solving the problem of unifying the sensor coordinate system and solving the spatial alignment problem of cross-modal data (images and point clouds).

[0077] ;

[0078] in, is the homogeneous transformation matrix from the laser radar to the camera; is the 3D rotation matrix; is the translation vector; More accurate transformation matrices calculated for optimization methods; 、 are the corresponding point coordinates of the lidar and camera respectively; is the transformation matrix.

[0079] Furthermore, the YOLO-based door detection and point cloud mapping positioning technology mainly includes the following steps: First, the RGB image is detected in real time through a pre-trained YOLO model (such as YOLOv5) to identify the 2D bounding box of the door frame. The model input is The output includes the center pixel coordinates, width, height and confidence of the bounding box. The detection results need to be filtered by confidence (threshold 0.7) and non-maximum suppression (NMS, loU threshold 0.45) to eliminate false detections and overlapping frames. Subsequently, combined with the depth map of the RGBD camera or the lidar point cloud data, the two-dimensional pixel coordinates are converted into three-dimensional space coordinates. Specifically, the depth value of the corresponding point in the depth map is used to calculate the depth of the camera through the intrinsic parameters (focal length and main point ) Calculate the three-dimensional position of the door frame center using the formula: ,

[0080] in, is the X coordinate in the camera coordinate system; is the pixel coordinate; is the column coordinate of the principal point of the image; is the focal length in the X-axis direction; is the depth value; is the Y coordinate in the camera coordinate system; is the focal length in the Y-axis direction; is the row coordinate of the principal point of the image.

[0081] For lidar data, the coordinates in the camera coordinate system need to be converted to the radar coordinate system through a pre-calibrated extrinsic matrix.

[0082] Furthermore, the LiDAR point cloud is processed in multiple stages to verify and optimize the rough detection results. First, the intelligent filtering algorithm is used to remove ground reflection points (such as floors and carpets) to avoid interference. Among them, the ground point cloud is removed using

[0083] ;

[0084] in, is the ground plane normal vector (fitted by RANSAC), is the ground plane equation offset, Represents a collection of raw point clouds.

[0085] Next, we extract point cloud clusters near the door frame location provided by the coarse detection. A robust fitting algorithm is used to construct a door frame plane model, calculating the precise 3D dimensions and normal orientation of the door. Robust RANSAC is used to fit the door frame plane, suppressing the influence of outliers and accurately fitting the door frame plane.

[0086] ;

[0087] in, represents the door frame plane unit normal vector, is the offset from the plane to the origin, represents the German-McClure robust kernel parameter, A three-dimensional point in space : is a point on the door frame plane.

[0088] Finally, by analyzing the density and distribution of the point cloud inside the door frame, we can determine whether the door is open or closed. If the door is closed, the point cloud will be blocked by the door panel and concentrated. If the door is open, the point cloud can penetrate the door frame area, and the density is significantly reduced. The door state is determined by the point cloud density and the direction of the normal vector.

[0089] ;

[0090] in, Indicates the point cloud density inside the door frame, is the number of point clouds in the valid area. Indicates the width and height of the door frame (unit: meter), where the height and height of the door frame are related to the point cloud density , represents the set of door states; The door is in the closed state; The door is in the open state; for Other circumstances other than

[0091] It can be obtained through pre-measurement or testing to facilitate accurate identification of the door and judgment of the door's open and closed state.

[0092] Furthermore, a real-time topological map of the environment is constructed to provide geometric constraints for position correction. Based on a two-dimensional grid map created from lidar data, a dynamic wavefront algorithm is used to locally update the grid affected by obstacle changes. A single-pixel wide Voronoi diagram is generated by combining conditional comparison and skeletonization methods. Using integer square distance optimization, an incremental update algorithm dynamically generates a Voronoi skeleton, a network structure that reflects the centerline of the environment's traffic area. Skeleton points are located near the equidistant boundary between two obstacles. Skeleton points are determined as follows:

[0093] ;

[0094] in, is any point on the grid map; is the Voronoi skeleton point set; is the coordinate set of all obstacles in the environment; is any point on the grid map; is any point on the grid map; is the resolution of the raster map. Considering that points on the Voronoi skeleton are at the same distance from nearby obstacles, the center of a door is usually at the same distance from the door frames on both sides and is also located at or very close to the environment's Voronoi skeleton. Therefore, we propose to use the Voronoi skeleton to correct the detected door position.

[0095] Furthermore, multi-source information is integrated to achieve high-precision positioning. First, the door frame coordinates, processed from the point cloud, are projected onto the Voronoi skeleton. A curvature optimization algorithm is used to avoid bifurcations or corners in the skeleton, ensuring that the projected point is located on the straight part of the Voronoi skeleton. Finally, the optimized door center point is mapped onto a two-dimensional grid map.

[0096] During the robot's movement, the camera outputs image data at a high frequency, and the robot observes the same door multiple times. The door detection module continuously returns the door position, and the door position in the map coordinate system is recorded as , ,in Indicates the number of times the door has been observed. At the same time, due to the large uncertainty of the robot's positioning information during the mapping process, there is often a deviation between the positions of the door returned multiple times. Therefore, the door position output by the door detection module is stored and its center of mass is found using the following formula. The center of mass position is used as the initial value of the door. In order to further correct the position of the door, the generalized Voronoi diagram is used. Since the generalized Voronoi diagram stores points that are equidistant from the obstacle, the center line of the door is generally included in it. In order to improve the accuracy of the correction and also increase the search speed, the edge branches on the generalized Voronoi diagram that are directly in contact with the obstacle are removed. Taking into account the curvature Reflects the local bending degree of the skeleton. :The frame is straight, usually the center of the door also appears here, which is the ideal projection area; when : The skeleton needs to avoid sharp turns or bifurcations. Therefore, it is proposed to search for the nearest point along the skeleton and combine it with curvature constraint optimization. First, direct nearest point search: only use Euclidean distance as an indicator to find the nearest point on the Voronoi skeleton with the initial coordinates. The nearest point, based on the distance, introduces the local geometric features (curvature) of the skeleton as a penalty term, and the optimization goal is:

[0097] ;

[0098] in, is the weight coefficient, is the door center coordinate of the initial detection, is the corrected door center coordinate position, is the curvature; is any point in the Voronoi skeleton point set; is the Voronoi skeleton point set;

[0099] About weight coefficient choice, , can be adjusted Controlling curvature affects strength; degenerates to the direct closest point; The door center is projected onto the low-curvature skeleton segment to avoid topological ambiguity.

[0100] The curvature is calculated as follows:

[0101] ;

[0102] in, and are the first-order and second-order derivatives of the skeleton curve respectively; the curvature calculation is optimized using discretized skeleton points Approximate derivatives are obtained by differencing at adjacent points:

[0103] ;

[0104] ;

[0105] in, is the skeleton point spacing.

[0106] ;

[0107] in, The set of skeleton points for finding the center of the door on the Voronoi skeleton. is the door center coordinate of the initial detection, The corrected door center coordinates. Search within a radius of r = 1.0m to avoid global traversal and finally output the corrected door center position And marked on the two-dimensional raster map.

[0108] like Figure 2 As shown, this embodiment provides a multimodal door detection and position correction system with dynamic Voronoi skeleton constraints, including:

[0109] Multi-sensor spatiotemporal alignment module, used to synchronize the spatiotemporal data of RGBD cameras and multi-line lidar;

[0110] The door rough detection module is used to identify the door frame in the RGB image through the improved YOLO network and output the initial 3D coordinates;

[0111] Point cloud refinement processing module, used to fuse point cloud data to perform ground removal, door frame plane fitting, and open / close status determination;

[0112] Dynamic Voronoi skeleton generation module, used to construct the environment topology skeleton in real time through incremental distance field update algorithm;

[0113] Position correction module, used to map the detection results to the skeleton centerline through the curvature constrained projection algorithm;

[0114] The coordinate output module is used to output the coordinates of the door center on the two-dimensional grid map.

[0115] In this embodiment, each module forms a closed-loop processing flow, significantly improving the robustness and navigation reliability of door positioning in complex dynamic environments. In this embodiment, each module has a clear division of labor and is closely linked to form a complete closed loop from environmental perception to precise positioning.

[0116] like Figure 3 As shown in the figure, the method proposed in this example is used to identify doors during the autonomous exploration process of a robot equipped with a laser radar and an RGBD camera. Figure 3 On the left, you can see that there are 9 doors in the simulation environment. During the robot's autonomous exploration, it continuously recognizes the doors and uses Figure 3 The purple square on the right represents the projection of the center position of the door on the grid map. It can be seen that the robot can finally fully identify the nine doors and correctly mark their locations.

[0117] Figure 4 is the recognition of the door when the robot explores the actual environment, Figure 4The left side shows the door in the actual environment, and the right side shows the marking of the door position during autonomous exploration. It can be seen that the robot can accurately mark the actual door position in the grid map, which also verifies the effectiveness of the method proposed in this embodiment.

[0118] The technical advantages of the present invention are:

[0119] (1) Multimodal sensor fusion and spatiotemporal alignment technology: Through the weighted time synchronization model and joint calibration algorithm, the spatiotemporal data of RGBD camera and lidar are accurately aligned, solving the problems of timing misalignment and coordinate deviation in cross-modal data fusion, and improving the reliability of detection and consistency of environmental perception.

[0120] (2) Dynamic Voronoi skeleton generation and real-time update mechanism: Using an incremental distance field update method, the skeleton topology structure is dynamically adjusted in combination with the obstacle movement speed to ensure that the environmental representation reflects the latest status (such as moving obstacles and temporary piles) in real time, overcoming the defect that traditional static skeletons cannot adapt to dynamic scenes.

[0121] (3) Curvature-constrained topological projection optimization algorithm: A curvature penalty term is introduced into the skeleton mapping to avoid projection ambiguity in high-curvature areas such as corners and bifurcation points, ensuring that the center position of the door is always mapped to the safe center line of the passage area.

[0122] (4) Door opening and closing status verification mechanism: Through the sparsity of the point cloud inside the door frame (low density when closed), the door opening and closing status can be effectively judged.

[0123] Through the above innovations, the present invention overcomes the bottlenecks of traditional solutions in terms of dynamic environment adaptability, multi-sensor collaboration, and complex door status determination, providing mobile robots with high-precision and robust door positioning capabilities. It can be widely used in indoor autonomous navigation scenarios such as warehousing and logistics, and medical services. The module decoupling design: each module can be optimized independently (such as replacing YOLO with other detectors) to improve system flexibility; dynamic adaptability: the Voronoi skeleton is updated in real time to support moving obstacles and temporary environmental changes; precision-efficiency balance: a hierarchical strategy of coarse detection (YOLO) and fine processing (point cloud) takes into account both real-time and accuracy.

[0124] The above are merely preferred embodiments of the present application, but the scope of protection of the present application is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this application should be included in the scope of protection of the present application. Therefore, the scope of protection of the present application should be based on the scope of protection of the claims.

Claims

1. A multimodal door detection and position correction method based on dynamic Voronoi skeleton constraints, characterized by: include: Acquire sensor data through RGBD cameras and multi-line lidar; The improved YOLO deep learning network is used to process the RGB image to achieve rough door positioning and output the initial 3D coordinates of the door frame. Combine point cloud data fusion for 3D geometry verification, including ground point cloud elimination, door frame plane fitting, and point cloud density analysis to determine the door open or closed status; Build a dynamic Voronoi skeleton model and modify the environment topology in real time through an incremental distance field update algorithm; The detection results are mapped to the skeleton centerline using a curvature constrained projection algorithm; Output the coordinates of the door center on the two-dimensional grid map; The process of constructing the dynamic Voronoi skeleton model includes: Create a two-dimensional raster map based on lidar data; The environment topology skeleton network is dynamically generated by an incremental distance field update algorithm, and the skeleton points of the environment topology skeleton network meet the following conditions: Where x is any point on the grid map; V is the set of Voronoi skeleton points; O is the set of coordinates of all obstacles in the environment; o1 is any point on the grid map; o2 is any point on the grid map; η is the resolution of the grid map; The process of the curvature constrained projection algorithm includes: Project the initial portal coordinates to the Voronoi skeleton; Within the skeleton search radius, the optimal projection point of the low curvature area is found by combining the curvature optimization objective function. The objective function is: Among them, x corrected is the corrected door center coordinate position; v is any point in the Voronoi skeleton point set; V is the Voronoi skeleton point set; x initial is the coordinate of the door center of the initial detection; λ is the weight coefficient; Curvature(v) is the curvature.

2. The multimodal door detection and position correction method based on dynamic Voronoi skeleton constraints as claimed in claim 1, characterized in that: The process of acquiring sensor data includes: Perform time synchronization and build a weighted least squares model to align the timestamps of the RGBD camera and the lidar. The model is: Among them, t sync is the optimal synchronization time offset; t is time; ω rgbd is the sensor weight coefficient of the RGBD camera; ω lidar is the sensor weight coefficient of the lidar; Δt rgbd , Δt lidar are the timestamp deviations of the RGBD camera and the lidar respectively; Perform spatial calibration, solve the homogeneous transformation matrix through the calibration plate, and unify the sensor coordinate system. The matrix is: in, is the homogeneous transformation matrix from the laser radar to the camera; R is the 3D rotation matrix; ∈ is the translation vector; ΔT is the more accurate transformation matrix calculated by the optimization method; p lidar 、p cam are the corresponding point coordinates of the lidar and camera respectively; T is the transformation matrix.

3. The multimodal door detection and position correction method based on dynamic Voronoi skeleton constraints as claimed in claim 1, characterized in that: The process of door body rough positioning includes: The YOLO network is used to identify the door frame bounding box in the RGB image, and then processed by confidence filtering and non-maximum suppression; Combined with the depth map or lidar point cloud data, the two-dimensional pixel coordinates are converted into three-dimensional space coordinates. The conversion formula is: Among them, x cam is the X coordinate in the camera coordinate system; u, v are pixel coordinates; c x is the column coordinate of the principal point of the image; f x is the focal length in the X-axis direction; d is the depth value; y cam is the Y coordinate in the camera coordinate system; f y is the focal length in the Y-axis direction; c y is the row coordinate of the principal point of the image.

4. The multimodal door detection and position correction method based on dynamic Voronoi skeleton constraints as claimed in claim 1, characterized in that: The process of 3D geometry verification includes: Perform ground point cloud removal, use the RANSAC algorithm to fit the ground plane and filter out interference points; Perform door frame plane fitting and use a robust fitting algorithm to construct a door frame plane model. The door frame plane model is: Where n is the unit normal vector of the door frame plane; d is the offset from the plane to the origin; p is a three-dimensional point in space; P door is a point on the door frame plane; ∈ is the German-McClure robust kernel parameter; The door opening and closing status is analyzed based on the point cloud density, and the point cloud density inside the door frame is calculated to determine the status. The determination formula is: Among them, State is the door state collection; Closed means the door is closed; Open means the door is open; ρ inner is the point cloud density inside the door frame; τ is the point cloud density; else is ρ inner ≥τpts / m 2 Other situations besides.

5. The multimodal door detection and position correction method based on dynamic Voronoi skeleton constraints as claimed in claim 1, characterized in that: The process of outputting the door center coordinates includes mapping the corrected coordinates to a two-dimensional grid map and marking them.

6. The system of the multimodal door detection and position correction method based on dynamic Voronoi skeleton constraints according to any one of claims 1 to 5, characterized in that: include: Multi-sensor spatiotemporal alignment module, used to synchronize the spatiotemporal data of RGBD cameras and multi-line lidar; The door rough detection module is used to identify the door frame in the RGB image through the improved YOLO network and output the initial 3D coordinates; Point cloud refinement processing module, used to fuse point cloud data to perform ground removal, door frame plane fitting, and open / close status determination; Dynamic Voronoi skeleton generation module, used to construct the environment topology skeleton in real time through incremental distance field update algorithm; Position correction module, used to map the detection results to the skeleton centerline through the curvature constrained projection algorithm; The coordinate output module is used to output the coordinates of the door center on the two-dimensional grid map.

Images