Handheld / backpack SLAM device and positioning method

Abstract

A handheld / backpack SLAM device and positioning method. The present invention is divided into two parts: a handheld part and a backpack part. The two parts can be connected by bolts and aviation plugs. The data collection and positioning method using the handheld / backpack SLAM device are as follows: S1. Connect the handheld part to the backpack part through the aviation interface, put the backpack part into the backpack, fix the screen on the body with a strap, and start the device. S2. Install the ROS system and the above-mentioned sensor drivers. S3. Enable the low-latency mode of the serial port so that the combined navigation data can be transmitted to the host at a high frequency of 200Hz. S4. Publish binocular camera image data, IMU data, laser point cloud data, and GNSS data. S5. Integrate the positioning of the camera, lidar, GNSS, and IMU. The present invention proposes a high-precision semi-tight-coupled visual-IMU-lidar-INS fusion SLAM positioning algorithm for supporting the device.

Description

Technical Field

[0001] This invention belongs to the field of SLAM devices, and specifically relates to a handheld / backpack SLAM device and positioning method. Background Technology

[0002] Land resource surveys currently rely primarily on satellite remote sensing data, supplemented by field investigations. However, satellite remote sensing has drawbacks in rural land surveys, including poor accuracy of localized information and difficulty in obtaining three-dimensional data. SLAM (Simultaneous Localization and Mapping) technology can overcome these shortcomings, constructing accurate three-dimensional maps in real time and determining its own location.

[0003] Currently, multi-sensor fusion has become a major development trend in the field of SLAM, as single sensors are no longer sufficient to meet the demands of real-time positioning and mapping. GNSS (Global Navigation Satellite System) can provide precise location information, but it is susceptible to obstruction, and in rural environments, it is easily affected by trees and buildings, leading to a significant drop in accuracy. Meanwhile, while GNSS-RTK (Real-Time Dynamic Carrier Phase Differential) achieves centimeter-level accuracy, its low frequency makes it difficult to meet real-time requirements. The fusion of GNSS and IMU (Inertial Measurement Unit) to form an INS system can output high-frequency position information and also possesses a certain positioning capability even in obstructed environments. LiDAR can meet the requirements for rapid real-time mapping, but point cloud images lack texture information. Vision can acquire rich details of the external environment, but it is susceptible to lighting conditions, and in complex rural environments, it suffers from high computational demands and algorithmic limitations. Multi-sensor fusion can effectively overcome the shortcomings of single sensors, achieving better positioning and mapping results.

[0004] In the field of SLAM data acquisition equipment, many wearable products have emerged in China. A representative example is the LiBackpack DGC50 backpack-style LiDAR scanning system from Digital Green Earth. Equipped with LiDAR sensors in both horizontal and vertical directions, it supports optional high-precision GNSS equipment and panoramic cameras. Combined with SLAM technology, it can acquire high-precision 3D point cloud data within the scanning range regardless of GNSS information intensity in the scanning environment, making it suitable for indoor and outdoor multi-scene measurements. Another representative product is the NavVis VLX wearable indoor mobile mapping system, equipped with Velodyne LiDAR sensors to provide high-quality 3D measurement data capture. The NavVis VLX combines Velodyne image data with SLAM technology to provide measurement-grade point clouds via mobile devices. Its compact and versatile design allows the system to map small, scattered, and narrow spaces, as well as environments with numerous obstacles and uneven terrain. Furthermore, four 20-megapixel cameras mounted on its top form a panoramic camera system, capturing high-resolution images in every direction without obstruction of the operator's view.

