Multi-label fusion positioning method based on geometric constraint

Abstract

The invention relates to a geometric constraint-based multi-label fusion positioning method, which comprises the following steps of: acquiring a visual label image, and determining relative attitude information between a target and a visual label; relative attitude information between the target and the visual label is converted into a global attitude under a world coordinate system, and a target observation value is obtained through calculation; based on a plurality of target observation values at the same moment, screening and reserving reliable observation values by constructing geometric constraints; and obtaining a positioning result through iterative fusion processing based on the plurality of reliable observation values. Compared with the prior art, the multi-label fusion positioning is carried out by utilizing the visual labels, so that the positioning result can be simply, conveniently and accurately obtained at low cost.

Description

Technical Field

[0001] This invention relates to the field of mobile carrier positioning technology, and in particular to a multi-label fusion positioning method based on geometric constraints. Background Technology

[0002] With the continuous development and improvement of the Global Navigation Satellite System (GNSS), the technology for applying GNSS to mobile vehicles such as drones, robots, and autonomous vehicles to determine their attitude is becoming increasingly mature. A traditional approach involves installing multiple GNSS antennas on the mobile vehicle and determining its attitude based on observations from these antennas combined with direct calculation methods. However, this multi-GNSS antenna positioning method suffers from high installation costs, high installation complexity, and poor real-time performance.

[0003] Furthermore, considering that GNSS signals are susceptible to atmospheric interference and / or signal reflection interference from objects near the moving vehicle, patent CN115494532A proposes a mobile vehicle positioning scheme. This scheme receives GNSS satellite signals from GNSS satellites and determines the distance information between the vehicle and the GNSS satellites transmitting the relevant signals. Then, it uses image information obtained from environmental sensors to determine environmental information about the vehicle's surroundings. These environmental sensors can acquire images of the vehicle's surroundings from different perspectives. Correction information is then determined using this environmental information, and the distance information is corrected accordingly. This method, utilizing image information acquired by environmental sensors, can identify and discard erroneous GNSS measurements when necessary, but it still suffers from high hardware costs and system complexity.

[0004] Another patent, CN116793354A, proposes a vehicle positioning initialization method that employs a particle filtering approach. Based on the vehicle's velocity-related data, it uses a vehicle motion model to predict the next position of the updated particles, thus obtaining the new particle positions. Based on these new particle positions, the target position of the vehicle is determined to complete the vehicle's positioning initialization. This method relies on motion model prediction, and due to the complexity and potential inaccuracies of model prediction, it is difficult to guarantee accurate positioning results. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the existing technology and provide a multi-label fusion localization method based on geometric constraints, which can obtain localization results in a low-cost, simple, convenient and accurate manner.

[0006] The objective of this invention can be achieved through the following technical solution: a multi-label fusion localization method based on geometric constraints, comprising the following steps:

[0007] S1. Acquire visual label images and determine the relative pose information between the target and the visual label;

[0008] S2. Convert the relative pose information between the target and the visual label into the global pose in the world coordinate system, and calculate the target observation value;

[0009] S3. Based on multiple target observations at the same time, reliable observations are selected and retained by constructing geometric constraints;

[0010] S4. Based on multiple reliable observations, the positioning results are obtained through iterative fusion processing.

[0011] Furthermore, in step S1, visual labels are pre-positioned at corresponding locations in the target movement area.

[0012] Furthermore, step S1 specifically involves acquiring visual label images using a camera mounted on the target.

[0013] Furthermore, step S1 includes the following process;

[0014] The acquired visual label images are processed by image filtering, image binarization, edge detection, quadrilateral fitting, perspective correction, and sharpening decoding to output the relative pose information between the target and the visual label.

[0015] Furthermore, step S2 includes the following process:

[0016] S21. Based on the right-hand rule, define the world coordinate system W, the visual label coordinate system T, the camera coordinate system C, and the target's own coordinate system M respectively;

