Adaptive fusion of visual-inertial positioning method and device of single frequency RTK

Abstract

The application relates to a visual inertial positioning method and device of adaptive fusion of single-frequency RTK, first state data of each key frame in a sliding window under a local coordinate system, i.e. a trajectory under the local coordinate system, is obtained by using a nonlinear optimization model, basic visual inertial positioning is realized, then a trajectory under a global coordinate system is calculated by using a single-frequency RTK algorithm, transformation data are obtained, finally, RTK residuals are calculated, and the single-frequency RTK algorithm is adaptively fused into the nonlinear optimization model according to whether there is a fixed solution, the best estimated measurement state information is obtained by minimizing the residual term including the RTK residuals, and the visual inertial positioning of adaptive fusion of the single-frequency RTK is realized. The RTK residuals are adaptively fused into the nonlinear optimization model based on a factor graph according to whether there is a fixed solution of the single-frequency RTK algorithm, since the RTK does not require a loop in a moving path, the problem of low universality existing in the existing loop detection is solved or partially solved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of positioning, in particular to a visual-inertial positioning method and device fusing single-frequency real-time kinematic (RTK) adaptively. BACKGROUND

[0002] Autonomous positioning is a key technology in the field of mobile robots, and accurate and real-time positioning results are the most basic prerequisite for subsequent perception, planning and control. The autonomous positioning of mobile robots relies on the sensors used. To improve overall performance, multi-sensor fusion is a common and effective means. The visual-inertial positioning method based on monocular camera and inertial measurement unit (IMU) fusion has the advantages of high local accuracy, good real-time calculation, small size, light weight, low power consumption and low cost, and has been widely used in the field of mobile robots.

[0003] Chinese patent application No. CN202210737648.1 discloses a multi-mobile robot cooperative positioning method and system based on visual-IMU fusion. The method comprises the following steps: step S1, calibrating the parameters of the camera and the IMU; step S2, calculating the IMU pre-integral and aligning the data; step S3, extracting image ORB features and matching the features according to the BRIEF descriptor; step S4, establishing an optimization problem and solving to obtain the optimal pose estimation of the robot terminal in the local coordinate system; step S5, sending the optimized pose and the extracted image ORB feature data to the server end; step S6, performing loop detection using the bag-of-words model; and step S7, performing pose graph optimization in the global unified coordinate and sending the optimization result to the robot terminal.

[0004] The above-mentioned application uses the bag-of-words model for loop detection, and finally outputs the pose of the multi-robot in the global unified coordinate system. However, the visual-inertial positioning method has the problem of global cumulative drift, and the positioning error will gradually accumulate over time until it completely fails. A common solution is to add an additional loop detection step, but this method has high computational cost and will affect real-time performance. Moreover, it should be noted that in actual applications, the path of the mobile robot does not necessarily have a loop, which greatly limits the universality of this method. SUMMARY

[0005] The present application is to overcome the defects of the prior art and provide a visual-inertial positioning method and device fusing single-frequency real-time kinematic (RTK) adaptively, to solve or partially solve the problem of low universality of existing visual-inertial positioning methods.

[0006] The object of the present application can be achieved by the following technical solutions:

[0007] One aspect of the present invention provides an adaptive fusion single-frequency RTK visual-inertial localization method, comprising the following steps:

[0008] Multiple sliding measurements are performed on multiple keyframes based on a sliding window. During a single measurement, the state data of each keyframe in the sliding window under the local coordinate system is obtained using a nonlinear optimization model based on factor graphs to achieve visual inertial positioning. The trajectory under the global coordinate system is calculated using a single-frequency RTK algorithm. The transformation data between the local coordinate system and the global coordinate system under this measurement is calculated. The measurement state information of this measurement is constructed based on the state data and the transformation data.

[0009] Based on the measurement state information from multiple measurements, the RTK residual is calculated and adaptively incorporated into the nonlinear optimization model according to whether the single-frequency RTK algorithm has a fixed solution. By minimizing the residual term including the RTK residual, the best estimated measurement state information is obtained, thereby realizing the visual inertial positioning of adaptive fusion single-frequency RTK.

[0010] As a preferred technical solution, the acquisition of the RTK residual includes the following steps:

