Heterogeneous sensor fusion positioning method and system based on line segments

Abstract

The invention belongs to the technical field of autonomous mobile robot multi-sensor fusion positioning measurement, and discloses a line segment-based heterogeneous sensor fusion positioning method and system. The method comprises the steps that a heterogeneous sensor group collects environment information, extracts two-dimensional feature line segments and point clouds respectively and completes initial pose estimation; filtering, coplanar segmentation and three-dimensional line segment extraction optimization are carried out on the point cloud; and constructing a loss function through 3D-2D and 2D-3D bidirectional reprojection, and outputting a positioning result through weighted summation and factor graph optimization. The system comprises a heterogeneous sensor module, a point cloud preprocessing and three-dimensional line segment segmentation module and a bidirectional reprojection fusion optimization module. By means of complementarity of multiple sensors and an efficient data processing strategy, real-time performance and positioning precision are both considered, and the method is suitable for positioning of the autonomous mobile robot in a complex environment.

Description

Technical Field

[0001] This invention relates to the field of industrial product quality inspection technology, specifically to a heterogeneous sensor fusion positioning method and system based on line segments. Background Technology

[0002] Autonomous mobile robots possess both autonomous mobility and operational capabilities, leading to a wide range of applications. However, their autonomous movement control relies heavily on their localization algorithms. Currently, robot localization algorithms include single-sensor algorithms and multi-sensor fusion localization algorithms. For the complex environments primarily faced by autonomous robots, single-sensor algorithms, limited by their single modality, lack robustness. Multi-sensor algorithms, utilizing multiple modalities, can ensure robustness across different scenarios. However, due to the inherent speed of autonomous mobile robots, multi-modal fusion localization algorithms suffer from issues such as large information requirements, algorithmic complexity, and insufficient real-time performance, thus limiting their application in autonomous mobile robot development and impacting their real-time autonomous movement control. Summary of the Invention

[0003] The purpose of this invention is to provide a heterogeneous sensor fusion positioning method and system based on line segments to solve the problems mentioned in the background art.

[0004] To achieve the above objectives, the present invention provides the following technical solution: a heterogeneous sensor fusion positioning method based on line segments, comprising the following steps:

[0005] S1. Heterogeneous Sensor Data Acquisition and Initial Processing: Environmental information is acquired using a heterogeneous sensor group consisting of a camera, LiDAR, and millimeter-wave radar. Each sensor performs initial pose estimation and feature / point cloud extraction.

[0006] S11. The camera extracts two-dimensional feature line segments from the image based on the VO algorithm. And estimate pose information with camera noise. ;

[0007] S12. The lidar estimates pose information with lidar noise based on the LO algorithm. And generate a 3D point cloud for lidar. ;

[0008] S13. The millimeter-wave radar uses the RO algorithm to perform multi-dimensional spectral analysis on the echo signal to estimate its own pose. And generate millimeter-wave radar point clouds. ;

[0009] S2. Rapid 3D line segmentation of point cloud: This involves segmenting the laser radar point cloud... and millimeter-wave radar point clouds Perform 3D line segment extraction and optimization, specifically including:

[0010] S21. Radius Filtering Pruning: Determine the radius based on the camera field of view and the number of point clouds. , definition point domain point set

[0011]

[0012] Eliminate those that meet the requirements The isolated points, of which The threshold for the minimum number of points in the domain;

[0013] S22. Point cloud segmentation into coplanar planes: Construct a kdtree to search for the k nearest neighbors of each point, calculate the covariance matrix and decompose the eigenvalues ​​using PCA to obtain the normal vector and curvature; Based on the region growing algorithm, use low curvature points as seeds to aggregate neighboring points that satisfy the normal vector angle is less than 15° and the orthogonal distance does not exceed the neighborhood scale of the seed point, then analyze the coplanarity of adjacent regions and merge scattered planes to obtain a set of stable planes;

[0014] S23. 3D Line Segment Extraction and Optimization: Based on the plane, a local coordinate system is constructed, and points in the plane are mapped to the local 2D coordinate system. Using 0.9 times the point scale distribution as an adaptive meshing parameter, the projected points are quantized into a binary image, and holes are eliminated through dilation-erosion operations. The contours of the binary image are extracted and fitted using least squares to obtain 2D line segments. These segments are then back-projected onto the original 3D plane to obtain the initial 3D line segments. ; Filter out abnormal line segments according to the structural consistency criterion, and then merge adjacent line segments through latitudinal binning clustering to obtain the optimized three-dimensional line segments;