[0005] Representative existing wearable data acquisition devices include the LiBackpack DGC50 backpack LiDAR scanning system and the NavVis VLX wearable indoor mobile mapping system from Digital Green Earth. The LiBackpack DGC50 backpack LiDAR scanning system only provides post-processing GNSS-RTK; its positioning algorithm does not incorporate visual information, and real-time processing of LiDAR point cloud and image data is impossible. High-precision positioning information can only be obtained through post-processing software. It lacks a handheld component and therefore lacks support for handheld scenarios. The NavVis VLX is not equipped with a GNSS module and cannot achieve satellite positioning. Its SLAM algorithm also lacks visual information and similarly lacks support for handheld scenarios. Furthermore, although integrated products are available on the market, most are expensive, lack handheld terminals, and fail to fully utilize data from various sensors. Summary of the Invention

[0006] To address the above issues, a handheld / backpack-style SLAM device and positioning method are proposed. A binocular camera, LiDAR, and GNSS integrated navigation module are installed in the handheld unit, while the processor and power module are installed in the backpack unit. This allows for both handheld data acquisition and data display when the handheld unit is mounted on the backpack. Furthermore, this invention incorporates a high-precision, high-frequency integrated navigation device. In environments with minimal occlusion, it can fuse RTK positioning data with IMU data to achieve centimeter-level pose output at up to 200Hz. Even in scenarios with significant occlusion, high-precision positioning output can be achieved through a semi-tightly coupled vision-IMU-LiDAR-INS positioning algorithm. Moreover, due to the integration of the camera, LiDAR, and IMU, high-precision positioning can be provided even in environments with little texture or poor lighting.

[0007] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0008] A handheld / backpack SLAM device, consisting of a handheld part and a backpack part, is characterized by:

[0009] The handheld device includes a lidar, a GNSS cylindrical antenna, a combined navigation device, and a camera. The head of the handheld device is cylindrical, with an inverted buckle connected to the top of the cylinder via a steel post along the edge of the cylinder. The buckle secures the lidar. The tail of the handheld device is shaped like a long handle. The GNSS cylindrical antenna is magnetically attached to the top of the handheld device. The combined navigation device is secured inside the cylinder of the handheld device with screws, and the camera is secured to the front of the handheld device.

[0010] The backpack section includes an antenna for the communication module and a screen. The backpack section is cuboid in shape. The handheld part has a connection interface between the handheld part and the backpack section at the top. The handheld part has a port at the rear. The handheld part has an aviation port at the top, which is connected to the handheld part's port via the aviation port and a data cable. The antenna of the communication module is magnetically attached to the backpack section's outer shell. The backpack section houses a processor, expansion cards, and a power module. The screen is connected to the backpack section via straps.

[0011] As a further improvement of the present invention, the lidar is a 32-line lidar.

[0012] As a further improvement of the present invention, the camera is a ZED camera.

[0013] As a further improvement of the present invention, the processor is a JETSON AGX XAVIER processor.

[0014] As a further improvement of the present invention, the integrated navigation device is an Inertial Labs INS-D integrated navigation device.

[0015] This invention provides a positioning method for a handheld / backpack SLAM device, characterized in that:

[0016] (1) Determine whether the satellite positioning of the integrated navigation is in RTK mode by the flag bit output by the integrated navigation device. If it is, only output the high-frequency positioning information of 200Hz through the integrated navigation. If not, perform pose estimation of loose coupling between vision-IMU and laser-INS.

[0017] (2) Pose estimation of laser-INS tight fusion;

[0018] 1) For each line of data obtained from a 32-line lidar scan, the curvature of each scan point is calculated using the following method:

[0019]

[0020] Where P k These are the coordinates of the laser point in the laser coordinate system;

[0021] 2) Classify the calculated points: points with high curvature are classified as edge points, and points with low curvature are classified as planar points. At the same time, ground points are removed.

[0022] 3) For adjacent LiDAR point clouds in frames i and (i+1) of the LiDAR, add a point-to-line constraint d. ek and point-to-surface constraint d pk k, u, v, w represent that these lidar points are located on different lines;

[0023]

[0024]

[0025] Define a sliding window, and for the LiDAR frames within the sliding window, optimize the following residuals using the Levenberg-Marquardt algorithm:

[0026]

[0027] The optimization variables are:

[0028] [t x ,t y ,t z ,θ roll ,θ pitch ,θ yaw (5)

[0029] The relative pose parameters in the lidar coordinate system were obtained;

[0030] 4) Calculate the relative transformation of the carrier using GNSS positioning data, acceleration and angular velocity data collected by the integrated navigation equipment, as well as feature information extracted from two consecutive frames of point cloud;

[0031] Since GNSS data is output at a frequency of 1Hz, much lower than the 10Hz frequency of lidar, the pose output by the lidar is interpolated to obtain a continuous trajectory. Using linear interpolation, the pose at time τ is calculated using the following formula: For τ k <τ<τ k+1 Interpolation ratio α = (τ - τ) k ) / (τ k+1 -τ k ),but

[0032] R τ =R k e α[ω]× (6)

[0033] t τ =(1-α)t k +αt k+1 (7)

[0034] in, R∈SO(3);

[0035] Using the acceleration and angular velocity data output by the integrated navigation device, constraints are added to the trajectory. The displacement of the trajectory is differentiated twice, and the acceleration of the trajectory in the IMU frame is obtained using the extrinsic parameters between the laser and the integrated navigation device. Similarly, the attitude of the trajectory is differentiated once to obtain the angular velocity, resulting in the following constraint equations:

[0036]

[0037] e ω =∑||γ τ -ω τ +b ω || 2 (9)

[0038] Where, α τ and γ τ For the acceleration and angular velocity output by the integrated navigation device, t τ Let ω be the displacement of the trajectory. τ The angular velocity is obtained by differentiating the trajectory attitude.

[0039] When the time or distance of movement exceeds the set threshold th, a GPS constraint is added. Since GPS elevation is inaccurate, only the longitude and latitude data of GPS data are used and converted to the local coordinate system L.

[0040]

[0041] Solve the following least squares problem to optimize the continuous lidar trajectory:

[0042]

[0043] (3) Pose estimation with tight visual-IMU coupling;

[0044] 1) Use optical flow to track feature points in the acquired visual information;

[0045] 2) Establish a sliding window, add visual constraints and IMU constraints to the visual data and IMU data in the sliding window, and optimize the pose;

[0046] 3) The variables to be optimized are:

[0047]

[0048]

[0049] Among them, I k It represents the state of the IMU at the k-th camera frame, including the IMU's position, velocity, attitude, and random walk noise at that moment, s l It is the depth of the feature points in the l-th frame;

[0050] 4) The visual constraint is: project the feature points in the i-th frame of the camera onto the j-th frame, and construct the residual as follows:

[0051]

[0052] Among them, u ci With v ci These are the coordinates of the feature points of the i-th frame image on the normalized plane. Let be the coordinates of the feature points of the j-th frame image in the camera coordinate system;

[0053] The IMU constraints are as follows: for the IMU corresponding to the k-th frame and the (k+1)-th frame of the camera, the b-th frame... k and b k+1 Frame, with the following constraints

[0054]

[0055] in, It is a pre-integrated measurement;

[0056] (4) Loosely coupled pose estimation of vision-IMU and laser-INS;

[0057] When using the Earth coordinate system as the navigation coordinate system, attitude, velocity, position, gyroscope drift, and accelerometer random walk errors are used as state variables X. GVLI :

[0058]

[0059] The system state and measurement equations are established as follows:

[0060]

[0061] Z GVLI (t)=H GVLI (t)X GVLI (t)+w GVLI (t)

[0062] Among them, A GVLI (t) is the state equation coefficient matrix for loose combination, which is related to the vision-IMU subsystem, W GVLI (t) represents the system noise in the state equation under loose combination; Z GVLI (t) represents the external measurement value during combination, obtained from the INS-laser subsystem, H GVLI (t) is the coefficient matrix of the measurement equation under loose combination; w GVLI (t) represents the measurement noise during loose assembly;

