CN116772834A - A mapping and positioning method for automatic inspection robots in open outdoor scenes - Google Patents

Abstract

The application provides a method for constructing and positioning an automatic inspection robot in an outdoor open scene. The method comprises the following steps: acquiring a first data set acquired by a plurality of sensors of the robot in an outdoor open scene; according to laser point cloud data, IMU data and GPS data in the first data set, constructing a three-dimensional point cloud map of an outdoor open scene through a LIO-SAM mapping algorithm, and taking the three-dimensional point cloud map as a pcd global map; acquiring a second data set acquired by the multi-sensor when the robot runs to an outdoor open scene again, wherein the second data set comprises laser point cloud data, IMU data, odometer data and GPS data; and determining target position information of the robot in the pcd global map based on the pcd global map and the second data set. Therefore, the map is built and the fusion positioning is carried out through various data, so that the positioning modes can be enriched, and the positioning precision of the robot can be improved.

Description

A mapping and positioning method for automatic inspection robots in open outdoor scenes

Technical field

The invention relates to the technical field of mobile robots, and specifically to a mapping and positioning method for an automatic inspection robot in an outdoor open scene.

Background technique

Traditional industrial inspections are inefficient, require a lot of time and human resources, are prone to omissions and misjudgments, and cannot guarantee the accuracy and comprehensiveness of inspections. In addition, in some dangerous areas, manual inspections pose great challenges. Security risks. Therefore, the development of autonomous inspection robots has become a trend. The autonomous movement of mobile robots depends on the map information of the target environment. At the same time, in order to ensure the safety of autonomous navigation, certain requirements are put forward for the positioning accuracy of mobile robots in the environment.

The existing positioning input sources for mobile robots mainly include Global Navigation Satellite System (GNSS), dynamic real-time differential system (Real-time Kinematic, RTK) and inertial navigation system (Inertial Navigation System, INS). However, positioning through satellite means is easily affected by the environment, and the positioning effect will be greatly reduced in scenes such as high-rise buildings, forests, and mountains, and the use of inertial navigation systems alone does not meet the demand for high-precision positioning.

Contents of the invention

In view of this, the purpose of the embodiments of the present application is to provide a mapping and positioning method for an automatic inspection robot in an outdoor open scene, which can improve the problem that a single robot positioning method can easily lead to inaccurate positioning.

In order to achieve the above technical objectives, the technical solutions adopted in this application are as follows:

The embodiment of the present application provides a mapping and positioning method for an automatic inspection robot in an outdoor open scene, which is applied to robots based on automatic inspection. The method includes:

Obtain the first data set collected by the robot's multi-sensor in an outdoor open scene. The first data set includes laser point cloud data, IMU data and GPS data;

According to the laser point cloud data, IMU data and GPS data in the first data set, a three-dimensional point cloud map of the outdoor open scene is constructed through the preset LIO-SAM mapping algorithm, and the three-dimensional point cloud map is used as pcd global map;

Obtain a second data set collected by the multi-sensor when the robot travels to the outdoor open scene again. The second data set includes laser point cloud data, IMU data, odometer data and GPS data;

Based on the pcd global map and the second data set, the target position information of the robot in the pcd global map is determined.

In some optional implementations, obtaining the first data set collected by the robot's multi-sensors in an outdoor open scene includes:

Obtain the laser point cloud data collected by the lidar in the multi-sensor in the outdoor open scene, and convert the format of the laser point cloud data into the velodyne point cloud format;

Obtain the IMU data collected by the inertial measurement unit in the multi-sensor in the outdoor open scene;

Obtain the GPS data collected by the GPS module in the multi-sensor in the outdoor open scene;

Based on the laser point cloud data and the IMU data, an external parameter rotation matrix is obtained;

The laser point cloud data, IMU data, GPS data and external parameter rotation matrix in velodyne point cloud format are used as the first data set.

In some optional implementations, based on the laser point cloud data, IMU data and GPS data in the first data set, a three-dimensional point cloud map of the outdoor open scene is constructed through a preset LIO-SAM mapping algorithm, include:

Perform de-distortion processing on the laser point cloud data to obtain de-distorted laser point cloud data;

Feature extraction is performed on the dedistorted laser point cloud data, and the curvature of the laser point cloud data is determined according to the curvature formula, and laser points with curvature greater than the preset threshold are used as corner points in the environment point cloud, and laser points with curvature less than the curvature are The laser point with the preset threshold is used as a plane point, and the curvature formula is as follows:

Among them, c refers to the curvature, S refers to the continuous point set i returned by the lidar in the multi-sensor in the same scan, i refers to the laser point, refers to the position of the laser point in the coordinate system of the lidar, L refers to the coordinate system of the lidar, k refers to the scanning number of the lidar, and j refers to the laser point near i;