[0015] S3. Bidirectional reprojection multi-sensor fusion optimization: based on the aforementioned two-dimensional feature line segments and optimized 3D line segments The loss function is constructed through bidirectional reprojection, and the final localization result is obtained by combining it with factor graph optimization. Specifically, this includes:

[0016] S31. 3D→2D Constraint Construction: Constructing 3D line segments Transformation matrix of camera coordinate system Transform to camera coordinate system, then via projection matrix Projected onto a two-dimensional image plane to obtain ;calculate and overlap distance Filter to meet the angle threshold and distance threshold Construct a loss function from pairs of line segments.

[0017] ,in and These are the conversion factors;

[0018] S32.2D→3D constraint construction: constructing two-dimensional feature lines Reverse extension to a plane Calculate three-dimensional line segments With plane angular distance and vertical distance Filter to meet The line segments with the smallest distance are selected as the matching line segments, and a loss function is constructed.

[0019] ,in and These are the conversion factors;

[0020] S33. Total Loss Optimization: Setting Weighting Factors , Initially, empirical values ​​are assigned and incorporated into the optimization system for real-time adjustment. The total loss function is obtained through weighted summation. The total loss function is optimized based on the factor graph, and the final localization result is output.

[0021] Furthermore, in step S11, the camera extracts two-dimensional feature line segments from the image using a pre-trained convolutional neural network. The training samples of the convolutional neural network include environmental images under different lighting and occlusion scenarios, as well as corresponding line segment annotation information.

[0022] Furthermore, in step S21, the minimum number of points in the area threshold... The value range is 3-10, and the radius... The value range is 0.05m-0.3m.

[0023] Furthermore, in step S23, the adaptive threshold is 0.3-0.5 times the average length of all initial three-dimensional line segments in the plane.

[0024] Furthermore, in step S31, the two-dimensional feature line segment The first The endpoints of a line segment are represented as Two-dimensional line segments are represented by their coefficients as follows:

[0025]

[0026] ,in for The line segment parameters, The coordinates of the start and end points;

[0027] The three-dimensional line segment Using Plück's notation to represent lines in three-dimensional space

[0028]

[0029] ,in It is the direction vector of the line segment. It is the normal vector of the plane defined by the line segment and the origin;

[0030] The projection of a 3D point or line segment onto the 2D camera plane, and the transformation of the 3D line segment into the camera coordinate system, are as follows:

[0031]

[0032] in, and They are Representation in camera and world coordinate system It is the transformation matrix between the camera and the world;

[0033] 3D line segment It can be projected onto a two-dimensional image plane as follows, where, It is a line segment projected onto the image plane. refer to The first three lines;

[0034]

[0035] The projection matrix The parameters satisfy: , These are the focal length parameters of the camera along the x and y axes of the image, respectively. , These are the x and y coordinates of the principal point of the camera image in the image coordinate system, respectively.

[0036] The and To perform pairing, first perform the parameterization shown in the following formula. This is the parametric expression for the two-dimensional line segment of the camera, where, For camera two-dimensional feature lines The starting coordinates are in the form of ( , ), For camera two-dimensional feature lines The endpoint coordinates are in the form of ( , ), belongs to the image plane coordinate system, It can represent the two endpoints of a two-dimensional projection of a three-dimensional line segment; This represents the point on a two-dimensional line segment that is the smallest distance from a three-dimensional line segment within the legal distance range;

[0037]

[0038] The Preliminary screening of line segment pairs, including the overlap distance between 3D and 2D line segments:

[0039]

[0040] ,in For when for The starting point At that time, the calculated Values, For when for The End At that time, the calculated Values.

[0041] 7. The heterogeneous sensor fusion positioning method based on line segments according to claim 1, characterized in that, in step S33, the weighting coefficients... The initial empirical value range is 0.3-0.7. The initial empirical value range is 0.3-0.7, and it satisfies... .

[0042] A heterogeneous sensor fusion positioning system based on line segments, comprising:

[0043] Heterogeneous sensor module, point cloud preprocessing and 3D line segmentation module, bidirectional reprojection fusion optimization module;

