A Visual Localization Method Based on Point Cloud Map

Abstract

The present invention provides a visual positioning method based on a point cloud map, including a point cloud map generation module, a visual inertial odometer construction module, and a visual matching and positioning module based on the point cloud map. Among them, the point cloud map module establishes a high-precision point cloud map by fusing laser, IMU, and GPS information; the visual inertial odometer construction module first extracts visual feature points, uses the optical flow method to track the feature points of the front and rear frames, and performs pre-integration on the IMU, and fuses with the visual feature points to construct a visual inertial odometer, outputs the initial pose of each frame, and restores the depth of the feature points; the visual matching and positioning module based on the existing map extracts a sub-map from the point cloud map according to the initial position, projects the feature points restored by vision into the 3D space map coordinate system, and queries the nearest points in the current sub-map. For the nearest points matched by vision and the map, the RANSAC algorithm based on dual quaternion is used to optimize the pose of the current frame.

Description

Technical Field

[0001] The present invention relates to the field of robot positioning, and particularly to a visual positioning method based on a point cloud map. Background Art

[0002] With the rapid development of intelligent chips, 5G communication, and artificial intelligence technologies, intelligent robots have been widely used in various environments, such as handling robots in the industrial field, food delivery robots in the commercial field, and household sweeping robots. Positioning is the core of robot technology, and achieving accurate self-positioning of a robot is one of the most basic and important functions. The positioning module is the basis for a robot to complete subsequent tasks such as perception, planning, and grasping.

[0003] Robot positioning refers to calculating the current position in the environment through sensors carried by itself, such as cameras, lidar, IMU, wheel speedometers, millimeter-wave radars, etc. Among them, sensors such as wheel speedometers and IMU cannot perform accurate positioning alone due to the limitations of the devices themselves. Currently, there are two mainstream positioning schemes: laser-based schemes and vision-based schemes. Laser-based positioning schemes are relatively mature, and the accuracy is generally higher than that of vision, but the price of lasers is expensive, and the number of laser beams in the vertical direction is limited, resulting in less information obtained and affecting the positioning accuracy; vision-based positioning schemes are less stable than laser-based schemes and have poor robustness in dynamic environments with less feature texture and large scene changes.

[0004] According to the above introduction, the following problems still exist in the current field of robot positioning: 1) Vision-based schemes are sensitive to scene changes, have poor positioning accuracy, and cannot solve the scale only relying on a monocular camera. 2) Laser-based schemes have a high cost and are difficult to implement. Although fusing vision and laser improves the accuracy, there are disadvantages in terms of cost and system resource occupancy. In response to the above problems, this paper proposes visual inertial positioning based on a point cloud map. When a known point cloud map is available, only a camera and an IMU are used, which improves the positioning accuracy and reduces the positioning cost. Summary of the Invention

[0005] Object of the Invention: The problem to be solved by the present invention is the problems of insufficient accuracy and high cost of the current positioning schemes. A visual inertial positioning method based on a known map is proposed. By fusing laser, IMU, and GPS (Global Positioning System), a high-precision map is established, and the map is matched with the feature points restored by the visual inertial odometer to optimize the robot pose, improving the positioning accuracy while reducing the hardware cost and ensuring the robustness of the system.

[0006] To solve the above technical problems, the present invention discloses a visual-inertial positioning method based on a point cloud map. First, a point cloud map construction module, a visual-inertial odometer construction module, and a positioning module based on the point cloud map are established. Among them, the point cloud map construction module provides the map required for positioning, and the visual-inertial odometer construction module provides the initial pose for the positioning module.

[0007] The method of the present invention specifically includes: establishing a point cloud map construction module, a visual-inertial odometer construction module, and a visual matching positioning module;

[0008] The point cloud map construction module is used to fuse the inertial measurement unit IMU, laser, and GPS (Global Positioning System) to achieve multi-sensor fusion laser mapping;

[0009] The visual-inertial odometer construction module provides the initial pose based on the visual positioning of the point cloud map, and solves the pose and feature point depth of each frame of image by fusing monocular vision and the inertial measurement unit IMU;