[0063] Finally, the pose of the carrier is obtained by using Kalman filtering;

[0064] (5) Re-evaluate every t time interval. If the satellite positioning re-enters RTK positioning mode, correct the past position information and redetermine the position using the pose output by the integrated navigation equipment.

[0065] Compared with the prior art, the present invention has the following advantages and beneficial effects:

[0066] This invention proposes a handheld / backpack-compatible SLAM device integrating multiple sensors. A binocular camera, LiDAR, and GNSS integrated navigation module are mounted in the handheld unit, while the processor and power module are mounted in the backpack unit. This allows for both handheld data acquisition and data display when the handheld unit is mounted on the backpack. Furthermore, this invention incorporates a high-precision, high-frequency integrated navigation device. In environments with minimal occlusion, it can fuse RTK positioning data with IMU data to achieve centimeter-level pose output at up to 200Hz. Even in scenarios with significant occlusion, it can achieve high-precision positioning output through a semi-tightly coupled vision-IMU-LiDAR-INS positioning algorithm. Moreover, due to the integration of the camera, LiDAR, and IMU, it can provide high-precision positioning even in environments with little texture and low lighting. Attached Figure Description

[0067] Figure 1 This is a partial design of the handheld SLAM device of the present invention. Figure 1 ;

[0068] Figure 2 This is a partial design of the handheld SLAM device of the present invention. Figure 2 ;

[0069] Figure 3 This is a partial design of the handheld SLAM device of the present invention. Figure 3 ;

[0070] Figure 4 This is a partial design of the handheld SLAM device of the present invention. Figure 4 ;

[0071] Figure 5 This is a schematic diagram of the structure of an embodiment of the handheld SLAM device of the present invention;

[0072] Figure 6 This is a design diagram of the positioning algorithm invented in this invention;

[0073] Component Description:

[0074] 1. Handheld device head; 2. Handheld device tail; 3. LiDAR; 4. Camera; 5. GNSS cylindrical antenna; 6. Socket; 7. Connection interface; 8. Aviation socket; 9. Antenna; 10. Backpack section; 11. Processor; 12. 4G communication module; 13. Expansion board; 14. Battery module. Detailed Implementation

[0075] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0076] This invention relates to a handheld / backpack SLAM device and positioning method.

[0077] (a) A handheld / backpack dual-use SLAM device

[0078] A handheld / backpack-compatible SLAM device includes internal components such as a ZED camera, a 32-line LiDAR (Velodyne VLP-32C), an Inertial Labs INS-D integrated navigation system, a 4G communication module, a GNSS cylindrical antenna, a JETSON AGX XAVIER processor, a Raspberry Pi touchscreen display, a battery module, and an expansion board. The ZED camera, the Inertial Labs INS-D integrated navigation system, and the expansion board are connected to the processor. The 4G communication module and the GNSS cylindrical antenna are also connected to the Inertial Labs INS-D integrated navigation system. The expansion board, display, and 32-line LiDAR are connected to the processor.

[0079] The invention consists of two parts: a handheld part and a backpack part, which can be connected by bolts and aviation plugs.

[0080] Combination Figure 1-5 As shown, the handheld part has a cylindrical head (1) with an inverted buckle connected to the top of the cylinder via a steel post along its edge. The 32-line LiDAR 3 is fixed within this buckle. The tail (2) of the handheld part is a long handle. The GNSS cylindrical antenna 5 is magnetically attached to the top of the handheld part. An Inertial Labs INS-D integrated navigation device is secured inside the cylindrical part (1) with screws, and a ZED2 camera is fixed at the front (4) of the handheld part. The backpack part is rectangular. The handheld part has a connection interface (7) at the top, and a connector (6) at the tail (2). An aviation connector (8) at the top of the handheld part connects to the connector (6) via an aviation connector and data cable. The 4G communication module's antenna (9) is magnetically attached to the outer casing. The backpack part (10) houses a processor, expansion cards, a power module, etc. The screen can be attached to the backpack part via straps.