[0011] Determine whether the single-frequency RTK algorithm has a fixed solution. If so, calculate the loosely coupled position constraint residual by solving the least squares problem based on the transformed data, the fixed solution obtained by the RTK algorithm, and the coordinates in the local coordinate system obtained by visual inertial positioning. If not, the RTK residual includes the tightly coupled Doppler measurement residual and the double-difference pseudorange measurement residual.

[0012] As a preferred technical solution, the process of calculating the transformation data between the local coordinate system and the global coordinate system includes the following steps:

[0013] Determine whether the single-frequency RTK algorithm has a fixed solution. If so, align the trajectories in the local and global coordinate systems by registering the corresponding point sets, and calculate the rotation and translation transformation terms between the coordinate systems as the transformation data. If not, first use coarse position coordinates and Doppler measurements to align the rotation, then use double-difference pseudorange measurements to align the translation, and calculate the rotation and translation transformation terms between the coordinate systems as the transformation data.

[0014] As a preferred technical solution, the residual term further includes at least one of point feature reprojection residual, line feature reprojection residual, and IMU pre-integration residual.

[0015] As a preferred technical solution, the calculation of the trajectory in the global coordinate system using the single-frequency RTK algorithm specifically includes the following steps:

[0016] The rover receiver discards satellites with elevation angles less than a preset angle and selects one satellite with the highest elevation angle from both GPS and BDS as a reference satellite.

[0017] The rover receiver subtracts its own measurement of a satellite from the measurement of the same satellite from the base station receiver to obtain the inter-station single-difference pseudorange measurement and single-difference carrier phase measurement of the satellite. It subtracts the single-difference measurement of all other satellites in each constellation from the single-difference measurement of its reference satellite to obtain the double-difference measurement.

[0018] Based on double-difference measurement, a single-frequency RTK algorithm is executed to obtain the trajectory of the position with 3 degrees of freedom in the global coordinate system.

[0019] As a preferred technical solution, the process of obtaining the state data of each key frame in the sliding window under the local coordinate system using a factor graph-based nonlinear optimization model specifically includes the following steps:

[0020] Acquire multiple frames of images, extract and track point features, select key frames based on preset key frame selection rules, and calculate IMU pre-integration terms;

[0021] Pose information is acquired based on visual SfM and aligned with the pre-integrated terms of the IMU (Inertial Measurement Unit);

[0022] A nonlinear optimization model based on factor graphs is constructed. By minimizing all factors in a sliding window, the optimal estimates of vehicle pose, velocity, point feature position, and IMU bias are obtained and used as the state data to achieve visual inertial positioning.

[0023] As a preferred technical solution, the key frame selection process also includes line feature extraction. By extracting and tracking point features and line features, key frames are selected based on preset key frame selection rules.

[0024] As a preferred technical solution, the optimally estimated measurement state information includes vehicle pose, speed, point feature position, IMU deviation, RTK receiver clock drift, and transformation between the global coordinate system and the local coordinate system.

[0025] As a preferred technical solution, the local coordinate system is the initial pose of the IMU coordinate system, and the global coordinate system is the northeast-sky coordinate system.

[0026] Another aspect of the present invention provides an adaptive fusion single-frequency RTK visual inertial positioning device, including an industrial control computer and a monocular camera, an IMU, an RTK rover receiver, and a receiving antenna connected to the industrial control computer. The monocular camera and the IMU are stacked and located on the longitudinal centerline of the vehicle body. The RTK receiving antenna is connected to the RTK rover receiver. The industrial control computer is used to acquire multiple frames of images from the monocular camera and filter multiple key frames. Based on a sliding window, multiple sliding measurements are performed on the multiple key frames to construct the measurement state information of the current measurement. Based on the measurement state information of multiple measurements, the RTK residual is calculated and adaptively integrated into a nonlinear optimization model according to whether the single-frequency RTK algorithm has a fixed solution. By minimizing the residual term including the RTK residual, the best estimated measurement state information is obtained, thereby realizing adaptive fusion single-frequency RTK visual inertial positioning.

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

