Wheel speed-vision-inertia tight coupling positioning method and system

Abstract

The invention discloses a wheel speed-vision-inertia tight coupling positioning method and system, and the method comprises the steps: determining a fusion wheel speed-vision-inertia positioning algorithm framework based on a maximum posteriori estimation and factor graph state estimation theory, carrying out the re-parameterization of ground parameters of a mobile robot, defining a ground motion manifold constraint model, and carrying out the positioning of a wheel speed-vision-inertia. Performing joint optimization on the camera, the IMU, the wheel speed and the ground motion manifold model; pose constraints of a ground robot on a ground motion manifold are modeled by adopting random constraints, pose estimation and global positioning of a mobile platform are realized, finally, a real object mobile platform is established, a real environment data set is acquired in a campus environment, and the effectiveness of an algorithm in the embodiment in the real environment is verified. Experimental verification is carried out on a public data set KAIST and a real vehicle, and the effectiveness and the overall performance of the method are proved.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned vehicle positioning, and particularly to a wheel speed-vision-inertia tightly coupled positioning method and system. Background Art

[0002] Existing VIO algorithms perform well on devices with six degrees of freedom of motion such as unmanned aerial vehicles and handheld devices. However, relying solely on vision and inertial sensors may lead to motion degradation problems such as scale drift, unobservability, and reduced robustness, and the likelihood of ground robots encountering these motion degradation situations is significantly greater than that of unmanned aerial vehicles and other handheld mobile devices. Therefore, recent research has begun to introduce wheel speedometers into VIO systems to further improve system performance.

[0003] Wu, Kejian J et al. demonstrated through observability analysis and experiments in Vins on wheels that when a wheeled robot performs uniformly accelerated motion, the lack of acceleration excitation in the IMU results in unobservable system scale; and in non-rotational motion, the roll, yaw, and pitch angles are all unobservable. Therefore, a tightly coupled non-linear optimization method integrating a camera, IMU, and wheel speedometer was proposed. By integrating the linear and angular velocities of the wheel speedometer and introducing a planar manifold random constraint into the system error function, this method can solve the above-mentioned system unobservability problem. However, since the gravity and bias of the IMU must be initialized, the algorithm does not perform well on mobile platforms. M Quan et al. proposed a tightly coupled probabilistic monocular vision-inertial SLAM system incorporating a wheel speedometer, using a new manifold odometry pre-integration theory to integrate wheel speedometer measurements and gyroscope measurements into relative motion constraints independent of the linearization point, introducing an odometry error term and tightly coupling it to the vision optimization framework. Woosik Lee et al. proposed a vision-inertial wheel odometry system (VIWO) for ground vehicles based on the MSCKF algorithm and an online measurement model of the wheel speedometer to calibrate the internal and external (spatiotemporal) parameters of multiple sensors, and analyzed the observability of the system. Rong Kang et al. proposed VINS-Vehicle to address the problems of difficult initialization and low accuracy of the VINS system and common degradation motions of vehicle motion, such as uniform linear motion or uniform circular motion, and proposed a tightly coupled vision-inertial wheel speedometer odometry system based on sliding window optimization. The proposed method is robust in both textureless underground parking lots and dynamic outdoor environments.

[0004] However, it should be noted that the above methods are not optimized for the manifold characteristics of ground robots. Different from devices such as drones or intelligent headsets that have degrees of freedom of movement in a three-dimensional space environment, ground robots are restricted to move on a manifold (such as the ground) due to their mechanical structure design. This ground manifold constraint provides an opportunity to derive supplementary mathematical constraints to improve computational performance. Lee et al. proposed a monocular visual odometry method and an online self-supervised ground plane parameter estimation scheme. The ground parameters are modeled by a quadratic polynomial, as Figure 1 shown, to solve the monocular scale problem by learning ground features and estimating ground parameters. However, the online learning method has a high computational cost, and this appearance-based method does not use probability estimation to describe, which may lead to a decline in long-term positioning accuracy.

[0005] Zuo Xingxing et al. pioneered a low-cost method for robot kinematic constraints based on the instantaneous center of rotation (ICR) model to improve the positioning accuracy of skid-steering robots. At the same time, it is integrated into the visual inertial odometry based on the sliding window bundle adjustment (BA) in a tightly coupled manner. By analyzing the observability of the model, the feasibility of the state estimation of the system is verified. However, these motion constraints are not integrated into a unified manifold representation, and most of them ignore the consideration of measurement noise adjustment, thus reducing the positioning robustness in actual scenarios. Ouyang Ming et al. introduced a planar manifold constraint in the proposed visual-gyro-odometry (VGWO) method, using three-dimensional Euler angles and three-dimensional positions instead of SE(3) to parameterize the robot pose to reduce the drift error of planar motion. However, this method is only verified on a dataset, and its robustness and accuracy are not verified in a real environment, and it is difficult to ensure the real-time performance of the added image semantic segmentation module.

[0006] In summary, the current methods for fusing the wheel speedometer and the VIO system have achieved some results and have a certain improvement in the pose estimation effect. However, these methods are rarely verified on large-scale public datasets with a long time span, and few physical platforms are built for testing. Most of the SLAM research based on ground motion manifold constraints does not integrate motion constraints into a unified manifold representation and ignores the consideration of measurement noise adjustment.

[0007] The current mainstream solution to improve the positioning accuracy of visual SLAM is to fuse the IMU to construct a VIO system. However, deploying the VIO system on a mobile robot still faces many challenges: when moving at a constant speed or with a constant acceleration, the system scale is unobservable due to insufficient IMU excitation, resulting in the degradation or even divergence of the VIO system accuracy; due to the mechanical characteristics of the ground mobile robot itself, noise will be generated during driving due to the unevenness of the ground and the vibration of the vehicle. Summary of the Invention

[0008] In view of this, the purpose of the present invention is to provide a wheel speed-vision-inertial tightly coupled positioning method, which integrates the vision inertial odometer system WVIO of the vehicle wheel speedometer and uses stochastic constraints to model the pose constraints of the ground robot on the ground motion manifold, realizing the pose estimation and global positioning of the mobile platform.

[0009] To achieve the above object, the present invention provides the following technical solutions:

[0010] The wheel speed-vision-inertial tightly coupled positioning method provided by the present invention includes the following steps:

[0011] S1: A sensor measurement value processing module, which is used to obtain the wheel speedometer measurement value, image data, and IMU measurement value;

[0012] S2: A front-end feature initialization module, which obtains the positions of new feature points by extracting FAST corner points and pre-integrates the measurement values of the IMU and the wheel speedometer between two consecutive frames;

[0013] S3: A back-end sliding window optimization module, which is used to jointly optimize all measurement values through a factor graph based on a sliding window.

[0014] Furthermore, the front-end feature initialization module is processed in the following manner:

[0015] S21: Perform wheel speedometer pre-integration calculation on the wheel speedometer measurement value;

[0016] S22: Perform FAST corner point detection and tracking on the image data;

[0017] S23: Perform IMU pre-integration calculation on the IMU measurement value;

[0018] S24: Set up a sliding window for the initialization process.

[0019] Furthermore, the initialization process in the initialization module adopts the same process as VINS-mono and follows some criteria and operation sequences of VINS-mono in feature point initialization. The specific steps are as follows:

[0020] S241: Use only visual information for structure from motion estimation, that is, estimate the pose change of the camera and the three-dimensional structure of the scene by analyzing the movement of points in the image sequence;

[0021] S242: Gyroscope bias correction, which is used to correct the bias error in the gyroscope measurement;

[0022] S243: Initialize the speed and scale, and initialize the speed and scale information of the robot.

[0023] Furthermore, the backend sliding window optimization module in step S3 specifically operates according to the following steps:

