A VINS-Wheel Odometry Fusion Method

Abstract

The present invention relates to the field of image processing technology, and in particular to a VINS-wheel odometry fusion method, comprising pre-integrating wheel odometry and gyroscope data, fusing the pre-integrated data with optical flow keypoint tracking data to obtain robot pose data; performing local mapping; and performing path planning. The present invention places the angular velocity and linear velocity obtained by the wheel odometry into the gyroscope data to measure the residuals for overall Bundle Adjustment optimization, and also incorporates the wheel odometry into relocalization and pose graph optimization; binocular alignment is achieved by aligning the coordinate system of the binocular depth camera with the original RGB camera coordinate system, generating an obstacle map, applying the RANSAC algorithm to fit the wall, and then transforming the coordinate system of the RGB camera into the world coordinate system. In combination with the Manhattan hypothesis, the binocular depth camera completes the construction of a three-dimensional map of the wall.

Description

Technical Field

[0001] The present invention relates to the field of image processing technology, and in particular to a VINS-wheel odometry fusion method. Background Art

[0002] In the construction industry, there is an increasing demand for house area measurement, decoration preview, and building BIM models. Existing solutions require a large amount of manpower for measurement and software annotation. The average cost of measuring a house is 300 yuan per 100 square meters, and the price of a building's BIM model varies from hundreds of thousands to tens of millions depending on the area.

[0003] Traditional robot indoor positioning is based on UWB base station positioning (Robomaster UWB localization system) or laser positioning (DJI guidance system), but the measurement accuracy of the above two solutions is very low and cannot meet the subsequent mapping requirements.

[0004] In addition, the second problem to be solved in robot indoor three-dimensional mapping is the mapping problem. General mapping requires the use of laser surveying tools and manual observation methods for surveying, which is time-consuming, requires manual error verification, and is costly. Summary of the Invention

[0005] To address the shortcomings of existing algorithms, the present invention integrates VINS and wheel odometry through tight coupling, adds the angular velocity and linear velocity obtained by the wheel odometry to the gyroscope data, and measures the residuals for overall BundleAdjustment optimization. The wheel odometry is also incorporated into relocalization and pose graph optimization, achieving precise indoor positioning of wheeled robots. Binocular alignment is achieved by aligning the coordinate system of the binocular depth camera with the original RGB camera coordinate system. The depth is then sampled to generate an obstacle map. The RANSAC algorithm is used to fit the wall surface, and the coordinate system of the RGB camera is then converted to the world coordinate system. Combined with the Manhattan hypothesis, the binocular depth camera is used to create a three-dimensional map of the wall.

[0006] The technical solution adopted by the present invention is: a VINS-wheel odometer fusion method includes the following steps:

[0007] Step 1: Pre-integrate the wheel odometry and gyroscope data, fuse the pre-integrated data with the optical flow keypoint tracking data to obtain the robot's pose data, and build a fusion positioning module;

[0008] Furthermore, step one specifically includes:

[0009] S11. Use the RGB camera to take pictures, perform optical flow tracking through OpenCV, and select multiple landmark points before tracking the optical flow features;

[0010] S12, interrupt reading of the photoelectric encoder of the wheel motor, read the number of pulses of the motor's direction of rotation, and convert it into radians. Combined with the McKenna mother wheel model, the heading angular velocity and horizontal linear velocity data of the wheel odometer are output;

[0011] S13, using the angular velocity and linear acceleration data of the gyroscope in combination with the IMU measurement model, pre-integrate the gyroscope data and the wheel odometry data between two image frames;

[0012] Furthermore, the pre-integration formula is:

[0013]

[0014] in, Corresponding to the pre-integrated values of position, linear velocity and attitude respectively; Represents the rotation matrix of the IMU coordinate system at time t; and It is the acceleration and angular velocity measurements obtained by the IMU from the wheels and odometer; and is the offset between acceleration and angular velocity.

[0015] Furthermore, when the change range of the acceleration bias and the angular velocity bias is small, the bias is adjusted using Taylor expansion, and the formula is:

[0016]

[0017] Where J is the Jacobi and δ is the time differential.

[0018] S14. Pre-integration is applied in the time sequence between two image frames to calculate the residuals of the gyroscope, landmark points in the RGB image, camera, and wheel odometry, and obtain the VINS-wheel odometry objective function.

[0019] Furthermore, the formula of the VINS-wheel odometry objective function is:

[0020]

[0021] Among them, r p is the observation position of the landmark point, H p is the actual position of the landmark point in the camera coordinate system, Represents the current pose of the camera coordinate system, and are the residuals of the IMU wheel odometry and the residuals of the visual measurement, It is the set of IMU measurement values and wheel odometry measurement time synchronization data, and C is the set of feature points observed more than twice in the current state.