[0044] The heterogeneous sensor module, including a camera, lidar, and millimeter-wave radar, is used to acquire environmental information and perform initial processing; the camera outputs two-dimensional feature line segments. and pose information The lidar outputs a three-dimensional point cloud. and pose information The millimeter-wave radar output point cloud and pose information ;

[0045] The point cloud preprocessing and 3D line segmentation module is communicatively connected to the heterogeneous sensor module for receiving... and The process sequentially performs radius filtering pruning, point cloud coplanar segmentation, and 3D line segment extraction and optimization, outputting the optimized 3D line segments. ;

[0046] The bidirectional reprojection fusion optimization module is communicatively connected to the heterogeneous sensor module and the point cloud preprocessing and 3D line segmentation module, respectively, for receiving... , and Constructed through bidirectional reprojection and The total loss function is then obtained by weighted summation, and the total loss function is optimized based on the factor graph to output the final localization result.

[0047] Furthermore, in the heterogeneous sensor module, the camera, lidar, and millimeter-wave radar are installed at the same height, and the overlap ratio of their field of view is not less than 30%, ensuring the complementarity of environmental information acquisition.

[0048] Furthermore, the point cloud preprocessing and 3D line segmentation module is implemented using an FPGA chip or an embedded processor. The embedded processor has a main frequency of not less than 1GHz and supports single instruction multiple data stream operations to improve the point cloud processing speed.

[0049] Furthermore, the factor graph optimization in the bidirectional reprojection fusion optimization module employs the Gauss-Newton iterative algorithm, with the iteration termination condition being that the difference in the total loss function between two adjacent iterations is less than 10⁻. 4 Or the number of iterations may reach 5-20 times.

[0050] Compared with the prior art, the beneficial effects of the present invention are:

[0051] The multi-sensor approach leverages the complementary advantages of cameras, LiDAR, and millimeter-wave radar to enhance localization robustness in complex scenarios and resist interference from lighting and occlusion. A rapid point cloud 3D line segmentation process simplifies data redundancy, and dedicated hardware acceleration ensures real-time performance. The combined use of bidirectional reprojection constraints and factor graph optimization reduces sensor noise and significantly improves localization accuracy. The algorithm is highly adaptable, capable of handling various environmental changes, and provides stable and reliable localization support for autonomous mobile robots. Attached Figure Description

[0052] Figure 1 This is a schematic diagram of the overall process of the method of the present invention;

[0053] Figure 2 This is an illustration of the point cloud algorithm of the present invention;

[0054] Figure 3 This is a diagram illustrating the fusion module algorithm of the present invention. Detailed Implementation

[0055] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0056] Please see Figure 1 —3. This invention provides a technical solution: a heterogeneous sensor fusion positioning method based on line segments, comprising the following steps:

[0057] S1. Heterogeneous Sensor Data Acquisition and Initial Processing: Environmental information is acquired using a heterogeneous sensor group consisting of a camera, LiDAR, and millimeter-wave radar. Each sensor performs initial pose estimation and feature / point cloud extraction.

[0058] S11. The camera extracts two-dimensional feature lines from the image based on the VO algorithm. And estimate pose information with camera noise. The camera extracts two-dimensional feature lines from the image using a pre-trained convolutional neural network. The training samples for the convolutional neural network include environmental images and corresponding line segment annotations under different lighting and occlusion scenarios.

[0059] S12. LiDAR uses the LO algorithm to estimate pose information with lidar noise. And generate a 3D point cloud for lidar. ;

[0060] S13. The millimeter-wave radar uses the RO algorithm to perform multi-dimensional spectral analysis on the echo signal to estimate its own pose. And generate millimeter-wave radar point clouds. ;

[0061] First, the initial poses of each sensor are obtained through VO, LO, and RO algorithms to reduce the impact of initial deviations on subsequent fusion. At the same time, core features are extracted in a targeted manner, and redundant information is filtered to reduce the burden on subsequent line segment matching and fusion. A pre-trained convolutional neural network is used to extract two-dimensional feature lines. Its training samples cover different lighting and occlusion scenarios to improve the robustness of line segment extraction, avoid feature loss or mis-extraction due to environmental changes, and provide a stable two-dimensional benchmark for cross-sensor matching.

[0062] S2. Rapid 3D line segmentation of point clouds: For LiDAR point clouds... and millimeter-wave radar point clouds Perform 3D line segment extraction and optimization, specifically including:

[0063] S21. Radius Filtering Pruning: Determine the radius based on the camera field of view and the number of point clouds. , definition point domain point set

[0064]

[0065] Eliminate those that meet the requirements The isolated points, of which The minimum number of points in the neighborhood is set as the threshold. The core purpose of radius filtering pruning is to remove isolated noise points from the point cloud. These points are usually caused by sensor noise and environmental interference. If they are not removed, false planes will appear in subsequent plane segmentation, affecting the accuracy of 3D line segment extraction. The radius r is determined by the camera field of view and the number of points in the cloud to adapt to the acquisition range and point cloud density of different sensors and ensure the universality of the filtering effect. In step S21, the minimum number of points in the neighborhood is set as the threshold. The value range is 3-10, and the radius is... The value range is 0.05m-0.3m.

[0066] S22. Point cloud segmentation into coplanar planes: Construct a kdtree to search for the k nearest neighbors of each point, calculate the covariance matrix and decompose the eigenvalues ​​using PCA to obtain the normal vector and curvature; the normal vector can reflect the spatial pose of the plane where the point is located, and the curvature can distinguish between the plane and the edge region. Based on the region growing algorithm, using low curvature points as seeds, aggregate neighboring points that satisfy the normal vector angle is less than 15° and the orthogonal distance does not exceed the neighborhood scale of the seed point, and then analyze the coplanarity of the adjacent regions and merge the scattered planes to obtain a stable set of planes;

[0067] S23. 3D Line Segment Extraction and Optimization: A local coordinate system is constructed based on the plane, mapping points in the plane to the local 2D coordinate system. An adaptive meshing parameter of 0.9 times the point scale distribution is used to adapt to point clouds of different densities, avoiding feature loss due to excessively large meshes or redundant computation due to excessively small meshes. The core function of the dilation-erosion operation is to eliminate holes formed by point cloud projection. Binary image contours are extracted and least-squares fitted to obtain 2D line segments, which are then back-projected onto the original 3D plane to obtain the initial 3D line segments. ; Filter out abnormal line segments according to the structural consistency criterion, and then merge adjacent line segments through latitudinal binning clustering to obtain optimized three-dimensional line segments; In step S23, the adaptive threshold is 0.3-0.5 times the average length of all initial three-dimensional line segments in the plane;

[0068] S3. Bidirectional reprojection multi-sensor fusion optimization: based on two-dimensional feature line segments and optimized 3D line segments The loss function is constructed through bidirectional reprojection, and the final localization result is obtained by combining it with factor graph optimization. Specifically, this includes:

[0069] S31. 3D→2D Constraint Construction: Constructing 3D line segments Transformation matrix of camera coordinate system Transform to camera coordinate system, then via projection matrix Projected onto a two-dimensional image plane to obtain ;calculate and overlap distance ,

[0070] Two-dimensional feature line segments The first The endpoints of a line segment are represented as Two-dimensional line segments are represented by their coefficients as follows:

[0071]

[0072] ,in for The line segment parameters, The coordinates of the start and end points;

[0073] 3D line segment Using Plück's notation to represent lines in three-dimensional space

[0074]

[0075] ,in It is the direction vector of the line segment. It is the normal vector of the plane defined by the line segment and the origin;

[0076] We construct the loss function using the mutual projection of 3D and 2D line segments to optimize the efficient coupling between them; the projection of 3D point line segments onto the 2D camera plane, and the transformation of 3D line segments into the camera coordinate system, are as follows:

[0077]

[0078] in, and They are Representation in camera and world coordinate system It is the transformation matrix between the camera and the world;

[0079] 3D line segment It can be projected onto a two-dimensional image plane as follows, where, It is a line segment projected onto the image plane. refer to The first three lines;

[0080]

[0081] Projection matrix The parameters satisfy: , These are the focal length parameters of the camera along the x and y axes of the image, respectively. , These are the x and y coordinates of the principal point of the camera image in the image coordinate system, respectively.

[0082] and To perform pairing, first perform the parameterization shown in the following formula. This is the parametric expression for the two-dimensional line segment of the camera, where, For camera two-dimensional feature lines The starting coordinates are in the form of ( , ), For camera two-dimensional feature lines The endpoint coordinates are in the form of ( , ), belongs to the image plane coordinate system, It can represent the two endpoints of a two-dimensional projection of a three-dimensional line segment; This represents the point on a two-dimensional line segment that is the smallest distance from a three-dimensional line segment within the legal distance range;