[0081] (II) A semi-tightly coupled positioning method integrating vision, IMU, lidar, and INS. The design diagram of the positioning algorithm of this invention is shown below. Figure 6 As shown.

[0082] (1) Determine whether the satellite positioning of the integrated navigation is in RTK mode by the flag bit output by the integrated navigation device. If it is, only output the high-frequency positioning information of 200Hz through the integrated navigation. If not, perform pose estimation of loose coupling between visual-IMU and laser-INS.

[0083] (2) Pose estimation of laser-INS tight fusion.

[0084] 1) For each line of data obtained from a 32-line lidar scan, the curvature of each scan point is calculated using the following method:

[0085]

[0086] Where P k The coordinates of the laser point in the laser coordinate system.

[0087] 2) Classify the calculated points: points with high curvature are classified as edge points, and points with low curvature are classified as planar points. At the same time, ground points are removed.

[0088] 3) For adjacent LiDAR point clouds in frames i and (i+1) of the LiDAR, add point-to-line constraints. and point-to-surface constraints k, u, v, w represent that these lidar points are located on different lines.

[0089]

[0090]

[0091] Define a sliding window, and for the LiDAR frames within the sliding window, optimize the following residuals using the Levenberg-Marquardt algorithm:

[0092]

[0093] The optimization variables are:

[0094] [t x ,t y ,t z ,θ roll ,θ pitch ,θ yaw (5)

[0095] The relative pose parameters in the lidar coordinate system were obtained.

[0096] 4) The relative transformation of the carrier is calculated using GNSS positioning data, acceleration and angular velocity data collected by the integrated navigation equipment, as well as feature information extracted from two consecutive frames of point cloud.

[0097] Since GNSS data is output at a frequency of 1Hz, much lower than that of lidar (10Hz), to obtain the pose at the moment of lidar output, the pose of the lidar output is interpolated to obtain a continuous trajectory. To ensure real-time performance, a linear interpolation method is used. The pose at time τ can be calculated using the following formula: For τ k <τ<τ k+1 Interpolation ratio α = (τ - τ) k ) / (τk+1 -τ k ),but

[0098] R τ =R k e α[ω]× (6)

[0099] t τ =(1-α)t k +αt k+1 (7)

[0100] in, R∈SO(3).

[0101] To improve the accuracy of continuous trajectories, constraints are added to the trajectory using acceleration and angular velocity data output by the integrated navigation equipment. The trajectory displacement is differentiated twice, and the acceleration under IMU frames is obtained using extrinsic parameters between the laser and the integrated navigation equipment. Similarly, the trajectory attitude is differentiated once to obtain the angular velocity, resulting in the following constraint equations:

[0102]

[0103] e ω =∑||γ τ -ω τ +b ω || 2 (9)

[0104] Where, α τ and γ τ For the acceleration and angular velocity output by the integrated navigation device, t τ Let ω be the displacement of the trajectory. τ The angular velocity is obtained by differentiating the trajectory attitude.

[0105] When the time or distance of movement exceeds the set threshold th, a GPS constraint is added. Since GPS elevation is inaccurate, only the longitude and latitude data of GPS data are used and converted to the local coordinate system L.

[0106]

[0107] Solve the following least squares problem to optimize the continuous lidar trajectory:

[0108]

[0109] (3) Pose estimation with tight visual-IMU coupling.

[0110] 5) Use optical flow to track feature points in the acquired visual information.

[0111] 6) Create a sliding window, add visual constraints and IMU constraints to the visual data and IMU data in the sliding window, and optimize the pose.

[0112] 7) The variables to be optimized are:

[0113]

[0114]

