Mapping positioning method based on inertial navigation-laser-loopback-identification positioning tight coupling

Abstract

A mapping positioning method based on inertial navigation-laser-loopback-identification positioning tight coupling comprises an IMU odometer module, a laser radar odometer module, a loopback detection module and a visual identification positioning module, wherein the IMU odometer module and the laser radar odometer module are arranged on a mobile robot. According to the method, the basic pose of the mobile robot is estimated, the map is constructed, the global position information is generated according to detection, the corresponding weight is assigned according to the autonomously established evaluation standard, integration is carried out, the mapping function is finally completed, and high-precision positioning is achieved.

Description

Technical Field

[0001] The present invention relates to a technology in the field of robot control, specifically a mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning. Background Art

[0002] Agricultural robots are widely used in agricultural production, such as planting, watering, fertilizing and picking. To achieve autonomous agricultural production, agricultural robots need to accurately perceive and understand the environment in the greenhouse, and be able to perform high-precision positioning and navigation in complex scenes. However, the existing simultaneous localization and mapping (SLAM) technology faces the problem of signal attenuation and precision loss in indoor agricultural greenhouse scenes with severe occlusion and high repetition. The binocular camera and lidar are severely blocked by leaves and branches, and the extracted environmental information is relatively limited. At the same time, the geometric feature degradation induced by environmental repetition makes the SLAM framework based purely on the surrounding environment sensors easy to lose precision in the direction of geometric feature degradation. It is difficult to complete global positioning and loop detection in the case of repetitive and circling work. When the robot returns to the original position after circling a circle under the interference of shaking sensors, the odometer estimation result of its map construction cannot return to the original position, resulting in cumulative errors and affecting actual use. Summary of the invention

[0003] In view of the above-mentioned deficiencies in the prior art, the present invention proposes a mapping and positioning method based on the tight coupling of inertial navigation-laser-loopback-marker positioning. By estimating the basic pose of the mobile robot and building a map, and generating a global position information based on detection, the method assigns corresponding weights according to independently established evaluation criteria and performs synthesis, thereby finally completing the mapping function and achieving positioning with ideal accuracy.

[0004] The present invention is achieved through the following technical solutions:

[0005] The invention relates to a mapping and positioning method based on the tight coupling of inertial navigation-laser-loopback-marker positioning. The method collects the three-dimensional velocity, angular velocity and acceleration data of a moving robot and calculates the position, velocity and rotation matrix of a vehicle at any time and the change in the velocity, position and rotation matrix of the vehicle after a specific time; further collects laser point cloud data of the surrounding environment, performs feature extraction and matching after downsampling processing, and then generates an odometer based on a laser radar; then loopback detection is performed by calculating the Euclidean distance to obtain a transformation matrix from a laser radar coordinate system to a laser odometer coordinate system or a map coordinate system; finally, an image of the robot's surrounding environment is collected, and the position of the center point of the image in a camera coordinate system and the direction of an external normal of the image are obtained through three-dimensional reconstruction, and then the position and posture information of the robot in a base station coordinate system is obtained.

[0006] The changes in the speed, position, and rotation matrix of the car after the specific moment are obtained in the following way:

[0007] Step 1: Collect the measured angular velocity and acceleration data and eliminate the error components, specifically: ω mt =ω t + b ωt +n ωt , a mt =R BW,t (a t -g)+b at +n at , where: the variable assigned with subscript t is the relevant parameter at time t, the variable assigned with subscript m is the data collected by the sensor, parameter b is the inherent measurement deviation of the sensor, parameter n is the measurement error caused by environmental white noise, g is the gravity constant in the environment, R BW,t is the rotation matrix from the IMU coordinate system, that is, the mobile robot coordinate system, to the world coordinate system at time t; after eliminating the corresponding error components, the usable IMU angular velocity and acceleration data are obtained.

[0008] Step 2: Calculate the motion parameters of the car at the next moment: At time t+Δt, the motion parameters of the mobile system and its motion parameters at time t satisfy: v t+Δt =v t +gΔt+R t (a mt -b at -n at )Δt, Where: v is the moving speed of the car, p is the position vector of the car, is the rotation matrix from the IMU coordinate system, that is, the mobile robot coordinate system, to the world coordinate system at time t.

[0009] Step 3: Extract the position, velocity and rotation matrix of the car at any time and i to j At the moment, the change in the speed, position, and rotation matrix of the car, that is, the relative position during the movement, is: Where: Δt ij That is, from t i to j The time interval between moments, Δv ij From t i to j The change in the car's speed at time Δp ij From t i to j The change in the position of the car at the moment, ΔRij From t i to j The posture of the car at time t is the change in the rotation matrix. Among the other parameters, the parameters assigned subscripts i and j are respectively i ,t j The parameter value at the moment.

[0010] Step 4: Calculate the acceleration data of the car collected by the sensor at the initial moment, specifically: (R0-I)(g×a m,0 )=0, where: a m,0 is the acceleration data of the car collected by the sensor at the initial moment, R0 is the attitude information matrix of the car collected by the sensor at the initial moment, and I is a third-order unit matrix.

[0011] Step 5. According to the angular velocity and acceleration data obtained in steps 1-4, the motion parameters of the car, the relative position during the movement, and the acceleration data of the car collected by the sensor at the initial moment, estimate the changes in the speed, position, and rotation matrix of the car after a specific moment, that is, the speed, position, and rotation matrix at any moment are the superposition of the speed, position, and rotation matrix at the initial moment and the changes in the speed, position, and rotation matrix during the process.