Based on the IMU data in the first data set, construct the corresponding IMU pre-integration factor;

Convert the GPS data in the first data set into GPS data in the Cartesian coordinate system, and perform linear interpolation under the timestamp corresponding to the lidar to obtain the GPS factor;

Based on the Euclidean distance, according to the corner point, the plane point, the IMU pre-integration factor and the GPS factor, a closed-loop detection factor is constructed to perform loop detection, and a corresponding pcd file is obtained. The pcd file includes the The corner points, the plane points, the mapping trajectory and the three-dimensional point cloud map.

In some optional implementations, determining the target position information of the robot in the pcd global map based on the pcd global map and the second data set includes:

Using a preset normal distribution algorithm to perform point cloud registration on the target point cloud and the source point cloud, the first position information of the robot is obtained, where the target point cloud refers to the laser point cloud in the pcd global map Data, the source point cloud refers to the laser point cloud data in the second data set;

Through the preset extended Kalman filter algorithm, perform data fusion on the first location information and the IMU data, GPS data, and odometer data in the second data set, and output global positioning data;

The target position information of the robot in the outdoor open scene is determined based on the global positioning data and the odometer data released by the robot's chassis.

In some optional implementations, the source point cloud is the data obtained by voxel downsampling of the original point cloud data collected by the lidar in the multi-sensor and filtered with preset filtering conditions;

The target point cloud is the data obtained by performing voxel downsampling on the laser point cloud data in the pcd global map and filtering it according to the preset filtering conditions.

In some optional implementations, a preset normal distribution algorithm is used to perform point cloud registration on the target point cloud and the source point cloud to obtain the first position information of the robot, including:

Rasterize the target point cloud and divide _it into grids of specified sizes, and form a set The normal distribution parameters of , where the target point cloud mean μ and covariance matrix Σ are calculated as follows:

Computation falls to a single source point cloud in each raster The probability density of , the calculation function is as follows:

Denote the transformation parameter between the two frame point clouds as P. For all points in the source point cloud, calculate the probability density of any point in the source point cloud falling into the grid and sum it up to obtain the target The matching confidence score (P) between the point cloud and the source point cloud:

Calculate the minimum value of score(R) according to the preset Newton iteration method, that is, the optimal value of the transformation parameter P. The calculation functions of the Jacobian matrix g and Hessian matrix H of the Newton iteration method are as follows:

According to the Jacobian matrix g and Hessian matrix H, the transformation parameter P is updated as follows:

ΔP＝-H ^-1 g (7)

P←P+ΔP (8)

Iterative calculation is performed through formula (3) to formula (8) until the transformation parameter P after the iterative calculation reaches the preset value, so that the joint probability density distribution of the source point cloud and the target point cloud reaches the maximum, so as to obtain the desired first location information.

In some optional implementations, the first location information is fused with the IMU data, GPS data, and odometer data in the second data set through a preset extended Kalman filter algorithm, and the global Location data, including:

Divide the first location information, the IMU data, GPS data and odometer data in the second data set into a first group of data representing Pose type data, and a second group of data representing Twist type data;

According to the first set of data and the second set of data, through the extended Kalman filter algorithm, the current position status information of the robot is and state covariance/> To make an estimate, the calculation function is as follows:

Among them, f is the nonlinear function obtained from the motion equation and the state equation, F _t is the motion transformation matrix, Q _t is the noise covariance; update the Kalman gain K and the posterior probability distribution And calculate the final Kalman estimate/> The update method is as follows:

Among them, G _t is the observation matrix, R _t is the measurement state covariance matrix, x _t is the motion equation, and g is the nonlinear observation equation, is the output global positioning data.

In some optional implementations, the method further includes:

The three-dimensional point cloud map is converted into a two-dimensional grid map, and the two-dimensional grid map is used for path planning of the robot.

In some optional implementations, converting the three-dimensional point cloud map into a two-dimensional raster map includes:

Based on the preset height range, perform pass-through filtering on the laser point cloud data in the three-dimensional point cloud map to obtain filtered first laser point cloud data;

Calculate the proximity of each laser point in the first laser point cloud data, and filter out the laser point cloud data whose proximity is less than the preset proximity threshold in the first laser point cloud data to obtain the second laser point cloud data. Point cloud data, wherein the proximity amount is the total number of laser points existing in the area within a specified radius with any laser point as the center, and the total number of laser points does not include the any laser point as the center;

Convert the new three-dimensional point cloud map corresponding to the second laser point cloud data into a two-dimensional grid map, and use a denoising tool to erase the noise data in the two-dimensional grid map.

The invention adopting the above technical solution has the following advantages:

In the technical solution provided by this application, when the robot is mapping, it uses the LIO-SAM algorithm to combine laser point cloud data, IMU data, GPS data and other data to create a three-dimensional point cloud map. This is conducive to obtaining high-precision map. In addition, during the subsequent robot positioning, based on the re-collected laser point cloud data, IMU data, odometer data, GPS data and other data, high-precision three-dimensional point cloud maps are used for fusion positioning. This can enrich the positioning methods. , improve the positioning accuracy of the robot, and improve the problem that the single positioning method of the robot easily leads to inaccurate positioning.

Description of drawings

The application can be further illustrated by the non-limiting examples given in the accompanying drawings. It should be understood that the following drawings only show certain embodiments of the present application, and therefore should not be regarded as limiting the scope. For those of ordinary skill in the art, without exerting creative efforts, they can also Other relevant drawings are obtained based on these drawings.

Figure 1 is a schematic flowchart of a mapping and positioning method for an automatic inspection robot in an outdoor open scene provided by an embodiment of the present application.

Figure 2 is a schematic diagram of a two-dimensional grid map obtained by mapping in an outdoor open grassland scene provided by an embodiment of the present application.

Detailed ways

The present application will be described in detail below with reference to the drawings and specific embodiments. It should be noted that in the drawings or the description of the specification, similar or identical parts use the same figure numbers. Implementations not shown or described in the drawings The method is a form known to those of ordinary skill in the technical field. In the description of the present application, the terms "first", "second", etc. are only used to differentiate the description and cannot be understood as indicating or implying relative importance.

Please refer to Figure 1. This application provides a mapping and positioning method for an automatic inspection robot in an outdoor open scene, which is referred to as the mapping and positioning method for short. This mapping and positioning method can be applied to a robot with automatic inspection function, and the robot executes or realizes each step of the method.

Among them, the robot can include a processing module, a storage module and multiple sensors. Among them, multi-sensors can include lidar, inertial measurement unit (Inertial Measurement Unit, IMU), global positioning system (Global Positioning System, GPS) module and odometer. The processing module can be used for mapping or fusion positioning of data collected by multiple sensors. The storage module can store the created map and data collected by multiple sensors.

In this embodiment, the mapping and positioning method may include the following steps:

Step 110: Obtain the first data set collected by the robot's multi-sensor in an outdoor open scene. The first data set includes laser point cloud data, IMU data and GPS data;

Step 120: Construct a three-dimensional point cloud map of the outdoor open scene through the preset LIO-SAM mapping algorithm based on the laser point cloud data, IMU data and GPS data in the first data set, and add the three-dimensional point cloud map The cloud map serves as the pcd global map;

Step 130: Obtain the second data set collected by the multi-sensor when the robot travels to the outdoor open scene again. The second data set includes laser point cloud data, IMU data, odometer data and GPS data;

Step 140: Determine the target position information of the robot in the pcd global map based on the pcd global map and the second data set.

Each step of the mapping and positioning method will be explained in detail below, as follows:

Before executing step 110, the robot's system and multi-sensors need to be deployed. Among them, deployment operations can include:

Deploy the robot's operating system ROS in the Ubuntu system, and test topic services and other related communications;

Deploy a 16-line lidar on the robot body, connect the power cord and network cable of the 12V DC power box for data transmission, and configure relevant network segment information;

Add the inertial measurement unit, calibrate the internal parameters of the system, download the driver and communicate with ROS through the serial port;

Deploy a high-precision MEMS integrated navigation system to perform GPS data calibration in an open and unobstructed outdoor environment;