[0024] S31: Initialize the pose of the mobile robot and set the initial position and orientation of the robot;

[0025] S32: Initialize the landmarks, select and define some landmarks in the environment for subsequent positioning and map construction;

[0026] S33: Tightly coupled non-linear optimization, using non-linear optimization techniques to minimize the error between the predicted pose and the actual pose;

[0027] S34: Remove outliers, identify and exclude those observation data points that significantly deviate from the model prediction;

[0028] S35: Marginalization, in the sliding window optimization, the process of removing old poses and landmarks from the optimization window.

[0029] Furthermore, the tightly coupled non-linear optimization in step S33 operates according to the following steps:

[0030] S331: IMU residual: Calculate the difference between the IMU measurement and the model prediction;

[0031] S332: Visual residual: Calculate the difference between the visual measurement and the model prediction;

[0032] S333: Wheel odometer residual: Calculate the difference between the wheel odometer measurement and the model prediction.

[0033] Furthermore, the IMU pre-integration calculation in step S23 is carried out according to the following formula:

[0034]

[0035] where, represents the rotation matrix of the odometry coordinate system o of the j-th frame with respect to the world coordinate system w; represents the rotation matrix of the i-th frame coordinate system o with respect to the world coordinate system w; represents the position vector of the origin of the j-th frame coordinate system o in the world coordinate system w; represents the position vector of the origin of the i-th frame coordinate system o in the world coordinate system w; represents the velocity vector of the origin of the coordinate system o in the world coordinate system w at the k-th frame during the time interval from the i-th frame to the j-th frame; represents the angular velocity of the coordinate system o at the k-th frame during the time interval from the i-th frame to the j-th frame.

[0036] Further, the pre-integration calculation of the wheel speedometer in step S21 is performed according to the following formula:

[0037]

[0038] where represents the cumulative rotation matrix of the coordinate system o from i to j from the i-th frame to the j-th frame; represents the position change vector of the coordinate system o from i to j from the i-th frame to the j-th frame; represents the transpose of the rotation matrix of the coordinate system o relative to the world coordinate system ω at the i-th frame; represents the rotation matrix of the coordinate system o relative to the world coordinate system ω at the j-th frame; represents the rotation matrix of the coordinate system o from the i-th frame to the i+1-th frame; represents the rotation matrix of the coordinate system o from the i-th frame to the k-th frame; represents the linear velocity of the coordinate system o at the (k-1)-th frame; represents the linear velocity of the coordinate system o at the k-th frame; represents the angular velocity of the coordinate system o at the (k-1)-th frame; represents the angular velocity of the coordinate system o at the (k-1)-th frame; represents the estimated or measured value of the angular velocity of the world coordinate system ω at the (k-1)-th frame; represents the noise or error term of the linear velocity at the (k-1)-th frame; represents the noise or error term of the linear velocity at the k-th frame; represents the noise or error term of the angular velocity at the k-th frame.

[0039] Further, the factor graph in step S3 is a factor graph model for wheel speed-vision-inertia fusion. The factor graph for wheel speed-vision-inertia fusion is a probabilistic graph model, which represents the relationship between variables through nodes and edges. The nodes represent variables, and the edges represent the constraint relationships between variables. The nodes include

[0040] landmark node: representing the landmark points in the environment, which is used to assist in positioning and map construction;

[0041] pose node: representing the pose of the robot at different time points;

[0042] marginalized factor: representing the pose nodes marginalized in the sliding window optimization process;

[0043] IMU factor: representing the measurement data provided by the inertial measurement unit, which is used to estimate the acceleration and angular velocity of the robot;

[0044] Visual factor: Represents the measurement data provided by the visual sensor, which is used to estimate the relative pose between the robot and the road landmark;

[0045] Wheel speedometer factor: Represents the measurement data provided by the wheel speedometer, which is used to estimate the linear velocity of the robot.

[0046] Furthermore, the marginalization in step S35 is performed in the following manner:

[0047] Batch optimization is adopted, and the global map is solved through bundle adjustment. Considering real-time performance and computational efficiency, it runs in a sliding window, maintains the state variables in a time window of a fixed size, and only optimizes the state variables within the window;

[0048] The information before the marginalization window, and at the same time convert the values corresponding to the marginalized state into prior information.

[0049] The wheel speed-vision-inertial tightly coupled positioning system provided by the present invention includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, the above method is implemented.

[0050] The beneficial effects of the present invention are as follows:

[0051] The wheel speed-vision-inertial tightly coupled positioning method provided by the present invention first determines the positioning algorithm framework for fusing wheel speed-vision-inertial based on the maximum a posteriori estimation and factor graph state estimation theories. Secondly, it re-parameterizes the ground parameters of the mobile robot, defines the ground motion manifold constraint model, and jointly optimizes the camera, IMU, wheel speed, and ground motion manifold models; random constraints are used to model the pose constraints of the ground robot on the ground motion manifold, realizing the pose estimation and global positioning of the mobile platform. Finally, a physical mobile platform is built to collect real environment data sets in the campus environment and verify the effectiveness of the algorithm in this embodiment in the real environment. Through experiments on the public data set KAIST and the actual vehicle respectively, the effectiveness and overall performance of the method are proved.

[0052] Other advantages, objectives, and features of the present invention will be described to some extent in the subsequent description, and to some extent, will be obvious to those skilled in the art based on the study of the following text, or can be learned from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] In order to make the objectives, technical solutions, and beneficial effects of the present invention clearer, the present invention provides the following drawings for illustration.

[0054] Figure 1 It is a ground parameterization modeling method.

[0055] Figure 2 It is an Ackermann four-wheel steering model.

[0056] Figure 3 Wheel speed meter sampling time and pre-integration interval.

[0057] Figure 4 It is the flow chart of the wheel speed-vision-inertia tightly coupled positioning method.

[0058] Figure 5 It is the factor graph model of wheel speed-vision-inertia fusion.

[0059] Figure 6 It is a state variable node.

[0060] Figure 7 It is a marginalization diagram.

[0061] Figure 8 It is the factor graph of the optimization process.

[0062] Figure 9 It is the trajectory comparison of each algorithm in a large scene.

[0063] Figure 10 It is a mobile robot experimental platform.

[0064] Figure 11 It is the data acquisition flow chart.

[0065] Figure 12 It is the data set acquisition route.

[0066] Figure 13 It is the trajectory comparison of the campus data set operation. Specific implementation mode

[0067] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, so that those skilled in the art can better understand the present invention and be able to implement it, but the embodiments cited do not limit the present invention.

[0068] Embodiment 1

[0069] As Figure 2 shown, Figure 2 It is an Ackermann four-wheel steering model. In this embodiment, the vehicle chassis adopts an Ackermann four-wheel steering model, and the vehicle's turning mainly relies on the steering of the front wheels. Assume that the wheel linear velocities measured by the wheel speed meter for the left rear wheel and the right rear wheel are v L and v R, the wheelbase of the vehicle platform is L, the wheel track is W, the vehicle steering angle is θ, the turning radius is R, the rotational speed of the left front wheel is α, and the steering angle of the right front wheel is β. A right-handed coordinate system is used to represent the wheel speed meter coordinate system {O}. Assuming that the z-axis of the coordinate system is perpendicular to the ground, the kinematic inverse solution is used to obtain the steering angle θ of the vehicle:

[0070]

[0071] Among them,

[0072] The forward kinematic model is used to calculate the conversion from wheel speed to overall motion, and we get:

[0073]

[0074] Among them, represents the wheel speed in the x-axis direction in the wheel speed coordinate system; represents the wheel speed in the x-axis direction in the wheel speed coordinate system; represents the angular velocity of the vehicle body around the z-axis direction in the wheel speed coordinate system;

