Laser radar and stereoscopic vision complementary positioning fusion positioning method and system

Abstract

The invention discloses a laser radar and stereoscopic vision complementary positioning fusion positioning method and system, and relates to the technical field of radio wave positioning. The method comprises the following specific steps: S100, carrying out timestamp alignment and space calibration on an image and point cloud data; S200, identifying a dynamic object area; S300, calculating displacement to identify a dynamic interference area; S400, determining a final real dynamic interference area; according to the method, through cross-modal cooperation of visual semantic segmentation and laser radar motion vectorization analysis, accurate detection and real-time elimination of dynamic interference are realized, visual semantic segmentation provides pixel-level dynamic object recognition, laser radar point cloud cluster displacement analysis can quantify a motion trail, and through a space-time alignment mechanism, the visual semantic segmentation and the laser radar motion vectorization analysis can realize accurate detection and real-time elimination of the dynamic interference. Real dynamic targets and sensor noise are effectively distinguished, and positioning errors caused by data errors of a single sensor are avoided.

Description

Technical Field

[0001] The present invention relates to the field of radio wave positioning technology, and in particular to a positioning method and system for integrating complementary positioning of laser radar and stereo vision. Background Art

[0002] With the rapid development of science and technology, intelligent positioning technology has become the core support of many cutting-edge fields. In the field of intelligent logistics, automatic guided vehicles need precise positioning to achieve efficient handling and sorting of goods. The accuracy of positioning directly affects the efficiency and cost of logistics operations. In the field of security monitoring, patrol robots rely on precise positioning to patrol according to preset routes and discover safety hazards in time. In virtual reality and augmented reality applications, precise positioning can provide users with a more immersive experience and perfectly integrate virtual elements with the real environment. However, interference from moving objects in dynamic and complex environments and the problem of spatiotemporal asynchrony of sensor data are important factors that lead to positioning drift and map distortion in existing fusion solutions.

[0003] At present, when the existing positioning technology faces a dynamic environment and there are a large number of dynamic objects in the environment, the point cloud data obtained by the lidar will contain a large number of noise point clouds generated by dynamic objects. These noise point clouds will interfere with the extraction of static environment features by the positioning algorithm, resulting in large deviations in the positioning results. Although traditional stereo vision positioning can obtain rich visual information, in dynamic scenes, due to factors such as changes in lighting and rapid movement of objects, feature points in the visual image are prone to mismatching or loss. Moreover, the existing multi-sensor fusion positioning method often simply superimposes the lidar and stereo vision data without fully considering the motion characteristics of objects in the dynamic environment, and cannot effectively eliminate dynamic interference, resulting in a significant reduction in the performance of the positioning system in complex dynamic environments.

[0004] In summary, the limitations of current intelligent positioning technology in dynamic environments seriously restrict its widespread application in practical scenarios. Therefore, there is an urgent need for a new positioning method that can combine visual semantic segmentation and lidar point cloud analysis to achieve real-time elimination of dynamic interference and update the positioning benchmark, thereby significantly improving the robustness and accuracy of positioning in dynamic scenarios. Summary of the invention

[0005] The purpose of the present invention is to make up for the shortcomings of the prior art and to provide a positioning method and system that integrates lidar and stereo vision complementary positioning. It can accurately identify dynamic objects through visual semantic segmentation and lidar point cloud motion vector analysis, and eliminate interference point clouds in real time, thereby providing pure static background data for positioning and effectively reducing positioning deviations caused by dynamic interference.

