Multi-source fusion positioning and pose determination method and system based on lane line structure assistance

Abstract

The application provides a multi-source fusion positioning and pose determination method and system based on lane line structure assistance, comprising: fusing satellite pseudo-range observation values, phase observation values, acceleration values, gyroscope observation information and visual images for positioning processing; obtaining positions, velocities and attitudes at an initial time, performing initial alignment and mechanical arrangement through inertial navigation to obtain positions, velocities and attitudes at subsequent times; extracting lane lines in the visual images by using a preset lane line extraction algorithm; extracting and matching line features in the visual images by using a real-time straight line segment detection algorithm and a line band descriptor algorithm, and extracting and matching corner point features in the images by using a corner point detection algorithm and an optical flow method; obtaining a current world main direction based on the lane lines and the gravity direction, and initializing structure line features of the current world; and using the main direction to constrain the direction of the line features, continuously optimizing the positions, velocities and attitudes at the subsequent times through the corner point features and the constrained line features.

Description

Technical Field

[0001] This invention relates to the field of satellite navigation technology, and in particular to a multi-source fusion positioning and attitude determination method and system based on lane line structure assistance. Background Technology

[0002] In complex urban environments, achieving continuous, smooth, and high-precision vehicle positioning and attitude determination is a core foundation for the reliable operation of autonomous driving systems in intelligent connected vehicles. The BeiDou Navigation Satellite System (BDS), a crucial domestically developed and independently operated space infrastructure, presents unprecedented opportunities and challenges for navigation and location services. Leveraging its complementary characteristics with Inertial Navigation Satellite System (INS), the BDS / INS combined system can provide continuous, high-precision, and highly reliable positioning and attitude determination results in environments such as highways and urban expressways. However, in complex environments where satellite signals are obstructed, such as urban canyons, tunnels, or under overpasses, BDS positioning performance degrades to varying degrees, and INS rapidly diverges due to error accumulation, leading to a degraded or even failed positioning and attitude determination performance of the combined navigation system. Furthermore, such urban scenarios with obstructed BeiDou signals often possess rich visual features; therefore, fusing observational information from cameras, BDS, and INS sensors can overcome their individual limitations and achieve superior positioning and attitude determination performance.

[0003] In visual positioning, commonly used visual features include corner points, lines, surfaces, and semantic features. Among these, corner points are stable in outdoor scenes where satellite signals are obstructed, and their extraction and tracking methods are mature, making them the most widely used visual features in urban outdoor environments. In textured scenes, corner point features alone can achieve high-precision pose estimation. However, in weak-texture environments, the lack of sufficient grayscale variations and texture details leads to unstable extraction and tracking of corner point features, affecting the accuracy of pose estimation. In contrast, line features can be stably tracked in weak-texture environments, compensating for the shortcomings of corner point features, enhancing the robustness of visual observation, and thus improving the accuracy of visual positioning. In addition, heterogeneous visual features such as surface features and semantic features are also used to enrich visual observation information and improve the robustness of visual positioning in different observation environments. However, when BeiDou signals are unavailable for extended periods, although man-made buildings in urban scenes can provide rich heterogeneous visual observation information for visual positioning, the pose of the fusion system will still diverge linearly with distance due to the lack of global constraints. Therefore, introducing additional global constraints into a multi-sensor fusion system to suppress the accumulation and divergence of pose estimation errors is key to improving the positioning and pose determination accuracy of the fusion system when BeiDou fails for extended periods in urban outdoor scenarios.

[0004] Man-made structures in urban scenes exhibit strong structural regularity, and in most cases can be abstracted as blocks stacked together along a dominant direction. This characteristic provides a foundation for constructing the Structured World hypothesis and can provide global directional constraints for visual positioning systems, thereby effectively improving the accuracy of pose estimation. The Manhattan World hypothesis models a scene as three mutually orthogonal dominant directions. Within this framework, visual features parallel to the dominant directions are called structural features. Compared with traditional geometric visual features, structural features contain explicit global directional information, providing strong constraints during optimization and suppressing directional divergence. However, the Manhattan hypothesis only includes three fixed dominant directions, making it more suitable for indoor environments with regular structural heights and smaller scales, and cannot effectively describe large-scale outdoor structural features. To improve the adaptability of the world hypothesis to complex outdoor environments, researchers proposed the Atlanta World hypothesis based on the Manhattan World hypothesis. This hypothesis is composed of multiple dynamically detected and updated local Manhattan worlds, which can more flexibly represent diverse dominant directions in different regions, thereby more finely describing the structural properties in outdoor scenes and effectively improving the applicability of structure-based assisted positioning. However, structure-information-assisted localization methods still have significant limitations: 1) Strict structural constraints are insufficiently adaptable to highly complex and irregular urban environments; 2) The Atlanta world method heavily relies on the initial detection and multi-coordinate system merging strategy of the Manhattan world, which is prone to local optima or estimation divergence due to detection errors or improper threshold settings; 3) Existing methods do not fully utilize semantic elements rich in structural information, such as lane lines, and fail to effectively leverage semantic information to improve the accuracy and stability of structural constraints. Therefore, how to fully utilize semantic features highly correlated with structural information in a scene to accurately and robustly describe the structural characteristics of real-world scenes remains a key problem that has not yet been fully solved.