[0012] The laser radar-based odometer is generated in the following way:

[0013] Step a. Periodically mark the sampled point cloud data as key frames;

[0014] Step b. Downsampling the point cloud data acquired by the sensor: omitting radar data points that are too far away from the sensor, filtering the point cloud area with too high density, omitting invalid information, reducing the point cloud scale, and improving the real-time performance of subsequent registration;

[0015] Step c. Extract line and plane features, and group the point clouds that meet the screening conditions into several sets in a given Euclidean space.

[0016] The screening conditions include: (1) satisfying the same edge feature, that is, the spatial straight line equation or the distance to the straight line is less than a threshold; (2) satisfying the same plane feature, that is, the spatial plane equation or the vertical distance to the plane is less than a threshold; at least one of the above two conditions must be met.

[0017] The extracted straight line and plane features are preferably obtained by solving the verification problem of the smoothness index c, specifically: in: is the position coordinate of a point i in the downsampled point cloud in the laser radar coordinate system at the kth key frame moment, S is the set of all points in a neighborhood of the point in the point cloud (for example, in a sphere with a certain radius), and its norm |S| is defined as the number of points. For the point cloud in a certain area, calculate its smoothness index c and determine whether it is less than or equal to a preset threshold c0. If c≤c0, the verified point cloud is recognized as a point cloud with prominent straight line and plane features. According to this judgment method, the point cloud of the laser radar after step b is screened again to obtain a point cloud subset containing prominent straight line and plane features.

[0018] Step d. Match the corresponding straight line and plane features extracted in step c between adjacent key frames, and inversely infer the position and posture changes of the laser radar in the environment based on the translation and rotation transformation. Repeat this process between each adjacent key frame to obtain an odometer based on the laser radar, which specifically includes:

[0019] d1. If the latest key frame that has been matched is t k , the key frame that completes the matching should be in the coordinate system of the laser odometer, and its unknown posture is known; the key frame that does not complete the matching is t k+1 , the key frame has not been matched and should be in the laser radar coordinate system, and its position and posture are unknown. Then the position and posture transformation problem between the two key frames is converted into: in: is the distance between the kth edges describing the same feature in the environment between adjacent keyframes, in the coordinate system at two moments. It is the position distance between two k-th planes describing the same feature in the environment between adjacent key frames in the coordinate system at two moments.

[0020] d2. Set the position and posture of the new key frame with unknown position and posture as a variable, then are all functions of unknown position and posture variables. Through Newton iteration method, optimization Make it minimum, that is, by changing the position and posture parameters, when the position and posture parameters with the best feature overlap are obtained, they are regarded as the true position and posture parameters.

[0021] d3. Get t k+1 At time , the coordinate transformation matrix R from the laser radar measured by the IMU to the laser odometer t+1 With position p t+1 , and t k+1 The radar point cloud at the moment is transformed from the lidar coordinate system to the odometer coordinate system.

[0022] d4. For the completed transformation tk+1 The point cloud in i Any p in} i , after completing the transformation t k In the Euclidean space where the point cloud in is located, find the point that is consistent with p i The three nearest points p j ,p i ,p m , calculate the position distance Where: p i ,p j ,p l ,p m Both represent position vectors.

[0023] d5. Adjust R t+1 With p t+1 ,make The minimum value is obtained, and the result of iterative calculation is t after laser point cloud feature matching. k+1 The coordinate transformation matrix R' of the laser point cloud data at the moment to the laser odometer t+1 With position p' t+1 .

[0024] d6. By fusing the transformed and matched key frame point cloud information, we can obtain the real-time updated point cloud map and the laser odometer starting from the initial pose.

[0025] The loop detection specifically includes:

[0026] Step i) During the map construction process, the vehicle extracts sensor information at specific moments as key frames through a low-frequency information collection method. For example, in the information flow of the lidar sensor of about 10Hz, a key frame is collected every 1 second for map construction and information processing, which can ensure that the map construction frequency is at least 1Hz.

[0027] Step ii) For each newly marked key frame, the three-dimensional spatial position coordinates obtained by calculating the IMU and laser odometer are recorded as p i . i With the position of each historical key frame {p1,p2,…p i-1} Compare and calculate the Euclidean distance one by one, and calculate the obtained i The historical key frame K with the smallest Euclidean distance j.For example, when the vehicle is driving in a straight line, the current vehicle position information is recorded as a key frame every 1 second. The position information of the latest key frame is compared with the historical key frames one by one. It can be found that the key frame that is closest to the position information of the latest key frame must be the second-newest key frame; and when the vehicle is driving in circles repeatedly, the position information of the latest key frame is compared with the historical key frames one by one. The key frame that is closest to the position information of the latest key frame may not be the second-newest key frame.

[0028] If the historical key frame K j Not the next new keyframe, i.e. K j ≠K i-1 , then the loop is triggered and the key frames in the construction time sequence K are extracted from the historical key frames. j The first m and the last m key frames are centered, for a total of 2m+1 key frames {K j-m ,…,K j ,…,K j+m}, where m is a preset parameter. The key frame obtained by the latest mark is matched with the above series of key frames in sequence. After the matching is completed, the key frame K is obtained. i The transformation matrix from the laser radar coordinate system to the laser odometry coordinate system (map coordinate system). The result of this transformation matrix will replace the result of the laser odometry.