[0028] (1) High versatility and wide applicability: Unlike existing schemes that reduce global cumulative drift by adding extra loop closure detection steps, this invention first utilizes the state data of each key frame in the sliding window under the local coordinate system obtained by the nonlinear optimization model, i.e., the trajectory under the local coordinate system, to achieve basic visual inertial positioning. Then, the RTK algorithm is used to calculate the trajectory under the global coordinate system to obtain transformation data. Finally, the RTK residual is calculated and adaptively integrated into the nonlinear optimization model according to whether the single-frequency RTK algorithm has a fixed solution. By minimizing the residual term including the RTK residual, the best estimated measurement state information is obtained, thus achieving adaptive fusion of single-frequency RTK visual inertial positioning. By adaptively integrating the RTK residual into the factor graph-based nonlinear optimization model according to whether the single-frequency RTK algorithm has a fixed solution, and then achieving positioning by minimizing the residual term including the RTK residual, since RTK does not require the movement path to have loop closures, this solves or partially solves the problem of low versatility of existing loop closure detection methods.

[0029] (2) High positioning accuracy: When the single-frequency RTK algorithm has a fixed solution, the loosely coupled position constraint residual is used as the RTK residual; otherwise, the tightly coupled Doppler measurement residual and the double-difference pseudorange measurement residual are used as the RTK residual. Different initialization methods and fusion methods are selected by the state of the single-frequency RTK solution. This strategy can maximize the advantages of single-frequency RTK, thereby improving the positioning accuracy and real-time performance of this method. Attached Figure Description

[0030] Figure 1 This is a flowchart illustrating the implementation steps of the adaptive fusion single-frequency RTK visual-inertial localization method in this embodiment.

[0031] Figure 2This is a schematic diagram of the sensor device installation in the embodiment;

[0032] Figure 3 This is a schematic diagram illustrating the initialization between single-frequency RTK and visual inertial.

[0033] Figure 4 The flowchart illustrates the adaptive initialization process between single-frequency RTK and visual inertial communication.

[0034] Figure 5 A schematic diagram illustrating the factor graph optimization between single-frequency RTK and visual inertial mapping;

[0035] Figure 6 This is a flowchart illustrating the adaptive fusion process between single-frequency RTK and visual inertial systems. Detailed Implementation

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

[0037] Example 1

[0038] This embodiment addresses the cumulative drift problem in existing visual-inertial localization methods, as well as the high computational cost and lack of versatility of traditional loop closure detection methods. It provides an adaptive visual-inertial localization method that fuses single-frequency RTK, with the implementation process as follows: Figure 1 As shown, the specific steps are as follows:

[0039] Step S1: Equipment installation.

[0040] Mount the monocular camera, IMU, and RTK receiving antenna on the roof of the car, such as Figure 2 As shown in the diagram, the camera and IMU are mounted in approximately overlapping positions, located on the longitudinal centerline of the vehicle body. There are two RTK receiving antennas, symmetrically arranged on either side of the longitudinal centerline, aligned with the camera and IMU. The left antenna is used for positioning, and the right antenna for orientation; in this embodiment, only the left positioning antenna is used. The RTK rover receiver is installed in the trunk of the vehicle, and an industrial computer, also installed in the trunk, serves as the computing platform. The vehicle's own battery powers all the equipment.

[0041] The origin of the camera coordinate system is located at the optical center. The x-axis is defined to point directly to the right of the vehicle, the y-axis to point directly below the vehicle, and the z-axis to point directly in front of the vehicle. The origin of the IMU coordinate system is located at the IMU's centroid. The x-axis is defined to point directly to the right, the y-axis to point directly in front, and the z-axis to point directly upwards. The origin of the RTK positioning antenna coordinate system is located at the antenna phase center, and the directions of each axis can be arbitrarily defined. The transformation matrix T between the camera coordinate system and the IMU coordinate system is obtained through calibration. c b And the translation p between the RTK positioning antenna coordinate system and the IMU coordinate system r b The IMU coordinate system is used uniformly as the positioning coordinate system, also known as the vehicle coordinate system or the vehicle coordinate system. The RTK base station receiver and its antenna are installed in an open area near the vehicle's driving area, and obtain their own precise position coordinates by using a static multi-frequency RTK measurement algorithm implemented with the help of a local base station service provider.

[0042] Step S2: Data Acquisition.