[0005] In urban outdoor vehicle-mounted positioning environments, images captured by vehicle cameras are typically rich in semantic information about the ground and roads. Among this, the structured information contained in lane lines plays a crucial role in constraining the construction of the world model. On one hand, the extension direction of lane lines (such as straight lines and curves) directly reflects the geometric structure of the road, making it one of the most prominent and stable structural features in the image. On the other hand, urban buildings are usually distributed in a grid pattern along the road network, forming a regular street layout. The direction of lane lines often coincides with the expansion direction of the main urban roads, effectively mapping the macroscopic urban spatial structure. Therefore, the structural attributes reflected by lane lines in urban outdoor vehicle-mounted scenarios are consistent with the basis for constructing the Manhattan and Atlanta world hypotheses, providing key constraints for the construction of these hypotheses. Summary of the Invention

[0006] This invention provides a multi-source fusion localization and orientation method and system based on lane line structure assistance to address the shortcomings of existing technologies. Based on the mapping relationship between road semantic information and environmental structural characteristics, lane lines are used to quickly determine the current world structural characteristics, improving the flexibility of the structural world hypothesis in various urban scenarios. Furthermore, based on the determination of environmental structural characteristics, the principal direction is estimated based on lane lines, solving the problem of unstable principal direction estimation in the structural Atlanta world. In addition, in structural scenarios, structural lines are added as a new visual observation to the state to fully utilize visual information and improve the accuracy of fusion localization.

[0007] In a first aspect, the present invention provides a multi-source fusion localization and pose determination method based on lane line structure assistance, comprising:

[0008] The satellite pseudorange and phase observations are collected by a satellite receiver, acceleration values ​​and gyroscope observations are collected by an inertial navigation system, and visual images are collected by an RGB camera. The satellite pseudorange, phase, acceleration, gyroscope observations and visual images are then fused for positioning.

[0009] The initial position, velocity, and attitude are obtained, and initial alignment and mechanical choreography are performed using inertial navigation to obtain the position, velocity, and attitude at subsequent moments.

[0010] Lane lines in the visual image are extracted using a preset lane line extraction algorithm. Line features in the visual image are extracted and matched using a real-time straight line segment detection algorithm and a line strip descriptor algorithm. Corner features in the visual image are extracted and matched using a corner detection algorithm and an optical flow method. If the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, the main direction of the current world is determined based on gravity and lane lines, and the structural line features of the current world are initialized.

[0011] By utilizing the main direction to constrain the line feature direction, the position, velocity, and attitude at subsequent time steps are continuously optimized through corner point features and constrained line feature directions.

[0012] According to the present invention, a multi-source fusion localization and pose determination method based on lane line structure assistance is provided. This method employs a preset lane line extraction algorithm to extract lane lines from a visual image, a real-time straight line segment detection algorithm and a line strip descriptor algorithm to extract and match line features in the visual image, and a corner detection algorithm and optical flow method to extract and match corner features in the visual image. If the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, then the current world's principal direction is determined based on gravity and lane lines. The method initializes the structural line features of the current world, including:

[0013] Lane lines are detected using the Ultra Fast Lane Detection v2 method. Line features are extracted and matched using EDLine and LBD, while corner features are extracted and matched using FAST and optical flow methods.

[0014] By using perspective transformation, the visual image is converted into a bird's-eye view, and the curvature and radius of curvature of the lane lines are calculated.

[0015] The current environment is determined to conform to the Atlanta World hypothesis based on the lane line curvature. If the lane line curvature is less than the threshold, the current environment is determined to conform to the orthogonal constraint of the Manhattan hypothesis, and the lane line straight line parameters are fitted using least squares. Otherwise, the current environment is determined to not conform to the Manhattan hypothesis, the structure line is not initialized, and ordinary line features are used for updating.

[0016] According to the present invention, a multi-source fusion localization and pose determination method based on lane line structure assistance is provided, which calculates the lane line curvature and radius of curvature, including:

[0017]

[0018] In the formula, For lane curvature, The tangent direction angle at a point on the lane line. Let the arc length be , The radius of curvature of the lane line.

[0019] According to the present invention, a multi-source fusion localization and pose determination method based on lane line structure assistance is provided. The method determines whether the current environment conforms to the Atlanta world hypothesis based on the lane line curvature. If the lane line curvature is determined to be less than a threshold, the method determines that the current environment conforms to the orthogonal constraints of the Manhattan hypothesis. The method then uses least squares fitting of the lane line straight-line parameters, including:

[0020] The principal direction perpendicular to the Atlanta World hypothesis is determined based on the direction of gravity. :

[0021]

[0022] Vertical direction Main direction Corresponding vanishing point for:

[0023]

[0024] in, Let be the rotation matrix from the n-system to the e-system. Let be the rotation matrix from the e-frame to the c-frame. Let n be the camera intrinsic parameter matrix, n be the navigation coordinate system, e be the geocentric coordinate system, and c be the camera coordinate system.

[0025] Detect the first and second lane lines on both sides of the lane in the image within the sliding window:

[0026]

[0027]

[0028] In the formula, , and The first linear coefficient, , and The second linear coefficient;

[0029] The lane line vanishing point is determined based on the intersection of the first lane line and the second lane line. :

[0030]

[0031] Lane corresponding direction for:

[0032]

[0033] in, Let be the rotation matrix from the c-system to the n-system;