[0029] If the historical key frame obtained is the next new key frame, that is, K j =K i-1 , the loop is not triggered and the position and attitude results obtained by the laser odometer are recognized.

[0030] The visual identification positioning module includes several images fixed in the environment, which are shaped like QR codes, are square in shape, have fixed side lengths and sizes, and can obtain the ID number of the image after the camera scans and decodes it. When pre-deployed, the position of each image corresponding to the ID number in the environment is known. Based on this, a base station coordinate system fixed to the world coordinate system can be proposed, and the position and posture of each image in the base station coordinate system are all known in advance.

[0031] The position and posture information of the robot in the base station coordinate system is obtained in the following way:

[0032] Step I: Assume that for a certain image block, when the camera coordinate system rotates only in the horizontal plane, that is, its pitch angle is known and fixed, the rotation matrix from the camera coordinate system to the base station coordinate system is R = R z (θ)R pitch , where R z (θ)R pitch n C =n a , p+Rz (θ)R pitch p c =p a ,p is the position of the origin of the camera coordinate system in the base station coordinate system, p c ,n c is the position and external normal direction of the image block in the camera coordinate system, p a ,n a is the position and external normal direction of the image block in the base station coordinate system, θ and p are the position and posture of the camera coordinate system in the base station coordinate system, R pitch Describes the pitch angle of the camera.

[0033] Step II: When the camera scans N image blocks in one scan, each block is solved to obtain a corresponding position information p i , that is, by solving the optimization problem The coordinate p of the camera in the base station coordinate system is obtained, where: p is the weighted sum of the distance norms between p and the position information given by each image block, and the weight is set as a function of the distance from the image block to the camera coordinate system. That is, the farther the scanned image block is from the camera, the lower the weight of the position information provided by it to guide the camera position; if the distance is too close, the weight is set to an upper bound. At the same time, for all ω i Set the normalization condition to be met, that is, ∑ω i =1; By optimizing this function, the position p of the camera in the base station coordinate system is obtained.

[0034] Step III: Since all feature image blocks are oriented in only four directions, the feature image blocks oriented in a single fixed direction in the scan can be counted according to the weight ω i The accumulated weights are summed, and the posture of the feature block in the highest direction is selected to obtain the weighted average of the results to obtain the posture angle θ. Technical Effects

[0035] The present invention integrates visual identification positioning measurement data in indoor scenes, adds reliable observation constraints to the back-end pose graph of SLAM, and performs tight coupling and joint optimization on the data of IMU odometer, laser odometer, loop detection and visual identification positioning.

[0036] Compared with the prior art, the present invention makes up for the deficiency of lack of GPS observation constraints in indoor scenes, is conducive to controlling the cumulative error of indoor 3D laser SLAM, and improving the positioning accuracy and robustness of the algorithm, thereby further improving the accuracy of indoor maps constructed by laser SLAM; by adjusting factors and weights, the measurement upper limit of high-precision sensors can be fully utilized, which can further reduce the system error of positioning and improve the positioning accuracy of the algorithm, thereby achieving a positioning effect of the order of 8 cm under experimental conditions. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is the principle diagram of the present invention;

[0038] Figure 2 It is a flow chart of the present invention;

[0039] Figure 3 This is a schematic diagram of the example scene environment;

[0040] Figure 4 It is a schematic diagram of the effect of the embodiment. DETAILED DESCRIPTION

[0041] like Figure 1 As shown, this embodiment involves a map construction and positioning system based on TAG tight coupling for implementing the above method, including: an IMU odometer module and a lidar odometer module, a loop detection module and a visual identification positioning module arranged on a mobile robot.

[0042] The IMU odometer module includes: an inertial navigation sensor and an inertial navigation odometer unit deployed on an industrial control computer, wherein: the inertial navigation sensor can calculate the acceleration information and angular acceleration information of the sensor itself according to a fixed frequency, and transmit the information to the industrial control computer in real time through a data line. The inertial navigation odometer unit calculates the real-time position and posture information of the vehicle based on the inertial navigation sensor according to the above acceleration information and angular velocity information. The real-time information flow is the IMU odometer.

[0043] The laser radar odometer module includes: a laser radar sensor and a laser radar odometer unit deployed on an industrial control computer, wherein: the laser radar sensor can perform high-frequency information sampling of its surrounding environment information at a fixed frequency to obtain a three-dimensional point cloud of the environment surface, and transmit the three-dimensional point cloud to the industrial control computer in real time through a data line; the laser radar odometer unit extracts key frames, downsamples data, extracts straight line and plane features, fuses and updates the key frames with the previously obtained point cloud map according to the high-frequency laser radar data stream, obtains the fused point cloud map, and obtains the position and posture of the latest key frame in the map coordinate system according to the fused position transformation result. The real-time information stream is the laser odometer.

[0044] The loop detection module matches and fuses the latest key frame with the historical nearest key frame and a certain number of key frames before and after it based on the latest key frame and historical key frame queue provided by the laser odometer. This result will replace the result of the laser radar odometer and become a reference for the mobile robot to determine its own position.

[0045] The visual identification positioning module includes: a visual identification image stand deployed in the environment, a monocular camera fixed on the trolley, and a visual identification unit deployed in the industrial control computer, wherein: the visual identification image stand is fixedly deployed in the environment, each stand has a unique two-dimensional information code, and the deployment position and posture of each stand are known in advance; the monocular camera can collect environmental images in real time and transmit the image data stream to the industrial control computer, and the visual identification unit calculates the position and posture information of the trolley in the environment based on the deployment information of the image stand and several two-dimensional information codes collected in the camera.