[0010] The visual matching positioning module constructs a kd-tree (k-dimensional tree, where k is the dimension of the data) for the map points based on different modalities of point cloud and visual information, and uses a particle filter based on dual quaternions for positioning after matching with the feature points projected onto the map coordinate system.

[0011] The point cloud map construction module specifically performs the following steps:

[0012] Step 1-1: Taking the laser as the reference, align the inertial measurement unit IMU and GPS data relative to the laser. For each frame of laser point cloud, use the inertial measurement unit IMU at 100Hz for distortion removal processing;

[0013] Step 1-2: Perform inertial solution on the inertial measurement unit IMU, and use the solution result as the prediction update of the Kalman filter;

[0014] Step 1-3: For two adjacent frames of laser point clouds, use the iterative closest point ICP to calculate the relative pose, which is used as the initial pose for subsequent observation update of the Kalman filter;

[0015] Step 1-4: Perform Kalman filter prediction update;

[0016] Step 1-5: When there is a new laser point cloud frame, fuse the robot motion constraint and perform observation update;

[0017] Step 1-6: Use the current posterior pose as the next prior, and continuously iterate to solve the new posterior pose until the pose change between the last two times does not exceed the first threshold, that is, the pose converges;

[0018] Steps 1 - 7: Consider the first three frames as key frames. When the distance between the pose of the current frame and the previous key frame exceeds the second threshold, set the current frame as a key frame and add it as an optimization node to the graph optimization. The graph optimization takes the poses of consecutive N1 (usually 10) frames of key frames as optimization variables, the pixel feature points observed in the key frames as the edges of the graph optimization, and the projection error of the feature points as the cost of the graph. By increasing or decreasing the poses of the nodes in the graph, the cost of the graph is minimized. Graph optimization is a prior art;

[0019] Steps 1 - 8: When the following conditions are met: the current cumulative number of key frames exceeds X1 (usually 50) frames, or the cumulative GPS data exceeds X2 (usually 100) frames, or the number of loop detections reaches X3 (usually 10) times, perform a graph optimization;

[0020] Steps 1 - 9: For key frames, transform the key frame point cloud into the map coordinate system, take the pose of the first frame point cloud as the map origin, and filter the map to obtain a point cloud map. (A point cloud is a cluster of three - dimensional space points, and a point cloud map is composed of frames of lidar point clouds.)

[0021] In Step 1 - 2, the following formula is used for inertial solution:

[0022]

[0023]

[0024]

[0025] where,

[0026]

[0027] where is the prior rotation quantity of the inertial measurement unit IMU at time k, is the posterior rotation quantity at time k - 1, and the posterior rotation quantity at time 0 is the identity matrix. φ is the rotation vector of the inertial measurement unit IMU, and the superscript ∧ symbol represents taking the skew - symmetric matrix of the vector, is the prior velocity at time k, is the posterior velocity at time k - 1, and the posterior velocity at time 0 is 0, a k is the acceleration measured by the inertial measurement unit IMU at time k, is the bias of the inertial measurement unit IMU at time k, g is the gravity vector, and δt is the time interval of the inertial measurement unit IMU, is the prior position of the IMU at time k, is the posterior position of the inertial measurement unit IMU at time k - 1, ω kis the IMU angular velocity, and the above equation is used to calculate the prior values of rotation, velocity, and displacement from time k-1 to time k.

[0028] Steps 1-4 include: calculating the prior pose using the following formula:

[0029]

[0030]

[0031] where, is the state at time k, including velocity, position, and IMU bias; is the prior covariance of the state, is the posterior covariance of the state at time k-1, Q k is the system noise at time k, F k-1 and B k-1 are coefficient matrices, is the transpose of F k-1 as follows:

[0032]

[0033]

[0034] where I3 is the 3D identity matrix, T is the integration period of the inertial measurement unit IMU, and R k-1 is the rotation matrix at time k-1.

[0035] Steps 1-5 include:

[0036] calculating the observation y using the following formula:

[0037]