[0043] The camera captures images directly in front of the vehicle during its movement at a frequency of 20 Hz; the IMU captures the vehicle's linear acceleration and angular velocity at a frequency of 200 Hz. The RTK rover receiver acquires Doppler measurements, pseudorange measurements, and single-frequency carrier phase measurements between itself and the satellites, while simultaneously receiving pseudorange measurements and single-frequency carrier phase measurements between itself and the satellites from the base station receiver, both at a frequency of 1 Hz. The satellites include two constellations: the US GPS and the Chinese BeiDou, specifically at GPS L1C and BDS-2B1I frequencies. All measurement data is transmitted to an industrial control computer running Ubuntu, which uses ROS (Robot Operating System) for data communication.

[0044] Step S3: Implement visual inertial positioning function based on camera and IMU.

[0045] The purpose of this step is to fuse the camera and IMU to obtain the vehicle's trajectory in a local coordinate system, thereby achieving basic visual inertial positioning functionality.

[0046] (1) Preprocessing of camera and IMU

[0047] First, the Shi-Tomasi algorithm is used to extract point features from the first frame of the image, with a maximum number of features set to 300. Then, the Lucas-Kanade sparse optical flow algorithm is used to track point features in the next frame. If the number of tracked point features is less than 50, the Shi-Tomasi algorithm is used again to extract point features from that frame, ensuring that the total number of point features in each frame is between 100 and 300. Keyframes are selected from the acquired images, based on the criteria that the number of tracked point features in a frame is less than 20, or the average disparity of the tracked point features is greater than 10 pixels. All IMU measurements between each two selected keyframes are pre-integrated to obtain the IMU pre-integration term.

[0048] (2) Initialization of camera and IMU

[0049] The purpose of this step is to align the camera (vehicle) pose obtained in this step based on pure vision SfM (Structure From Motion) with the IMU pre-integration term from the previous step. First, the algorithm checks the correspondence of point features between the current keyframe and all previous keyframes in the sliding window. If the number of tracked point features exceeds 30 and the average disparity exceeds 20 pixels, the pose transformation between the current and previous frames is obtained based on epipolar geometry constraints. Next, triangulation is performed on all co-view point features in the two frames to recover the depth of the point features and obtain their 3D position coordinates. Then, the PnP (Perspective-n-Point) algorithm is used to obtain the pose of all keyframes in the window. Then, the reprojection residuals of all point features are minimized to obtain the optimal camera pose and point feature positions. Since the vehicle coordinate system is represented using the IMU coordinate system, it needs to be converted to the IMU coordinate system. Finally, the pose transformation result is aligned with the IMU pre-integration term to recover the scale of the translation component and obtain the initial values ​​of vehicle velocity, gravity vector, and gyroscope bias.

[0050] (3) Integration of camera and IMU

[0051] A nonlinear optimization model based on factor graphs is constructed. By minimizing all factors (point feature reprojection residual and IMU pre-integration residual) in the sliding window, the optimal estimate of all states (vehicle pose, velocity, point feature position and IMU bias) is obtained, realizing basic visual inertial localization function, also known as visual inertial odometry (VIO), which can obtain the trajectory (6 degrees of freedom pose) of autonomous vehicles in the local world coordinate system.

[0052] Step S4: Preprocessing of single-frequency RTK receiver.

[0053] First, the rover receiver eliminates satellites with elevation angles less than 15 degrees and selects one satellite with the highest elevation angle from both GPS and BDS as reference satellites. Next, the rover receiver subtracts its own measurements of a given satellite from the measurements taken by the base station receiver for the same satellite, obtaining the inter-station single-difference pseudorange and single-difference carrier phase measurements for that satellite. Then, the single-difference measurements of all other satellites within each constellation are subtracted from the single-difference measurements of their reference satellites, yielding double-difference measurements. Finally, based on these double-difference measurements, a single-frequency RTK algorithm is executed to obtain the trajectory (3-DOF position) of the autonomous vehicle in the Global Geocentric-Earth-Fixed (ECEF) coordinate system. This trajectory can then be transformed to the Northeast-Eastern-Upper-Heaven (ENU) coordinate system by setting anchor points.

[0054] Step S5: Adaptive initialization between single-frequency RTK and visual inertial

[0055] The purpose of initialization is to align the trajectory in the World coordinate system obtained in step 3 with the trajectory in the ENU coordinate system obtained in step 4, that is, to solve for the rotation between the World coordinate system and the ENU coordinate system. Peaceful relocation like Figure 3 As shown.

[0056] (1) Case where a fixed solution is obtained by single-frequency RTK