[0075] W represents the wheel track; v R represents the right wheel speed; v L represents the right wheel speed;

[0076] Let

[0077] Among them, v o represents the vehicle body speed in the wheel speed coordinate system; ω o represents the angular velocity of the vehicle body in the wheel speed coordinate system;

[0078] These two measured values are disturbed by Gaussian white noise. Assuming that the angular velocity measurement noise and the linear velocity measurement noise respectively follow:

[0079] n ω (t) ~ N(0, Q ω ), n ν (t) ~ N(0, Q ν )(3)

[0080] Then the true value of the wheel speed meter can be expressed as:

[0081]

[0082] Among them, n ω (t) represents the angular velocity measurement white noise; n V (t) represents the velocity measurement white noise; Q ω represents the angular velocity measurement variance; Q V represents the velocity measurement variance; Represents the true speed value; V o (t) represents the measured speed value; n v (t) represents the white noise of speed measurement; (t) represents the true angular velocity value; ω o (t) represents the measured angular velocity value;

[0083] Let the initial state of the mobile carrier be abbreviated as The previous state of the mobile carrier is abbreviated as The measured value of the current wheel speedometer is And the time stamp interval dt k = t k ―t k―1 The state of the current wheel speedometer can be obtained as:

[0084]

[0085] where, Represents the initial position vector of the wheel speedometer in the world coordinate system; Represents the direction vector of the wheel speedometer in the world coordinate system (represented by quaternion); Represents the position vector of the k-1th frame of the wheel speedometer in the world coordinate system; Represents the direction vector of the k-1th frame of the wheel speedometer in the world coordinate system; Represents the position vector of the kth frame of the wheel speedometer in the world coordinate system; Represents the direction vector of the kth frame of the wheel speedometer in the world coordinate system; Represents the x-axis speed component of the kth frame of the wheel speedometer in the world coordinate system; Represents the y-axis speed component of the kth frame of the wheel speedometer in the world coordinate system; Represents the angular velocity component around the z-axis of the kth frame of the wheel speedometer in the world coordinate system; ω k Represents the angular velocity value of the kth frame of the wheel speedometer in the world coordinate system; v k Represents the speed value of the kth frame of the wheel speedometer in the world coordinate system; x k Represents the state of the kth frame of the current wheel speedometer; Represents the x-axis position component of the kth frame of the wheel speedometer in the world coordinate system; Represents the y-axis position component of the kth frame of the wheel speedometer in the world coordinate system; θ k Represents the steering angle of the kth frame; dt k Represents the time interval between the kth frame and the previous frame;

[0086] To predict the movement of the robot, the derivative of the wheel speedometer coordinate system with respect to time can be expressed as:

[0087]

[0088] where, Represents the derivative of the rotation matrix for converting from the odometry coordinate system to the world coordinate system; Represents the derivative of the direction vector for converting from the odometry coordinate system to the world coordinate system; Represents the rotation matrix for converting from the odometry coordinate system to the world coordinate system; Represents the direction vector for converting from the odometry coordinate system to the world coordinate system; Represents the velocity of the odometry coordinate system in the world coordinate system; Represents the skew-symmetric matrix form of the angular velocity vector, used to describe rotation. Referring to the principle of IMU pre-integration, by introducing quantities independent of the initial state, the pre-integration model of the wheel speedometer is obtained, which is used to calculate and estimate the pre-integrated quantity of the wheel speedometer in continuous time to obtain the relative displacement and relative rotation information of the carrier:

[0089]

[0090] Among them, Represents the rotation matrix of the j-th frame odometry coordinate system o relative to the world coordinate system w; Represents the rotation matrix of the i-th frame coordinate system o relative to the world coordinate system w; Represents the position vector of the origin of the j-th frame coordinate system o in the world coordinate system w; Represents the position vector of the origin of the i-th frame coordinate system o in the world coordinate system w; Represents the velocity vector of the origin of the coordinate system o in the world coordinate system w at the k-th frame during the time interval from the i-th frame to the j-th frame; Represents the angular velocity of the coordinate system o at the k-th frame during the time interval from the i-th frame to the j-th frame;

[0091] To avoid repeated integration, the idea of Lupton and Foster is introduced to obtain the wheel speedometer integration between key frame i and key frame j:

[0092]

[0093] Among them, Represents the cumulative rotation matrix of the coordinate system o from i to j from the i-th frame to the j-th frame; Represents the position change vector of the coordinate system o from i to j from the i-th frame to the j-th frame; Represents the transpose of the rotation matrix of the coordinate system o relative to the world coordinate system ω at the i-th frame; Represents the rotation matrix of the coordinate system o relative to the world coordinate system ω at the j-th frame; Represents the rotation matrix of the coordinate system o from the i-th frame to the i + 1-th frame; Represents the rotation matrix of the coordinate system o from the i-th frame to the k-th frame; Denotes the linear velocity of the coordinate system o at the (k - 1)-th frame; Denotes the linear velocity of the coordinate system o at the k-th frame; Denotes the angular velocity of the coordinate system o at the (k - 1)-th frame; Denotes the angular velocity of the coordinate system o at the (k - 1)-th frame; Denotes the estimated or measured value of the angular velocity of the world coordinate system ω at the (k - 1)-th frame; Denotes the noise or error term of the linear velocity at the (k - 1)-th frame; Denotes the noise or error term of the linear velocity at the k-th frame; Denotes the noise or error term of the angular velocity at the k-th frame;

[0094] Such as Figure 3 shown, Figure 3 is a schematic diagram of the sampling time and pre-integration interval of the wheel speedometer, Figure 3 which describes the sampling time and pre-integration interval of different sensor data in a robot or an automated system. In the figure, the time axis represents the process from the i-th frame to the j-th frame, where Δt represents the time interval between two adjacent frames. Images (blue dots) and key frames (blue squares): Represent images or key frames captured at specific moments on the time axis. These are usually used in visual odometry or visual SLAM (Simultaneous Localization and Mapping) systems. IMU measurements (red crosses): Represent data collected by the Inertial Measurement Unit (IMU) at continuous time intervals. The IMU usually measures acceleration and angular velocity, and this data is used to estimate the motion of the robot. IMU pre-integrated quantities (orange squares): Represent the intermediate results obtained by pre-integrating the IMU measurements. Pre-integration is the process of converting continuous IMU measurements into estimates of position and rotation, and is usually used for time synchronization and fusion between visual and IMU data. Wheel speedometer measurements (green diamonds): Represent data collected by the wheel speedometer at continuous time intervals. The wheel speedometer measures the rotational speed of the robot's wheels and is used to estimate the linear and angular velocities of the robot. Wheel speedometer pre-integrated quantities (green squares): Represent the intermediate results obtained by pre-integrating the wheel speedometer measurements. These pre-integrated quantities are used to estimate the motion of the robot between two key frames.

[0095] The figure shows the sampling and pre-integration processes of different sensor data and their distributions on the time axis. Through these data, the motion state of the robot in continuous time intervals can be estimated more accurately.

[0096] To simplify the log-likelihood calculation, Equation 7 is rewritten as

[0097]

[0098] where, denotes random noise; Represents the estimated value of the position vector from coordinate system oi to coordinate system oj; Represents the estimated value of the rotation matrix from coordinate system oi to coordinate system oj;

[0099] Write Equation 9 in a recursive form:

[0100]

[0101] where, Represents the estimated value of the rotation matrix of coordinate system o from the i-th frame to the j-th frame; Represents the estimated value of the position vector of coordinate system o from the i-th frame to the j-th frame;