[0046] like Figure 3 As shown in the figure, it is a single-layer agricultural greenhouse with a high degree of repetition. Each orange box represents a column with a square cross-section, and the four arrows in each orange box represent four visual identification two-dimensional information code stands facing four directions. The test was carried out by an AGV car equipped with an Ubuntu operating system, including: Livox horizon laser radar sensor, motion control system, IMU sensor and depth camera. The car is equipped with a ROS control system, and the above components can realize real-time data transmission in the ROS system.

[0047] like Figure 2 As shown, the positioning method of this embodiment based on the above system includes:

[0048] Step 1: Parameter determination and mapping, that is, calibrate the world coordinate system, control the car to move in the scene, complete the map construction, and determine the transformation matrix from the visual base station coordinate system to the world coordinate system and the transformation matrix from the mapping coordinate system to the world coordinate system;

[0049] Step 2: Real-time high-precision positioning. During the mapping process, the car uses the mapping information it holds, its own lidar information, and the parameter information and noise information fed back by the acoustic beacon to obtain the transformation matrix from the car coordinate system to the world coordinate system at this moment. Specifically, it includes:

[0050] Step S201, configure the system hardware and software environment, build the ROS control platform, deploy and test the mapping and positioning algorithms, and test the acoustic beacon positioning system interface, specifically including: installing software drivers, installing other software for the computer by installing the Jetpack development tool. Install hardware drivers, and achieve data acquisition and interaction by installing the hardware drivers of the lidar, IMU, and acoustic beacon positioning system; install the ROS Melodic control system, and call and manage the above hardware and software and their communications in the ROS platform. Install the GTSAM factor graph optimization framework for calling the factor graph optimization algorithm in SLAM and relocalization projects; deploy the SLAM algorithm, relocalization algorithm, apriltag work package, initialize the working environment, and compile all projects; under the default configuration, run the SLAM algorithm test data set, complete the mapping test and verification; in the configuration file, set the SLAM map generation path, the model and topic name of the IMU sensor and the lidar sensor, which are consistent with the local sensor device.

[0051] Step S202, calling the IMU sensor, laser radar and corresponding program components to achieve real-time mapping, update the real-time IMU odometer and laser odometer, and run the visual tag global positioning and loop detection in real time, specifically including: deploying the mobile robot at any position and posture in the environment, starting the mobile robot, starting the SLAM project, starting bag recording in the terminal, controlling the mobile robot to move freely in the environment, and making the on-board laser radar scan the environment as completely and finely as possible; the IMU sensor continues to work, and feeds back the real-time speed, acceleration and angular velocity of the mobile robot according to the working frequency in the ROS system; pre-integrating the IMU sensor data, obtaining the IMU odometer and the position, velocity and rotation matrix of the IMU odometer compared to the initial posture at the current moment and any historical moment. The laser radar works continuously, and the radar point cloud scan of the current frame in the point cloud format is fed back in the ROS system according to the working frequency; the frame radar point cloud is marked as a key frame according to the time interval; the point cloud data obtained by the sensor is downsampled; the straight line and plane features are extracted, and the point clouds that meet the relevant requirements are grouped into several sets in the given Euclidean space; the key frame point cloud processed above is bound to the IMU sensor, and the key frame closest to the position obtained by the IMU sensor integration is retrieved in the historical key frame, and the key frame is taken out before and after with the time stamp as the center, and a total of 2m+1 key frames are recorded, and the features are aligned with the current key frame one by one. The implementation method of feature alignment refers to the relevant content in the aforementioned invention content section. If the registration is successful, the loop detection module is called to generate the pose transformation matrix from the historical keyframe to the current keyframe based on the laser point cloud registration based on the historical similar keyframes. At the same time, in the laser odometer, the pose transformation matrix from the first keyframe to the historical keyframe based on the laser point cloud registration is obtained. The two are synthesized to obtain the pose transformation matrix from the first keyframe to the current keyframe based on the laser point cloud registration. The pose transformation matrix is ​​stored in the laser odometer, and the factor function of the loop detection module is generated at the same time; if the registration fails, the loop detection module is not called. The pose transformation matrix from the first key frame to the previous key frame is obtained from the laser odometer, and the displacement and rotation of the mobile robot from the previous key frame to the current key frame is obtained from the IMU odometer, and the factor function of the laser odometer module is generated at the same time; the pose transformation matrix from the first key frame to the previous key frame is superimposed on the pose transformation matrix from the previous key frame to the current key frame measured based on the IMU odometer to obtain the initial value of the pose transformation matrix from the first key frame to the current key frame. The current key frame is feature matched with the previous key frame, and the initial value of the iterative algorithm is the initial value of the pose transformation matrix from the first key frame to the current key frame obtained above. After the iteration is completed, the pose transformation matrix from the first key frame to the current key frame is obtained, and the pose transformation matrix is ​​stored in the laser odometer.The above pose transformation matrix is ​​the global pose of the mobile robot obtained from the laser odometer, IMU odometer and loop detection module. The current key frame that has completed the pose transformation is fused with the historical key frames to obtain a real-time point cloud map containing the current key frame.

[0052] Step S203, calling the depth camera sensor data to obtain the visual positioning result, specifically including: the depth camera collects the visual sign image in the field of view in real time on the mobile robot ROS platform according to the working frequency. Through internal protocol and image recognition, obtain the ID number of each image, its position and orientation in the base station coordinate system and the world coordinate system; obtain the position and posture of the camera in the base station coordinate system by obtaining the transformation matrix; construct the camera factor function to be optimized through several base stations;