[0057] When a fixed solution is obtained by single-frequency RTK, due to its relatively high accuracy (sub-meter level), the corresponding point set registration method is selected for trajectory alignment.

[0058] Choose the position coordinate point corresponding to the third single-frequency RTK fixed solution as the reference point, define it as the anchor point and denot it as . By establishing an ENU coordinate system at the anchor point, the position coordinates subsequently obtained by the autonomous vehicle from the RTK receiver can be determined. You can then transform from the ECEF coordinate system to the ENU coordinate system using anchor points:

[0059]

[0060] in, It is the rotation matrix from the ECEF coordinate system to the ENU coordinate system, specifically in the form of:

[0061]

[0062] Where lon and lat are the longitude and latitude of the anchor point in the ECEF coordinate system, respectively, which can be directly obtained from the receiver. This can be considered a known quantity. Now consider the position coordinates obtained by the autonomous vehicle from VIO. Transform it from the World coordinate system to the ENU coordinate system:

[0063]

[0064] Theoretically, the position coordinates in the two ENU coordinate systems should be equal. However, due to VIO drift, a scale parameter s also needs to be estimated. In the sliding window, they are modeled as a least-squares problem, with the cost function being:

[0065]

[0066] By solving the above least squares problem, the rotation between the World coordinate system and the ENU coordinate system is obtained. Peaceful relocation

[0067] (2) Case where a fixed solution is not obtained for single-frequency RTK

[0068] When a fixed solution is not obtained for single-frequency RTK, due to its relatively low accuracy (meter-level), a stepwise refinement method is chosen for alignment. That is, first, coarse position coordinates and low-noise Doppler measurements are used for alignment rotation, and then double-difference pseudorange measurements are used for alignment translation.

[0069] First, without relying on any prior information, the average position coordinates of the window are obtained by combining all double-difference pseudorange measurements within the sliding window. This average position coordinate is defined as the anchor point and denoted as . Similarly, establish an ENU coordinate system at the anchor point.

[0070] (a) Alignment rotation using Doppler measurements

[0071] Vehicle speed from VIO As a priori, by minimizing all Doppler measurement residuals within the window, the rotation from the World coordinate system to the ENU coordinate system can be obtained.

[0072]

[0073] in,

[0074]

[0075]

[0076] At time k, the car body b k and satellites j The Doppler measurement residuals between them, Q is the covariance matrix, and M is the total number of satellites. These are Doppler measurements, where λ is the satellite carrier wavelength. This refers to satellite velocity. Anchor point coordinates. Used for calculation And the unit vector from the body to the satellite Because they are not sensitive to location, the accuracy can be calculated using coarse anchor point coordinates. These are satellite position coordinates. It's satellite clock drift. This refers to receiver clock drift. All satellite-related parameters can be obtained from ephemeris data; only [the ephemeris data is specified in the formula]. and unknown.

[0077] (b) Align translation using double-difference pseudorange measurement

[0078] Vehicle position coordinates from VIO As a priori, by minimizing all double-difference pseudorange measurement residuals within the window, the translation from the World coordinate system to the ENU coordinate system can be obtained. And can further refine the rough anchor point coordinates calculated at the beginning of step S5-(2):

[0079]

[0080] in,

[0081]

[0082]

[0083] At time k, the car body b k and base station ref for GPS satellites and The residuals of double-difference pseudorange measurement It is a reference satellite. BDS satellite and The residuals of double-difference pseudorange measurement It is a reference satellite. and These are the double-difference pseudorange measurements from the two constellations. and These are the single-difference geometric distances between the reference station (ref) and the satellites in the two constellations. All parameters related to the satellites and the reference station are known; only... and unknown.

[0084] At this point, the transformation between the global ENU coordinate system and the local World coordinate system has been fully calibrated, and the initialization between single-frequency RTK and visual inertial coordinates is complete.

[0085] Step S6: Adaptive fusion between single-frequency RTK and visual inertial.

[0086] This invention adds factors related to single-frequency RTK to the nonlinear optimization model based on factor graphs described in steps S3-(3), such as... Figure 5 As shown, by minimizing all factors within the sliding window (point feature reprojection residual, IMU pre-integration residual, and residuals related to single-frequency RTK), the optimal estimate of all states (vehicle pose, velocity, point feature position and IMU bias, as well as RTK receiver clock drift and transformation between the World coordinate system and the ENU coordinate system) is obtained. The states within the sliding window... for:

[0087]

[0088] in,

[0089]

[0090]

[0091] n+1 and m+1 are the total number of keyframes and point features in the sliding window, respectively. l It is the inverse depth of the l-th point feature when it is first observed. It is the transformation matrix from the World coordinate system to the ENU coordinate system. and These are rotation and translation, respectively. k This represents the state of the k-th keyframe, including the position of the autonomous vehicle in the World coordinate system. attitude and speed and accelerometer deviation b a and gyroscope deviation b ω Since double-difference measurement eliminates the receiver clock bias, it is no longer necessary to estimate δt. k However, since Doppler measurements are still used, it is necessary to estimate the clock drift. It is the same for different zodiac signs.

[0092] Optimal state estimation is a maximum a posteriori (MAP) problem, which seeks to find the state that satisfies the maximum a posteriori probability given all measurements. Assuming all measurements are independent and the noise follows a zero-mean Gaussian distribution, it can be further transformed into minimizing the sum of a series of cost functions, each corresponding to a specific measurement residual. The specific form is as follows:

[0093]

[0094] Where, rmarg It comes from marginalized prior information. This represents the measurement residual of the sensor. Among them, It is the IMU pre-integration residual. It is the point feature reprojection residual. It is the Doppler measurement residual. It is the residual of double-difference pseudorange measurement. This is the RTK fixed solution position constraint residual. M is the total number of tracked satellites, and u and v are the number of satellites tracked in GPS and BDS, respectively. and Each is its own reference satellite. This indicates the calculation of Mahalanobis distance, where Q is the covariance matrix.

[0095] This invention provides an adaptive fusion strategy, such as Figure 6 As shown, different fusion methods can be selected based on the state of the single-frequency RTK, i.e., whether a fixed solution is obtained: when the single-frequency RTK obtains a fixed solution, the loosely coupled position constraint residuals... When a fixed solution is not obtained for single-frequency RTK, the residual of tightly coupled Doppler measurement... and double-difference pseudorange measurement residuals

[0096] (1) Case where a fixed solution is obtained by single-frequency RTK

[0097] When a fixed solution is obtained using single-frequency RTK, the position constraints it provides are loosely coupled. The coordinates of the vehicle body in the ECEF coordinate system obtained from the fixed solution are then used. The following formula can be used to transform to the ENU coordinate system:

[0098]

[0099] Similarly, the vehicle body obtains its position coordinate system from VIO. It can also be transformed to the ENU coordinate system using the following formula:

[0100]

[0101] Therefore, at time k, the position constraint residual from the single-frequency RTK fixed solution is:

[0102]

[0103] (2) Case where a fixed solution is not obtained for single-frequency RTK

[0104] When a single-frequency RTK solution is not obtained, the double-difference pseudorange measurement and Doppler measurement provided by the algorithm are tightly coupled, while the double-difference carrier phase measurement with ambiguity is ignored to avoid the problem of ambiguity fixation and improve algorithm efficiency. The residual models for the Doppler measurement and double-difference pseudorange measurement are as follows:

[0105] (a) Doppler measurement residuals

[0106] At time k, the vehicle body b k and satellites j The Doppler measurement model is as follows:

[0107]

[0108]

[0109] in, Transformation to the World coordinate system can be achieved in the following ways:

[0110]

[0111] Therefore, at time k, the vehicle body b k and satellites j The Doppler measurement residuals between are:

[0112]

[0113]

[0114] (b) Residual of double-difference pseudorange measurement

[0115] The single-frequency RTK algorithm of this invention includes two constellations, GPS and BDS, and selects reference satellites in each constellation. and Taking GPS as an example, at time k, the vehicle body b k Simultaneous tracking of satellites with the base station ref and The double-difference pseudorange measurement model is as follows:

[0116]

[0117]

[0118] in, Transformations between the World coordinate system and the World coordinate system can be achieved through anchor points:

[0119]

[0120] Therefore, at time k, the vehicle body b kand base station ref for GPS satellites and The residual of the double-difference pseudorange measurement is:

[0121]

[0122] Similarly, vehicle body b k and the reference station ref for BDS satellite and The residual of the double-difference pseudorange measurement is:

[0123]