[0083]

[0084] Preliminary screening of line segment pairs, including the overlap distance between 3D and 2D line segments:

[0085]

[0086] ,in For when for The starting point At that time, the calculated Values, For when for The End At that time, the calculated Values.

[0087] Filtering based on angle threshold and distance threshold Construct a loss function from pairs of line segments.

[0088] ,in and These are the conversion factors;

[0089] S32.2D→3D constraint construction: constructing two-dimensional feature lines Reverse extension to a plane Calculate three-dimensional line segments With plane angular distance and vertical distance Filter to meet Find the line segments and select the one with the smallest distance as the matching line segment, construct... loss function

[0090]

[0091] ,in and These are the conversion factors;

[0092] S33. Total Loss Optimization: Setting Weighting Factors , Initially, empirical values ​​are assigned and incorporated into the optimization system for real-time adjustment. Considering the varying confidence levels of LiDAR and camera in different scenarios, a weighted average of the two is used to obtain the final total error function, as shown below. The weighting coefficients are initially assigned empirical values ​​and subsequently optimized in real-time within the optimization system. The total loss function is obtained through weighted summation. The total loss function is optimized based on the factor graph, and the final localization result is output; weighting coefficients The initial empirical value range is 0.3-0.7. The initial empirical value range is 0.3-0.7, and it satisfies... The conversion coefficient is introduced to adapt to the error characteristics of different sensors and to reasonably weight the angle and distance errors. The dynamic optimization design of the weighting coefficients α and β is to cope with the changes in the confidence of the sensors under different scenarios and to ensure that the fusion result always depends on the information of high-confidence sensors.

[0093] A heterogeneous sensor fusion positioning system based on line segments, comprising:

[0094] Heterogeneous sensor module, point cloud preprocessing and 3D line segmentation module, bidirectional reprojection fusion optimization module;

[0095] A heterogeneous sensor module, including a camera, LiDAR, and millimeter-wave radar, is used to acquire environmental information and perform initial processing: the camera outputs two-dimensional feature line segments. and pose information LiDAR outputs 3D point cloud and pose information Millimeter-wave radar output point cloud and pose information The consistent installation height ensures that the environmental information collected by each sensor is aligned in the vertical direction, avoiding spatial displacement of line segment features due to height differences; the overlap ratio of the field of view is not less than 30%, which ensures that there is a sufficient common observation area, providing a sufficient feature basis for cross-sensor line segment matching and reducing fusion failure caused by field of view misalignment.

[0096] Other sensors with line segment extraction or point cloud output capabilities, such as infrared cameras, ultrasonic radars, and binocular cameras, can be incorporated into this system through the adaptation feature extraction interface, demonstrating the strong scalability of the algorithm.

[0097] The point cloud preprocessing and 3D line segmentation module communicates with the heterogeneous sensor module to receive data. and The process sequentially performs radius filtering pruning, point cloud coplanar segmentation, and 3D line segment extraction and optimization, outputting the optimized 3D line segments. ;

[0098] The point cloud preprocessing and 3D line segmentation modules use FPGA chips or embedded processors with a main frequency of no less than 1GHz because the point cloud data of LiDAR and millimeter-wave radar is large. FPGA has parallel computing capabilities, and embedded processors support single instruction multiple data stream operations, both of which can realize hardware acceleration of point cloud processing, ensuring that the processing delay from point cloud input to 3D line segment output is controlled within the real-time requirements.

[0099] The bidirectional reprojection fusion optimization module is communicatively connected to the heterogeneous sensor module and the point cloud preprocessing and 3D line segmentation module, respectively, for receiving... , and Constructed through bidirectional reprojection and The total loss function is obtained by weighted summation, and then optimized based on the factor graph to output the final localization result. The factor graph optimization in the bidirectional reprojection fusion optimization module adopts the Gauss-Newton iterative algorithm, and the iteration termination condition is that the difference in the loss function is less than 10⁻. 4 At this point, the positioning accuracy already meets the needs of most scenarios, and further iterations will yield limited gains; the number of iterations is limited to 5-20 times to avoid the algorithm getting stuck in infinite iterations in extreme cases and to ensure stable system operation.