[0006] In order to solve the above technical problems, the present invention provides the following technical solutions: On the one hand, a positioning method for complementary positioning fusion of laser radar and stereo vision, the specific steps of the method are: S100, synchronously acquiring laser radar point cloud data and stereoscopic vision image data, and performing time stamp alignment and spatial calibration on the two; S200, performing real-time semantic segmentation on the stereoscopic vision image, identifying the dynamic object area and generating a pixel-level dynamic mask; S300, motion vector analysis of laser radar continuous frame point cloud, including: Extract point cloud clusters through clustering; Based on the displacement vector calculation of the centroid of the point cloud clusters in adjacent frames, the point cloud clusters whose displacement exceeds the dynamic threshold are selected as dynamic interference areas; S400: align the visual dynamic mask with the laser radar dynamic point cloud cluster in time and space, and determine the final real dynamic interference area, including: Project the visual dynamic mask to the lidar coordinate system and calculate its overlap rate with the dynamic point cloud cluster; For areas where the overlap rate exceeds the preset threshold, it is determined as a real dynamic interference area, and for areas where the overlap rate is lower than the preset threshold, it is determined as an uncertain area, and a secondary verification is performed in combination with IMU motion prediction; S500: Eliminate the point cloud and visual features in the real dynamic interference area, retain the static background point cloud, and update the positioning reference based on the geometric and texture features of the static point cloud.

[0007] Furthermore, the process of spatially calibrating the laser radar and the stereo vision camera in S100 is as follows: collecting multiple sets of images and point cloud data within the camera field of view and the laser radar scanning range, and extracting the coordinates of the corner points through the images taken by the camera. , and obtain the 3D coordinates of the corresponding corner points in the LiDAR point cloud , establish the overdetermined equations of the image coordinates and the lidar coordinate system, that is, for each corner point, there is an equation: ,in is the pixel coordinate of the corner point in the image, extracted from the image taken by the camera, is the scale factor, is the camera intrinsic parameter matrix and , , The camera is , The focal length of the direction, , are the coordinates of the principal point of the image, is the rotation matrix, which is used to describe the rotation relationship between the camera coordinate system and the lidar coordinate system. is a translation vector that describes the translation offset of the camera coordinate system relative to the lidar coordinate system. After collecting multiple sets of corner point coordinates, they are substituted into the equation to form an overdetermined set of equations, thereby obtaining the camera intrinsic parameter matrix , rotation matrix with translation vector , and establish the spatial mapping relationship between the lidar and the stereo vision camera.

[0008] Furthermore, in S200, the stereoscopic image is segmented in real time to obtain the input image. The features are continuously extracted through the downsampling path. During the downsampling process, the size of the feature map is halved and the number of channels is doubled, that is, the number of downsampling times is , No. After downsampling, the feature map size is ,satisfy , , , thereby extracting image features of different scales. The upsampling path fuses the high-dimensional features obtained by downsampling with the low-dimensional features of the corresponding layer. The upsampling restores the high-dimensional features to the same size as the corresponding low-dimensional features through deconvolution operations, and then splices and fuses them, finally outputting a pixel-level dynamic mask. ,During splicing and fusion, the feature channels at the same position are merged to accurately identify the pixel range of dynamic objects.

[0009] Furthermore, the step of extracting point cloud clusters in S300 is: In the lidar point cloud space, set the neighborhood search radius With minimum point threshold ,by is the seed point, and the data structure is used to search for the neighborhood whose distance is less than Point set ; Divide the space along the coordinate axis and construct a binary tree data structure; When searching, start from the root node and search the left and right subtrees according to the positional relationship between the query point and the node hyperplane to narrow the search range; when , then these points are grouped into a point cloud cluster, and continue with The points in are the seed points and the search process is repeated until all points are clustered.

[0010] Furthermore, the step S300 extracts the first Frame point cloud cluster Center of mass coordinates , No. Frame corresponding point cloud cluster Center of mass coordinates , then the displacement vector , its displacement , the coordinates of all points in each point cloud cluster are weighted averaged, with a weight of 1, that is, , , ,in Point cloud cluster The number of midpoints, Representative Frame point cloud cluster Middle Points in the laser radar coordinate system Axis coordinates, The point is The coordinates of the axes, For its The axis coordinates, when , the point cloud cluster is screened as a dynamic interference area.