[0115] Among them, I k It represents the state of the IMU at the k-th camera frame, including the IMU's position, velocity, attitude, and random walk noise at that moment, s l It is the depth of the feature points in the l-th frame.

[0116] 8) The visual constraint is: project the feature points in the i-th frame of the camera onto the j-th frame, and construct the residual as follows:

[0117]

[0118] Among them, u ci With v ci These are the coordinates of the feature points of the i-th frame image on the normalized plane. Let be the coordinates of the feature points of the j-th frame image in the camera coordinate system.

[0119] The IMU constraints are as follows: for the IMU corresponding to the k-th frame and the (k+1)-th frame of the camera, the b-th frame... k and b k+1 Frame, with the following constraints

[0120]

[0121] in, It is the measured value after pre-integration.

[0122] (4) Pose estimation with loose coupling between vision-IMU and laser-INS.

[0123] The loose combination is a loose combination of the vision-IMU and laser-INS subsystems. Since the laser-INS subsystem establishes a continuous-time trajectory model, the pose between the two subsystems at corresponding moments can be easily obtained.

[0124] When using the Earth coordinate system as the navigation coordinate system, attitude, velocity, position, gyroscope drift, and accelerometer random walk errors are used as state variables X. GVLI :

[0125]

[0126] The system state and measurement equations are established as follows:

[0127]

[0128] Z GVLI (t)=H GVLI (t)X GVLI (t)+w GVLI (t)

[0129] Among them, A GVLI (t) is the coefficient matrix of the state equation for loose combination, which is related to the vision-IMU subsystem. W GVLI (t) represents the system noise in the state equation under loose combination; Z GVLI (t) represents the external measurement value during combination, obtained from the INS-laser subsystem, H GVLI (t) is the coefficient matrix of the measurement equation under loose combination; w GVLI (t) represents the measurement noise during loose assembly.

[0130] Finally, the pose of the carrier is obtained by using Kalman filtering.

[0131] (5) Re-evaluate every t time interval. If the satellite positioning re-enters RTK positioning mode, correct the past position information and redetermine the position using the pose output by the integrated navigation equipment.