[0124] Thus, based on the adaptive fusion strategy, the residuals related to single-frequency RTK can be selectively incorporated into the nonlinear optimization model to achieve the localization of autonomous vehicles.

[0125] This invention provides a visual-inertial localization method that integrates single-frequency RTK, solving the cumulative drift problem of current visual-inertial localization methods and the high computational cost and lack of versatility associated with traditional loop closure detection steps. It achieves localized high accuracy and global drift-free localization. Different initialization and fusion methods are selected based on the state of the single-frequency RTK. This strategy maximizes the advantages of single-frequency RTK, thereby improving the localization accuracy and real-time performance of this method. The single-frequency RTK integrated in this invention balances accuracy and cost, with small, lightweight, and low-power hardware that is easy to install and use, matching the advantages of visual-inertial devices and making it highly suitable for widespread adoption.

[0126] Example 2

[0127] Compared to Example 1, this example differs in step S3 by extracting line features in addition to point features from the image. This better addresses point feature degradation and improves overall performance. The steps for adding the line features are as follows:

[0128] (1) Line features are extracted based on the EDlines algorithm. Then, by setting a length threshold, the line segments that are too short are removed. By setting angle and distance thresholds, the line segments that are too close are filtered and the shorter ones are removed.

[0129] (2) Line feature matching is performed based on LBD descriptors, and then incorrect matches are eliminated by setting a proportional verification step, as well as distance and angle thresholds.

[0130] (3) In the nonlinear optimization model based on factor graph described in steps S3-(3) of Example 1, add a factor related to the line feature, namely the line feature reprojection residual:

[0131]

[0132] in, The matching result of the h-th line feature in the j-th frame image. and These are the coordinates of the starting point and the ending point, respectively. Let d be the reprojection of the h-th line feature in the j-th frame image, and d represent the distance from the point to the line.

[0133] Example 3

[0134] See Figure 2 This embodiment provides an adaptive fusion single-frequency RTK visual inertial positioning device, including an industrial control computer and a monocular camera, an IMU, an RTK rover receiver, and a receiving antenna connected to the industrial control computer. The monocular camera and IMU are stacked and located on the longitudinal centerline of the vehicle body. The RTK receiving antenna is connected to the RTK rover receiver. The industrial control computer is used to acquire multiple frames of images from the monocular camera and filter multiple key frames. Based on a sliding window, multiple sliding measurements are performed on multiple key frames to construct the measurement state information of this measurement. Based on the measurement state information of multiple measurements, the RTK residual is calculated and adaptively integrated into a nonlinear optimization model according to whether the single-frequency RTK algorithm has a fixed solution. By minimizing the residual term including the RTK residual, the best estimated measurement state information is obtained, thereby realizing adaptive fusion single-frequency RTK visual inertial positioning.

[0135] The camera and IMU are mounted in approximately overlapping positions, located on the longitudinal centerline of the vehicle body. There are two RTK receiving antennas, symmetrically arranged on either side of the vehicle's longitudinal centerline, aligned with the camera and IMU. The left antenna is used for positioning, and the right antenna for orientation; in this embodiment, only the left positioning antenna is used. The RTK rover receiver is installed in the trunk of the vehicle, and an industrial computer, also installed in the trunk, serves as the computing platform. The vehicle's own battery powers all the equipment.

[0136] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should all be covered within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A visual-inertial localization method based on adaptive fusion of single-frequency RTK, characterized in that, Includes the following steps: Multiple sliding measurements are performed on multiple keyframes based on a sliding window. During a single measurement, the state data of each keyframe in the sliding window under the local coordinate system is obtained using a nonlinear optimization model based on factor graphs to achieve visual inertial positioning. The trajectory under the global coordinate system is calculated using a single-frequency RTK algorithm. The transformation data between the local coordinate system and the global coordinate system under this measurement is calculated. The measurement state information of this measurement is constructed based on the state data and the transformation data. Based on measurement state information from multiple measurements, RTK residuals are calculated and adaptively incorporated into the nonlinear optimization model depending on whether the single-frequency RTK algorithm has a fixed solution. By minimizing the residual term including the RTK residuals, the best estimated measurement state information is obtained, thus achieving adaptive fusion of single-frequency RTK visual-inertial localization. The acquisition of the RTK residual includes the following steps: Determine whether the single-frequency RTK algorithm has a fixed solution. If so, based on the transformed data, the fixed solution obtained from the RTK algorithm, and the coordinates in the local coordinate system obtained from visual-inertial positioning, calculate the loosely coupled position constraint residual by solving a least-squares problem, and use this as the RTK residual. If not, the RTK residual includes the tightly coupled Doppler measurement residual and the double-difference pseudorange measurement residual. The process of calculating the transformation data between the local coordinate system and the global coordinate system includes the following steps: Determine whether the single-frequency RTK algorithm has a fixed solution. If so, align the trajectories in the local and global coordinate systems by registering the corresponding point sets, and calculate the rotation and translation transformation terms between the coordinate systems as the transformation data. If not, first use coarse position coordinates and Doppler measurements to align the rotation, then use double-difference pseudorange measurements to align the translation, and calculate the rotation and translation transformation terms between the coordinate systems as the transformation data.