[0017] S22. Based on the relative pose information between the target and the visual label, the relative pose in the visual label coordinate system is transformed into the global pose in the world coordinate system through coordinate transformation, and the target observation value is calculated.

[0018] Furthermore, the target observation values ​​in step S2 include the target's coordinates and orientation angle in the world coordinate system.

[0019] Furthermore, step S3 includes the following process:

[0020] S31. For multiple target observations at the same time, calculate the distance difference and angle difference between any two target observations in the horizontal plane of the world coordinate system to construct geometric constraints;

[0021] S32. Based on geometric constraints, calculate the outlier factor for each target observation using a normalization method, and select and retain reliable observations based on the magnitude of the outlier factor.

[0022] Furthermore, step S32 specifically involves sorting the target observations according to the magnitude of the outlier factor, then removing the target observations whose outlier factor exceeds a preset threshold, and retaining the reliable observations.

[0023] Furthermore, step S4 specifically involves using a Kalman filter framework to iteratively fuse multiple reliable observations.

[0024] Furthermore, step S4 includes the following process:

[0025] S41. For the current predicted value, add new reliable observations to update the predicted value. Determine if there are any unfused reliable observations. If yes, return to the iteration to add new reliable observations to update the predicted value.

[0026] S42. Otherwise, output the positioning result at the current moment, calculate the error covariance matrix at the current moment, and then return to step S41 to perform iterative fusion processing at the next moment.

[0027] Compared with the prior art, the present invention has the following advantages:

[0028] This invention acquires visual tag images to determine the relative pose information between the target and the visual tag; then, it converts this relative pose information into a global pose in the world coordinate system and calculates the target observation value; next, based on multiple target observation values ​​at the same time, it constructs geometric constraints to filter and retain reliable observation values; finally, iterative fusion processing is performed on the multiple reliable observation values ​​to obtain the localization result. This provides a multi-tag fusion localization scheme using visual tags, eliminating the need for additional complex equipment and sensors, significantly reducing hardware costs and structural complexity, while also avoiding the complexity and potential inaccuracies of relying on motion model predictions, thus ensuring the accuracy of the localization result.

[0029] After acquiring the visual label image, this invention uses steps such as image filtering, image binarization, edge detection, quadrilateral fitting, perspective correction, and sharpening decoding to decode the relative pose, thus ensuring accurate acquisition of the relative pose information between the target and the visual label.

[0030] This invention defines a world coordinate system (W), a visual label coordinate system (T), a camera coordinate system (C), and a target self-coordinate system (M). By using coordinate transformation, the relative pose in the label coordinate system is converted into the global pose in the world coordinate system, thereby calculating the target's coordinates and orientation angle in the world coordinate system. This provides effective observation values ​​for the subsequent iterative fusion process.

[0031] This invention calculates the distance and angle differences between any two observations in the XOY plane of the world coordinate system for multiple tags detected at the same time. These differences are used to construct geometric constraints. By introducing an outlier factor based on geometric constraints, outliers can be effectively eliminated and observation noise can be dynamically adjusted. This reduces the possibility of unreasonable initial noise settings and improves the accuracy and stability of positioning.

[0032] This invention employs a Kalman filter framework, using reliable observations after outlier removal as the basis for iterative fusion. By gradually fusing information from multiple reliable observations, this iterative fusion process does not rely on kinematic equations to calculate predicted values, but rather makes predictions entirely based on observation data, thus obtaining accurate positioning results. Attached Figure Description

[0033] Figure 1 This is a schematic diagram of the method flow of the present invention;

[0034] Figure 2 This is a schematic diagram illustrating the processing of visual label images;

[0035] Figure 3a and 3b This is a schematic diagram of multiple coordinate systems defined in the embodiment;

[0036] Figure 4 This is a schematic diagram illustrating the construction of geometric constraints in the embodiment;

[0037] Figure 5 This is a schematic diagram of the iterative fusion process. Detailed Implementation