[0053] Step S204, using factor graphs to fuse the information of each component to obtain high-precision positioning, specifically including: laser odometer, IMU odometer and loop detection modules provide the location information of the laser odometer, and the acoustic beacon provides the acoustic location information. Construct the problem and the factor function of the four sensors. The specific construction method is as follows: The problem is to jointly optimize the position estimation result with the highest credibility based on the estimation results of the four modules. According to the factor graph theory, the position estimation result X map It can be described as: X map = argmax∏ t φ t (x t ,z t ), where: z t is the observation value of the mobile system algorithm on its position at time t; x t is the estimated result given by the algorithm at time t, φ t For x t is the probability of the optimal estimation result under the condition. Specifically, according to the iterative principle of the above four modules, the factors can be constructed as follows: Visual odometer factor Where: x k Describes the position prediction result of the car under the kth key frame, μ v represents the position of the car obtained by the visual tag positioning method, Σ v It means that μ v The covariance matrix of the measurement error determined by the two-bit information codes of several visual tags used in the image processing according to their position and angle from the camera; the laser radar odometer factor Where: x k 、x k+1 Describe the position prediction results of the car under the kth and k+1th key frames respectively, Δ LIDARrepresents the difference between two position estimation results, that is, the estimated position change between two adjacent frames, h LIDAR (x k ,x k+1 ) represents the position change between the two obtained by the laser radar mileage calculation method, Σ LIDAR It represents the covariance matrix of the parameters introduced in the measurement process; the loop detection factor Where: x k 、x k+1 Describe the position prediction results of the car under the kth and k+1th key frames respectively, Δ LOOP represents the difference between two position estimation results, that is, the estimated position change between two adjacent frames, h LOOP (x k ,x k+1 ) represents the position change between two key frames obtained by the loop detection algorithm, Σ LOOP It represents the covariance matrix of the parameters introduced in the measurement process; the inertial navigation odometer factor Where: x k 、x k+1 Describe the position prediction results of the car under the kth and k+1th key frames respectively, Δ IMU represents the difference between two position estimation results, that is, the estimated position change between two adjacent frames, h IMU (x k ,x k+1 ) represents the position change between the two obtained by the loop detection algorithm, Σ IMU Then it represents the final estimation result X of the covariance matrix of the parameters introduced in the measurement process map = argmaxf Fiducial (x k )×f LIDAR (x k ,x k+1 )×f LOOP (x k ,x k+1 )×f IMU (x k ,x k+1 ). The pre-designed factor function structure is called to import and fuse the four position information with the four factor functions, and optimization iterations are performed according to the pre-assigned corresponding weight information to finally obtain high-precision positioning information.

[0054] like Figure 3 As shown in the experimental environment, the vehicle is controlled by the above method to travel around in the real environment. The actual trajectory is compared with the path trajectory obtained by the algorithm described in this article and the typical open source algorithm LIO-SAM. Figure 4 shown.

[0055] Compared with the existing technology, in scenes with high repetition and severe occlusion, this method improves the accuracy of the map construction algorithm by introducing a visual identification positioning module, and the control system does not have large deviations; on the other hand, through the laser module and the back-end factor graph, the control system does not have jumps and instabilities in the map construction process like pure visual identification methods, thereby ensuring the stability of the system operation.

[0056] The above-mentioned specific implementation can be partially adjusted in different ways by those skilled in the art without departing from the principle and purpose of the present invention. The protection scope of the present invention shall be based on the claims and shall not be limited by the above-mentioned specific implementation. Each implementation scheme within its scope shall be subject to the constraints of the present invention.

Claims

1. A mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning, characterized in that: By collecting the three-dimensional velocity, angular velocity, and acceleration data of the moving robot and calculating the position, velocity, and rotation matrix of the car at any time, as well as the change in the velocity, position, and rotation matrix of the car after a specific time, the laser point cloud data of the surrounding environment is further collected, and feature extraction and matching are performed after downsampling processing to generate an odometer based on the laser radar. The Euclidean distance is then calculated for loop detection to obtain the transformation matrix from the laser radar coordinate system to the laser odometer coordinate system or the map coordinate system. Finally, the image of the robot's surrounding environment is collected and the position of the center point of the image in the camera coordinate system and the direction of the image's external normal are obtained through three-dimensional reconstruction, thereby obtaining the position and posture information of the robot in the base station coordinate system.