[0011] Furthermore, in S400, the visual dynamic mask is projected into the laser radar coordinate system, and the camera intrinsic parameter matrix , rotation matrix with translation vector , for the pixels in the visual dynamic mask , using the camera intrinsic parameter matrix The inverse matrix , through back projection Get the three-dimensional coordinates in the camera coordinate system ,in is the camera intrinsic parameter matrix The inverse matrix of Convert to the ray direction in the camera coordinate system, combined with depth information Get the three-dimensional coordinates, and then transform the coordinates Convert to the LiDAR coordinate system and calculate its overlap rate with the dynamic point cloud cluster , the overlap ratio The ratio of the number of points that fall into the laser radar dynamic point cloud cluster in the projected visual dynamic mask area to the total number of points in the visual dynamic mask area is obtained, that is, ,in is the point set in the projected visual dynamic mask area, is the point set of the laser radar dynamic point cloud cluster, for the overlap rate Exceeding the threshold area, that is , determined as the real dynamic interference area.

[0012] Furthermore, in S400, for the overlap rate Not exceeding the threshold area, that is , determined as an uncertain area, and combined with IMU motion prediction for secondary verification. During the verification process, IMU measures acceleration With angular velocity , the displacement is obtained by integration With posture changes , that is, in the time interval Internal displacement , displacement posture change ,in is the initial displacement, is the initial velocity, is the initial attitude, through the angular velocity The predicted displacement and posture are used to update the spatial position of the uncertain region, and re-evaluate whether it is a dynamic region. The re-evaluation is performed by transforming the initial coordinates of the uncertain region according to the displacement and posture changes, that is, ,in are the initial coordinates of the uncertainty region, are the updated coordinates, According to the change of posture The obtained rotation matrix is ​​compared with the lidar dynamic point cloud cluster information again to determine whether it is a real dynamic interference area.

[0013] Furthermore, the specific steps of S500 are: Eliminate the point cloud and visual features in the real dynamic interference area determined in S400 to eliminate the interference of dynamic factors on positioning; Extract plane features and corner features from the remaining static point cloud; According to the mapping relationship established by S100 spatial calibration, the plane features of the static point cloud are matched with the corner features in the visual image to fuse the static information of different sensors; The fused static information is used to predict and update the positioning benchmark.

[0014] On the other hand, the positioning system of laser radar and stereo vision complementary positioning fusion, the components of this system include: data acquisition module, dynamic object recognition module, dynamic interference division module, dynamic interference determination module, positioning fusion module; The data acquisition module uses a laser radar and a stereo vision camera to respectively collect point cloud data and visual image data of the environment, align the data according to timestamps, and perform spatial calibration to construct a spatial mapping relationship; The dynamic object recognition module performs semantic segmentation on the image captured by the stereo vision camera, identifies the dynamic object area in the image and generates a pixel-level dynamic mask; The dynamic interference division module performs motion vectorization analysis on the laser radar point cloud data, and selects point cloud clusters whose displacement exceeds a dynamic threshold as dynamic interference areas; The dynamic interference determination module combines the semantic segmentation results and the LiDAR point cloud data to determine the final real dynamic interference area; The positioning fusion module removes the dynamic interference point cloud and updates the positioning according to the remaining static background features.

[0015] Compared with the prior art, the positioning method and system of the complementary positioning fusion of laser radar and stereo vision have the following beneficial effects: 1. The present invention realizes accurate detection and real-time elimination of dynamic interference through cross-modal collaboration of visual semantic segmentation and LiDAR motion vector analysis. Visual semantic segmentation provides pixel-level dynamic object recognition, while LiDAR point cloud cluster displacement analysis can quantify motion trajectory. The two can effectively distinguish real dynamic targets from sensor noise through the spatiotemporal alignment mechanism, avoiding positioning errors caused by single sensor data errors.

[0016] 2. The present invention adopts a complementary positioning fusion method of lidar and stereo vision, which gives full play to the advantages of the two sensors and significantly enhances the adaptability of the positioning system to complex environments. Under different conditions, stereo vision can provide rich texture information to assist positioning, while lidar can stably obtain distance information, and fuse stereo vision images with lidar data. In dynamic scenes, the positioning benchmark is continuously updated based on the static background, so that the system can quickly adapt to environmental changes.