[0034] To ensure y The principal direction of the axis is orthogonal to the principal direction of gravity, thus affecting the direction of the lane lines. Projecting the lane lines onto the current ground plane yields the main direction of the projected lane lines. Based on detection within the sliding window m The lane lines on the frame image are used to calculate the current situation in Atlanta. y The main direction of the axis and n North direction The included angle for:

[0035]

[0036] Get Atlanta World y The principal direction of the axis and vanishing point for:

[0037]

[0038]

[0039] x Major direction along the axis It is obtained by cross product of the other two main directions:

[0040]

[0041] x Major direction along the axis Corresponding vanishing point for:

[0042] .

[0043] According to the present invention, a multi-source fusion localization and attitude determination method based on lane line structure assistance is provided. This method utilizes the principal direction to constrain the line feature direction, and continuously optimizes the position, velocity, and attitude at subsequent time points using corner features and the constrained line feature direction. The method includes:

[0044] The structure lines are initialized using a sliding window to obtain the initialized structure lines.

[0045] Triangulation of the initialized structure lines is performed based on a sliding window.

[0046] Visual updates are performed using the triangulated structural lines. The visual features to be updated include corner points, lines, and structural line features.

[0047] Perform satellite position updates, complete measurement updates, and obtain system errors;

[0048] The system state is continuously corrected and optimized based on system errors.

[0049] According to the present invention, a multi-source fusion localization and pose determination method based on lane line structure assistance is provided, which initializes the structure line based on a sliding window to obtain the initialized structure line, including:

[0050] Connector features midpoint With vanishing point When the line features and rays Line features meet the following distance and angle thresholds. For the structural lines corresponding to the main direction:

[0051]

[0052] In the formula, Line features Endpoint to Ray distance, Line features Length, Line features and rays The included angle between them;

[0053] The sliding window-based optimization and judgment method determines the final main direction of the line segment by comprehensively judging the main direction it belongs to over multiple frames, thus obtaining the initialized structure line.

[0054] According to the present invention, a multi-source fusion localization and pose determination method based on lane line structure assistance is provided, which triangulates the initialized structure lines based on a sliding window, including:

[0055] The direction vector of the structure line can be obtained from its principal direction. ;

[0056] Based on the common viewing relationship of the inner line features of the sliding window, the normal vector of the lower plane in the e-system is obtained. and the distance from the line to the origin .

[0057] According to the present invention, a multi-source fusion localization and pose determination method based on lane line structure assistance is provided. Visual updates are performed using triangulated structure lines. The visual features to be updated include corner points, lines, and structure line features, including:

[0058] The system state includes IMU position, attitude velocity, and zero bias of the accelerometer and gyroscope. When a new frame of image is recorded by the sliding window, the current IMU state is added to the estimated state vector.

[0059]

[0060] In the formula, This represents the estimated state vector. and This indicates the IMU's position error, velocity error, and misalignment angle. and This indicates the zero bias error of the accelerometer and gyroscope. Indicates the size of the sliding window. Indicates the IMU status within the sliding window:

[0061]

[0062] The observation information of visual features includes reprojection error and point feature state. With line feature state They are represented as follows:

[0063]

[0064]

[0065] In the formula, This represents the coordinates of the feature point in the e-frame. Represents a distance scalar. Rotation matrix The angle of misalignment, This represents the rotation matrix of the line feature.

[0066] According to the multi-source fusion positioning and attitude determination method based on lane line structure assistance provided by the present invention, satellite position updates are performed, measurement updates are completed, and system errors are obtained, including:

[0067] Given that the spatial relationship between the satellite receiver and the inertial navigation system is known, the position of the satellite receiver is corrected using a lever arm to obtain the position of the inertial navigation system's center.

[0068]

[0069] in The lever arm between the satellite receiver and the inertial navigation system. The phase center of the satellite receiver antenna;

[0070] Jacobian matrix corresponding to the pine combination observation model for:

[0071]

[0072] in, , These represent the zero bias of the accelerometer and gyroscope, respectively. It is the identity matrix. Let be the rotation matrix from system b to system e. The lever arm that serves as the phase center for inertial navigation and BeiDou. Indicates an antisymmetric matrix;

[0073] Turbo Edit and M-estimation were used to remove cycle slips and gross errors from satellites.

[0074] Secondly, the present invention also provides a multi-source fusion positioning and attitude determination system based on lane line structure assistance, comprising:

[0075] The data acquisition and input module is used to acquire satellite pseudorange and phase observations through a satellite receiver, acquire acceleration values ​​and gyroscope observation information through an inertial navigation system, and acquire visual images through an RGB camera. It performs fusion positioning processing on the satellite pseudorange observations, phase observations, acceleration values, gyroscope observation information, and visual images.

[0076] The inertial navigation initialization and mechanical orchestration module is used to obtain the position, velocity and attitude at the initial moment. It performs initial alignment and mechanical orchestration through inertial navigation to obtain the position, velocity and attitude at subsequent moments.

[0077] The visual front-end module is used to extract lane lines from the visual image using a preset lane line extraction algorithm, extract and match line features in the visual image using a real-time straight line segment detection algorithm and a line strip descriptor algorithm, extract and match corner features in the visual image using a corner detection algorithm and an optical flow method, and if the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, then the main direction of the current world is determined based on gravity and lane lines, and the structural line features of the current world are initialized.