[0038] The observation y includes the position deviation δp, the velocity deviations in the y and z directions [δv b yz , and the misalignment angle δθ. The observation equation is:

[0039] y = G k δx + C k n

[0040] where n is the IMU observation noise of the inertial measurement unit, δx is the current state quantity, including velocity deviation, position deviation, and IMU deviation; G k and C k are equation coefficient matrices, as follows:

[0041]

[0042] ​

[0043]

[0044] where [R bw yz represents the rotation in the y-axis and z-axis directions, represents the velocity in the y-axis and z-axis directions, and I2 is the second-order identity matrix. are the position noise in the x-direction, position noise in the y-direction, position noise in the z-direction, velocity noise in the y-direction, velocity noise in the z-direction, angle noise in the x-direction, angle noise in the y-direction, and angle noise in the z-direction, respectively. Subsequently, the measurement update is added to calculate the posterior pose:

[0045]

[0046]

[0047]

[0048] where K k is the Kalman gain, is the posterior covariance, and I is the identity matrix. is the posterior state, and y k is the measurement at time k. Finally, based on the posterior state quantity, the posterior pose is updated:

[0049]

[0050]

[0051]

[0052]

[0053]

[0054] where is the posterior position, is the posterior velocity, is the posterior rotation quantity, is the posterior inertial measurement unit (IMU) accelerometer bias, is the posterior inertial measurement unit (IMU) gyroscope bias, is the prior position, is the prior velocity, is the prior rotation quantity, is the prior inertial measurement unit (IMU) accelerometer bias, is the prior inertial measurement unit (IMU) gyroscope bias, is the posterior position error, is the posterior velocity error,​ is the posterior angle error, is the error of the posterior accelerometer bias, is the error of the posterior gyroscope bias.

[0055] The visual-inertial odometer construction module specifically performs the following steps:

[0056] Step 2-1, preprocessing stage: Extract Harris (a visual feature point detection method named after Harris) corners in each frame of the visual camera as visual feature points, use Lukas-Kanade optical flow (a visual feature point tracking method named after Lukas and Kanade) to track the feature points in adjacent frames, and use RANSAC (Random Sample Consensus) to remove abnormal tracking points. At the same time, pre-integrate the data of the inertial measurement unit IMU to obtain the position, velocity, and rotation at the current moment;

[0057] Step 2-2, initialization stage: Only use vision to recover the poses and feature point depths of the previous certain number of frames (usually 5 to 10 frames), and finally align and solve the depth with the IMU pre-integration;

[0058] Step 2-3, back-end optimization stage: Nonlinearly optimize the errors of the inertial measurement unit IMU and the vision errors together. Among them, the error of the IMU is the deviation between the actual measurement value and the quantities to be optimized (position, velocity, rotation, and IMU parameters), and the vision error is the error after the projection of the feature point positions. Optimize the position, velocity, rotation, inertial measurement unit IMU parameters, and feature point positions of each frame, output the initial value of the current pose, and output continuous poses and visual features as the visual-inertial odometer.

[0059] The visual matching and positioning module specifically performs the following steps:

[0060] Step 3-1, preprocess the point cloud map. Before matching, perform a breadth-first search BFS clustering on the point cloud map, filter out point cloud clusters with fewer than X4 points, divide the entire point cloud map into cuboid grids with a size of 40m * 40m * 40m, and construct a sub-map centered on the grid where the current position is located. The sub-map is a 5 * 5 * 3 grid;

[0061] Step 3-2, for the feature points of the current camera vision frame, their depths have been estimated in the visual-inertial odometer. Project the feature points into the map coordinate system according to the current frame pose, and use the kd-tree algorithm to search for the nearest map points in the current sub-map as the matching corresponding points;

[0062] Step 3-3: Set the number of corresponding point pairs of the visual feature points in the map in Step 3-2 as N. Randomly arrange the N pairs of points, and take every eight pairs of points as a group. Then the corresponding points are divided into N / 8 sets of corresponding point sets;

[0063] Step 3-4: Use the dual quaternion to represent the pose of the current frame, where q r is the rotation quaternion, and q t is the quaternion representing translation. ε is the dual number, satisfying ε 2 = 0;

[0064] Set the correct matching point pair ratio in Step 3-2 as X5. The set of eight pairs of points in Step 3-3 is used as the inliers, and the RANSAC algorithm is used to estimate the current pose;

[0065] Step 3-5: Use the current pose to project the visual feature points into the map coordinate system, and query the nearest point pair. Repeat Steps 3-3 to 3-4 until the change in the rotation angle of the latest estimated pose does not exceed the third threshold, and the translation does not exceed the fourth threshold, or the number of iterations exceeds the fifth threshold. Output the final pose as the positioning result.

[0066] Beneficial effects: 1. The present invention provides a high-precision method for establishing a point cloud map, which provides a basis for robot positioning and 3D reconstruction. 2. The present invention proposes a visual-inertial positioning method based on a point cloud map, which solves the pose drift problem existing in visual positioning and laser positioning schemes. 3. The present invention provides a low-cost positioning scheme, which is only based on a point cloud map and visual and IMU sensors, and the map can be reused. 4. The present invention proposes a method for matching vision and a point cloud map, which improves the positioning accuracy. Description of the Drawings

[0067] The following further specifically describes the present invention in conjunction with the drawings and specific embodiments, and the above and / or other advantages of the present invention will become clearer.

[0068] Figure 1 is the overall flow schematic diagram of the visual-inertial positioning system based on a point cloud map.

[0069] Figure 2 is the flow schematic diagram of establishing a point cloud map by multi-sensor fusion.

[0070] Figure 3 is the flow schematic diagram of the monocular visual-inertial odometer.

[0071] Figure 4 is the flow schematic diagram of positioning based on a known point cloud map.

[0072] Figure 5It is a schematic diagram of the experimental results of visual-inertial positioning based on a point cloud map. Specific implementation manners

[0073] Figure 1 The overall process of the visual-inertial positioning system based on a point cloud map is Figure 4 a schematic diagram of the positioning process based on a known point cloud map. The method of the present invention includes a point cloud map generation module, a visual-inertial odometer construction module, and a visual matching and positioning module based on an existing point cloud map.

[0074] Figure 2 A functional schematic diagram of the point cloud map generation module, which fuses IMU, laser, and GPS to achieve multi-sensor fusion laser mapping. The specific implementation steps include:

[0075] Step 1-1: Since the time stamps of laser, IMU, and GPS data are different, taking the laser as the reference, align the IMU and GPS data relative to the laser. For each frame of laser point cloud, use the high-frequency IMU for distortion removal processing.

[0076] Step 1-2: Perform inertial solution on the IMU, and the solution result is used as the prediction update of the Kalman filter. The inertial solution steps are as follows:

[0077]

[0078]

[0079]

[0080] Where:

[0081]

[0082] The above formula calculates the prior values of rotation, velocity, and displacement from time k-1 to time k.

[0083] Step 1-3: For two adjacent frames of laser point clouds, use ICP_SVD to calculate the relative pose as the initial pose for subsequent observation update of the Kalman filter.

[0084] Step 1-4: Perform Kalman filter prediction update to calculate the prior pose:

[0085]

[0086]

[0087] Where,

[0088]

[0089]

[0090] Steps 1 - 5, when there is a new laser point cloud frame, fuse the robot motion constraints and perform observation updates. The observations are position, deviation angle, and the velocities of the y - axis and z - axis:

[0091]

[0092] Observation equation:

[0093] y = G t δx + C t n

[0094] Where,

[0095]

[0096]

[0097]

[0098] Add measurement updates and calculate the posterior pose:

[0099]

[0100]

[0101]

[0102] Finally, update the posterior pose according to the posterior state variables:

[0103]

[0104]

[0105]

[0106]

[0107]

[0108] Steps 1 - 6, take the current posterior pose as the prior for the next time, and continuously iteratively solve for the new posterior pose until the pose change between the last two times does not exceed the thresholds of 0.1° and 0.05m, that is, the pose converges.

[0109] Steps 1 - 7, when the distance between the pose of the current frame and the previous key frame exceeds 2.5m, set the current frame as a key frame and add it as an optimization node to the graph optimization.

[0110] Steps 1-8, when the following conditions are met: the current cumulative number of key frames exceeds 50 frames, or the cumulative GPS data exceeds 100 frames, or the loop detection count reaches 10 times, the system performs a graph optimization once.

[0111] Steps 1-9, for key frames, transform the point cloud of this frame into the map coordinate system, and use the pose of the first frame of lidar point cloud as the map origin. And filter the map.

[0112] Figure 2 It is a functional schematic diagram of the visual-inertial odometry construction module, which provides an initial pose based on visual positioning of the point cloud map, and solves the pose of each frame of image and the depth of feature points by fusing monocular vision and IMU. The specific implementation steps are as follows:

[0113] Steps 2-1, in the preprocessing stage, extract Harris corner points in each frame, use LK optical flow to track feature points in adjacent frames, and use RANSAC to remove abnormal tracking points. At the same time, pre-integrate the IMU data to obtain the position, velocity and rotation at the current moment.

[0114] Steps 2-2, in the initialization stage, only use vision to recover the poses and feature point depths of the previous several frames, and finally align with the IMU pre-integration to solve the depth.

[0115] Steps 2-3, in the back-end optimization stage, perform non-linear optimization on the IMU constraints and visual constraints together to optimize the position, velocity, rotation and IMU parameters of each frame. Output the initial value of the current pose.

[0116] Figure 3 It is a functional schematic diagram of the visual matching and positioning module based on the point cloud map. The visual matching and positioning module constructs a kd-tree for map points based on different modalities of point cloud and visual information, and uses a particle filter based on dual quaternions for positioning after matching with the feature points projected into the map coordinate system. The process includes:

[0117] Steps 3-1, preprocess the point cloud map. The discrete points in the map will affect the matching effect with feature points. Therefore, perform BFS clustering on the point cloud map before matching, and filter out point cloud clusters with fewer than 30 points. The distance threshold for the same class is 0.3m. Divide the entire point cloud map into several cuboid grids with the size of 40m * 40m * 40m, and construct a sub-map with the grid where the current position is located as the center. The sub-map is a 5 * 5 * 3 grid.

[0118] Steps 3-2, for the feature points of the current frame, their depths have been estimated in the visual odometry. Project these feature points into the map coordinate system according to the current frame pose, and use the kd-tree algorithm to search for the nearest map points in the current sub-map as corresponding points.

[0119] Step 3-3: Assume that the number of corresponding point pairs of the visual feature points in the map in Step 3-2 is N. Randomly arrange the N pairs of points. Taking every eight pairs of points as a group, the corresponding points can be divided into N / 8 sets of corresponding point sets.

[0120] Step 3-4: Use a dual quaternion to represent the pose of the current frame, where q r is the rotation quaternion and q t is the quaternion representing translation. Assume that the proportion of correct matching point pairs in 2-2 is 0.6. The set of eight points in Step 3-3 is used as inliers, and the RANSAC algorithm is used to estimate the current pose.

[0121] Step 3-5: Use the current pose to project the visual feature points into the map coordinate system, and query the nearest point pair. Repeat Steps 3-3 and 3-4. Each time it is repeated, the proportion of correct matching point pairs increases by 0.05. Stop until the change in the rotation angle of the latest estimated pose does not exceed 0.5°, the translation does not exceed 0.05 m, or the number of iterations exceeds 10 times. Output the final pose as the positioning result. The positioning is as Figure 5 shown. The purple line is the positioning trajectory, the red points are the visual feature points, the light green point cloud is the current sub-map, and the yellow point cloud is the peripheral global point cloud map (since the attached drawings in the specification can only be grayscale images and the colors cannot be seen, this is hereby explained).

[0122] The present invention provides a visual positioning method based on a point cloud map. There are many methods and ways to specifically implement this technical solution. The above is only the preferred embodiment of the present invention. It should be noted that for those of ordinary skill in the art, without departing from the principle of the present invention, several improvements and refinements can be made, and these improvements and refinements should also be regarded as the protection scope of the present invention. Each component not clearly defined in this embodiment can be implemented using existing technologies.

Claims

1. A visual positioning method based on a point cloud map, characterized in that, Including: Establishing a point cloud map construction module, a visual-inertial odometer construction module, and a visual matching positioning module; The point cloud map construction module is used to fuse the inertial measurement unit IMU, laser, and GPS to achieve multi-sensor fusion laser mapping; The visual-inertial odometer construction module provides an initial pose based on the visual positioning of the point cloud map. By fusing monocular vision and the inertial measurement unit IMU, it solves the pose and feature point depth of each frame of image; The visual matching positioning module constructs a kd-tree for map points based on different modalities of point cloud and visual information, and uses a particle filter based on dual quaternions for positioning after matching with the feature points projected into the map coordinate system; The point cloud map construction module specifically performs the following steps: Step 1-1: Taking the laser as the reference, align the inertial measurement unit IMU and GPS data relative to the laser. For each frame of laser point cloud, use the inertial measurement unit IMU for distortion removal processing; Step 1-2: Perform inertial solution on the inertial measurement unit IMU, and the solution result is used as the prediction update of the Kalman filter; Step 1-3: For two adjacent frames of laser point clouds, use the iterative closest point ICP to calculate the relative pose, which is used as the initial pose for subsequent Kalman filter observation update; Step 1-4: Perform Kalman filter prediction update; Step 1-5: When there is a new laser point cloud frame, fuse the robot motion constraint and perform observation update; Step 1-6: Take the current posterior pose as the next prior, and continuously iterate to solve the new posterior pose until the pose change between the last two times does not exceed the first threshold, that is, the pose converges; Step 1-7: Take the first three frames as key frames. When the distance between the pose of the current frame and the previous key frame exceeds the second threshold, set the current frame as a key frame and add it as an optimization node to the graph optimization. The graph optimization takes the poses of consecutive N1 key frames as optimization variables, the pixel feature points observed in the key frames as the edges of the graph, and the projection error of the feature points as the cost of the graph. By increasing or decreasing the poses of the nodes in the graph, the cost of the graph is minimized; Step 1-8: When the following conditions are met: the current cumulative number of key frames exceeds X1 frames, or the cumulative GPS data exceeds X2 frames, or the loop detection count reaches X3 times, perform a graph optimization; Step 1-9: For key frames, convert the key frame point cloud to the map coordinate system, take the pose of the first frame of point cloud as the map origin, and filter the map to obtain the point cloud map; The visual-inertial odometer construction module specifically performs the following steps: Step 2-1: Preprocessing stage: Extract Harris corner points in each frame of the visual camera as visual feature points, use Lukas-Kanade optical flow to track the feature points of adjacent frames, and use RANSAC to remove abnormal tracking points. At the same time, pre-integrate the inertial measurement unit IMU data to obtain the position, velocity, and rotation amount at the current moment; Step 2-2: Initialization stage: Only use vision to recover the poses and feature point depths of the previous certain frames, and finally align with the IMU pre-integration to solve the depth; Step 2-3, Back-end optimization stage: Non-linearly optimize the errors of the inertial measurement unit (IMU) and the visual errors together, optimize the position, velocity, rotation, IMU parameters of each frame, and the positions of feature points, output the initial value of the current pose, and output continuous poses and visual features as visual-inertial odometry; The visual matching and positioning module specifically performs the following steps: Step 3-1, Preprocess the point cloud map. Before matching, perform a breadth-first search (BFS) clustering on the point cloud map, filter out point cloud clusters with fewer than X4 points, divide the entire point cloud map into cuboid grids with a size of 40m * 40m * 40m, and construct a sub-map centered on the grid where the current position is located. The sub-map is a 5 * 5 * 3 grid; Step 3-2, For the feature points in the current camera visual frame, their depths have been estimated in the visual-inertial odometry. Project the feature points into the map coordinate system according to the current frame pose, and use the kd-tree algorithm to search for the nearest map points in the current sub-map as matching corresponding points; Step 3-3, Set the number of corresponding point pairs of the visual feature points in the map in Step 3-2 as N. Randomly arrange the N point pairs, and take every eight point pairs as a group. Then the corresponding points are divided into N / 8 groups of corresponding point sets; Step 3-4, use dual quaternion to represent the pose of the current frame, where q r is the rotation quaternion, q t is the quaternion representing translation, ε is the dual number, and satisfies ε 2 = 0; Set the correct matching point pair ratio in Step 3-2 as X5. The set of 8 point pairs in Step 3-3 is used as inliers, and use the RANSAC algorithm to estimate the current pose; Step 3-5, Project the visual feature points into the map coordinate system using the current pose, and query the nearest point pair. Repeat Steps 3-3 to 3-4 until the change in the rotation angle of the latest estimated pose does not exceed the third threshold, and the translation does not exceed the fourth threshold, or the number of iterations exceeds the fifth threshold. Output the final pose as the positioning result.

2. The method according to claim 1, characterized in that, In Step 1-2, the following formula is used for inertial solution: Where, where is the prior rotation of the inertial measurement unit (IMU) at time k, is the posterior rotation at time k-1. The posterior rotation at time 0 is the identity matrix. φ is the rotation vector of the IMU. The superscript ∧ symbol represents taking the skew-symmetric matrix of the vector, is the prior velocity at time k, is the posterior velocity at time k-1. The posterior velocity at time 0 is 0, a k is the acceleration measured by the IMU at time k, is the bias of the IMU at time k, g is the gravity vector, and δt is the time interval of the IMU, is the prior position of the IMU at time k, is the posterior position of the IMU at time k-1, ω k is the angular velocity of the IMU.

3. The method according to claim 2, wherein Step 1-4 includes: Calculate the prior pose using the following formula: Among them, is the state at time k, including speed, position, and IMU deviation; is the prior covariance of the state, is the posterior covariance of the state at time k-1, Q k is the system noise at time k, F k-1 and B k-1 are coefficient matrices, is the transpose of F k-1 as follows: where I3 is the three-dimensional identity matrix, T is the integration period of the inertial measurement unit (IMU), and R k-1 is the rotation matrix at time k-1.

4. The method according to claim 3, wherein Step 1-5 includes: Calculate the observation y using the following formula: The observed quantity y includes the position deviation δp, the velocity deviations in the y- and z-directions [δv b yz , and the misalignment angle δθ. The observation equation is as follows:​ y = G k δx + C k n where n is the observation noise of the inertial measurement unit (IMU), and δx is the current state quantity, including velocity deviation, position deviation, and IMU deviation; G k and C k are the coefficient matrices of the equation, as shown below: where [R bw yz represents the rotation in the y-axis and z-axis directions, represents the velocity in the y-axis and z-axis directions, I2 is the second-order identity matrix, are the position noise in the x-direction, the position noise in the y-direction, the position noise in the z-direction, the velocity noise in the y-direction, the velocity noise in the z-direction, the angle noise in the x-direction, the angle noise in the y-direction, and the angle noise in the z-direction, respectively;​ Add measurement update and calculate the posterior pose: where K k is the Kalman gain, is the posterior covariance, I is the identity matrix, is the posterior state, y k is the measurement at time k, and finally, according to the posterior state quantity, update the posterior pose: wherein is the posterior position, is the posterior velocity, is the posterior rotation amount, is the posterior inertial measurement unit (IMU) accelerometer bias, is the posterior inertial measurement unit (IMU) gyroscope bias, is the prior position, is the prior velocity, is the prior rotation amount, is the prior inertial measurement unit (IMU) accelerometer bias, is the prior inertial measurement unit (IMU) gyroscope bias, is the posterior position error, is the posterior velocity error, is the posterior angle error, is the error of the posterior accelerometer bias, is the error of the posterior gyroscope bias.

Images