[0102] Assume that the wheel speedometer measurement error is linearly correlated between adjacent moments, and the mean and covariance of the next moment can be predicted using the measurement value at the current moment. The wheel speedometer measurement error consists of two parts: the error at the current moment and the measurement noise at the current moment. Then the linear transfer equation of the measurement error between adjacent moments is:

[0103]

[0104] where, Is the state measurement error at the previous moment,

[0105] Are the measurement noises at the previous moment and the next moment,

[0106] Is the state measurement error at the next moment,

[0107] F k-1 ,G k-1 Are the covariance transfer matrices at two moments. Then F k-1 ,G k-1 Are in the form of:

[0108]

[0109] where, Represents the noise term of a certain state or measurement at the k-th frame; F K―1 Represents the state transition matrix, which is used to describe the change of the system state from the (k - 1)-th frame to the k-th frame;

[0110] As Figure 4 shown, the wheel speed-vision-inertial tightly coupled positioning method provided in this embodiment has three threads in the overall algorithm, including the following steps:

[0111] S1: The sensor measurement value processing module is used to obtain the wheel speedometer measurement value, image data, and IMU measurement value;

[0112] S2: Front-end feature initialization module. The front-end feature initialization module uses FAST corner extraction instead of optical flow method, and then obtains the positions of new feature points through triangulation. The measured values of the IMU and the wheel speedometer are pre-integrated between two consecutive frames.

[0113] S3: Back-end sliding window optimization module. The back-end sliding window optimization module is used to jointly optimize all measured values through a factor graph based on a sliding window.

[0114] The front-end feature initialization module processes as follows:

[0115] S21: Perform wheel speedometer pre-integration calculation on the measured values of the wheel speedometer.

[0116] S22: Perform FAST corner detection and tracking on the image data.

[0117] S23: Perform IMU pre-integration calculation on the measured values of the IMU.

[0118] S24: Set the sliding window; perform the initialization process.

[0119] The initialization process adopts the same process as VINS-mono and follows some guidelines and operation sequences of VINS-mono in feature point initialization. The specific steps are as follows:

[0120] S241: Only visual SfM. Use only visual information for Structure from Motion (SfM) estimation, that is, estimate the pose change of the camera and the three-dimensional structure of the scene by analyzing the movement of points in the image sequence.

[0121] S242: Gyroscope bias correction. Correct the bias error in the gyroscope measurement to improve the accuracy of the angular velocity measurement.

[0122] S243: Velocity and scale initialization. Initialize the velocity and scale information of the robot, which usually involves estimating these parameters from the initial sensor data.

[0123] The back-end sliding window optimization module in step S3 specifically processes as follows:

[0124] S31: Initialize the pose of the mobile robot. Set the initial position and orientation of the robot, which is usually determined in some way (such as manual setting, sensor readings, or prior knowledge).

[0125] S32: Initialize the landmarks. Select and define some landmarks in the environment, and these points will be used for subsequent positioning and map building.

[0126] S33: Tightly-coupled non-linear optimization, which uses non-linear optimization techniques to minimize the error between the predicted pose and the actual pose, and this generally involves the tightly-coupled processing of IMU, vision, and wheel speedometer data;

[0127] S34: Remove outliers, identify and exclude those observation data points that significantly deviate from the model prediction, and these points may be caused by sensor noise or errors;

[0128] S35: Marginalization, in the sliding window optimization, the process of removing old poses and landmarks from the optimization window to reduce the computational load.

[0129] The tightly-coupled non-linear optimization in step S33 is carried out according to the following steps:

[0130] S331: IMU residuals, calculate the differences between the IMU measurement values and the model prediction values, and these differences are used for error correction in the optimization process;

[0131] S332: Vision residuals, calculate the differences between the vision measurement values (such as feature point matching) and the model prediction values, and are used for error correction in the optimization process;

[0132] S333: Wheel speedometer residuals, calculate the differences between the wheel speedometer measurement values and the model prediction values, and are used for error correction in the optimization process.

[0133] As Figure 5 shown, Figure 5 is the factor graph model for wheel speed-vision-inertia fusion, Figure 5Shows a factor graph model for wheel speed-vision-inertial fusion, which is used to describe the process of robot pose estimation and map construction. A factor graph is a probabilistic graphical model that represents the relationships between variables through nodes and edges. In this graph, nodes represent variables, and edges represent the constraints or measurement relationships between variables. The following are the specific explanations of each element in the graph: Landmark nodes (yellow circles l): Represent the landmarks in the environment, which are used to assist in positioning and map construction. Pose nodes (gray circles X): Represent the poses (position and orientation) of the robot at different time points, such as X0, X1, X2, X3, X4, X5. Marginalization factors (green squares p): Represent the pose nodes that are marginalized during the sliding window optimization process, such as p0. IMU factors (orange squares b): Represent the measurement data provided by the inertial measurement unit (IMU), which are used to estimate the acceleration and angular velocity of the robot, such as b1, b2, b3, b4, b5. Visual factors (blue squares c): Represent the measurement data provided by the visual sensor, which are used to estimate the relative pose between the robot and the landmarks, such as c01, c02, c12, c23, c34, c45. Wheel speedometer factors (light blue squares o): Represent the measurement data provided by the wheel speedometer, which are used to estimate the linear velocity of the robot, such as o1, o2, o3, o4, o5.

[0134] The edges in the figure represent the constraint relationships between different factors. For example: The pose node X0 is connected to the marginalization factor p0, indicating that the pose of X0 is obtained through marginalization. The pose node X1 is connected to the IMU factor b1, the visual factors c01 and c02, and the wheel speedometer factor o1, indicating that the pose of X1 is constrained by these measurement data. The pose node X2 is connected to the IMU factor b2, the visual factor c12, and the wheel speedometer factor o2, indicating that the pose of X2 is constrained by these measurement data. Through these nodes and edges, the factor graph model can represent complex multi-sensor fusion problems and solve these variables through optimization algorithms, thereby achieving accurate pose estimation and map construction.

[0135] The odometry factor graph model for wheel speed-vision-inertial fusion in this embodiment is used for the motion estimation of ground robots. This model makes observations based on visual data, combines the motion constraints provided by the IMU and wheel odometry, and achieves a balance between accuracy and efficiency through the sliding window algorithm. To handle these constraints, marginalization priors are also introduced.