[0078] The backend optimization module is used to continuously optimize the position, velocity, and attitude at subsequent time steps by using the main direction to constrain the line feature direction, and by using the corner features and the constrained line feature direction.

[0079] The beneficial effects of this invention are as follows:

[0080] (1) In view of the problem that the constraint accuracy and stability of BDS / INS / visual fusion positioning decrease when BeiDou fails for a long time, the present invention provides a BDS / INS / visual fusion positioning system suitable for urban outdoor structural scenarios. The system constructs the Atlanta World hypothesis model based on road semantic elements lane lines, provides global directional constraints, and uses structural lines to improve the positioning and attitude determination accuracy of the BDS / INS / visual fusion system.

[0081] (2) To address the problem that the Atlanta world hypothesis leads to a decrease in localization performance when used in unstructured scenarios, this invention provides a scene structure judgment method assisted by road semantic elements. This method is based on the mapping relationship between lane lines and scene structure. According to the lane line curvature detected within the sliding window, it quickly determines whether the current environment is suitable for the Atlanta world hypothesis, thereby improving the flexibility of the world hypothesis.

[0082] (3) In view of the problem of unstable estimation of the main direction of the Atlanta world, the present invention provides a method for determining and merging the main direction of the Atlanta world based on gravity and lane lines. The method uses gravity to determine the vertical main direction, uses lane lines to determine the horizontal main direction, and merges multiple local Manhattan worlds into the Atlanta world according to the horizontal main direction.

[0083] (4) In order to avoid the problem that the main direction is singular when the structural lines intersect at two vanishing points at the same time, and to retain more structural lines while avoiding the main direction singularity, this invention provides a structural line optimization and judgment method based on a sliding window. The main direction of the line is comprehensively judged by the main direction of the line feature in multiple frames within the sliding window. Attached Figure Description

[0084] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of this invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0085] Figure 1 This is one of the flowcharts of the multi-source fusion localization and attitude determination method based on lane line structure assistance provided by the present invention;

[0086] Figure 2 This is the second flowchart of the multi-source fusion positioning and attitude determination method based on lane line structure assistance provided by the present invention;

[0087] Figure 3 This is a schematic diagram of the structural lines provided by the present invention;

[0088] Figure 4 This is a comparison diagram of the relationship between lane lines and scene structural characteristics provided by the present invention;

[0089] Figure 5 This is a schematic diagram of the structural line direction optimization provided by the present invention;

[0090] Figure 6 These are the experimental trajectories and partial scene diagrams provided by this invention;

[0091] Figure 7 This is a positioning error sequence diagram provided by the present invention;

[0092] Figure 8 This is a schematic diagram of the multi-source fusion positioning and attitude determination system based on lane line structure assistance provided by the present invention;

[0093] Figure 9 This is a schematic diagram of the structure of the electronic device provided by the present invention. Detailed Implementation

[0094] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0095] To address the shortcomings of existing BeiDou / inertial / visual fusion positioning and attitude determination methods, which fail to fully utilize scene semantic information and whose structural constraints are insufficient to meet the high-precision positioning and attitude determination requirements when BeiDou positioning is unavailable for extended periods, this invention proposes a lane-line-assisted BeiDou / inertial / visual fusion positioning and attitude determination method, incorporating the structural properties of lane lines and the Manhattan world hypothesis. This method uses lane lines to determine the structural characteristics of the current environment, quickly identifies the main direction, and incorporates the Manhattan world hypothesis based on lane line changes. This provides stable global direction constraints even when BeiDou satellites are unavailable for extended periods, thereby improving the accuracy and continuity of positioning and attitude determination. Furthermore, this invention exhibits cross-system compatibility, applicable not only to the BeiDou Navigation Satellite System but also to other Global Navigation Satellite Systems (GNSS) such as GPS and Galileo.

[0096] Figure 1 This is one of the flowcharts illustrating the multi-source fusion localization and pose determination method based on lane line structure assistance provided in this embodiment of the invention, such as... Figure 1 As shown, it includes:

[0097] Step 100: Collect satellite pseudorange and phase observations using a satellite receiver, collect acceleration and gyroscope observations using an inertial navigation system, and collect visual images using an RGB camera. Perform fusion positioning processing on the satellite pseudorange, phase, acceleration, gyroscope observations, and visual images.

[0098] Step 200: Obtain the initial position, velocity, and attitude; perform initial alignment and mechanical choreography using inertial navigation to obtain the position, velocity, and attitude at subsequent moments;

[0099] Step 300: Use a preset lane line extraction algorithm to extract lane lines from the visual image, use a real-time straight line segment detection algorithm and a line strip descriptor algorithm to extract and match line features in the visual image, use a corner detection algorithm and optical flow method to extract and match corner features in the image, if the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, then determine the main direction of the current world based on gravity and lane lines, and initialize the structural line features of the current world;

[0100] Step 400: Utilize the main direction to constrain the line feature direction, and continuously optimize the position, velocity, and attitude at subsequent time steps using corner features and constrained line feature directions.

[0101] Specifically, such as Figure 2 As shown, it includes:

[0102] Step 1: The positioning sensors involved in this embodiment of the invention include a BeiDou / GNSS receiver, an inertial navigation system, and an RGB camera. Data is collected from typical urban scenes using the above equipment. The collected BeiDou pseudorange and phase observations, inertial navigation accelerometer and gyroscope observations, and visual images are input into the fusion positioning system.

