Positioning method and device based on visual label map
Through the combined positioning method of visual tag map and Kalman filtering, the problem of error accumulation in INS positioning technology is solved, the positioning accuracy in GNSS-free environment is improved, and it is suitable for applications such as smart cars.
Patent Information
- Application Number
- CN202410251328.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2024-03-06
- Publication Date
- 2024-11-15
- Estimated Expiration
- 2044-03-06
AI Technical Summary
The errors in INS positioning technology will accumulate over time, and the positioning accuracy needs to be further improved, especially in the absence of GNSS environment, which cannot meet the demand for positioning accuracy of smart cars.
Using a positioning method based on visual tag map, the label information of the visual tag is obtained through the combination of inertial sensors and visual sensors, the relative position and absolute position of the visual sensor and the target visual tag are determined, and coordinate conversion is performed to obtain the absolute positioning of the device to be located. Positioning and Kalman filtering is used to correct the INS positioning results to achieve combined positioning.
In the GNSS-free environment, the positioning accuracy is improved, the error accumulation is reduced, and the positioning needs in different scenarios are adapted to the positioning needs, achieving high-precision absolute positioning.
Smart Images

Figure CN117848331B_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of gyroscope positioning technology, and in particular to a positioning method and device based on a visual tag map. Background Art
[0002] Smart car technology has developed rapidly in recent years, among which positioning technology is a basic component. Accurate positioning is crucial for smart cars because it is related to the accuracy of the vehicle's relative position to the external environment. If the positioning error is too large, it may lead to driving safety problems and even cause traffic safety accidents.
[0003] Currently, the Global Navigation Satellite System (GNSS) positioning technology is the main positioning method that vehicles rely on. It uses the pseudo-range and position information provided by the satellite to calculate the antenna position and provide the vehicle with absolute positioning information without cumulative errors. However, the anti-interference ability of GNSS positioning technology is weak, and it is easily blocked by tall buildings, trees, overpasses, etc., as well as multipath effects and non-line-of-sight signals, resulting in reduced positioning accuracy. In indoor scenes such as tunnels and underground garages where satellite signals cannot be received, positioning fails and cannot meet the positioning accuracy requirements of smart cars.
[0004] Inertial Navigation System (INS) positioning technology calculates the carrier speed, position and attitude by integrating the three-axis acceleration and three-axis angular velocity provided by the inertial measurement unit. Compared with GNSS positioning technology, it has the characteristics of high sampling frequency and good real-time performance. In the process of implementing the embodiments of this application, it is found that there are at least the following technical problems in the related technology:
[0005] INS positioning technology errors will accumulate over time, and positioning accuracy needs to be further improved. Summary of the invention
[0006] The embodiments of the present application provide a positioning method and device based on a visual tag map to solve the problem that the errors of INS positioning technology will accumulate over time and the positioning accuracy needs to be further improved.
[0007] In a first aspect, an embodiment of the present application provides a positioning method based on a visual tag map, wherein a device to be positioned includes an inertial sensor and a visual sensor disposed on a body of the device to be positioned; the positioning method includes:
[0008] Acquire the tag information of the target visual tag collected by the visual sensor, perform recognition processing on the tag information to determine the relative position relationship between the visual sensor and the target visual tag, and perform recognition processing on the tag information to determine the tag code of the target visual tag;
[0009] Determining an absolute position of the target visual tag according to the tag encoding of the target visual tag and the visual tag map;
[0010] Performing coordinate transformation according to the relative posture relationship between the visual sensor and the target visual tag, the absolute posture of the target visual tag, and the relative posture relationship between the visual sensor and the device to be positioned, determining the absolute posture of the device to be positioned, and determining the visual positioning result according to the absolute posture of the device to be positioned;
[0011] Constructing the observation quantity of Kalman filter with the visual positioning result and the vehicle motion constraint, correcting the INS positioning result, and obtaining the combined positioning result of this epoch;
[0012] The INS positioning result is obtained by state recursive prediction using error state Kalman filtering based on the combined positioning result of the previous epoch and the acceleration and angular velocity collected by the inertial sensor.
[0013] In a possible implementation, the recursive prediction process of the INS positioning result includes:
[0014] The real state quantity corresponding to the combined positioning result of the previous epoch of the device to be positioned Nominal state quantity and error state Two state variables; among them, ;
[0015] According to the acceleration and angular velocity collected by the inertial sensor, the nominal state quantity and error state Recursively, we get the nominal state quantity The predicted value of , and according to the system noise and the state transition matrix Obtain the error state quantity of the device to be positioned The covariance matrix of .
[0016] In a possible implementation, constructing the observation quantity of Kalman filtering with the visual positioning result and the vehicle motion constraint, correcting the INS positioning result, and obtaining the combined positioning result of the current epoch includes:
[0017] The visual positioning result and the vehicle motion constraint are used to construct the observation quantity of the Kalman filter, and the INS positioning result is corrected to obtain the error state quantity. The posterior mean of and its covariance matrix ;
[0018] The nominal state The predicted value of and error state The posterior mean of Merge to obtain the combined positioning result of this epoch .
[0019] In one possible implementation, the nominal state quantity The predicted value of and error state The covariance matrix of The prediction formula is as follows:
[0020]
[0021] in, is the nominal state quantity The predicted value of (·) is the nominal state transfer function of the system; is the error state quantity; is the acceleration observed by the inertial sensor; is the angular velocity observed by the inertial sensor; is the time interval between two epochs; is the nominal state posterior value of the previous epoch; is the error state The predicted value of is the state transfer matrix of the error state quantity; is the error state posterior value of the previous epoch; is the error state covariance matrix of the system; is the covariance matrix posterior of the previous epoch; Assign a matrix to the system noise; is the system noise;
[0022] Among them, the system nominal state transfer function and the error state transfer matrix The nominal state quantity can be expressed as follows and error state The recursive formula is determined as follows:
[0023]
[0024]
[0025] in, is the position prediction quantity; is the position posterior of the previous epoch; is the velocity posterior of the previous epoch; is the time interval between two epochs; is the rotation matrix from the local coordinate system to the global coordinate system obtained in the previous epoch; is the acceleration measured by the accelerometer; is the accelerometer bias posterior of the previous epoch; is the local gravitational acceleration; is the speed prediction; is the attitude prediction quantity expressed in quaternion; is the attitude posterior of the previous epoch expressed as a quaternion; {} is the attitude quaternion corresponding to the rotation vector; is the angular velocity measured by the gyroscope; is the posterior value of the angular velocity deviation of the previous epoch; is the acceleration deviation prediction; is the predicted amount of angular velocity deviation; is the predicted amount of position error; is the position error posterior of the previous epoch; is the velocity error posterior of the previous epoch; is the speed error prediction; is the attitude error posterior of the previous epoch; is the acceleration bias error posterior of the previous epoch; Predict noise for velocity error; is the attitude error prediction; is the posterior value of the angular velocity deviation error of the previous epoch; Predict noise for attitude error; is the acceleration deviation error prediction; Predict noise for acceleration bias error; is the angular velocity deviation error prediction; is the angular velocity bias error prediction noise.
[0026] In a possible implementation, when the visual sensor collects multiple target visual tags, determining the visual positioning result according to the absolute position and posture of the device to be positioned includes:
[0027] According to a weighted fusion method based on detection distance, the absolute position and posture of the device to be positioned determined based on each target visual tag are fused to obtain a visual positioning result, and a standard deviation of the visual positioning result is determined.
[0028] In a possible implementation, the identifying and processing the tag information to determine the relative position relationship between the visual sensor and the target visual tag includes:
[0029] Performing nonlinear mapping adjustment on the RGB value of the original visual label image corresponding to the label information to obtain a modified visual label image;
[0030] Based on the Apriltag detection algorithm, the original visual label image and the modified visual label image are subjected to secondary detection to obtain visual label corner point detection results and label number recognition results; wherein the secondary detection includes: grayscale image, Gaussian filtering, binarization, union-find clustering, boundary segmentation, quadrilateral detection, and decoding;
[0031] The relative position relationship between the visual sensor and the collected visual label is determined according to the visual label corner detection result, the label number recognition result, the preset visual label size and the visual sensor internal parameter.
[0032] In a possible implementation, performing nonlinear mapping adjustment on the RGB value of the original visual label image corresponding to the label information to obtain the modified visual label image includes:
[0033] The RGB values of the original visual label image are mapped and adjusted according to the following nonlinear mapping formula:
[0034]
[0035] Among them, the parameters Defined as:
[0036] ;
[0037] is the RGB value of the original visual label image; To correct the RGB value of the visual label image; is the lower limit threshold of the input; is the upper threshold of the input amount; Output lower threshold; Output volume upper threshold.
[0038] In a possible implementation, determining the relative position relationship between the visual sensor and the collected visual label according to the visual label corner detection result, the label number recognition result, the preset visual label size and the visual sensor internal parameter includes:
[0039] According to the preset visual label size and the relationship between the corner points in the image coordinate system and the label coordinate system, the homography matrix is solved using the four pairs of corner points of the visual label corner point detection result; wherein the relationship between the corner points in the image coordinate system and the label coordinate system is as follows:
[0040]
[0041] in, and is the coordinate in the image coordinate system; and Coordinates of the label coordinate system is the camera intrinsic parameter matrix; is the first column of the rotation matrix; is the second column of the rotation matrix; is the displacement vector of the origin of the two coordinate systems;
[0042] According to the orthogonality of the camera intrinsic parameter matrix and the rotation matrix, the position and pose of the visual tag in the camera coordinate system are obtained to determine the relative position and pose relationship between the visual sensor and the collected visual tag.
[0043] In a possible implementation, performing coordinate transformation according to the relative posture relationship between the visual sensor and the target visual tag, the absolute posture of the target visual tag, and the relative posture relationship between the visual sensor and the device to be positioned to determine the absolute posture of the device to be positioned includes:
[0044] Coordinate conversion is performed based on the following conversion formula:
[0045]
[0046]
[0047] in, and is the relative position and posture of the visual sensor and the device to be positioned; and is the position and posture of the target visual label in the camera coordinate system; and is the absolute position of the target visual label; is the absolute position of the device to be located; and is the position and posture of the visual label in the coordinate system of the device to be positioned.
[0048] In a second aspect, an embodiment of the present application provides a positioning device based on a visual tag map, wherein the device to be positioned includes an inertial sensor and a visual sensor arranged on the body of the device to be positioned; the positioning device includes:
[0049] A first pose determination module is used to obtain the label information of the target visual label collected by the visual sensor, identify the label information to determine the relative pose relationship between the visual sensor and the target visual label, and identify the label information to determine the label code of the target visual label;
[0050] A second posture determination module is used to determine the absolute posture of the target visual tag according to the tag code of the target visual tag and the visual tag map;
[0051] A third posture determination module is used to perform coordinate transformation according to the relative posture relationship between the visual sensor and the target visual tag, the absolute posture of the target visual tag, and the relative posture relationship between the visual sensor and the device to be positioned, determine the absolute posture of the device to be positioned, and determine the visual positioning result according to the absolute posture of the device to be positioned;
[0052] A positioning module, used to construct a Kalman filter observation quantity based on the visual positioning result and the vehicle motion constraint, correct the INS positioning result, and obtain a combined positioning result of this epoch;
[0053] The INS positioning result is obtained by state recursive prediction using error state Kalman filtering based on the combined positioning result of the previous epoch and the acceleration and angular velocity collected by the inertial sensor.
[0054] In a third aspect, an embodiment of the present application provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the steps of the method described in the first aspect or any possible implementation of the first aspect are implemented.
[0055] In a fourth aspect, an embodiment of the present application provides a computer-readable storage medium, which stores a computer program. When the computer program is executed by a processor, it implements the steps of the method described in the first aspect or any possible implementation method of the first aspect.
[0056] The embodiment of the present application provides a positioning method and device based on a visual label map, which identifies and processes the label information of the target visual label collected by the visual sensor, determines the relative position and posture relationship between the visual sensor and the target visual label, and determines the label code of the target visual label. On this basis, the absolute position and posture of the target visual label is solved in combination with the label code of the target visual label and the visual label map. The relative position and posture relationship between the visual sensor and the target visual label, the absolute position and posture of the target visual label, and the relative position and posture relationship between the visual sensor and the device to be positioned are transformed into coordinates to determine the absolute position and posture of the device to be positioned. Furthermore, according to the absolute position and posture of the device to be positioned, a visual positioning result is obtained. In addition, in order to improve the positioning accuracy, the visual positioning result is combined with the vehicle motion constraint, the observation quantity of the Kalman filter is constructed, and the INS positioning result is corrected to obtain the combined positioning result of this epoch. In an embodiment of the present application, a visual tag map is constructed by low-cost, easy-to-identify, and highly robust visual tags. The relative position between the camera and the tag can be obtained through a single observation, and then the absolute positioning result is obtained, thereby correcting the accumulated error of INS and adapting to the requirement of limiting the accumulated error of INS positioning in different scenarios, thereby obtaining a higher-precision positioning result in a non-GNSS environment. BRIEF DESCRIPTION OF THE DRAWINGS
[0057] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0058] Figure 1 It is a flowchart of an implementation of a positioning method based on a visual tag map provided in an embodiment of the present application;
[0059] Figure 2 is a schematic diagram of a visual label provided by an embodiment of the present application;
[0060] Figure 3 This is a schematic diagram of a scenario for setting a visual tag provided by an embodiment of the present application;
[0061] Figure 4 is a schematic diagram of a scenario for setting a visual tag provided by another embodiment of the present application;
[0062] Figure 5 It is a schematic diagram of the Apriltag detection algorithm provided in an embodiment of the present application performing secondary detection on visual tags;
[0063] Figure 6is a flowchart of an implementation of a positioning method based on a visual tag map provided in an embodiment of the present application.
[0064] Figure 7 An embodiment of the present application provides a positioning verification device;
[0065] Figure 8 is a schematic diagram of positioning results of a positioning verification device provided by an embodiment of the present application;
[0066] Fig. 9 This is a schematic diagram of a positioning verification result provided by an embodiment of the present application;
[0067] Fig.10 It is a structural schematic diagram of a positioning device based on a visual tag map provided in an embodiment of the present application;
[0068] Fig.11 It is a schematic diagram of the structure of an electronic device provided in one embodiment of the present application. DETAILED DESCRIPTION
[0069] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.
[0070] The terms "first", "second", etc. in the specification and claims of the embodiments of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the embodiments of the present application described here. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions.
[0071] Unless otherwise specified, the term "plurality" means two or more. The character " / " indicates that the objects before and after are in an "or" relationship. For example, A / B means: A or B. The term "and / or" is a description of the association relationship between objects, indicating that there can be three relationships. For example, A and / or B means: A or B, or, A and B.
[0072] The words used in this application are only used to describe the embodiments and are not used to limit the claims. As used in the description of the embodiments and claims, unless the context clearly indicates otherwise, the singular forms "a", "an" and "the" are intended to include the plural forms as well. Similarly, the term "and / or" as used in this application refers to any and all possible combinations of one or more associated listings. In addition, when used in this application, the term "comprise" and its variants "comprises" and / or including (comprising) refer to the existence of stated features, wholes, steps, operations, elements, and / or components, but do not exclude the existence or addition of one or more other features, wholes, steps, operations, elements, components and / or groups of these. In the absence of more restrictions, the elements defined by the sentence "including one..." do not exclude the existence of other identical elements in the process, method or device including the elements.
[0073] In this application, each embodiment may focus on the differences from other embodiments, and the same or similar parts between the embodiments may refer to each other. For the methods, products, etc. disclosed in the embodiments, if they correspond to the method part disclosed in the embodiments, then the relevant parts may refer to the description of the method part.
[0074] The present invention discloses a vehicle INS and vision combined positioning method based on a visual tag map, the purpose of which is to solve the problem that INS cannot independently perform long-term accurate positioning in a GNSS-free environment. By introducing a visual sensor and a visual tag map, the visual tag of a known absolute position is observed, and the image contrast adjustment algorithm and the multi-tag weighted fusion positioning algorithm are combined to obtain an absolute positioning result. At the same time, the error state Kalman filter assisted by the vehicle motion constraint is fused with the INS positioning result to achieve INS and vision combined positioning in a GNSS-free environment. In an embodiment of the present application, in order to achieve the positioning of the device to be positioned, the device to be positioned needs to include at least an inertial sensor and a visual sensor arranged on its body. The inertial sensor can detect the roll angle, pitch angle, yaw angle and acceleration of the device to be positioned. Optionally, the inertial sensor includes a gyroscope and an accelerometer.
[0075] The positioning solution provided by the present invention has the following advantages: the visual tag map is constructed using low-cost, easy-to-identify, and highly robust visual tags, so that the relative position between the camera and the tag can be determined in a single observation, and then the absolute positioning result of the device to be positioned can be obtained. The visual sensor is compact and low-cost, and can provide rich environmental information. For example, the visual sensor can use a monocular camera to simplify the configuration of the visual sensor. This will help correct the INS cumulative error and achieve high-precision positioning in a non-GNSS environment.
[0076] The device to be positioned can be an autonomous driving vehicle, an unmanned delivery logistics vehicle, an unmanned delivery robot, etc. When used for positioning of autonomous driving vehicles, it is of great significance to reduce traffic accidents, improve traffic efficiency, and accelerate the application of autonomous driving technology.
[0077] In order to make the purpose, technical solutions and advantages of the present application clearer, specific embodiments will be described below in conjunction with the accompanying drawings.
[0078] Figure 1 is a flowchart of a positioning method based on a visual tag map provided in an embodiment of the present application, such as Figure 1 As shown, the method comprises the following steps:
[0079] S101, obtaining the label information of the target visual label collected by the visual sensor, identifying and processing the label information to determine the relative posture relationship between the visual sensor and the target visual label, and identifying and processing the label information to determine the label code of the target visual label.
[0080] When visual tags are arranged in the target area, the visual tags need to be encoded and a tag map is constructed based on the tag encoding results so that during the positioning process, the approximate position of the device to be located in the tag map can be determined based on the recognition results of the visual tags by the visual sensor.
[0081] The label code can be displayed in the form of a one-dimensional barcode or a two-dimensional code, or displayed in the form of numbers and text on a visual label for easy identification and processing. The embodiment of the present application is mainly described in the form of a two-dimensional code.
[0082] S102, determining the absolute position and posture of the target visual tag according to the tag encoding of the target visual tag and the visual tag map.
[0083] The positioning method based on the visual tag map provided in the embodiment of the present application is mainly executed by the controller of the device to be positioned or the vehicle navigation system. The execution body is responsible for integrating the information of the visual sensor and the inertial sensor to achieve positioning.
[0084] like Figure 2 is a schematic diagram of a visual label according to a specific embodiment. Figure 3 and Figure 4 The following are schematic diagrams of scenes according to different embodiments. The visual tags are set at positions that can be captured by the visual sensor of the device to be positioned according to the specific application scenario.
[0085] In this context, there are multiple visual tags for positioning in the environment around the device to be positioned. Depending on the location of the device to be positioned, some positioning tags are collected by the visual sensor. The target visual tag can be all tags collected by the visual sensor at the current moment, or the tag of the complete tag image collected at the current moment, or the tag collected at the current moment that is relatively close to the visual sensor.
[0086] Before positioning, a label map needs to be constructed for the environment with positioning labels. Before constructing the label map, each visual label is mechanically encoded and the absolute position of each positioning label in the environment is determined. The image of each visual label can reflect the unique encoding information and absolute positioning information to ensure accurate positioning after recognition.
[0087] During implementation, Figure 2 As shown, it is necessary to establish a device coordinate system based on the device to be positioned, a sensor coordinate system based on the visual sensor, and a tag coordinate system based on the visual tag. Then, the relative position and posture relationship between the visual sensor and the target visual tag is determined based on the position and size relationship of the target visual tag in the sensor coordinate system and the tag coordinate system.
[0088] S103, performing coordinate transformation according to the relative posture relationship between the visual sensor and the target visual tag, the absolute posture of the target visual tag, and the relative posture relationship between the visual sensor and the device to be positioned, determining the absolute posture of the device to be positioned, and determining the visual positioning result according to the absolute posture of the device to be positioned;
[0089] like Figure 3 As shown in FIG, the visual sensor is often set at the front end or rear end of the device to be positioned. Figure 3 As shown, the visual sensor is usually installed at the front or rear end of the device to be located. Taking a vehicle as an example, the center of the rear axle of the vehicle can better represent the position of the entire vehicle, while the front camera or rear camera can only reflect the local position information of the vehicle. When it is necessary to determine the position of the vehicle, the center of the rear axle of the vehicle is usually used as the reference, rather than the front camera or rear camera. Therefore, after determining the relative pose of the target visual label and the visual sensor and the absolute pose of the target visual label, it is necessary to further determine the absolute pose of the device to be located based on the relative pose relationship between the visual sensor and the device to be located. As shown in Figure 3 As shown, the coordinates are established based on the center of the rear wheel axle of the vehicle, and the coordinate axis is x b , y b andz b ; Establish coordinates based on the camera, the coordinate axis is x c , y c and z c ; Establish coordinates based on visual labels, and the coordinate axes are x t , y t and z t .
[0090] In the specific implementation process, it is necessary to preset the relative posture relationship between the center of gravity (or other points representing the position of the device to be positioned) and the visual sensor in the system of the device to be positioned, so that after obtaining the absolute posture of the target visual tag, the coordinate conversion can be performed according to the preset relative posture relationship to obtain the absolute posture of the device to be positioned. This visual positioning method has higher accuracy and reliability.
[0091] S104, constructing the observation quantity of Kalman filter with the visual positioning result and the vehicle motion constraint, correcting the INS positioning result, and obtaining the combined positioning result of the current epoch; wherein the INS positioning result is based on the combined positioning result of the previous epoch and the acceleration and angular velocity collected by the inertial sensor, and is obtained by state recursive prediction using the error state Kalman filter.
[0092] The INS positioning result depends on the combined positioning results of the previous epoch and the acceleration and angular velocity data obtained by inertial sensors (such as gyroscopes and accelerometers). The INS positioning result is obtained by state recursive prediction through error state Kalman filtering. This process is continuously executed in each epoch to achieve real-time update of the vehicle's motion state.
[0093] The combined positioning method proposed in the embodiment of the present application can obtain the relative position relationship between the visual sensor and the visual tag with only one observation. On this basis, according to the visual tag map, the absolute positioning result of the vehicle can be obtained. Compared with the traditional positioning method based on recurrent neural network, this method does not need to accumulate multiple frames of images for calculation, thereby improving the positioning efficiency. At the same time, this method avoids the gradual amplification of the positioning error based on the image recognition result during the accumulation process, further improving the positioning accuracy.
[0094] In the positioning and navigation applications of autonomous vehicles, by correcting the INS positioning results in real time, the vehicle can more accurately perceive its own position in complex environments, thereby improving driving safety and stability.
[0095] In this embodiment, the label information of the target visual label collected by the visual sensor is identified and processed to determine the relative position and posture relationship between the visual sensor and the target visual label, and to determine the label code of the target visual label. On this basis, the label code of the target visual label and the visual label map are combined to solve the absolute position and posture of the target visual label. The relative position and posture relationship between the visual sensor and the target visual label, the absolute position and posture of the target visual label and the relative position and posture relationship between the visual sensor and the device to be positioned are transformed into coordinates to determine the absolute position and posture of the device to be positioned. Further, according to the absolute position and posture of the device to be positioned, the visual positioning result is obtained. In addition, in order to improve the positioning accuracy, the visual positioning result is combined with the vehicle motion constraint, the observation amount of the Kalman filter is constructed, and the INS positioning result is corrected to obtain the combined positioning result of this epoch. In the embodiment of the present application, the visual label map is constructed by low-cost, easy-to-identify, and highly robust visual labels, and the relative position and posture between the camera and the label can be obtained by one observation, thereby obtaining the absolute positioning result, realizing the correction of the INS cumulative error, and adapting to the restriction requirements of the INS positioning cumulative error in different scenarios, so as to obtain a higher-precision positioning result in a non-GNSS environment.
[0096] In summary, the combined positioning method based on Kalman filtering proposed in the embodiment of the present application improves positioning accuracy and real-time performance while reducing the possibility of error accumulation, which is of great significance to the development of autonomous driving technology. In addition, the method is also applicable to other automatic delivery fields or automatic navigation fields.
[0097] In a possible implementation, in step S104, the recursive prediction process of the INS positioning result includes:
[0098] The actual state quantity corresponding to the combined positioning result of the previous epoch of the device to be positioned Nominal state quantity and error state Two state variables; among them, ;
[0099] According to the acceleration and angular velocity collected by the inertial sensor, the nominal state quantity and error state Recursively, we get the nominal state quantity The predicted value of , and according to the system noise and the state transition matrix Get the error state of the device to be positioned The covariance matrix of .
[0100] Among them, the real state quantity corresponding to the combined positioning result of the previous epoch of the device to be positioned is divided into two parts: the nominal state quantity and the error state quantity. , refers to the real state of the device to be positioned in the actual application environment. , represents the positioning result of the device to be positioned under ideal conditions. Error state quantity It indicates the positioning error caused by various factors during the actual positioning process.
[0101] In a specific embodiment, the nominal state vector There are 16 dimensions in total, which are the position, velocity, and orientation quaternion of the northeast celestial direction, as well as the three-axis deviations of the accelerometer and gyroscope; the error state vector There are 15 dimensions in total, which are the northeast celestial position error, velocity error, rotation vector error, and the errors of the three-axis measurement deviation of the accelerometer and gyroscope, respectively, as follows:
[0102]
[0103]
[0104] In this embodiment, the INS positioning system performs recursive prediction on the positioning result of the device to be positioned, thereby achieving more accurate positioning in a continuously updated state, and providing reliable positioning information for the device to be positioned.
[0105] In a possible implementation, in step S104, the observation quantity of the Kalman filter is constructed using the visual positioning result and the vehicle motion constraint, and the INS positioning result is corrected to obtain the combined positioning result of the current epoch, including:
[0106] The Kalman filter observation is constructed based on the visual positioning result and the vehicle motion constraint, and the INS positioning result is corrected to obtain the error state quantity. The posterior mean of and its covariance matrix ;
[0107] The nominal state The predicted value of and error state The posterior mean of Merge to obtain the combined positioning result of this epoch .
[0108] Among them, the visual positioning results and vehicle motion constraints are used to construct the observation quantity of the Kalman filter, aiming to correct the INS positioning results and obtain the combined positioning results of this epoch. In the correction process, the error state quantity is obtained The posterior mean of and its covariance matrix , which aims to reduce the error between the positioning result and the actual positioning.
[0109] Among them, for ground vehicles, special conditions such as tire side slip are not considered. Under normal driving conditions, the lateral velocity and longitudinal velocity in the coordinate system of its own carrier are both 0. In the specific implementation process, this property is used to construct vehicle motion constraints to correct errors in the combined positioning system.
[0110] In one possible implementation, the Kalman filter observation for .
[0111] In this embodiment, the combined positioning result obtained by integrating the information of visual positioning and vehicle motion constraints is more accurate than the single INS positioning result. In addition, the positioning accuracy is improved while the error is reduced by cyclic positioning, and the accumulated error of INS is continuously corrected, so that the navigation system can still maintain a high accuracy in complex environments.
[0112] In one possible implementation, the nominal state quantity The predicted value of and error state The covariance matrix of The prediction formula is as follows:
[0113] .
[0114] in, is the nominal state quantity The predicted value of (·) is the nominal state transfer function of the system; is the error state quantity; is the acceleration observed by the inertial sensor; is the angular velocity observed by the inertial sensor; is the time interval between two epochs; is the nominal state posterior value of the previous epoch; is the error state The predicted value of is the state transfer matrix of the error state quantity; is the error state posterior value of the previous epoch; is the error state covariance matrix of the system; is the covariance matrix posterior of the previous epoch; Assign a matrix to the system noise; is the system noise;
[0115] Among them, the system nominal state transfer function and the error state transfer matrix The nominal state quantity can be expressed as follows and error state The recursive formula is determined as follows:
[0116]
[0117]
[0118] in, is the position prediction quantity; is the position posterior of the previous epoch; is the velocity posterior of the previous epoch; is the time interval between two epochs; is the rotation matrix from the local coordinate system to the global coordinate system obtained in the previous epoch; is the acceleration measured by the accelerometer; is the accelerometer bias posterior of the previous epoch; is the local gravitational acceleration; is the speed prediction; is the attitude prediction quantity expressed in quaternion; is the attitude posterior of the previous epoch expressed as a quaternion; {} is the attitude quaternion corresponding to the rotation vector; is the angular velocity measured by the gyroscope; is the posterior value of the angular velocity deviation of the previous epoch; is the acceleration deviation prediction; is the predicted amount of angular velocity deviation; is the predicted amount of position error; is the position error posterior of the previous epoch; is the velocity error posterior of the previous epoch; is the speed error prediction; is the attitude error posterior of the previous epoch; is the acceleration bias error posterior of the previous epoch; Predict noise for velocity error; is the attitude error prediction; is the posterior value of the angular velocity deviation error of the previous epoch; Predict noise for attitude error; is the acceleration deviation error prediction; Predict noise for acceleration bias error; is the angular velocity deviation error prediction; is the angular velocity bias error prediction noise.
[0119] Correspondingly, in step S104, the observation quantity of Kalman filtering is constructed based on the visual positioning result and the vehicle motion constraint, and the process of correcting the INS positioning result is as follows:
[0120]
[0121]
[0122]
[0123] in, is the gain matrix, is the covariance matrix predicted value, is the observation matrix, is the observation noise matrix, is the posterior value of the error state at this epoch, is the observed value, is the prediction of the observed value.
[0124] After the above correction processing, the corrected system error state quantity is obtained and its covariance matrix , then the nominal state quantity and error state Merge and reset the error state parameters:
[0125]
[0126]
[0127] Among them, the variance reset matrix :
[0128] .
[0129] Among them, the nominal state quantity and error state After the merging, the error state parameters are reset to complete the correction for the next epoch.
[0130] In a possible implementation, when the visual sensor collects multiple target visual tags, in step S103, the visual positioning result is determined according to the absolute position and posture of the device to be positioned, including:
[0131] According to the weighted fusion method based on the detection distance, the absolute position and posture of the device to be positioned determined based on the visual labels of each target are fused to obtain the visual positioning result, and the standard deviation of the visual positioning result is determined.
[0132] Among them, considering that Apriltags of different sizes will be used in actual applications, the distance factor is designed:
[0133]
[0134] in, , are the size of the reference label and the size of the i-th label respectively; is the relative distance between the i-th tag and the device to be located;
[0135] Based on the distance factor, the positioning results are fused and the corresponding standard deviation is constructed:
[0136]
[0137]
[0138] in, is the absolute positioning result of the device to be positioned obtained by the i-th Apriltag : is the distance factor of the i-th Apriltag;
[0139] Constructing the measurement matrix Used for subsequent Kalman filter updates and solving the measurement matrix by chain rule :
[0140] .
[0141] In a specific embodiment, the visual positioning update is performed based on the above relationship.
[0142] The observation quantity of Kalman filter is:
[0143]
[0144] Measurement equation It is expressed as:
[0145]
[0146] The corresponding measurement matrix It is expressed as:
[0147]
[0148] Among them, the components are:
[0149]
[0150] In a specific embodiment, the vehicle motion constraint is updated based on the above relationship.
[0151] In a possible implementation, in step S101, identifying and processing the tag information to determine the relative position relationship between the visual sensor and the target visual tag includes:
[0152] Perform nonlinear mapping adjustment on the RGB value of the original visual label image corresponding to the label information to obtain a corrected visual label image;
[0153] Based on the Apriltag detection algorithm, the original visual label image and the modified visual label image are re-detected to obtain the visual label corner detection result and label number recognition result; the re-detection includes: grayscale image, Gaussian filtering, binarization, union-find clustering, boundary segmentation, quadrilateral detection, and decoding;
[0154] According to the visual label corner detection results, label number recognition results, preset visual label size and visual sensor internal parameters, the relative position relationship between the visual sensor and the collected visual label is determined.
[0155] In the specific implementation process, the color of the visual label is usually selected to have a high contrast so that it is easier to identify under a white background. However, the white background may become darker under poor lighting conditions, affecting the recognition effect of the label. Therefore, in the embodiment of the present application, before performing label recognition, the RGB value of the image is adjusted using nonlinear mapping technology to enhance the contrast between the visual label and the white background, which can effectively avoid the influence of the darker white background on the label boundary and improve the accuracy of the detection algorithm. Figure 5 As shown in the figure, after the RGB value adjustment is completed, the second-generation Apriltag detection algorithm is used to perform secondary detection on the original image and the corrected image of the visual tag, which can improve the contrast of the visual tag image, thereby improving the accuracy and reliability of image processing. Figure 5 The white area in the center of the visual label is the QR code area.
[0156] In a possible implementation, nonlinear mapping adjustment is performed on the RGB value of the original visual label image corresponding to the label information to obtain a modified visual label image, including:
[0157] The RGB values of the original visual label image are mapped and adjusted according to the following nonlinear mapping formula:
[0158]
[0159] Among them, the parameters Defined as:
[0160] ;
[0161] is the RGB value of the original visual label image; To correct the RGB value of the visual label image; is the lower limit threshold of the input; is the upper threshold of the input amount; Output lower threshold; Output volume upper threshold.
[0162] Among them, the input is the RGB value of the original visual label image; the output is the RGB value of the corrected visual label image.
[0163] In different embodiments, the manners of performing nonlinear mapping adjustment on the RGB values of the original visual label image corresponding to the label information are different.
[0164] In a possible implementation, for each channel of the color image, the above mapping formula is used to adjust the value of each pixel.
[0165] In this embodiment, since there are differences in pixel information in each channel, for each channel of the color image, the mapping accuracy can be improved by adjusting the value of each pixel using the above mapping formula.
[0166] In a possible implementation, for each channel of the color image, the above mapping formula is used to adjust the value of each pixel in the target range of the label, where the target range corresponds to part of the label content and the target range at least includes the corner points of the label. Optionally, the target range is all points on the four sides of the label that are outside the set distance from the center area, and the area surrounded by these points. Set the distance to the length of the label side. .
[0167] In this embodiment, in view of the differences in pixel information in each channel, for each channel of the color image, the above mapping formula is used to adjust the value of each pixel within the label target range, which can improve the mapping accuracy and the mapping efficiency.
[0168] In a possible implementation, the relative position relationship between the visual sensor and the collected visual label is determined according to the visual label corner detection result, the label number recognition result, the preset visual label size and the visual sensor internal parameter, including:
[0169] According to the preset visual label size and the relationship between the corner points in the image coordinate system and the label coordinate system, the homography matrix is solved using the four pairs of corner points of the visual label corner point detection results; among which, the relationship between the corner points in the image coordinate system and the label coordinate system is as follows:
[0170]
[0171] in, and is the coordinate in the image coordinate system; and Coordinates of the label coordinate system is the camera intrinsic parameter matrix; is the first column of the rotation matrix; is the second column of the rotation matrix; is the displacement vector of the origin of the two coordinate systems;
[0172] According to the orthogonality of the camera intrinsic parameter matrix and the rotation matrix, the position and pose of the visual tag in the camera coordinate system are obtained to determine the relative position and pose relationship between the visual sensor and the collected visual tag.
[0173] In a possible implementation, in step S103, coordinate transformation is performed according to the relative pose relationship between the visual sensor and the target visual tag, the absolute pose of the target visual tag, and the relative pose relationship between the visual sensor and the device to be positioned, so as to determine the absolute pose of the device to be positioned, including:
[0174] Coordinate conversion is performed based on the following conversion formula:
[0175]
[0176]
[0177] in, and is the relative position and posture of the visual sensor and the device to be positioned; and is the pose of the target visual label in the camera coordinate system; and is the absolute pose of the target visual label; is the absolute position of the device to be located; and is the position and posture of the visual label in the coordinate system of the device to be positioned.
[0178] Figure 6 is a flowchart of a positioning method based on a visual tag map provided in an embodiment of the present application, such as Figure 6 As shown, a visual tag map is constructed in advance. Based on the improved AprilTag detection algorithm, the visual tag image acquired by the visual sensor is detected to determine the visual tag corner detection result and tag number recognition result. Then, the absolute position of the tag is determined based on the visual tag map and tag number recognition result ( and ), and determine the relative position of the visual sensor and the device to be positioned based on the visual label corner point detection results ( and ). According to the absolute pose of the target visual label ( and ) and the relative position relationship between the visual sensor and the device to be positioned ( and ) to perform coordinate transformation to determine the absolute position of the positioning device ( ). When the number of target visual tags acquired by the visual sensor is greater than 1, the absolute position of the device to be positioned determined based on each target visual tag is fused according to the weighted fusion method based on the detection distance to obtain the visual positioning result, and the standard deviation of the visual positioning result is determined ( and At the same time, the observation of Error State Kalman Filter (ESKF) is constructed with vehicle motion constraints ( ), use the visual positioning results and vehicle motion constraints to update the ESKF measurement and obtain the posterior value of the error state ( ), further combined with the combined positioning result based on the previous epoch and the acceleration and angular velocity collected by the inertial sensor (Inertial Measurement Unit, IMU) and ) to perform ESKF state recursive prediction to obtain the ESKF state prediction ( ) to merge the ESKF states and obtain the final posterior state quantity ( ). Among them, IMU consists of two units: an accelerometer and a gyroscope. The accelerometer collects acceleration, and the gyroscope collects angular velocity.
[0179] It should be understood that the size of the serial numbers of the steps in the above embodiments does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.
[0180] In order to verify the effectiveness of the method provided in the embodiment of the present application, based on Figure 7 The experimental vehicle shown in Figure 4 The experiment was conducted in the scene shown. The experimental vehicle is equipped with a laser radar, a camera, a control unit (with a built-in inertial measurement unit) and a drive motor. The solution provided in the embodiment of the present application is used to conduct a positioning experiment based on the visual tag information collected by the camera, and the laser radar SLAM results are used for verification.
[0181] Figure 8 The positioning result of the solution provided in the embodiment of the present application is shown in FIG. 1 , wherein the multiple circles are the position distribution of the visual tags determined by the Apriltag detection algorithm, and the range covered by the multiple circles is constructed. Figure 8 During the experiment, Figure 7 The experimental vehicle shown is Figure 4 In the scenario shown, the vehicle travels counterclockwise for one circle. The positioning trajectory result obtained based on the method provided in the embodiment of the present application is as follows: Figure 8 The ESKF curve in the figure shows the reference positioning trajectory obtained based on the laser SLAM method. Figure 8 True curve.
[0182] Fig. 9 is a schematic diagram of positioning error. Fig. 9 The LiDAR SLAM result with cm-level accuracy is used as the true value. At the i-th epoch (n epochs in total), the positioning result and the true value result obtained by the proposed method are recorded as , , positioning error It is expressed as:
[0183]
[0184] The positioning error Error-T curve during the entire experiment is as follows: Fig. 9 As shown, the ordinate corresponds to the positioning error , the horizontal axis is the time axis.
[0185] The root mean square error (RMSE) is used as a statistical indicator of positioning accuracy and is calculated using the following formula:
[0186]
[0187] The root mean square of the positioning error of this experiment obtained by the above formula is 0.1363 m. Therefore, it is proved that the solution provided by the embodiment of the present application can obtain a very high positioning accuracy in a non-GNSS environment.
[0188] The following is an embodiment of the device of the present application. For details not described in detail, please refer to the corresponding method embodiment described above.
[0189] Fig.10 is a structural diagram of a positioning device based on a visual tag map provided in an embodiment of the present application, such as Fig.10 For the sake of convenience, only the parts related to the embodiment of the present application are shown. Fig.10 As shown, the device comprises:
[0190] The first pose determination module 1001 is used to obtain the tag information of the target visual tag collected by the visual sensor, identify the tag information to determine the relative pose relationship between the visual sensor and the target visual tag, and identify the tag information to determine the tag code of the target visual tag;
[0191] A second posture determination module 1002 is used to determine the absolute posture of the target visual tag according to the tag code of the target visual tag and the visual tag map;
[0192] The third posture determination module 1003 is used to perform coordinate transformation according to the relative posture relationship between the visual sensor and the target visual tag, the absolute posture of the target visual tag, and the relative posture relationship between the visual sensor and the device to be positioned, determine the absolute posture of the device to be positioned, and determine the visual positioning result according to the absolute posture of the device to be positioned;
[0193] A positioning module 1004 is used to construct a Kalman filter observation quantity based on the visual positioning result and the vehicle motion constraint, and to correct the INS positioning result to obtain a combined positioning result of this epoch;
[0194] The INS positioning result is obtained by state recursive prediction using error state Kalman filtering based on the combined positioning result of the previous epoch and the acceleration and angular velocity collected by the inertial sensor.
[0195] In the embodiment of the present application, the label information of the target visual label collected by the visual sensor is identified and processed to determine the relative position and posture relationship between the visual sensor and the target visual label, and to determine the label code of the target visual label. On this basis, the label code of the target visual label and the visual label map are combined to solve the absolute position and posture of the target visual label. The relative position and posture relationship between the visual sensor and the target visual label, the absolute position and posture of the target visual label and the relative position and posture relationship between the visual sensor and the device to be positioned are transformed into coordinates to determine the absolute position and posture of the device to be positioned. Further, according to the absolute position and posture of the device to be positioned, the visual positioning result is obtained. In addition, in order to improve the positioning accuracy, the visual positioning result is combined with the vehicle motion constraint, the observation amount of the Kalman filter is constructed, and the INS positioning result is corrected to obtain the combined positioning result of this epoch. In the embodiment of the present application, the visual label map is constructed by low-cost, easy-to-identify, and highly robust visual labels, and the relative position and posture between the camera and the label can be obtained by one observation, thereby obtaining the absolute positioning result, realizing the correction of the INS cumulative error, and adapting to the restriction requirements of the INS positioning cumulative error in different scenarios, so as to obtain a higher-precision positioning result in a non-GNSS environment.
[0196] Fig.11 Schematic diagram of the structure of an electronic device provided by an embodiment of the present application. Fig.11As shown, the electronic device 11 of this embodiment includes: a processor 110, a memory 111, and a computer program 112 stored in the memory 111 and executable on the processor 110. When the processor 110 executes the computer program 112, the steps in the above-mentioned positioning method embodiments based on the visual tag map are implemented, for example Figure 1 Alternatively, when the processor 110 executes the computer program 112, the functions of the modules / units in the above-mentioned device embodiments are realized, for example Fig.10 The functions of each module are shown.
[0197] Exemplarily, the computer program 112 may be divided into one or more modules / units, which are stored in the memory 111 and executed by the processor 110 to complete the present application. The one or more modules / units may be a series of computer program instruction segments that can complete specific functions, and the instruction segments are used to describe the execution process of the computer program 112 in the electronic device 11. For example, the computer program 112 may be divided into Fig.10 The modules shown.
[0198] The electronic device 11 may be a computing device such as a desktop computer, a notebook, a PDA, or a cloud server. The electronic device 11 may include, but is not limited to, a processor 110 and a memory 111. Those skilled in the art will appreciate that Fig.11 It is only an example of the electronic device 11 and does not constitute a limitation of the electronic device 11. It may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the electronic device may also include input and output devices, network access devices, buses, etc.
[0199] The processor 110 may be a central processing unit (CPU), or other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or any conventional processor, etc.
[0200] The memory 111 may be an internal storage unit of the electronic device 11, such as a hard disk or memory of the electronic device 11. The memory 111 may also be an external storage device of the electronic device 11, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the electronic device 11. Further, the memory 111 may also include both an internal storage unit of the electronic device 11 and an external storage device. The memory 111 is used to store the computer program and other programs and data required by the electronic device. The memory 111 may also be used to temporarily store data that has been output or is to be output.
[0201] The technicians in the relevant field can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of this application. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.
[0202] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.
[0203] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this application.
[0204] In the embodiments provided in the present application, it should be understood that the disclosed devices / electronic devices and methods can be implemented in other ways. For example, the device / electronic device embodiments described above are merely schematic. For example, the division of the modules or units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.
[0205] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
[0206] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.
[0207] If the integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present application implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, it can implement the steps of the above-mentioned positioning method embodiments based on the visual tag map. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or device that can carry the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal and software distribution medium, etc.
[0208] The embodiments described above are only used to illustrate the technical solutions of the present application, rather than to limit them. Although the present application has been described in detail with reference to the aforementioned embodiments, a person skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. Such modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present application, and should all be included in the protection scope of the present application.
Claims
1. A positioning method based on a visual label map, characterized in that: The device to be positioned includes an inertial sensor and a visual sensor arranged on the body of the device to be positioned; the visual sensor is arranged at the front end or the rear end of the device to be positioned; Taking the center of the rear wheel axle of the vehicle as a reference, presetting the relative position and posture relationship between the visual sensor and the device to be positioned in the system of the device to be positioned; The positioning method includes: Acquire the tag information of the target visual tag collected by the visual sensor, perform recognition processing on the tag information to determine the relative position relationship between the visual sensor and the target visual tag, and perform recognition processing on the tag information to determine the tag code of the target visual tag; Determining an absolute position of the target visual tag according to the tag encoding of the target visual tag and the visual tag map; Performing coordinate transformation according to the relative posture relationship between the visual sensor and the target visual tag, the absolute posture of the target visual tag, and the relative posture relationship between the visual sensor and the device to be positioned, determining the absolute posture of the device to be positioned, and determining the visual positioning result according to the absolute posture of the device to be positioned; Constructing the observation quantity of Kalman filter with the visual positioning result and the vehicle motion constraint, correcting the positioning result of the inertial navigation system INS, and obtaining the combined positioning result of this epoch; The INS positioning result is obtained by state recursive prediction using error state Kalman filtering based on the combined positioning result of the previous epoch and the acceleration and angular velocity collected by the inertial sensor; The step of identifying and processing the tag information to determine the relative position relationship between the visual sensor and the target visual tag includes: Performing nonlinear mapping adjustment on the RGB value of the original visual label image corresponding to the label information to obtain a modified visual label image; Based on the Apriltag detection algorithm, the original visual label image and the modified visual label image are subjected to secondary detection to obtain visual label corner point detection results and label number recognition results; wherein the secondary detection includes: grayscale image, Gaussian filtering, binarization, union-find clustering, boundary segmentation, quadrilateral detection, and decoding; Determine the relative position relationship between the visual sensor and the collected visual label according to the visual label corner detection result, the label number recognition result, the preset visual label size and the visual sensor internal parameter; The recursive prediction process of the INS positioning result includes: The real state quantity corresponding to the combined positioning result of the previous epoch of the device to be positioned Nominal state quantity and error state Two state variables; among them, ; According to the acceleration and angular velocity collected by the inertial sensor, the nominal state quantity and error state Recursively, we get the nominal state quantity The predicted value of , and according to the system noise and the state transition matrix Obtain the error state quantity of the device to be positioned The covariance matrix of ; The method of constructing the observation quantity of Kalman filtering based on the visual positioning result and the vehicle motion constraint, and correcting the INS positioning result to obtain the combined positioning result of the current epoch includes: The visual positioning result and the vehicle motion constraint are used to construct the observation quantity of the Kalman filter, and the positioning result of the inertial navigation system INS is corrected to obtain the error state quantity The posterior mean of and its covariance matrix ; The nominal state The predicted value of and error state The posterior mean of Merge to obtain the combined positioning result of this epoch ; When the visual sensor collects multiple target visual tags, determining the visual positioning result according to the absolute position and posture of the device to be positioned includes: According to the weighted fusion method based on the detection distance, the absolute position and posture of the device to be positioned determined based on the visual labels of each target are fused to obtain the visual positioning result, and the standard deviation of the visual positioning result is determined; Among them, the design distance factor is: in, , are the size of the reference label and the size of the i-th label respectively; is the relative distance between the i-th tag and the device to be located; Based on the distance factor, the positioning results are fused and the corresponding standard deviation is constructed: in, is the fusion positioning result; is the absolute positioning result of the device to be positioned obtained by the i-th Apriltag : Constructing the measurement matrix Used for subsequent Kalman filter updates and solving the measurement matrix by chain rule : Update the visual positioning based on the above relationship; The observation quantity of Kalman filter is: Measurement equation It is expressed as: The corresponding measurement matrix It is expressed as: Among them, the components are: The vehicle motion constraints are updated based on the above relationship.
2. The positioning method based on visual tag map according to claim 1 is characterized in that: Nominal state quantity The predicted value of and error state The covariance matrix of The prediction formula is as follows: in, is the nominal state quantity The predicted value of (·) is the nominal state transfer function of the system; is the error state quantity; is the acceleration observed by the inertial sensor; is the angular velocity observed by the inertial sensor; is the time interval between two epochs; is the nominal state posterior value of the previous epoch; is the error state The predicted value of is the state transfer matrix of the error state quantity; is the error state posterior value of the previous epoch; is the error state covariance matrix of the system; is the covariance matrix posterior of the previous epoch; Assign a matrix to the system noise; is the system noise; Among them, the system nominal state transfer function and the error state transfer matrix The nominal state quantity can be expressed as follows and error state The recursive formula is determined as follows: in, is the position prediction quantity; is the position posterior of the previous epoch; is the velocity posterior of the previous epoch; is the time interval between two epochs; is the rotation matrix from the local coordinate system to the global coordinate system obtained in the previous epoch; is the acceleration measured by the accelerometer; is the accelerometer bias posterior of the previous epoch; is the local gravitational acceleration; is the speed prediction; is the attitude prediction quantity expressed in quaternion; is the attitude posterior of the previous epoch expressed as a quaternion; {} is the attitude quaternion corresponding to the rotation vector; is the angular velocity measured by the gyroscope; is the posterior value of the angular velocity deviation of the previous epoch; is the acceleration deviation prediction; is the predicted amount of angular velocity deviation; is the predicted amount of position error; is the position error posterior of the previous epoch; is the velocity error posterior of the previous epoch; is the speed error prediction; is the attitude error posterior of the previous epoch; is the acceleration bias error posterior of the previous epoch; Predict noise for velocity error; is the attitude error prediction; is the posterior value of the angular velocity deviation error of the previous epoch; Predict noise for attitude error; is the acceleration deviation error prediction; Predict noise for acceleration bias error; is the angular velocity deviation error prediction; is the angular velocity bias error prediction noise.
3. The positioning method based on visual tag map according to claim 1 is characterized in that: When the visual sensor collects multiple target visual tags, determining the visual positioning result according to the absolute position and posture of the device to be positioned includes: According to a weighted fusion method based on detection distance, the absolute position and posture of the device to be positioned determined based on each target visual tag are fused to obtain a visual positioning result, and a standard deviation of the visual positioning result is determined.
4. The positioning method based on visual tag map according to claim 1 is characterized in that: The non-linear mapping adjustment is performed on the RGB value of the original visual label image corresponding to the label information to obtain a modified visual label image, including: The RGB values of the original visual label image are mapped and adjusted according to the following nonlinear mapping formula: Among them, the parameters Defined as: ; is the RGB value of the original visual label image; To correct the RGB value of the visual label image; is the lower limit threshold of the input; is the upper threshold of the input amount; Output lower threshold; Output volume upper threshold.
5. The positioning method based on visual tag map according to claim 1 is characterized in that: Determining the relative position relationship between the visual sensor and the collected visual label according to the visual label corner detection result, the label number recognition result, the preset visual label size and the visual sensor internal parameter includes: According to the preset visual label size and the relationship between the corner points in the image coordinate system and the label coordinate system, the homography matrix is solved using the four pairs of corner points of the visual label corner point detection result; wherein the relationship between the corner points in the image coordinate system and the label coordinate system is as follows: in, and is the coordinate in the image coordinate system; and Coordinates of the label coordinate system is the camera intrinsic parameter matrix; is the first column of the rotation matrix; is the second column of the rotation matrix; is the displacement vector of the origin of the two coordinate systems; According to the orthogonality of the camera intrinsic parameter matrix and the rotation matrix, the position and pose of the visual tag in the camera coordinate system are obtained to determine the relative position and pose relationship between the visual sensor and the collected visual tag.
6. The positioning method based on visual tag map according to claim 1 is characterized in that: The step of performing coordinate transformation according to the relative posture relationship between the visual sensor and the target visual tag, the absolute posture of the target visual tag, and the relative posture relationship between the visual sensor and the device to be positioned to determine the absolute posture of the device to be positioned includes: Coordinate conversion is performed based on the following conversion formula: in, and is the relative position and posture of the visual sensor and the device to be positioned; and is the position and posture of the target visual label in the camera coordinate system; and is the absolute position of the target visual label; is the absolute position of the device to be located; and is the position and posture of the visual label in the coordinate system of the device to be positioned.
7. A positioning device based on a visual tag map for executing the positioning method based on a visual tag map according to any one of claims 1 to 6, characterized in that: The device to be positioned includes an inertial sensor and a visual sensor arranged on the body of the device to be positioned; the visual sensor is arranged at the front end or the rear end of the device to be positioned; Taking the center of the rear wheel axle of the vehicle as a reference, presetting the relative position and posture relationship between the visual sensor and the device to be positioned in the system of the device to be positioned; The positioning device comprises: A first pose determination module is used to obtain the label information of the target visual label collected by the visual sensor, identify the label information to determine the relative pose relationship between the visual sensor and the target visual label, and identify the label information to determine the label code of the target visual label; A second posture determination module is used to determine the absolute posture of the target visual label according to the label code of the target visual label and the visual label map; A third posture determination module is used to perform coordinate transformation according to the relative posture relationship between the visual sensor and the target visual tag, the absolute posture of the target visual tag, and the relative posture relationship between the visual sensor and the device to be positioned, determine the absolute posture of the device to be positioned, and determine the visual positioning result according to the absolute posture of the device to be positioned; A positioning module, used to construct a Kalman filter observation quantity based on the visual positioning result and the vehicle motion constraint, correct the INS positioning result, and obtain a combined positioning result of this epoch; The INS positioning result is obtained by state recursive prediction using error state Kalman filtering based on the combined positioning result of the previous epoch and the acceleration and angular velocity collected by the inertial sensor.