[0100] In the heterogeneous sensor module, the camera, lidar and millimeter-wave radar are installed at the same height, and the overlap ratio of their field of view is not less than 30%, ensuring the complementarity of environmental information acquisition.

[0101] The point cloud preprocessing and 3D line segmentation modules are implemented using FPGA chips or embedded processors. The embedded processors have a main frequency of no less than 1GHz and support single instruction multiple data stream operations to improve the point cloud processing speed.

Claims

1. A heterogeneous sensor fusion positioning method based on line segments, characterized in that, Includes the following steps: S1. Heterogeneous Sensor Data Acquisition and Initial Processing: Environmental information is acquired using a heterogeneous sensor group consisting of a camera, LiDAR, and millimeter-wave radar. Each sensor performs initial pose estimation and feature / point cloud extraction. S11. The camera extracts two-dimensional feature line segments from the image based on the VO algorithm. And estimate pose information with camera noise. ; S12. The lidar estimates pose information with lidar noise based on the LO algorithm. And generate a 3D point cloud for lidar. ; S13. The millimeter-wave radar uses the RO algorithm to perform multi-dimensional spectral analysis on the echo signal to estimate its own pose. And generate millimeter-wave radar point clouds. ; S2. Rapid 3D line segmentation of point cloud: This involves segmenting the laser radar point cloud... and millimeter-wave radar point clouds Perform 3D line segment extraction and optimization, specifically including: S21. Radius Filtering Pruning: Determine the radius based on the camera field of view and the number of point clouds. , definition point domain point set ， Eliminate satisfied The isolated points, of which The threshold for the minimum number of points in the domain; S22. Point cloud segmentation into coplanar planes: Construct a kdtree to search for the k nearest neighbors of each point, calculate the covariance matrix and decompose the eigenvalues ​​using PCA to obtain the normal vector and curvature; Based on the region growing algorithm, use low curvature points as seeds to aggregate neighboring points that satisfy the normal vector angle is less than 15° and the orthogonal distance does not exceed the neighborhood scale of the seed point, then analyze the coplanarity of adjacent regions and merge scattered planes to obtain a set of stable planes; S23. 3D Line Segment Extraction and Optimization: Based on the plane, a local coordinate system is constructed, and points in the plane are mapped to the local 2D coordinate system. Using 0.9 times the point scale distribution as an adaptive meshing parameter, the projected points are quantized into a binary image, and holes are eliminated through dilation-erosion operations. The contours of the binary image are extracted and fitted using least squares to obtain 2D line segments. These segments are then back-projected onto the original 3D plane to obtain the initial 3D line segments. ; Filter out abnormal line segments according to the structural consistency criterion, and then merge adjacent line segments through latitudinal binning clustering to obtain the optimized three-dimensional line segments; S3. Bidirectional reprojection multi-sensor fusion optimization: based on the aforementioned two-dimensional feature line segments and optimized 3D line segments The loss function is constructed through bidirectional reprojection, and the final localization result is obtained by combining it with factor graph optimization. Specifically, this includes: S31. 3D→2D Constraint Construction: Constructing 3D line segments Transformation matrix of camera coordinate system Transform to camera coordinate system, then via projection matrix Projected onto a two-dimensional image plane to obtain ;calculate and overlap distance Filter to meet the angle threshold and distance threshold Construct a loss function from pairs of line segments. , in and These are the conversion factors; S32.2D→3D constraint construction: constructing two-dimensional feature lines Reverse extension to a plane Calculate three-dimensional line segments With plane angular distance and vertical distance Filter to meet The line segments with the smallest distance are selected as the matching line segments, and a loss function is constructed. , in and These are the conversion factors; S33. Total Loss Optimization: Setting Weighting Factors , Initially, empirical values ​​are assigned and incorporated into the optimization system for real-time adjustment. The total loss function is obtained through weighted summation. The total loss function is optimized based on the factor graph, and the final localization result is output.