[0103] Step 2: The inertial sensors, consisting of accelerometers and gyroscopes, provide the acceleration and angular velocity of the carrier relative to the inertial frame. Given the initial position, velocity, and attitude, the position, velocity, and attitude at subsequent moments can be obtained through initial alignment of the inertial navigation system and mechanical arrangement.

[0104] Step 3: Use Ultra-Fast-Lane-Detection-v2 to extract lane lines, EDLine and LBD to extract and match line features, and FAST and optical flow to extract and match corner features. Calculate the lane line curvature radius and determine whether the current environment conforms to the Atlanta world hypothesis based on the curvature. If the current environment conforms to the Atlanta world hypothesis, quickly determine the current world's principal direction based on the lane lines and initialize the structure line features. Here, Ultra-Fast-Lane-Detection-v2 is a lane line extraction algorithm, EDLine is a real-time straight line detection algorithm, and LBD is a line band descriptor algorithm.

[0105] Step 4: Use the main direction constraint line feature direction to improve the accuracy of line feature direction and positioning performance in artificial scenes.

[0106] Based on the above embodiments, the specific implementation of step 3 includes:

[0107] Step 3.1: Lane detection is performed using the Ultra Fast Lane Detection v2 (UFLDv2) method. This method combines row and column anchor points to model lane positions using sparse coordinates, transforming the detection problem into an ordinal classification task, significantly reducing computational complexity. Tests on mainstream datasets such as TuSimple and CULane demonstrate that this method can balance lane detection speed and accuracy, and is particularly adept at handling curves and lane change scenarios.

[0108] Step 3.2: Convert the image to a bird's-eye view using perspective transformation, and calculate the lane line curvature and radius of curvature:

[0109] (1)

[0110] In the formula, For lane curvature, The tangent direction angle at a point on the lane line. Let the arc length be , The radius of curvature of the lane line.

[0111] Step 3.3: Determine whether the current environment conforms to the Atlanta World (AW) hypothesis based on the curvature. If the detected lane line curvature is less than the corresponding threshold, the current environment is determined to conform to the orthogonal constraints of the Manhattan hypothesis, and the lane line straight line parameters are fitted using least squares. If the detected lane line curvature is greater than the corresponding threshold, the current environment is determined not to conform to the Atlanta World hypothesis, the structure lines are not initialized, and ordinary line features are used for updating.

[0112] In positioning, to maintain global orientation, AW includes a globally shared vertical direction (gravity axis) and multiple local horizontal directions. The main vertical direction... It can be determined based on the direction of gravity:

[0113] (2)

[0114] Vertical direction Main direction Corresponding vanishing point for:

[0115] (3)

[0116] in, Let be the rotation matrix from the n-system to the e-system. Let be the rotation matrix from the e-frame to the c-frame. Let n be the camera intrinsic parameter matrix, n be the navigation coordinate system, e be the geocentric coordinate system, and c be the camera coordinate system.

[0117] When the two lines on the left and right of the lane are detected in the image within the sliding window, namely the first lane line and the second lane line:

[0118] (4)

[0119] (5)

[0120] In the formula, , and The first linear coefficient, , and The second linear coefficient;

[0121] Lane line disappearance point It can be quickly determined based on the intersection of lane lines:

[0122] (6)

[0123] Lane corresponding direction for:

[0124] (7)

[0125] in, Let be the rotation matrix from the c-system to the n-system;

[0126] To ensure y The principal direction of the axis is orthogonal to the principal direction of gravity, thus affecting the direction of the lane lines. Projecting the lane lines onto the current ground plane yields the main direction of the projected lane lines. Based on detection within the sliding window m The lane lines on the frame image can be used to calculate the current situation in Atlanta. y The main direction of the axis and n North direction The included angle for:

[0127] (8)

[0128] At this time, Atlanta World y The principal direction of the axis and vanishing point for:

[0129] (9)

[0130] (10)

[0131] x Major direction along the axis It is obtained by cross product of the other two main directions:

[0132] (11)

[0133] x Major direction along the axis Corresponding vanishing point for:

[0134] (12)

[0135] After the above steps, the current world main direction can be quickly determined based on the lane lines, and the structural line features can be initialized.

[0136] Based on the above embodiments, in step 4, based on the current world main direction constraint line feature direction determined in step 3, the accuracy of line feature direction and positioning performance in artificial scenes are improved. The specific implementation method is as follows:

[0137] Step 4.1, Initialize the structure lines based on the sliding window:

[0138] Structural lines along the same principal direction intersect at corresponding vanishing points. Whether a line feature is a structural line can be determined based on the geometric relationship between the vanishing point and the line feature itself. Figure 3 The structural line diagram shown typically includes three scenarios, such as... Figure 4 The diagram showing the relationship between lane lines and scene structural characteristics is as follows.

[0139] First, the characteristics of the connecting lines. midpoint With vanishing point ,like Figure 5 As shown, when the line and Line features meet the following distance and angle thresholds. For the structural lines corresponding to the main direction:

[0140] (13)

[0141] In the formula, Line features Endpoint to Ray distance, Line features Length, Line features and rays The included angle between them;

[0142] The sliding window-based optimization and judgment method determines the final main direction of the line segment by comprehensively judging the main direction it belongs to over multiple frames, thus obtaining the initialized structure line.