[0017] Other advantages, objectives and features of the present invention will be set forth in part in the following description and, in part, will be apparent to those skilled in the art based on an examination of the following or may be taught from the practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the prior art descriptions are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention, and for ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0019] Figure 1 The operation flow chart of the positioning method for complementary positioning fusion of laser radar and stereo vision; Figure 2 This is a block diagram of the positioning system that integrates complementary positioning of lidar and stereo vision. DETAILED DESCRIPTION

[0020] In order to further explain the technical means and effects adopted by the present invention to achieve the predetermined invention purpose, the specific implementation method, structure, characteristics and effects of the present invention are described in detail below in combination with the accompanying drawings and preferred embodiments.

[0021] Embodiment 1 like Figure 1 As shown, this embodiment elaborates on the specific steps of the positioning method of the complementary positioning fusion of LiDAR and stereo vision. By elaborating on the whole process from data collection to positioning reference update, the working principle of this technology in a complex dynamic environment is demonstrated. By using visual semantic segmentation and LiDAR point cloud motion vector analysis, dynamic interference is effectively identified and eliminated, and the positioning reference is updated based on static background features to achieve high-precision and high-robustness positioning.

[0022] First, enter the data acquisition stage (S100) to obtain the original data of the environment. Use the laser radar and stereo vision camera to work simultaneously to collect point cloud data and visual image data of the environment respectively. The laser radar obtains the distance information of the surrounding objects by emitting laser beams and measuring the time delay of the reflected light, thereby generating three-dimensional point cloud data. These point cloud data can accurately represent the spatial position of the object, but lack texture information. The stereo vision camera obtains rich texture information by taking images of the surrounding environment. However, it is difficult to accurately measure the distance of the object only by relying on the stereo vision camera. In order to make the data collected by the two sensors work together, timestamp alignment and spatial calibration are required. Timestamp alignment ensures that the laser radar point cloud data and stereo vision image data are collected at the same time to avoid data inconsistency caused by time differences. Spatial calibration is to establish the conversion relationship between the laser radar coordinate system and the camera coordinate system. The calibration object is within the camera field of view and the laser radar scanning range. The camera takes an image and extracts the coordinates of the corner points. , and obtain the 3D coordinates of the corresponding corner points in the LiDAR point cloud , for each corner point, there exists an equation: ,in is the pixel coordinate of the corner point in the image, extracted from the image taken by the camera, is the scale factor, the camera intrinsic parameter matrix , The camera is , The focal length of the direction, , are the coordinates of the principal point of the image, is the rotation matrix, which describes the rotation relationship between the camera coordinate system and the lidar coordinate system. is a translation vector that describes the translation offset of the camera coordinate system relative to the lidar coordinate system. After collecting multiple sets of corner point coordinates, they are substituted into the equation to form an overdetermined set of equations, thereby obtaining the camera intrinsic parameter matrix , rotation matrix with translation vector , establish an accurate spatial mapping relationship between the two, so that the information in the camera image can be accurately converted to the lidar coordinate system for unified processing.

[0023] Then, the dynamic object recognition stage (S200) is entered to perform real-time semantic segmentation on the image captured by the stereo vision camera, identify the dynamic object area in the image and generate a pixel-level dynamic mask. Downsampling is performed. In this process, the features of the image are continuously extracted. During the downsampling process, the size of the feature map is halved and the number of channels is doubled, that is, the number of downsampling times is , No. After downsampling, the feature map size is ,satisfy , , In this way, the network can extract image features at different scales and capture detailed information in the image, such as the edges and textures of objects. After downsampling, the image features are compressed into a small high-dimensional space and then enter the upsampling path. The upsampling restores the high-dimensional features to the same size as the corresponding low-dimensional features through deconvolution operations, and then performs splicing and fusion. The channel information of the high-dimensional features is expanded to restore the size of the image. During splicing and fusion, the feature channels at the same position are merged to enrich the feature information. Finally, the network outputs a pixel-level dynamic mask. ,This mask classifies each pixel in the image, explicitly indicating whether the pixel belongs to a dynamic object.