2. The mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning according to claim 1 is characterized in that: The changes in the speed, position, and rotation matrix of the car after the specific moment are obtained in the following way: Step 1: Collect the measured angular velocity and acceleration data and eliminate the error components, specifically: ω mt =ω t +b ωt +n ωt , a mt =R BW,t (a t -g)+b at +n at , where: the variable assigned with subscript t is the relevant parameter at time t, the variable assigned with subscript m is the data collected by the sensor, parameter b is the inherent measurement deviation of the sensor, parameter n is the measurement error caused by environmental white noise, g is the gravity constant in the environment, R BW,t is the rotation matrix from the IMU coordinate system, that is, the mobile robot coordinate system, to the world coordinate system at time t; after eliminating the corresponding error components, the usable IMU angular velocity and acceleration data are obtained; Step 2: Calculate the motion parameters of the car at the next moment: At time t+Δt, the motion parameters of the mobile system and its motion parameters at time t satisfy: v t+Δt =v t +gΔt+R t (a mt -b at -n at )Δt, Where: v is the moving speed of the car, p is the position vector of the car, is the rotation matrix from the IMU coordinate system, i.e. the mobile robot coordinate system, to the world coordinate system at time t; Step 3: Extract the position, velocity and rotation matrix of the car at any time and i to j At the moment, the change in the speed, position, and rotation matrix of the car, that is, the relative position during the movement, is: Where: Δt ij That is, from t i to j The time interval between moments, Δv ij From t i to j The change in the car's speed at time Δp ij From t i to j The change in the position of the car at the moment, ΔR ij From t i to j The posture of the car at time t is the change in the rotation matrix. Among the other parameters, the parameters assigned subscripts i and j are respectively i ,t j The parameter value at the moment; Step 4: Calculate the acceleration data of the car collected by the sensor at the initial moment, specifically: Among them: a m,0 is the acceleration data of the car collected by the sensor at the initial moment, R0 is the attitude information matrix of the car collected by the sensor at the initial moment, and I is a third-order unit matrix; Step 5. According to the angular velocity and acceleration data obtained in steps 1-4, the motion parameters of the car, the relative position during the movement, and the acceleration data of the car collected by the sensor at the initial moment, estimate the changes in the speed, position, and rotation matrix of the car after a specific moment, that is, the speed, position, and rotation matrix at any moment are the superposition of the speed, position, and rotation matrix at the initial moment and the changes in the speed, position, and rotation matrix during the process.

3. The mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning according to claim 1 is characterized in that: The laser radar-based odometer is generated in the following way: Step a. Periodically mark the sampled point cloud data as key frames; Step b. Downsampling the point cloud data acquired by the sensor: omitting radar data points that are too far away from the sensor, filtering the point cloud area with too high density, omitting invalid information, reducing the point cloud scale, and improving the real-time performance of subsequent registration; Step c. extracting line and plane features, and grouping the point clouds that meet the screening conditions into several sets in a given Euclidean space; Step d. Between adjacent key frames, match the corresponding straight line and plane features extracted in step c, and inversely infer the position and posture changes of the lidar in the environment based on the translation and rotation transformation. Repeat this process between each adjacent key frame to obtain a lidar-based odometer.

4. The mapping and positioning method based on tight coupling of inertial navigation-laser-loopback-marker positioning according to claim 3 is characterized in that: The screening conditions include: 1) satisfying the same edge feature, that is, the spatial straight line equation or the distance to the straight line is less than a threshold; 2) satisfying the same plane feature, that is, the spatial plane equation or the vertical distance to the plane is less than a threshold; at least one of the above two conditions must be met.

5. The mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning according to claim 3 is characterized in that: The extracted straight line and plane features are obtained by solving the verification problem of the smoothness index c, specifically: in: is the position coordinate of a point i in the downsampled point cloud in the laser radar coordinate system at the kth key frame moment, S is the set of all points in a neighborhood of the point in the point cloud, and its norm |S| is defined as the number of points. For the point cloud in a certain area, its smoothness index c is calculated to determine whether it is less than or equal to a preset threshold c0. If c≤c0, the verified point cloud is recognized as a point cloud with prominent straight line and plane features. According to this judgment method, the point cloud after the laser radar after step b is screened again to obtain a point cloud subset containing prominent straight line and plane features.

6. The mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning according to claim 3 is characterized in that: The step d specifically comprises: d1. If the latest key frame that has been matched is t k , the key frame that completes the matching should be in the coordinate system of the laser odometer, and its unknown posture is known; the key frame that does not complete the matching is t k+1 , the key frame has not completed the matching and should be in the laser radar coordinate system. Its position and posture are unknown. Then the posture transformation problem between the two key frames is converted into: in: is the distance between the kth edges describing the same feature in the environment between adjacent keyframes, in the coordinate system at two moments. is the position distance between two k-th planes describing the same feature in the environment between adjacent key frames in the coordinate system at two moments; d2. Set the position and posture of the new key frame with unknown position and posture as a variable, then They are all functions of unknown position and posture variables. Through Newton iteration method, optimization Make it minimum, that is, by changing the position and posture parameters, when the position and posture parameters with the best feature overlap are obtained, they are regarded as the true position and posture parameters; d3. Get t k+1 At the moment, the coordinate transformation matrix p from the laser radar measured by the IMU to the laser odometer t+1 With position p t+1 , and t k+1 The radar point cloud at the time is transformed from the laser radar coordinate system to the odometer coordinate system; d4. For the completed transformation t k+1 The point cloud in i Any p in} i , after completing the transformation t k In the Euclidean space where the point cloud in is located, find the point that is consistent with p i The three nearest points p j ,p l ,p m , calculate the position distance Where: p i ,p j ,p l ,p m Both represent position vectors; d5. Adjust R t+1 With p t+1 ,make The minimum value is obtained, and the result of iterative calculation is t after laser point cloud feature matching. k+1 The coordinate transformation matrix R' of the laser point cloud data at the moment to the laser odometer t+1 With position p' t+1 ; d6. By fusing the transformed and matched key frame point cloud information, we can obtain the real-time updated point cloud map and the laser odometer starting from the initial pose.