Initialize the robot chassis system, supply power to other modules of the robot (such as the robot's motor, processing module, etc.) through the chassis power supply, and connect the chassis network cable to test whether the robot's chassis communication is normal. After the robot completes the deployment of the system and multi-sensors and initializes the system, the operation of step 110 can be performed.

In step 110, the frequency at which each sensor in the multi-sensor collects the first data set and the second data set described below can be flexibly set according to the actual situation.

The robot can perform mapping based on the first data set collected in real time to update the map. In addition, the robot can also perform fusion positioning based on the second data set collected in real time, and update the positioning.

In this embodiment, step 110 obtains the first data set collected by the robot's multi-sensors in an outdoor open scene, which may include:

Step 111, obtain the laser point cloud data collected by the lidar in the multi-sensor in the outdoor open scene, and convert the format of the laser point cloud data into the velodyne point cloud format;

Step 112: Obtain the IMU data collected by the inertial measurement unit in the multi-sensor in the outdoor open scene;

Step 113, obtain the GPS data collected by the GPS module in the multi-sensor in the outdoor open scene;

Step 114: Obtain an external parameter rotation matrix based on the laser point cloud data and the IMU data;

Step 115: Use the laser point cloud data, IMU data, GPS data and external parameter rotation matrix in the velodyne point cloud format as the first data set.

In step 112, the original IMU data collected by the inertial measurement unit needs to undergo coordinate transformation, that is, the original IMU data is transformed into the coordinate system of the lidar. Among them, the coordinates of the IMU and the lidar are usually 90° different on the Z axis. Therefore, the coordinate transformation can be achieved by simply rotating the yaw of the IMU by 90°. Among them, IMU data and laser point cloud data are calibrated data.

In step 113, after downloading the GPS data reading source code, you can fix the GPS serial port number and modify the serial port number and baud rate in the code. The modified baud rate can be flexibly set according to the actual situation to ensure that the modified baud rate is conducive to improving the integrity of data transmission while ensuring the transmission rate. For example, if the baud rate is set too high, data loss or incomplete transmission may occur during the transmission process; if the baud rate is set too low, the data transmission speed will slow down, affecting transmission efficiency.

In step 114, based on the calibrated laser point cloud data and IMU data, an external parameter rotation matrix can be created.

In this embodiment, step 120 constructs a three-dimensional point cloud map of the outdoor open scene through the preset LIO-SAM mapping algorithm based on the laser point cloud data, IMU data and GPS data in the first data set. include:

Step 121: Perform de-distortion processing on the laser point cloud data to obtain de-distorted laser point cloud data;

Step 122: Perform feature extraction on the dedistorted laser point cloud data, determine the curvature of the laser point cloud data according to the curvature formula, and use laser points with a curvature greater than a preset threshold as corner points in the environment point cloud. Laser points with curvature less than the preset threshold are regarded as plane points, and the curvature formula is as follows:

Among them, c refers to the curvature, S refers to the continuous point set i returned by the lidar in the multi-sensor in the same scan, i refers to the laser point, refers to the position of the laser point in the coordinate system of the lidar, L refers to the coordinate system of the lidar, k refers to the scanning number of the lidar, and j refers to the laser point near i;

Step 123: Construct the corresponding IMU pre-integration factor based on the IMU data in the first data set, where the IMU pre-integration factor is used to provide a priori pose information for the radar factor;

Step 124: Convert the GPS data in the first data set into GPS data in the Cartesian coordinate system, and perform linear interpolation under the timestamp corresponding to the lidar to obtain the GPS factor;

Step 125: Based on the Euclidean distance, according to the corner point, the plane point, the IMU pre-integration factor, and the GPS factor, a closed-loop detection factor is constructed for loop closure detection, and the corresponding pcd file (pcd is a A file format for storing point cloud data), the pcd file includes the corner points, the plane points, the mapping trajectory and the three-dimensional point cloud map (ie, pcd global map).

Understandably, when mapping, in an open and unobstructed outdoor environment, engineers can remotely control the robot to drive on the open grass to test the operation of the robot in the grass environment. During the test, each sensor of the robot can collect data in real time, and use various positioning data collected in each frame as the first data set.

In step 121, the robot can calculate the movement of the lidar during the acquisition process, and perform rotation and translation compensation according to the relative time of each laser point in each frame, thereby achieving dedistortion processing of the laser point cloud data.

In step 122, feature extraction can be understood as extracting corner points and plane points from laser points. That is, laser points with curvature greater than the preset threshold are regarded as corner points in the environment point cloud, and laser points with curvature less than the preset threshold are regarded as plane points. The calculation method of the curvature formula can be understood as: the sum of curvature = (the distance difference between the current laser point and the nearby laser point/the value of the current laser point) and then average it.

In this embodiment, various factors refer to the processed data of each sensor. For example, the various factors function as follows:

IMU pre-integration factor: The IMU pre-integration factor is used to associate the IMU measurement value with the robot's motion state, thereby improving the accuracy of state estimation. The IMU preintegration factor includes the robot's acceleration and angular velocity measurements, as well as timestamp information.

Radar factors: Radar factors are used to correlate lidar measurements with the robot's motion state for mapping. Radar factors include LiDAR point cloud data and pose information related to the robot's position and state.

GPS factor: GPS factor is used to associate the measurement value of the GPS module with the motion status of the robot to provide global positioning information. GPS factors include the position and timestamp information of the GPS module, as well as pose information related to the robot's position and status.

Loop closure detection factor: The loop closure detection factor is used to detect closed loops in the map and optimize the map. The loop closure detection factor includes the relative transformation information between the two poses, and the covariance information between them.

In step 125, mapping can be achieved by utilizing the LIO-SAM mapping algorithm. Among them, loop detection in the LIO-SAM mapping algorithm is implemented through scan-to-map matching. The specific steps can be as follows:

Based on the point cloud data collected at different times in the first data set, match the point cloud data of the current frame with the previous map point cloud data to obtain an initial pose estimate;

Use the ICP algorithm to refine the initial pose to obtain a more accurate pose estimate;

Determine which of the current frame and previous frames form a closed loop. If the closed-loop conditions are met, mark the current frame and previous frames that meet the closed-loop conditions as closed-loop, and add closed-loop constraints to the optimization problem;

An optimization algorithm is used to jointly optimize all constraints to obtain the optimal map and robot trajectory, thereby obtaining a three-dimensional point cloud map.

In step 130, each data in the second data set is a collection of data collected by the robot's multi-sensors after the robot has stored the three-dimensional point cloud map of the outdoor open scene and then drives in the outdoor open scene again. The second data set is used to provide data support for the fusion positioning of the robot.

In this embodiment, step 140 determines the target position information of the robot in the pcd global map based on the pcd global map and the second data set, which may include:

Step 141: Use a preset normal distribution algorithm (Normal Distribution Transform, NDT) to perform point cloud registration on the target point cloud and the source point cloud to obtain the first position information of the robot, where the target point cloud refers to The laser point cloud data in the pcd global map, the source point cloud refers to the laser point cloud data in the second data set;

Step 142: Perform data fusion on the first location information and the IMU data, GPS data, and odometer data in the second data set through the preset Extended Kalman Filter algorithm (EKF), and output global positioning data;

Step 143: Determine the target position information of the robot in the outdoor open scene based on the global positioning data and the odometer data released by the robot's chassis.

Understandably, in step 141, the robot can use the normal distribution algorithm to perform point cloud registration, preload the pcd global map built in advance, and use the point cloud data in the pcd global map as the target point cloud, and then match it with The source point cloud input by the lidar is matched frame by frame to obtain the optimal conversion relationship, and the position information of the robot is finally output as the first position information.

In step 141, both the target point cloud and the source point cloud are preprocessed data. The preprocessing method is: perform voxel downsampling on the original point cloud data collected by lidar and the laser point cloud data in the pcd global map, that is, set the specified sampling density, and then perform voxel downsampling on the point cloud data in the pcd global map and the The original point cloud data input by the lidar is filtered. By performing preprocessing operations, the point cloud density can be reduced while retaining most of the characteristics of the point cloud data, which has a significant effect on improving the data processing speed.

Understandably, the source point cloud is the data obtained by voxel downsampling of the original point cloud data collected by the lidar in the multi-sensor and filtered with preset filtering conditions. The target point cloud is the data obtained by voxel downsampling of the laser point cloud data in the PCD global map and filtered with preset filtering conditions. The preset filtering conditions can be flexibly set according to the actual situation.

In this embodiment, step 141 may include:

Rasterize the target point cloud and divide _it into grids of specified sizes, and form a set The normal distribution parameters of , where the target point cloud and the source point cloud are regarded as two frame point clouds S1 and S2 respectively. The target point cloud mean μ and covariance matrix Σ are calculated as follows:

Computation falls to a single source point cloud in each raster Probability density of , calculation function/> as follows:

Denote the transformation parameter between the two frame point clouds as P. For all points in the source point cloud, calculate the probability density of any point in the source point cloud falling into the grid and sum it up to obtain the target The matching confidence score (P) between the point cloud and the source point cloud:

Among them, n refers to the total number of point clouds in the source point cloud, and i is an integer from 1 to n;

Calculate the minimum value of score(R) according to the preset Newton iteration method, that is, the optimal value of the transformation parameter P. The calculation functions of the Jacobian matrix g and Hessian matrix H of the Newton iteration method are as follows:

According to the Jacobian matrix g and Hessian matrix H, the transformation parameter P is updated as follows:

ΔP＝-H ^-1 g (7)

P←P+ΔP (8)

For each laser point in the source point cloud, iterative calculation is performed through formula (3) to formula (8), so that the updated P reaches the preset value to optimize and solve the optimal point cloud between the two frames. Transform until the joint probability density distribution of the source point cloud and the target point cloud reaches the maximum after iterative calculation, and achieve the best match between the two point clouds to obtain the first position information.

In step 142, the robot uses the extended Kalman filter algorithm to perform data fusion in a loosely coupled manner. The input pose data used for fusion includes point cloud registration and positioning information, IMU data, GPS data, and odometer data. The fusion process may include first classifying each input data, dividing the data into Pose type data and Twist type data, and putting them into two different structures respectively as two different sets of measurement data. Once the preprocessing of data classification is complete, it can be processed through the extended Kalman filter. As an example, step 142 may include:

Divide the first location information, the IMU data, GPS data and odometer data in the second data set into a first group of data representing Pose type data, and a second group of data representing Twist type data;

According to the first set of data and the second set of data, through the extended Kalman filter algorithm, the current position status information of the robot is and state covariance/> To make an estimate, the calculation function is as follows:

Among them, f is the nonlinear function obtained from the motion equation and the state equation, F _t is the motion transformation matrix, Q _t is the noise covariance; update the Kalman gain K and the posterior probability distribution And calculate the final Kalman estimate/> The update method is as follows:

Among them, G _t is the observation matrix, R _t is the measurement state covariance matrix, x _t is the motion equation, and g is the nonlinear observation equation, is the output global positioning data after fusion.

In step 143, based on the global positioning data and combined with the local positioning data output by the robot chassis (that is, the odometer data), the precise positioning of the inspection robot can be completed. Among them, in step 143, the odometer output data is used as local positioning data to estimate the initial pose of the robot after it is started. In step 142, the odometer data is used to correct the point cloud registration and positioning during the data fusion process to prevent inaccurate positioning of the robot after deviations in point cloud registration. In the early mapping process, no need to Use odometer data.

In this embodiment, the method may also include:

Step 150: Convert the three-dimensional point cloud map into a two-dimensional grid map, and the two-dimensional grid map is used for path planning of the robot.

Please refer to Figure 2. As an example, after mapping in an outdoor open grassland scene to obtain a three-dimensional point cloud map, and then converting the three-dimensional point cloud map into a two-dimensional raster map, the resulting two-dimensional raster map can be as shown in picture 2.

Further, step 150 converts the three-dimensional point cloud map into a two-dimensional grid map, which may include:

Step 151: Perform pass-through filtering on the laser point cloud data in the three-dimensional point cloud map based on the preset height range to obtain filtered first laser point cloud data. In this way, point cloud data outside the height range can be filtered out;

Step 152: Calculate the proximity of each laser point in the first laser point cloud data, and filter out the laser point cloud data whose proximity is less than the preset proximity threshold in the first laser point cloud data to obtain The second laser point cloud data, in this way, can filter out some isolated points, where the proximity amount is the total number of laser points existing in the area within a specified radius centered on any laser point, and the total number of laser points does not include Said any laser point as the center;

Step 153: Convert the new three-dimensional point cloud map corresponding to the second laser point cloud data into a two-dimensional raster map, and erase the noise in the two-dimensional raster map using a denoising tool (such as ArcGIS software tool). Noisy data.

In this embodiment, the preset height range, the preset proximity threshold, and the specified radius can be flexibly set according to actual conditions.

Understandably, the robot reads the multi-sensor data, extracts the key frames of the lidar scan (that is, the laser point cloud data), and integrates the IMU pre-integrated data for pose optimization. Through the tightly coupled IMU and radar odometry structure, Combined with GPS data for precise positioning and mapping, the obtained point cloud map is finally converted into a two-dimensional raster map through filtering. Among them, the obtained two-dimensional grid map is conducive to providing higher-precision path planning and navigation when the robot subsequently plans a navigation path.

In this embodiment, LIO-SAM is a three-dimensional mapping algorithm based on sensors such as lidar, IMU, and GPS. It can build high-precision maps in large outdoor scene environments by constructing factor graph constraints. Therefore, for large areas of outdoor grassland with few feature points, using a variety of sensors combined with advanced mapping algorithms can help us improve positioning accuracy, establish high-precision maps, and effectively promote the development of mobile robot autonomous inspection technology. .

In this embodiment, for mapping large outdoor scenes, 3D lidar is mainly used and integrated with multiple sensors. Compared with using cameras, the multi-line lidar used by the robot in this embodiment has long range, It has the advantages of large scanning range, high precision and not affected by light.

Based on the above design, the robot combines multiple sensors such as radar, IMU, GPS, odometer and MEMS, and uses the LIO-SAM algorithm to complete simultaneous positioning and mapping in large outdoor scenes, and generates a clear two-dimensional grid map for easy movement. Autonomous inspection and navigation of robots. In addition, the extended Kalman filter algorithm can be used to achieve fusion positioning of multi-sensor data, which is beneficial to ensuring the accuracy and stability of positioning.

It should be noted that when the robot uses the method provided by this application for mapping and positioning, it can not only be used in outdoor open scenes, but also can be used in scenes such as high buildings, forests, and mountains for mapping and positioning. It can be constructed through a variety of data fusions. Mapping and positioning are helpful to improve the mapping accuracy and positioning accuracy of robots in complex environments.

In this embodiment, the processing module in the robot may be an integrated circuit chip with signal processing capabilities. The above-mentioned processing module may be a general-purpose processor. For example, the processor can be a central processing unit (Central Processing Unit, CPU), a digital signal processor (Digital Signal Processing, DSP), an application specific integrated circuit (Application Specific Integrated Circuit, ASIC), a field programmable gate array (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, and discrete hardware components can implement or execute the disclosed methods, steps, and logical block diagrams in the embodiments of this application.

The memory module may be, but is not limited to, random access memory, read only memory, programmable read only memory, erasable programmable read only memory, electrically erasable programmable read only memory, etc. In this embodiment, the storage module can be used to store pcd files, first data sets, second data sets, etc. Of course, the storage module can also be used to store a program, and the processing module executes the program after receiving the execution instruction.

Through the above description of the embodiments, those skilled in the art can clearly understand that the present application can be implemented by hardware or by software plus a necessary general hardware platform. Based on this understanding, the technical solution of the present application It can be embodied in the form of a software product, which can be stored in a non-volatile storage medium (which can be a CD-ROM, a USB flash drive, a mobile hard disk, etc.) and includes a number of instructions to make a computer device (which can It is a personal computer, server, or network device, etc.) that executes the methods described in each implementation scenario of this application.

In the embodiments provided in this application, it should be understood that the disclosed robots and methods can also be implemented in other ways. The robot and method embodiments described above are only illustrative. For example, the flowcharts and block diagrams in the accompanying drawings show possible implementation architectures of systems, methods and computer program products according to multiple embodiments of the present application. Functionality and operation. In this regard, each block in the flowchart or block diagrams may represent a module, segment, or portion of code that contains one or more components for implementing the specified logical function(s). Executable instructions. It will also be noted that each block of the block diagram and/or flowchart illustration, and combinations of blocks in the block diagram and/or flowchart illustration, can be implemented by special purpose hardware-based systems that perform the specified functions or acts. , or can be implemented using a combination of specialized hardware and computer instructions. In addition, each functional module in each embodiment of the present application can be integrated together to form an independent part, each module can exist alone, or two or more modules can be integrated to form an independent part.

The above descriptions are only examples of the present application and are not intended to limit the scope of protection of the present application. For those skilled in the art, the present application may have various modifications and changes. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of this application shall be included in the protection scope of this application.

Claims (9)

1. The method for constructing and positioning the automatic inspection robot in the outdoor open scene is characterized by being applied to the automatic inspection-based robot, and comprises the following steps:

acquiring a first data set acquired by a multi-sensor of a robot in an outdoor open scene, wherein the first data set comprises laser point cloud data, IMU data and GPS data;

according to laser point cloud data, IMU data and GPS data in the first data set, constructing a three-dimensional point cloud map of the outdoor open scene through a preset LIO-SAM mapping algorithm, and taking the three-dimensional point cloud map as a pcd global map;

acquiring a second data set acquired by the multi-sensor when the robot travels to the outdoor open scene again, wherein the second data set comprises laser point cloud data, IMU data, odometer data and GPS data;

and determining target position information of the robot in the pcd global map based on the pcd global map and the second data set.

2. The method of claim 1, wherein acquiring a first dataset acquired by a multisensor of a robot in an outdoor open scene comprises:

acquiring laser point cloud data acquired by a laser radar in the multi-sensor in the outdoor open scene, and converting the format of the laser point cloud data into a velodyne point cloud format;

acquiring IMU data acquired by an inertial measurement unit in the multi-sensor in the outdoor open scene;

acquiring GPS data acquired by a GPS module in the multi-sensor in the outdoor open scene;

obtaining an external parameter rotation matrix based on the laser point cloud data and the IMU data;

and taking the laser point cloud data, IMU data, GPS data and an external parameter rotation matrix in the velodyne point cloud format as the first data set.

3. The method of claim 1, wherein constructing the three-dimensional point cloud map of the outdoor open scene by a preset LIO-SAM mapping algorithm based on the laser point cloud data, IMU data, and GPS data in the first dataset comprises:

performing de-distortion treatment on the laser point cloud data to obtain de-distorted laser point cloud data;

feature extraction is carried out on the undistorted laser point cloud data, the curvature of the laser point cloud data is determined according to a curvature formula, a laser point with the curvature larger than a preset threshold value is used as a corner point in an environment point cloud, a laser point with the curvature smaller than the preset threshold value is used as a plane point, and the curvature formula is as follows:

wherein c refers to curvature, S refers to a set of successive points of i returned by the lidar in the multisensor in the same scan, i refers to a laser point,the method comprises the steps of referring to the position of a laser point in a coordinate system of the laser radar, wherein L refers to the coordinate system of the laser radar, k refers to the scanning times of the laser radar, and j refers to the laser point near i;

constructing a corresponding IMU pre-integration factor based on the IMU data in the first data set;

converting the GPS data in the first data set into GPS data under a Cartesian coordinate system, and performing linear interpolation under a time stamp corresponding to the laser radar to obtain a GPS factor;

based on Euclidean distance, constructing a closed loop detection factor according to the angular point, the plane point, the IMU pre-integration factor and the GPS factor to carry out loop detection, and obtaining a corresponding pcd file, wherein the pcd file comprises the angular point, the plane point, the map building track and the three-dimensional point cloud map.

4. The method of claim 1, wherein determining target location information for the robot in the pcd global map based on the pcd global map and the second data set comprises:

performing point cloud registration on a target point cloud and a source point cloud by adopting a preset normal distribution algorithm to obtain first position information of the robot, wherein the target point cloud refers to laser point cloud data in the pcd global map, and the source point cloud refers to laser point cloud data in the second data set;

carrying out data fusion on the first position information, IMU data, GPS data and odometer data in the second data set through a preset extended Kalman filtering algorithm, and outputting global positioning data;

and determining the target position information of the robot in the outdoor open scene according to the global positioning data and the odometer data issued by the chassis of the robot.

5. The method according to claim 4, wherein the source point cloud is data obtained by performing voxel downsampling on original point cloud data acquired by a laser radar in the multi-sensor and performing filtering under a preset filtering condition;

and the target point cloud is data obtained by carrying out voxel downsampling on laser point cloud data in the pcd global map and filtering under the preset filtering condition.

6. The method of claim 5, wherein performing point cloud registration on the target point cloud and the source point cloud using a preset normal distribution algorithm to obtain the first position information of the robot comprises:

rasterizing the target point cloud, dividing the target point cloud into grids with specified sizes, and forming a set X by k points falling in the ith grid, wherein the set X is marked as X _i And calculating normal distribution parameters of the target point cloud, wherein the calculation mode of the target point Yun Junzhi mu and the covariance matrix sigma is as follows:

computing a single source point cloud falling into each gridThe probability density of (2) is calculated as follows:

recording the transformation parameters between two frames of point clouds as P, calculating probability density of any point in the source point clouds falling into a grid aiming at all points in the source point clouds, and summing to obtain matching confidence score (P) between the target point clouds and the source point clouds:

wherein n refers to the total number of point clouds in the source point clouds, and i is an integer from 1 to n;

the minimum value of score (R), namely the optimal value of transformation parameter P, is calculated according to a preset Newton iteration method, and the calculation functions of the Jacobian matrix g and the Heisen matrix H of the Newton iteration method are as follows:

and updating the transformation parameters P according to the jacobian matrix g and the hessian matrix H, wherein the transformation parameters P are as follows:

Ap＝-H ^-1 g (7)

P←P+AP (8)

and (3) carrying out iterative computation through a formula (3) to a formula (8) until a transformation parameter P after the iterative computation reaches a preset value, so that the joint probability density distribution of the source point cloud and the target point cloud is maximum, and the first position information is obtained.

7. The method of claim 4, wherein data fusion of the first location information with IMU data, GPS data, odometry data in the second dataset and outputting global positioning data by a predetermined extended kalman filter algorithm comprises:

dividing the first location information, IMU data, GPS data, and odometry data in the second dataset into a first set of data characterizing a else type of data, and a second set of data characterizing a Twist type of data;

according to the first group of data and the second group of data, the current position state information of the robot is obtained through the extended Kalman filtering algorithmState covariance->The prediction is carried out, and the calculation function is as follows:

wherein F is a nonlinear function obtained by a motion equation and a state equation, F _t For the motion conversion matrix, Q _t Is the noise covariance; updating Kalman gain K and posterior probability distributionAnd calculate the final Kalman estimation result +.>The updating mode is as follows:

wherein G is _t For observing matrix, R _t For measuring state covariance matrix, x _t For the equation of motion, g is the nonlinear observation equation,and outputting the global positioning data.

8. The method according to claim 1, wherein the method further comprises:

and converting the three-dimensional point cloud map into a two-dimensional grid map, wherein the two-dimensional grid map is used for path planning of the robot.

9. The method of claim 8, wherein converting the three-dimensional point cloud map to a two-dimensional grid map comprises:

performing through filtering on the laser point cloud data in the three-dimensional point cloud map based on a preset height range to obtain filtered first laser point cloud data;

calculating the adjacent quantity of each laser point in the first laser point cloud data, and filtering the laser point cloud data with the adjacent quantity smaller than a preset adjacent quantity threshold value in the first laser point cloud data to obtain second laser point cloud data, wherein the adjacent quantity is the total number of laser points existing in an area within a specified radius with any laser point as a center, and the total number of laser points does not comprise any laser point as the center;

and converting the new three-dimensional point cloud map corresponding to the second laser point cloud data into a two-dimensional grid map, and erasing noise point data in the two-dimensional grid map through a noise removing tool.