[0024] Then, the dynamic interference division stage (S300) is entered to perform motion vectorization analysis on the laser radar point cloud data, and select point cloud clusters whose displacement exceeds the dynamic threshold as dynamic interference areas. By extracting point cloud clusters, a neighborhood search radius is set in the laser radar point cloud space. With minimum point threshold ,by As the seed point, use the data structure to search for its neighborhood with a distance less than Point set , by continuously dividing the space along the coordinate axis, a binary tree structure is constructed. When searching, starting from the root node, the left subtree or right subtree is searched according to the position relationship between the query point and the node hyperplane to quickly narrow the search range and improve the search efficiency. , then these points are grouped into a point cloud cluster, and then continue with The points in are used as seed points and the search process is repeated until all points are clustered to complete the extraction of point cloud clusters. The displacement vector and displacement amount are calculated for the point cloud clusters after clustering. For the point cloud clusters after clustering, the first Frame point cloud cluster Center of mass coordinates , No. Frame corresponding point cloud cluster Center of mass coordinates , the centroid coordinates are obtained by weighted averaging the coordinates of all points in each point cloud cluster, with a weight of 1, that is, ,in Point cloud cluster The number of midpoints, Representative Frame point cloud cluster Middle Points in the laser radar coordinate system Axis coordinates, The point is The coordinates of the axes, For its Axis coordinates, displacement vector , displacement , pre-set a dynamic threshold ,when , it means that the point cloud cluster has shifted between the two frames and belongs to a dynamic object, so the point cloud cluster is filtered as a dynamic interference area.

[0025] Next, the dynamic interference determination stage (S400) is entered. The semantic segmentation result (i.e., pixel-level dynamic mask) and the LiDAR point cloud data are combined to determine the final real dynamic interference area. The visual dynamic mask is projected into the LiDAR coordinate system. For the pixel points in the visual dynamic mask, , using the camera intrinsic parameter matrix The inverse matrix , through back projection Get the three-dimensional coordinates in the camera coordinate system ,here The pixel coordinates Convert to the ray direction in the camera coordinate system, combined with depth information Get the three-dimensional coordinates, after coordinate transformation Convert to the LiDAR coordinate system and calculate the overlap rate between the projected visual dynamic mask area and the LiDAR dynamic point cloud cluster , overlap ratio The ratio of the number of points that fall into the laser radar dynamic point cloud cluster in the projected visual dynamic mask area to the total number of points in the visual dynamic mask area is obtained, that is, ,in is the point set in the projected visual dynamic mask area is the point set of the laser radar dynamic point cloud cluster, for the overlap rate Exceeding the threshold area, that is , determined as the real dynamic interference area, for the overlap rate Not exceeding the threshold area, that is , determined as an uncertain area, for the uncertain area, a secondary verification is performed in combination with IMU motion prediction, and MU measures acceleration With angular velocity , in the time interval The displacement is obtained by integration. With posture changes , displacement ,in is the initial displacement, is the initial velocity, and the attitude change ,in is the initial posture, and the predicted displacement and posture are used to update the spatial position of the uncertain area, and the initial coordinates of the uncertain area are transformed according to the displacement and posture changes, that is, ,in are the initial coordinates of the uncertainty region, are the updated coordinates, According to the change of posture The obtained rotation matrix is ​​compared with the lidar dynamic point cloud cluster information again to determine whether it is a real dynamic interference area.

[0026] Finally, the positioning fusion stage (S500) is entered. The positioning fusion module removes the point cloud and visual features in the determined real dynamic interference area, eliminates the interference of dynamic factors on positioning, and extracts plane features and corner features from the remaining static point cloud. For the static point cloud, the plane features are extracted, and the plane features of the static point cloud are matched with the corner features in the visual image according to the mapping relationship established by the spatial calibration, and the static information of different sensors is fused. Through this matching, the geometric information of the lidar can be combined with the texture information of the stereo vision to provide a richer and more accurate description of the environment. The fused static information is used to predict and update the positioning reference, and the predicted posture is corrected to obtain a more accurate positioning result and update the positioning reference.