7. The mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning according to claim 1 is characterized in that: The loop detection specifically includes: Step i) During the process of map construction, the vehicle extracts sensor information at specific moments as key frames through a low-frequency information acquisition method; Step ii) For each newly marked key frame, the three-dimensional spatial position coordinates obtained by calculating the IMU and laser odometer are recorded as p i , p i With the position of each historical key frame {p1,p2,…p i-1 } Compare and calculate the Euclidean distance one by one, and calculate the obtained i The historical key frame K with the smallest Euclidean distance j ; If the historical key frame K j Not the next new keyframe, i.e. K j ≠K i-1 , then the loop is triggered and the key frames in the construction time sequence K are extracted from the historical key frames. j The first m and the last m key frames are centered, for a total of 2m+1 key frames {K j-m ,…,K j ,…,K j+m }, where m is a preset parameter. The key frame obtained by the latest mark is matched with the above series of key frames in sequence. After the matching is completed, the key frame K is obtained. i The transformation matrix from the laser radar coordinate system to the laser odometer coordinate system. The result of this transformation matrix will replace the result of the laser odometer. If the historical key frame obtained is the next new key frame, that is, K j =K i-1 , the loop is not triggered and the position and attitude results obtained by the laser odometer are recognized.

8. The mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning according to claim 1 is characterized in that: The visual identification positioning module includes several images fixed in the environment, which are shaped like QR codes and are square in shape with fixed side lengths and sizes. When the camera scans and decodes them, the ID number of the image is obtained. When pre-deployed, the position of the image corresponding to each ID number in the environment is known. Based on this, a base station coordinate system fixed to the world coordinate system is proposed, and the position and posture of each image in the base station coordinate system are all known in advance.

9. The mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning according to claim 1 is characterized in that: The position and posture information of the robot in the base station coordinate system is obtained in the following way: Step I: Assume that for a certain image block, when the camera coordinate system rotates only in the horizontal plane, that is, its pitch angle is known and fixed, the rotation matrix from the camera coordinate system to the base station coordinate system is R = R z (θ)R pitch , where R z (θ)R pitch n C =n a , p+R z (θ)R pitch p c =p a ,p is the position of the origin of the camera coordinate system in the base station coordinate system, p c ,n c is the position and external normal direction of the image block in the camera coordinate system, p a ,n a is the position and external normal direction of the image block in the base station coordinate system, θ and p are the position and posture of the camera coordinate system in the base station coordinate system, R pitch Describes the pitch angle of the camera; Step II: When the camera scans N image blocks in one scan, each block is solved to obtain a corresponding position information p i , that is, by solving the optimization problem The coordinate p of the camera in the base station coordinate system is obtained, where: the weighted sum of the distance norm between p and the position information given by each image block, and the weight is set as a function of the distance from the image block to the camera coordinate system, that is, the farther the scanned image block is from the camera, the lower the weight of the position information provided by it to guide the camera position; if the distance is too close, the upper bound of the weight is set, and at the same time, for all ω i Set the normalization condition to be met, that is, ∑ω i =1; by optimizing the function, the position p of the camera in the base station coordinate system is obtained; Step III: Since all feature image blocks are oriented in only four directions, the feature image blocks oriented in a single fixed direction in the statistical scan are calculated according to the weight ω. i The accumulated weights are summed, and the posture of the feature block in the highest direction is selected to obtain the weighted average of the results to obtain the posture angle θ.