2. The adaptive fusion single-frequency RTK visual-inertial localization method according to claim 1, characterized in that, The residual term also includes at least one of point feature reprojection residual, line feature reprojection residual, and IMU pre-integration residual.

3. The adaptive fusion single-frequency RTK visual-inertial localization method according to claim 1, characterized in that, The specific steps for calculating the trajectory in the global coordinate system using the single-frequency RTK algorithm are as follows: The rover receiver discards satellites with elevation angles less than a preset angle and selects one satellite with the highest elevation angle from both GPS and BDS as a reference satellite. The rover receiver subtracts its own measurement of a satellite from the measurement of the same satellite from the base station receiver to obtain the inter-station single-difference pseudorange measurement and single-difference carrier phase measurement of the satellite. It subtracts the single-difference measurement of all other satellites in each constellation from the single-difference measurement of its reference satellite to obtain the double-difference measurement. Based on double-difference measurement, a single-frequency RTK algorithm is executed to obtain the trajectory of the position with 3 degrees of freedom in the global coordinate system.

4. The adaptive fusion single-frequency RTK visual-inertial localization method according to claim 1, characterized in that, The process of obtaining the state data of each keyframe in a sliding window in a local coordinate system using a factor graph-based nonlinear optimization model includes the following steps: Acquire multiple frames of images, extract and track point features, select key frames based on preset key frame selection rules, and calculate IMU pre-integration terms; Pose information is acquired based on visual SfM and aligned with the IMU pre-integration term; A nonlinear optimization model based on factor graphs is constructed. By minimizing all factors in a sliding window, the optimal estimates of vehicle pose, velocity, point feature position, and IMU bias are obtained and used as the state data to achieve visual inertial positioning.

5. The adaptive fusion single-frequency RTK visual-inertial localization method according to claim 4, characterized in that, The keyframe selection process also includes line feature extraction. Keyframes are selected based on preset keyframe selection rules by extracting and tracking point and line features.

6. The visual-inertial localization method based on adaptive fusion single-frequency RTK according to claim 1, characterized in that, The optimal estimated measurement state information includes vehicle pose, velocity, point feature position, IMU bias, RTK receiver clock drift, and transformation between the global and local coordinate systems.

7. The adaptive fusion single-frequency RTK visual-inertial localization method according to claim 1, characterized in that, The local coordinate system is the initial pose of the IMU coordinate system, and the global coordinate system is the northeast-sky coordinate system.

8. An adaptive fusion single-frequency RTK visual-inertial positioning device, characterized in that, To implement the visual inertial positioning method as described in any one of claims 1-7, the device includes an industrial control computer and a monocular camera, an IMU, an RTK rover receiver, and a receiving antenna connected to the industrial control computer. The monocular camera and IMU are stacked and located on the longitudinal centerline of the vehicle body. The RTK receiving antenna is connected to the RTK rover receiver. The industrial control computer is used to acquire multiple frames of images from the monocular camera and filter multiple key frames. Based on a sliding window, multiple sliding measurements are performed on the multiple key frames to construct the measurement state information of the current measurement. Based on the measurement state information of multiple measurements, the RTK residual is calculated and adaptively integrated into the nonlinear optimization model according to whether the single-frequency RTK algorithm has a fixed solution. By minimizing the residual term including the RTK residual, the best estimated measurement state information is obtained, thereby realizing adaptive fusion of single-frequency RTK visual inertial positioning.

Images