[0022] Furthermore, the residual formula of the camera and wheel odometry is:

[0023]

[0024] in, is the rotation matrix of the kth frame in the world coordinate system, is the position of the k+1th frame in the world coordinate system, Δt k is the frame interval time, g w is the gravitational acceleration in the world coordinate system, is the acceleration value of the kth frame, is the estimated score of the k-th frame pose, is the attitude of the k-th frame gyroscope in the world coordinate system, is the attitude of the k+1th frame gyroscope in the world coordinate system, is the pre-integrated value of the k-th frame velocity, is the world coordinate position of the gyroscope at the initial moment, is the estimated score at the k-th frame position, is the acceleration bias of the k+1th frame, is the acceleration bias of the kth frame, is the bias of the angular velocity of the k+1th frame, is the bias of the angular velocity of the kth frame.

[0025] Step 2: Collect data through the depth camera and perform local mapping;

[0026] Further, specifically including: using the depth camera, first aligning the depth camera coordinate system with the RGB camera; secondly sampling the depth to obtain an obstacle map, and at the same time sampling the wall; finally using the RANSAC algorithm to fit the wall surface to obtain the wall surface equation in the RGB camera coordinate system;

[0027] Step 3: Plan the route;

[0028] Further, it specifically includes:

[0029] S31. Based on the obstacle map, the A* algorithm is first used to calculate the feasible path points with grid accuracy. Then, the minimum-snap algorithm is used to plan the path and obtain the control variables of the robot's position, velocity, and acceleration with time sequence. Finally, the control commands are sent to the chassis through ROS robot communication to achieve traversal.

[0030] S32. Based on the wall equation in the RGB camera coordinate system and the robot's real-time pose, it is converted into the wall equation in the world coordinate system. Combined with the Manhattan hypothesis, global mapping is performed.

[0031] Beneficial effects of the present invention:

[0032] 1. The VINS and wheel odometry are tightly coupled and integrated. The angular velocity and linear velocity obtained by the wheel odometry are added to the gyroscope data to measure the residuals for overall bundle adjustment optimization. The wheel odometry is also incorporated into relocalization and pose graph optimization, achieving precise indoor positioning of wheeled robots.

[0033] 2. The proposed infrared binocular depth camera mapping method achieves binocular alignment by aligning the binocular depth camera's coordinate system with the original RGB camera's coordinate system. Then, the depth is sampled to generate an obstacle map. The RANSAC algorithm is used to fit the wall surface. The RGB camera's coordinate system is then converted to the world coordinate system. Combined with the Manhattan hypothesis, the binocular depth camera completes the construction of a three-dimensional map of the wall.

[0034] 3. Mapping and positioning alone are not enough to complete the robot's autonomous mapping. Therefore, we propose to integrate local mapping, positioning, and path planning into three modules. Through the ROS system, the functional nodes of all modules are integrated and communicated to complete the entire system design. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 is a flow chart of the VINS-wheel odometer fusion method of the present invention;

[0036] Figure 2 This is a key point detection diagram of the wall surface of the present invention;

[0037] Figure 3 is a sampling map of the depth map of the present invention;

[0038] Figure 4 is a path planning diagram of the present invention;

[0039] Figure 5 It is a hardware structure diagram of the present invention;

[0040] Figure 6 This is the effect diagram of the global mapping of the present invention. DETAILED DESCRIPTION

[0041] The present invention will be further described below in conjunction with the accompanying drawings and embodiments. This figure is a simplified schematic diagram, which only illustrates the basic structure of the present invention in a schematic manner, and therefore only shows the components related to the present invention.

[0042] like Figure 1As shown, a VINS-wheel odometry fusion method includes the following steps:

[0043] Step 1: Pre-integrate the wheel odometry and gyroscope data, fuse the pre-integrated data with the optical flow keypoint tracking data to obtain the robot's pose data, and build a fusion positioning module;

[0044] Specifically include:

[0045] S11. Use the RGB camera to take pictures and use OpenCV to perform optical flow tracking. Select the first 300 landmark points to track the optical flow features.

[0046] S12. Use the STM32 encoder mode to interrupt the reading of the photoelectric encoder of the four wheel motors, read the number of pulses of the four motors' steering direction, and convert it into radians. Combined with the McKenna mother wheel model, the output of the wheel odometer value includes: heading angular velocity, horizontal x, y linear velocity;

[0047] S13. Using the 3-axis angular velocity and 3-axis acceleration data of the DJI A3's gyroscope, combined with the IMU measurement model, pre-integrate the gyroscope data and the wheel odometry data in step S12 between two image frames.

[0048] Furthermore, the gyroscope-wheel odometer pre-integration formula is:

[0049]