10. The mapping and positioning method based on tight coupling of inertial navigation, laser, loopback and marker positioning according to any one of claims 1 to 9, characterized in that: include: Step 1: Parameter determination and mapping, that is, calibrate the world coordinate system, control the car to move in the scene, complete the map construction, and determine the transformation matrix from the visual base station coordinate system to the world coordinate system and the transformation matrix from the mapping coordinate system to the world coordinate system; Step 2: Real-time high-precision positioning. During the mapping process, the car uses the mapping information it holds, its own lidar information, and the parameter information and noise information fed back by the acoustic beacon to obtain the transformation matrix from the car coordinate system to the world coordinate system at this moment. Specifically, it includes: Step S201, configure the system hardware and software environment, build the ROS control platform, deploy and test the mapping and positioning algorithms, and test the acoustic beacon positioning system interface, specifically including: installing software drivers, installing other software for the computer by installing the Jetpack development tool, installing hardware drivers, and realizing data acquisition and interaction by installing the hardware drivers of the lidar, IMU, and acoustic beacon positioning system; installing the ROS Melodic control system, calling and managing the above hardware and software and their communications in the ROS platform, and installing the GTSAM factor graph optimization framework for calling the factor graph optimization algorithm in SLAM and relocalization projects; deploying the SLAM algorithm, relocalization algorithm, apriltag work package, initializing the working environment, and compiling all projects; under the default configuration, running the SLAM algorithm test data set, completing the mapping test and verification; in the configuration file, setting the SLAM map generation path, the model and topic name of the IMU sensor and the lidar sensor, which are consistent with the local sensor device; Step S202, calling the IMU sensor and the laser radar and the corresponding program components to realize real-time mapping, update the real-time IMU odometer and laser odometer, and run the visual tag global positioning and loop detection in real time, specifically including: deploying the mobile robot at any position and posture in the environment, starting the mobile robot, starting the SLAM project, starting bag recording in the terminal, controlling the mobile robot to move freely in the environment, and making the on-board laser radar perform a complete and detailed scan of the environment as much as possible; the IMU sensor continues to work, and in the ROS system, the real-time speed, acceleration and angular velocity of the mobile robot are fed back according to the working frequency; the IMU sensor data is pre-integrated to obtain the IMU odometer and the position, speed and rotation matrix of the IMU odometer compared to the initial posture at the current moment and any historical moment; the laser radar continues to work, and in the ROS system, the radar point cloud scan of the current frame in the point cloud format is fed back according to the working frequency; according to the time interval, the frame radar point cloud is marked as a key frame; the point cloud data obtained by the sensor is downsampled; the straight line and plane features are extracted, and the point clouds that meet the relevant requirements are grouped into several sets in a given Euclidean space; the above processing The key frame point cloud is bound to the IMU sensor. In the historical key frames, the key frame closest to the position obtained by the IMU sensor integration is retrieved, and the timestamp is taken as the center, and m key frames are taken before and after, and a total of 2m+1 key frames are recorded. The features are aligned with the current key frame one by one. The implementation method of feature alignment refers to the relevant content in the aforementioned invention content section. If the alignment is successful, the loop detection module is called to generate a pose transformation matrix based on the laser point cloud alignment from the historical key frame to the current key frame based on the historical similar key frames. At the same time, in the laser odometer, the pose transformation matrix from the historical key frame to the current key frame is obtained. The pose transformation matrix from the first key frame to the historical key frame based on laser point cloud registration is synthesized to obtain the pose transformation matrix from the first key frame to the current key frame based on laser point cloud registration, and the pose transformation matrix is ​​stored in the laser odometer, and the factor function of the loop detection module is generated at the same time; if the registration fails, the loop detection module is not called, and the pose transformation matrix from the first key frame to the previous key frame is obtained from the laser odometer, and the displacement and rotation of the mobile robot from the previous key frame to the current key frame are obtained from the IMU odometer, and the factor function of the laser odometer module is generated at the same time;The pose transformation matrix from the first key frame to the previous key frame is superimposed on the pose transformation matrix from the previous key frame to the current key frame measured based on the IMU odometer to obtain the initial value of the pose transformation matrix from the first key frame to the current key frame. The current key frame is feature matched with the previous key frame. The initial value of the iterative algorithm is the initial value of the pose transformation matrix from the first key frame to the current key frame obtained above. After the iteration is completed, the pose transformation matrix from the first key frame to the current key frame is obtained and stored in the laser odometer. The above pose transformation matrix is ​​the global pose of the mobile robot obtained from the laser odometer, IMU odometer and loop detection module. The current key frame that has completed the pose transformation is point cloud fused with the historical key frames to obtain a real-time point cloud map containing the current key frame. Step S203, calling the depth camera sensor data to obtain the visual positioning result, specifically including: the depth camera collects the visual sign image in the field of view in real time on the mobile robot ROS platform according to the working frequency, obtains the ID number of each image, its position and orientation in the base station coordinate system and the world coordinate system through internal protocol and image recognition; obtains the position and posture of the camera in the base station coordinate system by obtaining the transformation matrix; constructs the camera factor function to be optimized through several base stations; Step S204, using the factor graph to fuse the information of each component to obtain high-precision positioning, specifically including: the laser odometer, IMU odometer and loop detection modules provide the location information of the laser odometer, and the acoustic beacon provides the acoustic location information, and constructs the factor function of the problem and the four sensors, specifically including: Jointly optimize the position estimation result with the highest credibility. According to the factor graph theory, the position estimation result X map Description: X map = arg max∏ t φ t (x t ,z t ), where: z t is the observation value of the mobile system algorithm on its position at time t; x t is the estimated result given by the algorithm at time t, φ t For x t is the probability of the optimal estimation result under the conditions. Specifically, according to the iterative principle of the above four modules, the factors can be constructed as follows: Visual odometer factor Where: x k Describes the position prediction result of the car under the kth key frame, μ v is the position of the car obtained by the visual tag positioning method, Σ v To get μ v The covariance matrix of the measurement error determined by the two-bit information codes of several visual tags used in the image processing according to their position and angle from the camera; the laser radar odometer factor Where: x k 、x k+1 Describe the position prediction results of the car under the kth and k+1th key frames respectively, Δ LIDAR is the difference between the two position estimation results, that is, the estimated position change between two adjacent frames, h LIDAR (x k ,x k+1 ) is the position change between the two obtained by the laser radar mileage calculation method, Σ LIDAR is the covariance matrix of the parameters introduced in the measurement process; loop detection factor Where: x k 、x k+1 Describe the position prediction results of the car under the kth and k+1th key frames respectively, Δ LOOP is the difference between the two position estimation results, that is, the estimated position change between two adjacent frames, h LOOP (x k ,x k+1 ) is the position change between two key frames obtained by the loop detection algorithm, Σ LOOP is the covariance matrix of the parameters introduced in the measurement process; inertial navigation odometer factor Where: x k 、x k+1 Describe the position prediction results of the car under the kth and k+1th key frames respectively, Δ IMU is the difference between the two position estimation results, that is, the estimated position change between two adjacent frames, h IMU (x k ,x k+1 ) is the position change between the two obtained by the loop detection algorithm, Σ IMU The final estimated result X of the covariance matrix of the parameters introduced in the measurement process map = argmaxf Fiducial (x k )×f LIDAR (x k ,x k+1 )×f LOOP (x k ,x k+1 )×f IMU (x k ,x k+1 ), calling the pre-designed factor function structure, importing and fusing the four position information with the four factor functions, and performing optimization iterations according to the pre-assigned corresponding weight information, and finally obtaining high-precision positioning information.

Images