[0027] In summary, through the close cooperation of the above steps S100-S500, we can clearly see the workflow of the positioning method that integrates the complementary positioning of lidar and stereo vision in a complex dynamic environment, from obtaining raw data, through dynamic object recognition, dynamic interference division, dynamic interference determination, gradually accurately identifying and eliminating dynamic interference, and finally using static background features to update the positioning benchmark, achieving high-precision and high-robustness positioning.

[0028] Embodiment 2 Based on the first embodiment, Figure 2 As shown, this embodiment provides the system composition of a positioning system that integrates lidar and stereo vision complementary positioning. By elaborating on the functions and collaborative working methods of each module of the system, the role of the system in achieving high-precision positioning is demonstrated, and it can effectively cope with complex dynamic environments and provide reliable support for precise positioning applications.

[0029] The positioning system of complementary positioning fusion of laser radar and stereo vision includes: data acquisition module, dynamic object recognition module, dynamic interference division module, dynamic interference determination module, positioning fusion module, among which: The data acquisition module is responsible for acquiring raw data from the external environment and integrates a laser radar and a stereo vision camera. The laser radar accurately measures the distance between itself and surrounding objects by emitting laser beams and receiving reflected light, thereby generating three-dimensional point cloud data. The stereo vision camera acquires rich texture information by taking images of the surrounding environment, and performs time stamp alignment and spatial calibration on the data.

[0030] The dynamic object recognition module works based on the image data collected by the stereo vision camera. The module performs real-time semantic segmentation on the input image to determine whether it belongs to a dynamic object and further determines the category of the dynamic object. After identifying the dynamic object area, the module generates a pixel-level dynamic mask, which realizes the precise positioning and separation of the dynamic object, providing an important basis for subsequent interference processing.

[0031] The dynamic interference division module performs an in-depth analysis of the point cloud data collected by the lidar, processes the lidar point cloud data of continuous frames, divides the point cloud into different point cloud clusters by clustering, tracks the movement of each point cloud cluster between different frames, and determines whether the point cloud cluster is in motion by calculating the displacement of the centroid of the point cloud cluster in adjacent frames. If the displacement of the centroid of a point cloud cluster exceeds a preset dynamic threshold, then the point cloud cluster is determined to be a possible source of dynamic interference.

[0032] The dynamic interference determination module integrates the results of the dynamic object recognition module and the dynamic interference division module to accurately determine the actual dynamic interference area, projects the pixel-level dynamic mask generated by the dynamic object recognition module into the lidar coordinate system, and compares and analyzes it with the dynamic point cloud cluster obtained by the dynamic interference division module. By calculating the overlap rate of the two, it preliminarily determines whether a certain area is a dynamic interference area. For uncertain areas, the module will use the IMU data for secondary verification to determine again whether it is a dynamic interference area, thereby improving the accuracy of the judgment.

[0033] The positioning fusion module is responsible for the final positioning calculation and positioning benchmark update, eliminating the point cloud and visual features in the dynamic interference area identified by the dynamic interference determination module, extracting key features from the static background data, and matching and fusing the plane features of the static point cloud with the corner features in the visual image using the mapping relationship established by spatial calibration. Based on the fused static information, the positioning benchmark is predicted and updated. By continuously updating the positioning benchmark, the system can adapt to environmental changes in real time and provide high-precision positioning results.

[0034] To sum up, this embodiment introduces in detail the various component modules and working principles of the positioning system that integrates the complementary positioning of lidar and stereo vision. The modules work closely together to give full play to the complementary advantages of lidar and stereo vision, so that the entire positioning system can still work stably and accurately in complex dynamic environments.

[0035] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. Although the present invention has been disclosed as a preferred embodiment as above, it is not used to limit the present invention. Any technical personnel in this field can make some changes or modify the technical contents disclosed above into equivalent embodiments without departing from the scope of the technical solution of the present invention. However, any brief modifications, equivalent changes and modifications made to the above embodiments based on the technical essence of the present invention without departing from the content of the technical solution of the present invention are still within the scope of the technical solution of the present invention.

Claims