[0050] In the kth frame to k+1 frame of IMU, Corresponding to the pre-integrated values of position, linear velocity and attitude respectively; Represents the rotation matrix of the IMU coordinate system at time t; and It is the acceleration and angular velocity measurements obtained by the IMU from the wheels and odometer; and is the offset between acceleration and angular velocity.

[0051] When the change in acceleration bias and angular velocity bias is small, the bias can be adjusted using Taylor expansion:

[0052]

[0053] Where J is the Jacobi and δ is the time differential.

[0054] S14. Pre-integration is applied in the time sequence between two image frames. When the processing in step S11 is reached, 300 landmark points are successfully obtained from one image. The residuals of the gyroscope, landmark points in the RGB image, camera, and wheel odometry are calculated, and the minimized objective function is obtained.

[0055] The processing rate of step S11 is much lower than that of step S13. The camera frequency of S11 is 20Hz, and the IMU and wheel odometer frequency of S13 is 200Hz.

[0056] The state quantity formula for the process of calculating the posture is:

[0057]

[0058] Among them, x k represents the attitude and state of the gyroscope and wheel odometry in the kth frame image, including the position, velocity, attitude relative to the world coordinate system, and the bias of the accelerometer and gyroscope; n is the total number of key frames, m is the number of feature points in the sliding window;,λ n (n=1,2...m) is the inverse depth when the feature point is observed for the nth time. It can be seen that the observed ground quantity includes the system posture and the position of the feature point. They are the position, velocity and attitude of the world coordinate system of the kth frame of the gyroscope, b a ,b g is the offset between the gyroscope and wheel odometry acceleration and angular velocity. is the transformation from the camera coordinate system to the gyroscope coordinate system, including Position transformation and Posture change.

[0059] Furthermore, after obtaining an initial estimate of the robot's pose (through direct triangulation of the landmarks), combined with the observations of the landmarks, IMU, and wheel odometry, the equation for the VINS-wheel odometry objective function is obtained as:

[0060]

[0061] Among them, r p is the observation position of the landmark point, H p is the actual position of the landmark point in the camera coordinate system, Represents the current pose of the camera coordinate system, and are the residuals of the IMU wheel odometry and the residuals of the visual measurement, It is the set of data after the IMU measurement value and the wheel odometry measurement time are synchronized. C is the set of feature points observed more than twice in the current state. By solving the Bundle-adjustment problem, the robot's pose and the actual position of the landmark are obtained.

[0062] The residual error between two consecutive frames k and k+1 of gyroscope-wheel odometry synchronization data can be defined as:

[0063]

[0064] This formula represents the residual of all the gyroscope observations (acceleration, angular velocity, pre-integral of velocity, pre-integral of position, acceleration bias, angular velocity bias). is the rotation matrix of the kth frame in the world coordinate system, is the position of the k+1th frame in the world coordinate system, Δt k is the frame interval time, g w is the gravitational acceleration in the world coordinate system, is the acceleration value of the kth frame, is the estimated score of the k-th frame pose, is the attitude of the k-th frame gyroscope in the world coordinate system, is the attitude of the k+1th frame gyroscope in the world coordinate system, is the pre-integrated value of the k-th frame velocity, is the world coordinate position of the gyroscope at the initial moment, is the estimated score at the k-th frame position, is the acceleration bias of the k+1th frame, is the acceleration bias of the kth frame, is the bias of the angular velocity of the k+1th frame, is the bias of the angular velocity of the kth frame.

[0065] Step 2: Use depth camera to collect data and build local maps;

[0066] S21. Using the depth camera, first align the depth camera coordinate system with the RGB camera. Then, sample the depth to obtain an obstacle map and sample the wall. Finally, use the RANSAC algorithm to fit the wall and obtain the equation of the wall in the RGB camera coordinate system.

[0067] Step 3: Path planning;

[0068] S31. Based on the obstacle map in step S21, the A* algorithm is first used to calculate the feasible region path points with grid accuracy; secondly, the minimum-snap algorithm is used to plan the path to obtain the control variables of the robot's position, velocity, and acceleration with time sequence; finally, the control command is sent to the chassis through ROS robot communication to achieve traversal;

[0069] S32. Based on the wall equation in S21 in the RGB camera coordinate system and the real-time pose of the robot in step S14, the equation is converted into the wall equation in the world coordinate system, and the global map is constructed in combination with the Manhattan hypothesis.

[0070] Let’s take the construction of a room as an example:

[0071] The first step is to detect the key points of the wall, such as Figure 2 As shown, the red points are landmark points that are observed in the entire sliding window, and the blue points are key points of the current frame;

[0072] like Figure 3 This is the sampling of the depth map. The depth map sampling is shown as a broken line. The fitting equation for the wall is shown as the straight line in the upper right corner. The current coordinate system is in the coordinate system of the RGB camera.