[0143] Step 4.2, triangulation of the structure line based on the sliding window:

[0144] The direction vector of the structure line can be obtained from its principal direction. Based on the common viewing relationship of the inner line features of the sliding window, the plane normal vector in the e-system can be obtained. and the distance from the line to the origin .

[0145] Step 4.3, Visual Update: The visual features to be updated include corner points, lines, and structural line features.

[0146] The system state includes IMU position, attitude velocity, and zero bias of the accelerometer and gyroscope. Whenever a new frame of image is recorded by the sliding window, the current IMU state is added to the estimated state vector.

[0147] (14)

[0148] In the formula, This represents the estimated state vector. and This indicates the IMU's position error, velocity error, and misalignment angle. and This indicates the zero bias error of the accelerometer and gyroscope. Indicates the size of the sliding window. Indicates the IMU status within the sliding window:

[0149] (15)

[0150] The observation information of visual features includes reprojection error and point feature state. With line feature state They can be represented as:

[0151] (16)

[0152] (17)

[0153] In the formula, This represents the coordinates of the feature point in the e-frame. Represents a distance scalar. Rotation matrix The angle of misalignment, This represents the rotation matrix of the line feature.

[0154] The line features to be updated include structural lines and ordinary line features. The representation and update method of the two types of line features are consistent. When the scene satisfies the Atlanta World hypothesis, structural lines are used for updating; otherwise, ordinary line features are used.

[0155] Step 4.4, BeiDou / GNSS position update:

[0156] Given the known spatial relationship between the BeiDou receiver and the inertial navigation system (INS), the position of the INS center can be obtained by correcting the position of the BeiDou receiver using a lever arm:

[0157] (18)

[0158] in This is the lever arm between the BeiDou receiver and the inertial navigation system. This is the phase center of the Beidou receiver antenna.

[0159] The Jacobian matrix corresponding to the pine combination observation model is:

[0160] (19)

[0161] in, , These represent the zero bias of the accelerometer and gyroscope, respectively. It is the identity matrix. Let be the rotation matrix from system b to system e. The lever arm that serves as the phase center for inertial navigation and BeiDou. Indicates an antisymmetric matrix;

[0162] Due to environmental obstruction, BeiDou / GNSS sensors contain a certain amount of gross errors and cycle slips in their observations. To improve the quality of BeiDou / GNSS observation data, this invention employs Turbo Edit and M-estimation to eliminate cycle slips and gross errors.

[0163] Step 4.5 Loop Correction:

[0164] After the measurement update is completed, the estimated error will be fed back into the system to correct the system state.

[0165] To verify the positioning performance of the BDS / INS / vision fusion positioning system with lane line assistance in artificial scenes proposed in this invention, a dataset of complex urban environments was collected in a city. The BeiDou failure scenarios mainly included overpasses and tree tunnels. The trajectory map and some actual scene photos are shown below. Figure 6 As shown.

[0166] To analyze the improvement of BDS / INS / visual fusion localization performance by incorporating lane line and world assumptions, the localization and pose determination accuracy of the fusion systems with four different inputs was compared: using corner and ordinary line features (GVIO); using corner and structure line features, where the Atlanta world principal direction is obtained through RANSAN (GVIO-A); and using corner, ordinary line, and structure line features, where the Atlanta world principal direction is obtained based on lane lines (GVIO-AL). The localization and pose determination accuracy statistics for the corresponding methods are shown in Tables 1 and 2, and the localization error sequence diagram is shown below. Figure 7 As shown, the results indicate that using the Atlanta world hypothesis can improve the system's localization and orientation accuracy in urban scenarios, and the lane-line-based world determination hypothesis can further improve localization and orientation accuracy, especially lateral accuracy.

[0167] Table 1. RMSE Statistics for Location Error (m)

[0168]

[0169] Table 2. Attitude Error RMSE Statistics (°)

[0170]

[0171] The multi-source fusion positioning and attitude determination system based on lane line structure assistance provided by the present invention is described below. The multi-source fusion positioning and attitude determination system based on lane line structure assistance described below can be referred to in correspondence with the multi-source fusion positioning and attitude determination method based on lane line structure assistance described above.

[0172] Figure 8 This is a schematic diagram of the multi-source fusion positioning and attitude determination system based on lane line structure assistance provided in an embodiment of the present invention, as shown below. Figure 8 As shown, it includes: a data acquisition and input module 81, an inertial navigation initialization and mechanical arrangement module 82, a vision front-end module 83, and a back-end optimization module 84, wherein:

[0173] The data acquisition and input module 81 is used to acquire satellite pseudorange and phase observations through a satellite receiver, acceleration values ​​and gyroscope observations through an inertial navigation system, and visual images through an RGB camera. It performs fusion positioning processing on the satellite pseudorange, phase, acceleration, gyroscope observations, and visual images. The inertial navigation initialization and mechanical arrangement module 82 is used to obtain the position, velocity, and attitude at the initial moment. It performs initial alignment and mechanical arrangement through the inertial navigation system to obtain the position, velocity, and attitude at subsequent moments. The visual front-end module 83 is used to extract lane lines from the visual image using a preset lane line extraction algorithm, extract and match line features in the visual image using a real-time straight line segment detection algorithm and a line strip descriptor algorithm, and extract and match corner features in the visual image using a corner detection algorithm and an optical flow method. If the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, the current world's main direction is determined based on gravity and lane lines, and the structural line features of the current world are initialized. The back-end optimization module 84 is used to constrain the line feature direction using the main direction, and continuously optimize the position, velocity, and attitude at subsequent moments using corner features and constrained line feature directions.