2. A heterogeneous sensor fusion positioning system based on line segments, which is advantageous for the method described in claim 1, characterized in that, include: Heterogeneous sensor module, point cloud preprocessing and 3D line segmentation module, bidirectional reprojection fusion optimization module; The heterogeneous sensor module, including a camera, lidar, and millimeter-wave radar, is used to acquire environmental information and perform initial processing; the camera outputs two-dimensional feature line segments. and pose information The lidar outputs a three-dimensional point cloud. and pose information The millimeter-wave radar output point cloud and pose information ; The point cloud preprocessing and 3D line segmentation module is communicatively connected to the heterogeneous sensor module for receiving... and The process sequentially performs radius filtering pruning, point cloud coplanar segmentation, and 3D line segment extraction and optimization, outputting the optimized 3D line segments. ; The bidirectional reprojection fusion optimization module is communicatively connected to the heterogeneous sensor module and the point cloud preprocessing and 3D line segmentation module, respectively, for receiving... , and Constructed through bidirectional reprojection and The total loss function is then obtained by weighted summation, and the total loss function is optimized based on the factor graph to output the final localization result.

3. The heterogeneous sensor fusion positioning method based on line segments according to claim 1, characterized in that, In step S11, the camera extracts two-dimensional feature line segments from the image using a pre-trained convolutional neural network. The training samples of the convolutional neural network include environmental images under different lighting and occlusion scenarios, as well as corresponding line segment annotation information.

4. The heterogeneous sensor fusion positioning method based on line segments according to claim 1, characterized in that, In step S21, the minimum number of points in the region threshold The value range is 3-10, and the radius... The value range is 0.05m-0.3m.

5. The heterogeneous sensor fusion positioning method based on line segments according to claim 1, characterized in that, In step S23, the adaptive threshold is 0.3-0.5 times the average length of all initial three-dimensional line segments in the plane.

6. The heterogeneous sensor fusion positioning method based on line segments according to claim 1, characterized in that, In step S31, the two-dimensional feature line segment The first The endpoints of a line segment are represented as Two-dimensional line segments are represented by their coefficients as follows: ,in for The line segment parameters, The coordinates of the start and end points; The three-dimensional line segment Using Plück's notation to represent lines in three-dimensional space ， in It is the direction vector of the line segment. It is the normal vector of the plane defined by the line segment and the origin; The projection of a 3D point or line segment onto the 2D camera plane, and the transformation of the 3D line segment into the camera coordinate system, are as follows: in, and They are Representation in camera and world coordinate system It is the transformation matrix between the camera and the world; 3D line segment It can be projected onto a two-dimensional image plane as follows, where, It is a line segment projected onto the image plane. refer to The first three lines; The projection matrix The parameters satisfy: , These are the focal length parameters of the camera along the x and y axes of the image, respectively. , These are the x and y coordinates of the principal point of the camera image in the image coordinate system, respectively. The and To perform pairing, first perform the parameterization shown in the following formula. This is the parametric expression for the two-dimensional line segment of the camera, where, For camera two-dimensional feature lines The starting coordinates are in the form of ( , ), For camera two-dimensional feature lines The endpoint coordinates are in the form of ( , ), belongs to the image plane coordinate system, It can represent the two endpoints of a two-dimensional projection of a three-dimensional line segment; This represents the point on a two-dimensional line segment that is the smallest distance from a three-dimensional line segment within the legal distance range; The Preliminary screening of line segment pairs, including the overlap distance between 3D and 2D line segments: ， in For when for The starting point At that time, the calculated Values, For when for The End At that time, the calculated Values.

7. The heterogeneous sensor fusion positioning method based on line segments according to claim 1, characterized in that, In step S33, the weighting coefficients The initial empirical value range is 0.3-0.

7. The initial empirical value range is 0.3-0.7, and it satisfies... .

8. The heterogeneous sensor fusion positioning system based on line segments according to claim 2, characterized in that, In the heterogeneous sensor module, the camera, lidar, and millimeter-wave radar are installed at the same height, and the overlap ratio of their field of view is not less than 30%, ensuring the complementarity of environmental information acquisition.

9. The heterogeneous sensor fusion positioning system based on line segments according to claim 2, characterized in that, The point cloud preprocessing and 3D line segmentation module is implemented using an FPGA chip or an embedded processor. The embedded processor has a main frequency of not less than 1GHz and supports single instruction multiple data stream operation to improve the point cloud processing speed.

10. The heterogeneous sensor fusion positioning system based on line segments according to claim 2, characterized in that, The factor graph optimization in the bidirectional reprojection fusion optimization module employs a Gauss-Newton iterative algorithm, with the iteration termination condition being that the difference in the total loss function between two adjacent iterations is less than 10⁻⁻⁴. 4 Or the number of iterations may reach 5-20 times.

Images