[0073] like Figure 4 For path planning, for visualization of simulation results in Matlab, path points in a grid coordinate system are constructed and the path is obtained through minimum-snap;

[0074] Figure 5 This is a hardware diagram of a wheeled robot. The locations of the D435 camera (depth camera + RGB camera), gyroscope, and photoelectric encoder are all indicated.

[0075] Figure 6 This is the global mapping effect. From left to right is the entire mapping process. The green vertical line is the sampling of the wall; the red line parallel to the ground is the wall line, and the red arrow is the current position of the chassis robot.

[0076] With the above-described preferred embodiments of the present invention as a guide, and with reference to the above description, relevant personnel are fully capable of making various changes and modifications without departing from the technical scope of this invention. The technical scope of this invention is not limited to the contents of the specification and must be determined according to the scope of the claims.

Claims

1. A VINS-wheel odometry fusion method, characterized in that: The following steps are involved: Step 1: Pre-integrate the wheel odometry and gyroscope data, and fuse the pre-integrated data with the optical flow key point tracking data to obtain the robot's pose data and build a fusion positioning module; Pre-integration is applied in the time sequence between two image frames to calculate the residuals of the gyroscope, landmarks in the RGB image, camera, and wheel odometry, and obtain the VINS-wheel odometry objective function; The objective function formula of VINS-wheel odometry is: Among them, r p is the observation position of the landmark point, H p is the actual position of the landmark point in the camera coordinate system, X represents the current pose of the camera coordinate system, and are the residuals of the IMU wheel odometry and the visual measurement, respectively. B is the set of data after the IMU measurement value and the wheel odometry measurement time are synchronized. C is the set of feature points observed more than twice in the current state. The formula for the residual error of the camera and wheel odometry is: in, is the rotation matrix of the kth frame in the world coordinate system, is the position of the k+1th frame in the world coordinate system, Δt k is the frame interval time, g w is the gravitational acceleration in the world coordinate system, is the acceleration value of the kth frame, is the estimated score of the k-th frame pose, is the attitude of the k-th frame gyroscope in the world coordinate system, is the attitude of the k+1th frame gyroscope in the world coordinate system, is the pre-integrated value of the k-th frame velocity, is the world coordinate position of the gyroscope at the initial moment, is the estimated score at the k-th frame position, is the acceleration bias of the k+1th frame, is the acceleration bias of the kth frame, is the bias of the angular velocity of the k+1th frame, is the bias of the angular velocity of the kth frame; Step 2: The depth camera collects the depth map and performs local mapping; Step 3: Plan the route.

2. The VINS-wheel odometry fusion method according to claim 1, characterized in that: Step 1 specifically includes: S11. Use the RGB camera to take pictures and perform optical flow tracking using the CalcOpticalFlowPyrLK() function of OpenCV. Select multiple landmark points before tracking the optical flow feature. S12, interrupting and reading the photoelectric encoder of the wheel motor, converting the number of steering pulses into radians, and combining it with the McKenna mother wheel model to output the heading angular velocity and horizontal linear velocity of the wheel odometer; S13. Using the angular velocity and linear acceleration data of the gyroscope and combining it with the IMU measurement model, pre-integrate the gyroscope data and the wheel odometer data between two image frames.

3. The VINS-wheel odometry fusion method according to claim 2, characterized in that: The formula for pre-integrating gyroscope data and wheel odometry data between two image frames is: in, Corresponding to the pre-integrated values of position, linear velocity and attitude respectively; Represents the rotation matrix of the IMU coordinate system at time t; and It is the acceleration and angular velocity measurements obtained by the IMU from the wheels and odometer; and is the offset between acceleration and angular velocity.

4. The VINS-wheel odometry fusion method according to claim 3, characterized in that: When the change range of acceleration bias and angular velocity bias is small, the bias is adjusted using Taylor expansion. The formula is: Where J is the Jacobi and δ is the time differential.

5. The VINS-wheel odometry fusion method according to claim 1, characterized in that: Step 2 specifically includes: using the depth camera, first aligning the depth camera coordinate system with the RGB camera; secondly, sampling the depth to obtain an obstacle map and sampling the wall at the same time; finally, using the RANSAC algorithm to fit the wall to obtain the equation of the wall in the RGB camera coordinate system.

6. The VINS-wheel odometry fusion method according to claim 1, characterized in that: Step three specifically includes: S31. Based on the obstacle map, the A* algorithm is first used to calculate the feasible path points with grid accuracy. Then, the minimum-snap algorithm is used to plan the path and obtain the control variables of the robot's position, velocity, and acceleration with time sequence. Finally, the control command is sent to the chassis to realize traversal. S32. Based on the wall equation in the RGB camera coordinate system and the robot's real-time pose, it is converted into the wall equation in the world coordinate system. Combined with the Manhattan hypothesis, global mapping is performed.

Images