[0174] Figure 9 An example is a schematic diagram of the physical structure of an electronic device, such as... Figure 9As shown, the electronic device may include: a processor 910, a communication interface 920, a memory 930, and a communication bus 940, wherein the processor 910, the communication interface 920, and the memory 930 communicate with each other through the communication bus 940. The processor 910 can call logic instructions in the memory 930 to execute a multi-source fusion positioning and attitude determination method based on lane line structure assistance. This method includes: acquiring satellite pseudorange and phase observations via a satellite receiver, acquiring acceleration and gyroscope observations via an inertial navigation system (INS), and acquiring visual images via an RGB camera; performing fusion positioning processing on the satellite pseudorange, phase, acceleration, gyroscope observations, and visual images; obtaining the initial position, velocity, and attitude; performing initial alignment and mechanical arrangement via INS to obtain the position, velocity, and attitude at subsequent moments; extracting lane lines from the visual images using a preset lane line extraction algorithm; extracting and matching line features in the visual images using a real-time straight line segment detection algorithm and a line descriptor algorithm; extracting and matching corner features in the visual images using a corner detection algorithm and an optical flow method; if the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, determining the current world's principal direction based on gravity and lane lines, and initializing the structural line features of the current world; using the principal direction to constrain the line feature direction, and continuously optimizing the position, velocity, and attitude at subsequent moments using corner features and the constrained line feature direction.

[0175] Furthermore, the logical instructions in the aforementioned memory 930 can be implemented as software functional units and, when sold or used as independent products, can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0176] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0177] Through the above description of the embodiments, those skilled in the art can clearly understand that each embodiment can be implemented by means of software plus necessary general-purpose hardware platforms, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solutions, in essence or the part that contributes to the prior art, can be embodied in the form of a software product. This computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, magnetic disk, optical disk, etc., and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute the methods described in the various embodiments or some parts of the embodiments.

[0178] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A multi-source fusion localization and pose determination method based on lane line structure assistance, characterized in that, include: The satellite pseudorange and phase observations are collected by a satellite receiver, acceleration values ​​and gyroscope observations are collected by an inertial navigation system, and visual images are collected by an RGB camera. The satellite pseudorange, phase, acceleration, gyroscope observations and visual images are then fused for positioning. The initial position, velocity, and attitude are obtained, and initial alignment and mechanical choreography are performed using inertial navigation to obtain the position, velocity, and attitude at subsequent moments. Lane lines in the visual image are extracted using a preset lane line extraction algorithm. Line features in the visual image are extracted and matched using a real-time straight line segment detection algorithm and a line strip descriptor algorithm. Corner features in the visual image are extracted and matched using a corner detection algorithm and an optical flow method. If the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, the main direction of the current world is determined based on gravity and lane lines, and the structural line features of the current world are initialized. By utilizing the main direction to constrain the line feature direction, the position, velocity, and attitude at subsequent time steps are continuously optimized through corner features and constrained line feature directions. Lane lines are extracted from the visual image using a preset lane line extraction algorithm. Real-time straight line segment detection and line strip descriptor algorithms are used to extract and match line features in the visual image. Corner detection and optical flow methods are used to extract and match corner features in the visual image. If the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, the main direction of the current world is determined based on gravity and lane lines. The structural line features of the current world are initialized, including: Lane lines are detected using the Ultra Fast Lane Detection v2 method. Line features are extracted and matched using EDLine and LBD, while corner features are extracted and matched using FAST and optical flow methods. By using perspective transformation, the visual image is converted into a bird's-eye view, and the curvature and radius of curvature of the lane lines are calculated. The current environment is determined to conform to the Atlanta World hypothesis based on the lane line curvature. If the lane line curvature is less than the threshold, the current environment is determined to conform to the orthogonal constraint of the Manhattan hypothesis, and the lane line straight line parameters are fitted using least squares. Otherwise, the current environment is determined to not conform to the Manhattan hypothesis, the structure line is not initialized, and ordinary line features are used for updating. The current environment is determined to conform to the Atlanta World hypothesis based on the lane curvature. If the lane curvature is less than a threshold, the current environment is determined to conform to the orthogonal constraints of the Manhattan hypothesis. Least squares fitting is then used to fit the lane line straight-line parameters, including: The principal direction perpendicular to the Atlanta World hypothesis is determined based on the direction of gravity. : Vertical direction Main direction Corresponding vanishing point for: in, Let n be the rotation matrix from the n-system to the e-system. Let be the rotation matrix from the e-frame to the c-frame. Let n be the camera intrinsic parameter matrix, n be the navigation coordinate system, e be the geocentric coordinate system, and c be the camera coordinate system. Detect the first and second lane lines on both sides of the lane in the image within the sliding window: In the formula, , and The first linear coefficient, , and The second linear coefficient; The lane line vanishing point is determined based on the intersection of the first lane line and the second lane line. : Lane corresponding direction for: in, Let be the rotation matrix from the c-system to the n-system; To ensure y The principal direction of the axis is orthogonal to the principal direction of gravity, and the corresponding direction of the lane Projecting the lane lines onto the current ground plane yields the main direction of the projected lane lines. Based on detection within the sliding window m The lane lines on the frame image are used to calculate the current Atlanta world. y The main direction of the axis and n North direction The included angle for: Get Atlanta World y The main direction of the axis and vanishing point for: x Major direction along the axis It is obtained by cross product of the other two main directions: x Major direction along the axis Corresponding vanishing point for: 。 2. The multi-source fusion localization and attitude determination method based on lane line structure assistance according to claim 1, characterized in that, Calculate the lane line curvature and radius of curvature, including: In the formula, For lane curvature, The tangent direction angle at a point on the lane line. Let the arc length be , The radius of curvature of the lane line.