[0038] The present invention will now be described in detail with reference to the accompanying drawings and specific embodiments.

[0039] Example

[0040] like Figure 1 As shown, a multi-label fusion localization method based on geometric constraints includes the following steps:

[0041] S1. Acquire visual tag images and determine the relative pose information between the target and the visual tag. The visual tag is pre-positioned at the corresponding position in the target's movement area, and the visual tag images are acquired by a camera mounted on the target.

[0042] S2. Convert the relative pose information between the target and the visual label into the global pose in the world coordinate system, and calculate the target observation value (including the target's coordinates and orientation angle in the world coordinate system);

[0043] S3. Based on multiple target observations at the same time, reliable observations are selected and retained by constructing geometric constraints;

[0044] Specifically, for multiple target observations at the same time, the distance and angle differences between any two target observations in the horizontal plane of the world coordinate system are calculated to construct geometric constraints;

[0045] Then, based on geometric constraints, the outlier factor of each target observation is calculated using a normalization method. The target observations are sorted according to the magnitude of the outlier factor. Then, target observations with outlier factors exceeding a preset threshold are removed, and reliable observations are retained.

[0046] S4. Based on multiple reliable observations, the positioning result is obtained through iterative fusion processing. Specifically, a Kalman filter framework is used to iteratively fuse multiple reliable observations.

[0047] For the current predicted value, add new reliable observations to update the predicted value, determine whether there are any unfused reliable observations, and if so, return to the iteration to add new reliable observations to update the predicted value;

[0048] Otherwise, the current positioning result is output, and the error covariance matrix at the current moment is calculated. Then, iterative fusion processing is performed at the next moment.

[0049] This embodiment applies the above-described solution, and its main contents include:

[0050] I. Visual Label Detection

[0051] In this embodiment, AprilTag3 is selected as the visual label. Its advantages, such as being open source, having strong real-time processing capabilities, and high precision, are used to obtain the relative pose information between the target and the label.

[0052] The detection process includes steps such as image filtering, image binarization, edge detection, quadrilateral fitting, perspective correction, and sharpening decoding, ultimately decoding the relative pose. For example... Figure 2 As shown.

[0053] II. Coordinate Transformation

[0054] Define the world coordinate system (W), visual label coordinate system (T), camera coordinate system (C), and target's own coordinate system (M), all conforming to the right-hand rule, such as... Figure 3a As shown in Figure 3b, red represents the X-axis, green represents the Y-axis, and blue represents the Z-axis.

[0055] Based on the relative pose information between the camera and the tag, the relative pose in the tag coordinate system is converted into the global pose in the world coordinate system through a series of coordinate transformation formulas. The coordinates and orientation angle of the target in the world coordinate system are calculated to provide observation values ​​for subsequent fusion steps.

[0056] III. Geometric Constraint Construction and Outlier Removal

[0057] For observations of multiple labels detected at the same time, calculate the distance and angle differences between any two observations in the XOY plane of the world coordinate system, and construct geometric constraints. For example... Figure 4 As shown, it can intuitively present the positional relationship of multiple label observations in the world coordinate system.

[0058] Based on distance and angle differences, the outlier factor for each observation is calculated using a normalization method. The observations are then sorted according to the magnitude of the outlier factor, and observations with larger outlier factors are removed, while reliable observations are retained.

[0059] IV. Multi-observation fusion

[0060] In this embodiment, within the modified Kalman filter framework, iterative fusion is performed based on reliable observations after outlier removal, such as... Figure 5 As shown, the multi-label fusion proposed in this scheme is a variation of Kalman filtering. It does not rely on kinematic equations to calculate predicted values, but makes predictions entirely based on observation data.

[0061] In each fusion iteration, the information from multiple reliable observations is gradually fused by using the current predicted values ​​and new observations, through steps such as calculating the Kalman gain, updating the predicted values ​​and the error covariance matrix, and finally obtaining accurate positioning results.

[0062] In summary, this solution has the following advantages:

[0063] 1. Low cost and simple structure: It realizes a linear fusion method using only cameras, without the need for additional complex equipment and sensors, reducing hardware costs and system complexity, while avoiding the complexity and potential inaccuracies of relying on motion model prediction.

[0064] 2. Improved Positioning Accuracy: Introducing an outlier factor based on geometric constraints effectively eliminates outliers and dynamically adjusts observation noise, reducing the possibility of unreasonable initial noise settings and improving positioning accuracy and stability. Experimental results show that even under the influence of motion and obstacles, this method can still control the position error within 3 cm and the attitude error within 3°, demonstrating high positioning accuracy and stability.

[0065] 3. Wide applicability: This method is applicable to a variety of scenarios and applications, such as robots, drones and intelligent transportation systems. In environments without GNSS signals, it can provide a reliable positioning solution for vehicles or robots, and has broad application prospects and practical value.

Claims

1. A multi-label fusion localization method based on geometric constraints, characterized in that, Includes the following steps: S1. Acquire visual label images and determine the relative pose information between the target and the visual label; S2. Convert the relative pose information between the target and the visual label into the global pose in the world coordinate system, and calculate the target observation value; S3. Based on multiple target observations at the same time, reliable observations are selected and retained by constructing geometric constraints; S4. Based on multiple reliable observations, the positioning results are obtained through iterative fusion processing.

2. The multi-label fusion localization method based on geometric constraints according to claim 1, characterized in that, In step S1, visual labels are pre-placed at corresponding positions in the target movement area.

3. The multi-label fusion localization method based on geometric constraints according to claim 1, characterized in that, Step S1 specifically involves acquiring visual label images using a camera mounted on the target.

4. The multi-label fusion localization method based on geometric constraints according to claim 1, characterized in that, Step S1 includes the following process; The acquired visual label images are processed by image filtering, image binarization, edge detection, quadrilateral fitting, perspective correction, and sharpening decoding to output the relative pose information between the target and the visual label.

5. The multi-label fusion localization method based on geometric constraints according to claim 1, characterized in that, Step S2 The process includes the following: S21. Based on the right-hand rule, define the world coordinate system W, the visual label coordinate system T, the camera coordinate system C, and the target's own coordinate system M respectively; S22. Based on the relative pose information between the target and the visual label, the relative pose in the visual label coordinate system is transformed into the global pose in the world coordinate system through coordinate transformation, and the target observation value is calculated.

6. The multi-label fusion localization method based on geometric constraints according to claim 5, characterized in that, The target observation values ​​in step S2 include the target's coordinates and orientation angle in the world coordinate system.

7. The multi-label fusion localization method based on geometric constraints according to claim 6, characterized in that, Step S3 The process includes the following: S31. For multiple target observations at the same time, calculate the distance difference and angle difference between any two target observations in the horizontal plane of the world coordinate system to construct geometric constraints; S32. Based on geometric constraints, calculate the outlier factor for each target observation using a normalization method, and select and retain reliable observations based on the magnitude of the outlier factor.

8. The multi-label fusion localization method based on geometric constraints according to claim 7, characterized in that, Specifically, step S32 involves sorting the target observations according to the magnitude of the outlier factor, then removing the target observations whose outlier factor exceeds a preset threshold, and retaining the reliable observations.

9. The multi-label fusion localization method based on geometric constraints according to claim 1, characterized in that, Specifically, step S4 involves using a Kalman filter framework to iteratively fuse multiple reliable observations.

10. The multi-label fusion localization method based on geometric constraints according to claim 9, characterized in that, Step S4 The process includes the following: S41. For the current predicted value, add new reliable observations to update the predicted value. Determine if there are any unfused reliable observations. If yes, return to the iteration to add new reliable observations to update the predicted value. S42. Otherwise, output the positioning result at the current moment, calculate the error covariance matrix at the current moment, and then return to step S41 to perform iterative fusion processing at the next moment.

Images