[0132] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A positioning method for a handheld / backpack SLAM device, the device comprising a handheld part and a backpack part: The handheld device includes a lidar, a GNSS cylindrical antenna, a combined navigation device, and a camera. The head of the handheld device is cylindrical, and an inverted buckle is connected to the top of the cylinder via a steel post along the edge of the cylinder. The buckle secures the lidar. The tail of the handheld device is shaped like a long handle. The GNSS cylindrical antenna is magnetically attached to the top of the handheld device. The combined navigation device is secured inside the cylinder of the handheld device with screws, and the camera is secured at the front of the handheld device. The backpack section includes an antenna and a screen for the communication module. The backpack section is cuboid. The handheld part has a connection interface between the handheld part and the backpack section at the top. The handheld part has a port at the rear. The handheld part has an aviation port at the top, which is connected to the handheld part's port via the aviation port and a data cable. The antenna of the communication module is magnetically attached to the outer shell of the backpack section. The backpack section houses a processor, expansion cards, and a power module. The screen is connected to the backpack section via straps. The positioning method is specifically as follows, characterized in that: (1) Determine whether the satellite positioning of the integrated navigation is in RTK mode by the flag bit output by the integrated navigation device. If it is, only output the high-frequency positioning information of 200Hz through the integrated navigation. If not, perform pose estimation of loose coupling between vision-IMU and laser-INS. (2) Pose estimation of laser-INS tight fusion; 1) For each line of data obtained from a 32-line lidar scan, the curvature of each scan point is calculated using the following method: ; wherein is the coordinate of the laser point in the laser coordinate system; 2) Classify the calculated points: points with high curvature are classified as edge points, and points with low curvature are classified as planar points. At the same time, ground points are removed. 3) For adjacent i-th frame lidar point cloud and i+1-th frame lidar point cloud, add point-to-line constraints and point-to-plane constraints , k, u, v, w represent that these lidar points are located on different lines; ; Define a sliding window, and for the LiDAR frames within the sliding window, optimize the following residuals using the Levenberg-Marquardt algorithm: ; The optimization variables are: ; The relative pose parameters in the lidar coordinate system were obtained; 4) Calculate the relative transformation of the carrier using GNSS positioning data, acceleration and angular velocity data collected by the integrated navigation equipment, as well as feature information extracted from two consecutive frames of point cloud; Since the data of GNSS is outputted at 1 Hz which is much lower than the frequency of 10 Hz of the laser radar, the pose of the output of the laser radar is interpolated to obtain a continuous trajectory, and a linear interpolation method is adopted to calculate the pose at the time instant t according to the following formula: For , the interpolation ratio , then ; wherein , ; Using the acceleration and angular velocity data output by the integrated navigation device, constraints are added to the trajectory. The displacement of the trajectory is differentiated twice, and the acceleration of the trajectory in the IMU frame is obtained using the extrinsic parameters between the laser and the integrated navigation device. Similarly, the attitude of the trajectory is differentiated once to obtain the angular velocity, resulting in the following constraint equations: ; wherein, and are the acceleration and angular velocity output by the combined navigation device, is the displacement of the trajectory, is the angular velocity derived by differentiating the trajectory attitude; When the time or distance of movement exceeds the set threshold th, a GPS constraint is added. Since GPS elevation is inaccurate, only the longitude and latitude data of GPS data are used and converted to the local coordinate system L. ; Solve the following least squares problem to optimize the continuous lidar trajectory: ; (3) Pose estimation with tight visual-IMU coupling; 1) Use optical flow to track feature points in the acquired visual information; 2) Establish a sliding window, add visual constraints and IMU constraints to the visual data and IMU data in the sliding window, and optimize the pose; 3) The variables to be optimized are: ; in, It represents the state of the IMU at the k-th camera frame, including the IMU's position, velocity, orientation, and random walk noise at that moment. It is the depth of the feature points in the l-th frame; 4) The visual constraint is: project the feature points in the i-th frame of the camera onto the j-th frame, and construct the residual as follows: ; in, and These are the coordinates of the feature points of the i-th frame image on the normalized plane. , , Let be the coordinates of the feature points of the j-th frame image in the camera coordinate system; The IMU constraints are as follows: for the IMU corresponding to the k-th frame and the (k+1)-th frame of the camera... and Frame, with the following constraints ; in, It is a pre-integrated measurement; (4) Loosely coupled pose estimation of vision-IMU and laser-INS; When using the Earth coordinate system as the navigation coordinate system, attitude, velocity, position, gyroscope drift, and accelerometer random walk errors are used as state variables. : ; The system state and measurement equations are established as follows: ; in, The coefficient matrix of the state equations for loose combination is related to the vision-IMU subsystem. The noise of the state equation system under loose combination; These are external measurements taken during assembly, obtained from the INS-laser subsystem. The coefficient matrix of the measurement equation when using a loose combination; Measurement noise during loose assembly; Finally, the pose of the carrier is obtained by using Kalman filtering; (5) Re-evaluate every t time interval. If the satellite positioning re-enters RTK positioning mode, then correct the past position information and re-determine the position using the pose output by the integrated navigation equipment.

2. The positioning method of a handheld / backpack SLAM device according to claim 1, characterized in that: The lidar is a 32-line lidar.

3. The positioning method of a handheld / backpack SLAM device according to claim 1, characterized in that: The camera in question is a ZED camera.

4. The positioning method of a handheld / backpack SLAM device according to claim 1, characterized in that: The processor is a JETSON AGX XAVIER processor.

5. The positioning method of a handheld / backpack SLAM device according to claim 1, characterized in that: The integrated navigation device is the Inertial Labs INS-D integrated navigation device.

Images