1. A positioning method combining complementary positioning of laser radar and stereo vision, characterized in that: The specific steps of this method are: S100, synchronously acquiring laser radar point cloud data and stereoscopic vision image data, and performing time stamp alignment and spatial calibration on the two; S200, performing real-time semantic segmentation on the stereoscopic vision image, identifying the dynamic object area and generating a pixel-level dynamic mask; S300, motion vector analysis of laser radar continuous frame point cloud, including: Extract point cloud clusters through clustering; Based on the displacement vector calculation of the centroid of the point cloud clusters in adjacent frames, the point cloud clusters whose displacement exceeds the dynamic threshold are selected as dynamic interference areas; S400: align the visual dynamic mask with the laser radar dynamic point cloud cluster in time and space, and determine the final real dynamic interference area, including: Project the visual dynamic mask to the lidar coordinate system and calculate its overlap rate with the dynamic point cloud cluster; For areas where the overlap rate exceeds the preset threshold, it is determined as a real dynamic interference area, and for areas where the overlap rate is lower than the preset threshold, it is determined as an uncertain area, and a secondary verification is performed in combination with IMU motion prediction; S500: Eliminate the point cloud and visual features in the real dynamic interference area, retain the static background point cloud, and update the positioning reference based on the geometric and texture features of the static point cloud.

2. The positioning method of complementary positioning fusion of laser radar and stereo vision according to claim 1 is characterized in that: The process of spatially calibrating the laser radar and the stereo vision camera in S100 is as follows: collecting multiple sets of images and point cloud data within the camera field of view and the laser radar scanning range, and extracting the coordinates of the corner points through the images taken by the camera. , and obtain the 3D coordinates of the corresponding corner points in the LiDAR point cloud , establish the overdetermined equations of the image coordinates and the lidar coordinate system, that is, for each corner point, there is an equation: ,in is the pixel coordinate of the corner point in the image, extracted from the image taken by the camera, is the scale factor, is the camera intrinsic parameter matrix and , , The camera is , The focal length of the direction, , are the coordinates of the principal point of the image, is the rotation matrix, which is used to describe the rotation relationship between the camera coordinate system and the lidar coordinate system. is a translation vector that describes the translation offset of the camera coordinate system relative to the lidar coordinate system. After collecting multiple sets of corner point coordinates, they are substituted into the equation to form an overdetermined set of equations, thereby obtaining the camera intrinsic parameter matrix , rotation matrix with translation vector , and establish the spatial mapping relationship between the lidar and the stereo vision camera.

3. The positioning method of complementary positioning fusion of laser radar and stereo vision according to claim 1 is characterized in that: In S200, the stereoscopic image is segmented in real time to obtain the input image. The features are continuously extracted through the downsampling path. During the downsampling process, the size of the feature map is halved and the number of channels is doubled, that is, the number of downsampling times is , No. After downsampling, the feature map size is ,satisfy , , , thereby extracting image features of different scales. The upsampling path fuses the high-dimensional features obtained by downsampling with the low-dimensional features of the corresponding layer. The upsampling restores the high-dimensional features to the same size as the corresponding low-dimensional features through deconvolution operations, and then splices and fuses them, finally outputting a pixel-level dynamic mask. ,During splicing and fusion, the feature channels at the same position are merged to accurately identify the pixel range of dynamic objects.

4. The positioning method of complementary positioning fusion of laser radar and stereo vision according to claim 1 is characterized in that: The steps of extracting point cloud clusters in S300 are as follows: In the lidar point cloud space, set the neighborhood search radius With minimum point threshold ,by is the seed point, and the data structure is used to search for the neighborhood whose distance is less than Point set ; Divide the space along the coordinate axis and construct a binary tree data structure; When searching, start from the root node and search the left and right subtrees according to the positional relationship between the query point and the node hyperplane to narrow the search range; when , then these points are grouped into a point cloud cluster, and continue with The points in are the seed points and the search process is repeated until all points are clustered.