[0136] In this model, it also includes state variable nodes, and the state variable nodes include frame nodes (position velocity attitude as well as gyroscope bias b g and accelerometer bias b aVariables such as [variables] and landmark nodes (represented by modeling the inverse depth λ). In the figure, the color blocks corresponding to the variables represent the constraints on the variables. The factor nodes include visual factors, IMU factors, wheel speedometer factors, and marginalization factors, as Figure 6 shown, Figure 6 is the state variable node, Figure 6 shows a schematic diagram of a state variable node, which is usually used for state estimation in robot localization, visual odometry (VO), or simultaneous localization and mapping (SLAM) systems. Each letter in the figure represents a different state variable or measurement type, specifically as follows: X (gray circle): represents the pose node, usually containing position and orientation information. l (yellow circle): represents the landmark node, used for feature points or road signs in the map. p (green square): represents the marginalization factor, usually used to marginalize old pose nodes in the sliding window optimization. c (blue square): represents the visual factor, usually used to represent the measurement data obtained from visual sensors (such as cameras) for pose estimation. b (orange square): represents the IMU (inertial measurement unit) factor, used to represent the measurement data obtained from the IMU, such as acceleration and angular velocity, for pose estimation. o (light blue square): represents the wheel speedometer factor, used to represent the measurement data obtained from the wheel speedometer for pose estimation. λ (Greek letter in the yellow circle): represents a specific state variable or parameter. The table in the figure lists the relationships between different state variables and factors. These state variables and factors together form a complex system for accurately estimating the position, orientation, speed of the robot, and the biases of various sensors. By optimizing these variables, the overall localization accuracy and robustness of the system can be improved.

[0137] The wheel speedometer factor in this embodiment is calculated according to the following steps:

[0138] (1) State variables

[0139] After fusing the wheel speed factor, the state vector includes the states of n + 1 frames within the sliding window, including position speed rotation accelerometer bias b a and gyroscope bias b g 、the inverse depth λ of m + 1 landmark points, and the extrinsic parameters of the wheel speed odometer and IMU

[0140] Among them, X represents the state vector of the entire system, which contains all state variables such as the robot pose, sensor biases, and landmark positions; x n represents the state vector of the nth pose node; represents the calibration parameters or state variables related to the IMU; λ mIndicates the position of the m-th road punctuation point; Indicates the position of the robot in the world coordinate system ω at the k-th frame; Indicates the rotation represented by the quaternion of the robot in the world coordinate system ω at the k-th frame; Indicates the velocity of the robot in the world coordinate system ω at the k-th frame; Indicates the position of the IMU coordinate system o in the robot coordinate system b; Indicates the rotation represented by the quaternion of the IMU coordinate system o in the robot coordinate system b; ba represents the accelerometer bias; bg represents the gyroscope bias;

[0141] (2) Residual of the wheel speedometer pre-integrated quantity

[0142] According to the wheel speed sensor measurement model, the residual of the pre-integrated quantity is:

[0143]

[0144] Among them, r o Indicates the relative pose between frames of the variable to be optimized and the key frame pose The error distance between.

[0145] Substitute to obtain the residual term containing only the state variables to be optimized and the pre-integration representation:

[0146]

[0147] (3) Jocobian matrix

[0148] The optimization vector for backend optimization is:

[0149]

[0150] Take the partial derivative of the residual with respect to the above optimization vector as the target, and use the perturbation method to calculate the Jacobian matrix, where the size of the small perturbation is:

[0151]

[0152] Then the Jacobian matrix of the residual term with respect to the optimization vector is:

[0153]

[0154] (4) Covariance transfer matrix of the wheel speed sensor pre-integrated quantity

[0155] The covariance transfer matrix of the pre-integrated quantity of the wheel speedometer in this embodiment is Equation (12). In this embodiment, the wheel speed-vision-inertial sliding window optimization and marginalization are carried out as follows: This embodiment adopts batch optimization to solve the global map through bundle adjustment. Considering real-time performance and computational efficiency, it runs in the sliding window (i.e., local BA), maintains the state variables in a time window of a fixed size (10 frames are selected in this embodiment), only optimizes the state variables within the window, and marginalizes the information before the window, while converting the values corresponding to the marginalized states into prior information. In this way, the system can maintain a relatively small-scale optimization problem, reduce the computational complexity, and at the same time smoothly process historical information in time. The visual-inertial sliding window optimization strategy corresponding to VINS-mono is extended to the wheel speed-vision-inertial odometer system.

[0156] In this embodiment, it is judged whether the second-newest frame (the 9th frame) is a key frame by calculating the parallax magnitude of the co-visible feature points in the 8th and 9th frames within the current sliding window. If the second-newest frame (the 9th frame) is a key frame, the information of the second-newest frame is retained in the sliding window. After the backend optimization, the oldest frame (the 0th frame) in the sliding window and the measurements related to the oldest frame are marginalized, and the current frame (the 10th frame) enters the sliding window; if the second-newest frame is not a key frame, then after the sliding window optimization, the visual observations of the second-newest frame are removed, and only the IMU constraint and the wheel speedometer constraint are retained to ensure the coherence of the pre-integrated quantity.

[0157] As Figure 7 shown, Figure 7For the marginalization diagram, it shows a schematic diagram of a marginalization process, which is usually used in robot localization, visual odometry (VO), or simultaneous localization and mapping (SLAM) systems. In the figure, various nodes and factors are represented by different colors and shapes, and the specific meanings are as follows: Landmark node (yellow circle l): It represents the landmarks in the environment, which are used to assist in localization and map construction. Old node (gray circle X): It represents the old pose nodes that have been marginalized in the sliding window optimization. New node (blue circle X): It represents the current pose nodes, which are being optimized. Marginalization factor (green square p): It represents the factors that are marginalized in the sliding window optimization, and these factors are related to the old pose nodes. IMU factor (orange square b): It represents the measurement data provided by the inertial measurement unit (IMU), which is used to estimate the acceleration and angular velocity of the robot. Visual factor (blue square c): It represents the measurement data provided by the visual sensor, which is used to estimate the relative pose between the robot and the landmarks. Wheel odometer factor (light blue square o): It represents the measurement data provided by the wheel odometer, which is used to estimate the linear velocity of the robot. The figure shows the process of two sliding window optimizations: Upper part: Marginalization is performed at key frames, that is, the old pose nodes and related factors are removed from the optimization window to reduce the computational amount. Lower part: Marginalization is performed at non-key frames, and the old pose nodes and related factors are also removed.

[0158] The marginalization process is represented by dashed arrows, where the red dashed arrows represent the fixed state and the green dashed arrows represent the estimated state. In this way, the system can continuously update and optimize the pose and map information of the robot while maintaining computational efficiency.

[0159] This embodiment also includes the modeling of motion manifold constraints. Due to the limitations of its own mechanical characteristics, the ground mobile robot will generate noise during driving due to the uneven ground and vehicle vibrations, and it is necessary to add ground motion manifold constraints according to the planar characteristics.

[0160] Due to indoor and outdoor structured man-made scenes (such as roads, streets, and university campuses, etc.), these scenes usually have flat terrain and no sharp slope changes. The motion manifold of any three-dimensional spatial position p in the wheel odometer coordinate system {O} relative to the world coordinate system {W} is approximately described as:

[0161]

[0162] Therefore, the manifold parameters are defined as:

[0163] m = [c a1 a2 a3] T (21)

[0164] Also note that the roll angle and pitch angle of the ground robot should be consistent with the normal of the motion manifold, that is:

[0165]

[0166] For key frame i, the following equation holds:

[0167]

[0168] in, The first two rows of the skew-symmetric matrix representing the three-dimensional vector v, Λ mr represents the inverse matrix of the covariance matrix, i.e., the information matrix; M P () represents the manifold function of point po in the world coordinate system w; Wp o Represents the position vector of point po in the world coordinate system w; Represents the x, y, and z components of point po in the world coordinate system w; A ω represents the coefficient matrix related to the world coordinate system w; p o represents the position vector of point po; A represents the coefficient matrix, including a1, a2, a3.; a1, a2, a3 represent the coefficients related to point po on the x, y, z axis; c represents the constant term; m represents the manifold parameter; M r () represents the manifold function of the roll axis in the robot coordinate system; represents the rotation matrix from the robot coordinate system o to the world coordinate system w; M P Represents the manifold function of the pitch axis in the robot coordinate system; represents the product of the rotation matrix ow R and the unit vector e3; M

[0169] () represents the manifold function of a point in the world coordinate system w; Represents the position vector of point pok in the world coordinate system w;

[0170] represents the position vector of point poi in the world coordinate system w; however, in complex outdoor environments, the ground motion manifold is dynamically changing. In structured artificial scenes indoors and outdoors (such as roads, streets, and university campuses), the terrain usually presents smooth features and changes gradually and slowly. In order to pursue accuracy, a noise perturbation that follows a Gaussian distribution is added to the ground motion manifold parameters.

[0171]

[0172] Among them, m k+1 represents the ground motion manifold parameter at the k+1th frame; m k represents the ground motion manifold parameters at time step k; Denotes the noise perturbation introduced at time step k; Denotes the transpose of the i-th element in the noise vector;

[0173] The factor graph of the optimization process after adding the motion manifold constraint is as Figure 8 shown, Figure 8 is the factor graph of the optimization process, Figure 8 which shows a factor graph model of an optimization process, usually used in robot localization, visual odometry (VO) or simultaneous localization and mapping (SLAM) systems. The nodes and edges in the figure represent variables and the relationships between them, as follows:

[0174] Pose nodes (gray circles X): Represent the poses of the robot at different time points, such as X0, X1, X2, X3, X4, X5. Landmark nodes (yellow circles l): Represent the landmarks in the environment, which are used to assist in localization and mapping, such as l1, l2, l3, l4, l5. IMU factors (orange squares b): Represent the measurement data provided by the inertial measurement unit (IMU), which is used to estimate the acceleration and angular velocity of the robot, such as b1, b2, b3, b4, b5. Visual factors (blue squares c): Represent the measurement data provided by the visual sensor, which is used to estimate the relative pose between the robot and the landmarks, such as c01, c12, c23, c34, c45. Wheel odometer factors (light blue squares o): Represent the measurement data provided by the wheel odometer, which is used to estimate the linear velocity of the robot, such as o1, o2, o3, o4, o5. Motion manifold factors (pink squares p): Represent the factors related to the robot's motion manifold, such as p0, p1, p2, p3, p4, p5. The edges in the figure represent the constraint relationships between different factors. For example: The pose node X0 is connected to the IMU factor b1, the visual factor c01, and the motion manifold factor p0, indicating that the pose of X0 is obtained by constraining these measurement data. The pose node X1 is connected to the IMU factor b2, the visual factor c12, and the motion manifold factor p1, indicating that the pose of X1 is obtained by constraining these measurement data. Through these nodes and edges, the factor graph model can represent complex multi-sensor fusion problems and solve these variables through optimization algorithms, thereby achieving accurate pose estimation and mapping.

[0175] The state vector after fusing the ground motion manifold factor in this embodiment is as follows:

[0176]

[0177] The update method of the state variables of the newly added ground motion manifold constraint is:

[0178]

[0179] w po ← w p o +δ w p o (27)

[0180] Among them, X represents the state vector of the entire system, including all state variables such as the robot pose, sensor bias, and landmark position; X n represents the state vector of the nth pose node, including position and orientation information; represents the calibration parameters or state variables related to the IMU, used to describe the state of the IMU coordinate system b relative to a certain reference coordinate system c; represents the state variables of the IMU coordinate system b at a certain initial moment, including information such as position, velocity, and acceleration; λ m represents the position of the mth landmark; represents the rotation angle of the coordinate system o relative to the world coordinate system w; δ represents a general noise or error term; δ ω represents the noise or error term in the world coordinate system w, which may be used to describe the uncertainty of position or rotation;

[0181] Based on the foregoing definitions and assumptions, the states of velocity and pose can be restricted as follows:

[0182] (1) Velocity constraint

[0183] Considering that the wheels of the planar robot are in close contact with the ground during movement, the following equation can be obtained:

[0184]

[0185] Since the mobile robot still follows the motion manifold constraint in the lateral (y-axis direction), then:

[0186] o v y =0(29)

[0187] Among them, o v y represents the velocity component in the y-axis direction (lateral) in the robot coordinate system o;

[0188] In actual calculations, the constraint described by Equation 24 often has random noise. Considering the deviation from the real world, a velocity measurement model can be formulated:

[0189]

[0190] Among them, r νel represents the velocity residual, and J νel represents the Jacobian matrix, which is defined as follows:

[0191]

[0192] where, Λ1 = [e2 e3] T , assuming that the coordinate system of the mobile robot body coincides with the coordinate system of the wheel speedometer;

[0193] ov z represents the velocity component in the z-axis direction in the robot coordinate system o; represents the unit vector in the x-axis direction in the robot coordinate system o; represents the estimated or measured value of the rotation matrix from the robot coordinate system o to the world coordinate system w;

[0194] (2) Pose Constraint

[0195] Assuming that the mobile robot operates on the plane π, and the plane π is set as the initial x-y plane in the global coordinate system (where c = a1 = a2 = 0), the following equation can be obtained:

[0196]

[0197] where, Λ2 = [e1 e2] T . K represents the scaling factor used to adjust the size of the vector;

[0198] When the plane π corresponds to the initial x-y plane in the global coordinate system, the matrix is initialized to an identity matrix. The rotation constraint residuals of roll-pitch and the translation constraint residuals along the z-axis can be obtained as:

[0199]

[0200] where, r rot represents the residuals of the estimated roll angle and pitch angle, while r tran represents the residuals of the translation estimation along the z-axis.

[0201] Z rot represents the residuals of the estimated roll angle and pitch angle; Z tran represents the residuals of the translation estimation along the z-axis.;

[0202] The Jacobian matrix of the corresponding state variables of Equation 33 is expressed as follows:

[0203]

[0204] where, J rot represents the Jacobian matrix related to the rotation residuals. This matrix describes how the state variables affect the rotation residuals and is usually used to adjust the state variables in the optimization process to minimize the rotation error.; Jtran Represents the Jacobian matrix related to the displacement residual. This matrix describes how the state variables affect the displacement residual and is usually used to adjust the state variables in the optimization process to minimize the displacement error;

[0205] Taking the above-derived ground motion manifold constraint as one of the optimization objectives, together with other optimization objectives, it constitutes the tightly coupled optimization of wheel speed-vision-inertial odometry. By introducing optimization conditions, the accuracy and robustness of the positioning system are improved. Then, the bundle adjustment BA can be extended to:

[0206]

[0207] where, Represents the ground motion manifold constraint.

[0208] This embodiment verifies the effect of the above method through experiments. The computer configuration of the experimental platform is shown in Table 1 for details.

[0209] Table 1 Experimental platform configuration

[0210]

[0211] This embodiment uses the KAIST dataset as experimental data for algorithm evaluation. The KAIST dataset encompasses rich complex urban features, such as large-scale environments, multi-lane roads, complex building structures, and rich highly dynamic objects. It mainly includes highway scenarios and urban scenarios. In highway scenarios, vehicles often maintain a constant speed or move in a straight line with a high speed. In urban scenarios, there are a large number of traffic lights, vehicles need to start and stop frequently, the driving route involves multiple turns, and there are many moving objects. The experimental results of this embodiment are compared as follows:

[0212] Table 2 Comparison of average errors per 100 meters on the KAIST dataset sequence

[0213]

[0214] Table 3 Comparison of RMSE of absolute trajectory errors per 100 meters on the KAIST dataset sequence

[0215]

[0216]

[0217] The quantitative comparison results of the ATE average error and the absolute trajectory error of the WVIO algorithm proposed in this embodiment and other algorithms when running on the dataset are detailed in Tables 2 and 3. The optimal values are marked in bold, from which the performance differences of the three algorithms can be intuitively observed, and the absolute trajectory errors of them relative to the ground truth can be evaluated. After being verified by multiple groups of experimental data, the WVIO algorithm proposed in this embodiment shows smaller positioning errors in environments of different sizes compared with the VINS-Fusion and VINS-mono algorithms, demonstrating higher positioning accuracy. Taking the RMSE of the Urban38 sequence as an example, compared with VINS-mono, the error of the VINS-Fusion algorithm in this index is reduced by 18.5%, and the error of the algorithm in this embodiment is reduced by 47.7%.

[0218] Taking the processed VRS_GPS data in the KAIST dataset as the ground truth, the trajectory positioning results of the WVIO algorithm proposed in this embodiment and the VINS-Fusion algorithm are compared. Figure 9 The comparison results are shown, where the dotted line represents the trajectory ground truth in the dataset, the green solid line represents the trajectory positioning effect of VINS-Fusion, and the blue solid line represents the trajectory positioning result of the WVIO algorithm proposed in this embodiment. In the Urban29 sequence, the measured trajectory of the VINS-Fusion algorithm shows a large drift, and the algorithm in this embodiment is closer to the real trajectory; in the Urban30 sequence, it can be seen that the maximum error is at the closed loop. Compared with the VINS-Fusion algorithm, the trajectory estimated by the algorithm in this embodiment is more accurate at the closed loop; the Urban26 and Urban38 sequences are urban traffic sections, and the driving route involves multiple turns. The VINS-Fusion algorithm performs poorly in this route, and the trajectory shows obvious drift, while the algorithm in this embodiment has less drift at the turns and basically coincides with the real trajectory, with high global positioning accuracy.

[0219] Table 4 Comparison of the running time of the front end on the KAIST dataset sequence

[0220]

[0221] In addition, the real-time performance of the system is compared in this embodiment. The comparison of the front-end processing time per frame of the algorithm in this embodiment and the VINS-Fusion system is shown in Table 4. Taking the Urban18 sequence as an example, the front-end time consumption is shortened by 37.59%. It can be seen that the real-time performance of the algorithm in this embodiment has been greatly improved on the tested sequences.

[0222] The real-scene positioning experiment in this embodiment is as follows:

[0223] To further verify the accuracy and practicality of the method proposed in this embodiment in practical applications, a physical mobile robot platform was built to obtain a dataset of real campus scenarios for further comprehensively testing and evaluating the overall performance of the improved algorithm.

[0224] The experimental environment was set up as follows:

[0225] (1) Hardware test platform: The mobile robot platform built in this embodiment consists of an Ackermann mobile chassis (HUNTER2.0 chassis system) and multiple sensors. As Figure 10 shown, Figure 10 for the mobile robot experimental platform, the sensors carried include a binocular depth camera Intel RealSense D435i integrated with an inertial measurement unit, a wheel speed encoder (magnetic encoder 2500), and a multi-star multi-frequency GNSS RTK (model: HG-GOYH7156).

[0226] (2) Software test environment: In the data acquisition experiment, the computing unit of the industrial computer used is Nvidia Jetson Nano, which supports ROS development. The installed operating system is Ubuntu20.04 LTS. The data acquisition flowchart is as Figure 11 shown, Figure 11 for the data acquisition flowchart. The mobile chassis and the industrial computer with the ROS system communicate through the CAN bus. The ugv_sdk software package and the huner_ros2 software package implement the mapping between the data on the CAN bus and the ROS topics, including basic CAN information parsing and encoding functions, and establish the connection between the CAN information with different addresses and the corresponding ROS topics. These software packages also implement some basic functions, such as obtaining the integrated rotational speed from the built-in wheel speedometer and publishing relevant information on the "odom" topic. After collecting the rosbag file on the mobile trolley, the operating system and other environment configurations of the computer for algorithm testing and verification are the same as above.

[0227] The sensor calibration and experimental data acquisition of this embodiment are as follows:

[0228] (1) Calibration of the binocular camera and IMU: The kalibr toolbox developed by ETH Zurich was used to calibrate the main parameters of the RealSense D435i camera.

[0229] Table 5 Binocular camera calibration results

[0230]

[0231]

[0232] Use the imu_utils toolkit developed by the Hong Kong University of Science and Technology to calibrate the parameters of the IMU. The IMU is stationary to record data for about 2 hours. According to the calibrated parameters, the Gaussian noise and true zero biases of the accelerometer and gyroscope can be obtained from the measured values of the IMU. The calibration results of the IMU are shown in Table 6.

[0233] Table 6 IMU Calibration Results

[0234]

[0235] (2) Joint Calibration of Binocular Camera and IMU: After the individual calibrations of the binocular camera and IMU are completed, use a calibration board to perform joint calibration on the left-eye camera and IMU, and obtain the external parameter matrix of the camera and IMU sensors as:

[0236]

[0237] The time offset between the two sensors is:

[0238] t cam0-imu = 0.0076(38)

[0239] t cam1-imu = -0.0086 (39)

[0240] (3) Joint Calibration of Camera and Wheel Speed Sensor: Use the CamOdomCalibraTool calibration tool to calibrate the external parameters of the camera and wheel speedometer, and obtain the external parameter matrix of the camera and wheel speedometer as:

[0241]

[0242] (4) Joint Calibration of Camera and Wheel Speed Sensor:

[0243] For the campus dataset collection, use the above-built mobile trolley experimental platform to collect multi-sensor data around multiple sites. A total of four sequences are collected. The dataset collection route is as Figure 12The red trajectory is the moving route of the vehicle, and the white trajectory is the main road on campus. The collection lengths of the four sequences are 455.80m, 122.88m, 575.61m, and 417.48m respectively. The collection process includes a large number of challenging scenarios, such as textureless, lighting changes, uneven sections, moving vehicles and pedestrians, etc. The test scenarios are surrounded by a large number of trees and buildings. The average speed of the moving vehicle is 1m / s, the operating frequency of GNSS RTK is 200Hz, the camera resolution is set to 640×480, and the frame rate is set to 30FPS. The data collected from the camera, inertial measurement unit, wheel speedometer, and GNSS RTK are made into bag files, and the sizes of the bag files are 8.9GB, 2.8GB, 12.9GB, and 7.4GB respectively. Among them, Figure 12 is the data collection route. Among them, (a) Sequence 1 campus playground dataset; (b) Sequence 2 campus teaching building dataset - 1; (c) Sequence 3 campus teaching building dataset - 2; (d) Sequence 4 campus teaching building dataset - 3;

[0244] Experiment and analysis of this embodiment: The algorithm of this embodiment is tested on 4 campus dataset sequences. The obtained trajectory data results and the running results of the VINS - Fusion algorithm are compared with the ground truth trajectory of differential GNSS RTK, and the evo evaluation tool is used to give the quantitative results. The comparison of the running trajectories is as Figure 13 shown, Figure 13 is the comparison of the running trajectories of the campus dataset. Among them, (a) Sequence 1 campus playground dataset; (b) Sequence 2 campus teaching building dataset - 1; (c) Sequence 3 campus teaching building dataset - 2; (d) Sequence 4 campus teaching building dataset - 3;

[0245] Through Figure 13 the running trajectories, it can be seen that the WVIO method that tightly couples the wheel speedometer information has a higher degree of coincidence with the trajectory truth value on the four sequences than the VINS - Fusion algorithm.

[0246] Sequence 1 is the campus playground runway scene. The algorithm proposed in this embodiment successfully realizes the closed loop, while the VINS - Fusion algorithm does not detect the loop due to excessive drift during the running process.

[0247] Sequence 3 is the scene around the main teaching building of the school. Since the loop location is near the garage and there are many pedestrians and vehicles, the error of the algorithm in this embodiment is relatively large at the loop, but the loop is successfully detected, while the cumulative error of VINS - Fusion is relatively large and the closed loop is not successfully realized.

[0248] Therefore, comprehensively Figure 13Among the four trajectories, the estimated trajectory of the WVIO method proposed in this embodiment basically coincides with the true trajectory, indicating that the WVIO method of this embodiment provides the motion information of the robot on the ground by fusing the wheel speedometer data, effectively improving the positioning accuracy.

[0249] Table 8 Comparison of average errors on the campus dataset sequence

[0250]

[0251]

[0252] Table 9 Comparison of root mean square errors on the campus dataset sequence

[0253]

[0254] Table 8 and Table 9 respectively show the quantitative results comparison of the average error and root mean square error in the absolute trajectory error of the two algorithms on the campus dataset. The optimal results are shown in bold. From these two indicators, it can be seen that on the campus dataset collected by the real vehicle, compared with the VINS-Fsuion algorithm, the WVIO method of this embodiment has a greater improvement in positioning accuracy. The VINS-Fusion algorithm is easily affected by the error accumulation of the inertial measurement unit, especially prone to pose drift during long-term operation. The sensor information provided by the wheel speedometer can be used to correct this drift, thereby reducing the accumulation of positioning errors. The acquisition scenarios of sequences 2, 3, and 4 are surrounded by a large number of trees and lack reliable features, which may lead to incorrect feature matching results. After fusing the wheel speedometer information, the linear velocity measurement data and angular velocity data with absolute scale can be obtained, which can correct the incorrect feature matching results and effectively reduce the positioning error of the system.

[0255] In this embodiment, aiming at the problem that the VINS system is unobservable when restricted to move at a constant acceleration or stationary, a visual inertial odometer system WVIO that fuses the vehicle wheel speedometer is formed, and random constraints are used to model the pose constraints of the ground robot on the ground motion manifold, realizing the pose estimation and global positioning of the mobile platform; through experiments on the public dataset KAIST and real vehicles respectively, the effectiveness and overall performance of the multi-sensor fusion algorithm adopted in this embodiment are verified.

[0256] The above-described embodiments are only preferred embodiments given to fully illustrate the present invention, and the protection scope of the present invention is not limited thereto. Equivalent substitutions or transformations made by those skilled in the art on the basis of the present invention are all within the protection scope of the present invention. The protection scope of the present invention is subject to the claims.

Claims

1. Wheel speed-vision-inertial tightly coupled positioning method, characterized in that: It includes the following steps: S1: A sensor measurement processing module for obtaining wheel speedometer measurement values, image data, and IMU measurement values; S2: A front-end feature initialization module that obtains the positions of new feature points by extracting FAST corner points and pre-integrates the measurement values of the IMU and the wheel speedometer between two consecutive frames; S3: A back-end sliding window optimization module for jointly optimizing all measurement values through a factor graph based on a sliding window.

2. The wheel speed-vision-inertia tightly coupled positioning method according to claim 1, wherein: The front-end feature initialization module is processed in the following manner: S21: Perform wheel speedometer pre-integration calculation on the wheel speedometer measurement values; S22: Perform FAST corner detection and tracking on the image data; S23: Perform IMU pre-integration calculation on the IMU measurement values; S24: Set up a sliding window for the initialization process.

3. The wheel speed-vision-inertia tightly-coupled positioning method according to claim 1, characterized in that: The initialization process in the initialization module adopts the same process as VINS-mono and follows some guidelines and operation sequences of VINS-mono in feature point initialization. The specific steps are as follows: S241: Use only visual information for structure from motion estimation, that is, estimate the pose change of the camera and the three-dimensional structure of the scene by analyzing the movement of points in the image sequence; S242: Gyroscope bias correction for correcting the bias error in gyroscope measurements; S243: Velocity and scale initialization for initializing the velocity and scale information of the robot.

4. The wheel speed-vision-inertial tightly coupled positioning method according to claim 1, wherein: The back-end sliding window optimization module in step S3 is specifically carried out according to the following steps: S31: Initialize the pose of the mobile robot and set the initial position and orientation of the robot; S32: Initialize the landmarks, select and define some landmarks in the environment for subsequent positioning and map construction; S33: Tightly coupled non-linear optimization, using non-linear optimization techniques to minimize the error between the predicted pose and the actual pose; S34: Remove outliers, identify and exclude those observation data points that significantly deviate from the model prediction; S35: Marginalization, the process of removing old poses and landmarks from the optimization window in sliding window optimization.

5. The wheel speed-vision-inertia tightly coupled positioning method according to claim 1, wherein: The tightly coupled non-linear optimization in step S33 is carried out according to the following steps: S331: IMU residual: Calculate the difference between the IMU measurement value and the model prediction value; S332: Visual residual: Calculate the difference between the visual measurement value and the model prediction value; S333: Wheel speedometer residual: Calculate the difference between the wheel speedometer measurement value and the model prediction value.

6. The wheel speed-vision-inertial tightly-coupled positioning method according to claim 1, wherein: The IMU pre-integration calculation in step S23 is carried out according to the following formula: Among them, represents the rotation matrix of the odometry coordinate system o of the j-th frame with respect to the world coordinate system w; represents the rotation matrix of the coordinate system o of the i-th frame with respect to the world coordinate system w; represents the position vector of the origin of the coordinate system o of the j-th frame in the world coordinate system w; represents the position vector of the origin of the coordinate system o of the i-th frame in the world coordinate system w; represents the velocity vector of the origin of the coordinate system o at the k-th frame in the world coordinate system w during the time interval from the i-th frame to the j-th frame; represents the angular velocity of the coordinate system o at the k-th frame during the time interval from the i-th frame to the j-th frame.

7. The wheel speed-vision-inertia tightly coupled positioning method according to claim 1, wherein: The wheel speedometer pre-integration calculation in step S21 is carried out according to the following formula: Among them, represents the cumulative rotation matrix of the coordinate system o from the i-th frame to the j-th frame; represents the position change vector of the coordinate system o from the i-th frame to the j-th frame; represents the transpose of the rotation matrix of the coordinate system o relative to the world coordinate system ω at the i-th frame; represents the rotation matrix of the coordinate system o relative to the world coordinate system ω at the j-th frame; represents the rotation matrix of the coordinate system o from the i-th frame to the i+1-th frame; represents the rotation matrix of the coordinate system o from the i-th frame to the k-th frame; represents the linear velocity of the coordinate system o at the (k-1)-th frame; represents the linear velocity of the coordinate system o at the k-th frame; represents the angular velocity of the coordinate system o at the (k-1)-th frame; represents the angular velocity of the coordinate system o at the (k-1)-th frame; represents the estimated or measured value of the angular velocity of the world coordinate system ω at the (k-1)-th frame; represents the noise or error term of the linear velocity at the (k-1)-th frame; represents the noise or error term of the linear velocity at the k-th frame; represents the noise or error term of the angular velocity at the k-th frame.

8. The wheel speed-vision-inertial tightly-coupled positioning method according to claim 1, wherein: The factor graph in step S3 is a factor graph model for wheel-vision-inertia fusion. The factor graph for wheel-vision-inertia fusion is a probabilistic graph model that represents the relationship between variables through nodes and edges. The nodes represent variables, and the edges represent the constraint relationships between variables. The nodes include Landmark nodes: Represent the landmarks in the environment for assisting in positioning and map construction; Pose nodes: Represent the poses of the robot at different time points; Marginalization factors: Represent the pose nodes that are marginalized during the sliding window optimization process; IMU factor: Represents the measurement data provided by the inertial measurement unit and is used to estimate the acceleration and angular velocity of the robot; Vision factor: Represents the measurement data provided by the vision sensor and is used to estimate the relative pose between the robot and the road marking point; Wheel speedometer factor: Represents the measurement data provided by the wheel speedometer and is used to estimate the linear velocity of the robot.

9. The wheel speed-vision-inertial tightly coupled positioning method according to claim 1, characterized in that: The marginalization in step S35 is performed in the following manner: Batch optimization is adopted to solve the global map through bundle adjustment. Considering real-time performance and computational efficiency, it runs in a sliding window, maintains the state variables in a time window of a fixed size, and only optimizes the state variables within the window; The information before the marginalization window, and at the same time, convert the values corresponding to the marginalization state into prior information.

10. A wheel speed-vision-inertial tightly coupled positioning system, comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the method described in any one of claims 1 to 9 above.

Images