3. The multi-source fusion localization and attitude determination method based on lane line structure assistance according to claim 1, characterized in that, By utilizing the principal direction constraint line feature direction, and through corner point features and the constrained line feature direction, the position, velocity, and attitude at subsequent time steps are continuously optimized, including: The structure lines are initialized using a sliding window to obtain the initialized structure lines. Triangulation of the initialized structure lines is performed based on a sliding window. Visual updates are performed using the triangulated structural lines. The visual features to be updated include corner points, lines, and structural line features. Perform satellite position updates, complete measurement updates, and obtain system errors; The system state is continuously corrected and optimized based on system errors.

4. The multi-source fusion localization and attitude determination method based on lane line structure assistance according to claim 3, characterized in that, The structure lines are initialized using a sliding window to obtain the initialized structure lines. include: Connector features midpoint With vanishing point When the line features and rays Line features meet the following distance and angle thresholds. For the structural lines corresponding to the main direction: In the formula, Line features Endpoint to Ray distance, Line features Length, Line features and rays The included angle between them; The sliding window-based optimization and judgment method determines the final main direction of the line segment by comprehensively judging the main direction it belongs to over multiple frames, thus obtaining the initialized structure line.

5. The multi-source fusion localization and attitude determination method based on lane line structure assistance according to claim 4, characterized in that, Triangulation of the initialized structure lines based on a sliding window includes: The direction vector of the structure line can be obtained from its principal direction. ; Based on the common viewing relationship of the inner line features of the sliding window, the normal vector of the lower plane in the e-system is obtained. and the distance from the line to the origin .

6. The multi-source fusion localization and attitude determination method based on lane line structure assistance according to claim 5, characterized in that, Visual updates are performed using triangulated structural lines. The visual features to be updated include corner points, lines, and structural line features, including: The system state includes IMU position, attitude, velocity, and zero bias of the accelerometer and gyroscope. When a new frame of image is recorded by the sliding window, the current IMU state is added to the estimated state vector. In the formula, This represents the estimated state vector. and This indicates the IMU's position error, velocity error, and misalignment angle. and This indicates the zero bias error of the accelerometer and gyroscope. Indicates the size of the sliding window. Indicates the IMU status within the sliding window: The observation information of visual features includes reprojection error and point feature state. With line feature state They are represented as follows: In the formula, This represents the coordinates of the feature point in the e-frame. Represents a distance scalar. Rotation matrix The angle of misalignment, This represents the rotation matrix of the line feature.

7. The multi-source fusion localization and attitude determination method based on lane line structure assistance according to claim 6, characterized in that, Perform satellite position updates, complete measurement updates, and obtain systematic errors, including: Given that the spatial relationship between the satellite receiver and the inertial navigation system is known, the position of the satellite receiver is corrected using a lever arm to obtain the position of the inertial navigation system's center. in The lever arm between the satellite receiver and the inertial navigation system. The phase center of the satellite receiver antenna; Jacobian matrix corresponding to the pine combination observation model for: in, , These represent the zero bias of the accelerometer and gyroscope, respectively. It is the identity matrix. Let be the rotation matrix from system b to system e. The lever arm between the satellite receiver and the inertial navigation system. Indicates an antisymmetric matrix; Turbo Edit and M-estimation were used to remove cycle slips and gross errors from satellites.

8. A multi-source fusion positioning and attitude determination system based on lane line structure assistance, based on the multi-source fusion positioning and attitude determination method based on lane line structure assistance as described in any one of claims 1 to 7, characterized in that, include: The data acquisition and input module is used to acquire satellite pseudorange and phase observations through a satellite receiver, acquire acceleration values ​​and gyroscope observation information through an inertial navigation system, and acquire visual images through an RGB camera. It performs fusion positioning processing on the satellite pseudorange observations, phase observations, acceleration values, gyroscope observation information, and visual images. The inertial navigation initialization and mechanical orchestration module is used to obtain the position, velocity and attitude at the initial moment. It performs initial alignment and mechanical orchestration through inertial navigation to obtain the position, velocity and attitude at subsequent moments. The visual front-end module is used to extract lane lines from the visual image using a preset lane line extraction algorithm, extract and match line features in the visual image using a real-time straight line segment detection algorithm and a line strip descriptor algorithm, extract and match corner features in the visual image using a corner detection algorithm and an optical flow method, and if the current environment conforms to the Atlanta world hypothesis based on the lane line curvature radius, then the main direction of the current world is determined based on gravity and lane lines, and the structural line features of the current world are initialized. The backend optimization module is used to continuously optimize the position, velocity, and attitude at subsequent time steps by using the main direction to constrain the line feature direction, and by using the corner features and the constrained line feature direction.

Images