5. The positioning method of complementary positioning fusion of laser radar and stereo vision according to claim 4 is characterized in that: The S300 extracts the first Frame point cloud cluster Center of mass coordinates , No. Frame corresponding point cloud cluster Center of mass coordinates , then the displacement vector , its displacement , the coordinates of all points in each point cloud cluster are weighted averaged, with a weight of 1, that is, , , ,in Point cloud cluster The number of midpoints, Representative Frame point cloud cluster Middle Points in the laser radar coordinate system Axis coordinates, The point is The coordinates of the axes, For its The coordinates of the axis, when , the point cloud cluster is screened as a dynamic interference area.

6. The positioning method of complementary positioning fusion of laser radar and stereo vision according to claim 2 is characterized in that: In S400, the visual dynamic mask is projected into the laser radar coordinate system, and the camera intrinsic parameter matrix is ​​used , rotation matrix with translation vector , for the pixels in the visual dynamic mask , using the camera intrinsic parameter matrix The inverse matrix , through back projection Get the three-dimensional coordinates in the camera coordinate system ,in is the camera intrinsic parameter matrix The inverse matrix of Convert to the ray direction in the camera coordinate system, combined with depth information Get the three-dimensional coordinates, and then transform the coordinates Convert to the LiDAR coordinate system and calculate its overlap rate with the dynamic point cloud cluster , the overlap ratio The ratio of the number of points that fall into the laser radar dynamic point cloud cluster in the projected visual dynamic mask area to the total number of points in the visual dynamic mask area is obtained, that is, ,in is the point set in the projected visual dynamic mask area, is the point set of the laser radar dynamic point cloud cluster, for the overlap rate Exceeding the threshold area, that is , determined as the real dynamic interference area.

7. The positioning method of complementary positioning fusion of laser radar and stereo vision according to claim 1 is characterized in that: In S400, for the overlap rate Not exceeding the threshold area, that is , determined as an uncertain area, and combined with IMU motion prediction for secondary verification. During the verification process, IMU measures acceleration With angular velocity , the displacement is obtained by integration With posture changes , that is, in the time interval Internal displacement , displacement posture change ,in is the initial displacement, is the initial velocity, is the initial attitude, through the angular velocity The predicted displacement and posture are used to update the spatial position of the uncertain region, and re-evaluate whether it is a dynamic region. The re-evaluation is performed by transforming the initial coordinates of the uncertain region according to the displacement and posture changes, that is, ,in are the initial coordinates of the uncertainty region, are the updated coordinates, According to the change of posture The obtained rotation matrix is ​​compared with the lidar dynamic point cloud cluster information again to determine whether it is a real dynamic interference area.

8. The positioning method of complementary positioning fusion of laser radar and stereo vision according to claim 1 is characterized in that: The specific steps of S500 are: Eliminate the point cloud and visual features in the real dynamic interference area determined in S400 to eliminate the interference of dynamic factors on positioning; Extract plane features and corner features from the remaining static point cloud; According to the mapping relationship established by S100 spatial calibration, the plane features of the static point cloud are matched with the corner features in the visual image to fuse the static information of different sensors; The fused static information is used to predict and update the positioning benchmark.

9. A positioning system for complementary positioning fusion of laser radar and stereo vision, applicable to the positioning method for complementary positioning fusion of laser radar and stereo vision as claimed in any one of claims 1 to 8, characterized in that: The components of the system include: data acquisition module, dynamic object recognition module, dynamic interference division module, dynamic interference determination module, positioning fusion module; The data acquisition module uses a laser radar and a stereo vision camera to respectively collect point cloud data and visual image data of the environment, align the data according to timestamps, and perform spatial calibration to construct a spatial mapping relationship; The dynamic object recognition module performs semantic segmentation on the image captured by the stereo vision camera, identifies the dynamic object area in the image and generates a pixel-level dynamic mask; The dynamic interference division module performs motion vectorization analysis on the laser radar point cloud data, and selects point cloud clusters whose displacement exceeds a dynamic threshold as dynamic interference areas; The dynamic interference determination module combines the semantic segmentation results and the laser radar point cloud data to determine the final real dynamic interference area; The positioning fusion module removes the dynamic interference point cloud and updates the positioning according to the remaining static background features.

Images