Autonomous positioning method for mobile robot in complex underground environment and robot

Abstract

The application discloses a kind of mobile robot autonomous positioning methods and robot in complex underground environment, and the robot to be positioned is installed with multi-sensor positioning system, sensor in the multi-sensor positioning system includes: odometer, laser radar, UWB module and IMU module;The method comprises: the robot is placed in underground environment without GPS signal, and it is controlled to move to each position in environment;Based on the data collected in the process of robot movement by multi-sensor positioning system, environment map is constructed;The relative position of two time provided by UWB module is obtained, relative position constraint is constructed, and position constraint is added to pose graph optimization constraint, and the global map without offset is constructed, to complete fusion positioning.The positioning accuracy and stability can be effectively improved by using the mobile robot autonomous positioning method provided by the application.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot autonomous positioning, motion control and mapping analysis in underground scenes, and particularly relates to a mobile robot autonomous positioning method in underground complex environment and a robot. BACKGROUND

[0002] When the robot autonomously works in the underground environment, there is often no environment prior map information. In fact, the ultimate purpose of applying the robot in such a scene is to obtain an accurate environment map for subsequent tasks. Precise positioning and environment map construction, path planning, motion control and exploration method are the key technologies to improve the intelligence of the underground environment robot. Among them, path planning and obstacle avoidance require the use of environment map and precise positioning, so robot positioning and map construction is a more basic and paving technology research.

[0003] When the robot executes tasks in the underground scene, it not only needs to face the problem of no environment prior map, but also cannot rely on GPS for positioning. This requires the robot to run in an unknown environment under the condition of uncertain self-position. In typical underground scenes, the robot will face many severe challenges. With the improvement of the intelligence and autonomy of mobile robots, using mobile robots to execute tasks in underground scenes such as coal mine tunnels and subway tunnels has the advantages of high efficiency and fearlessness of danger. However, the underground scene has complex working conditions, poor road conditions, no GPS and poor communication, and other factors, which pose great challenges to mobile robot positioning and mapping, and may lead to the inability of commonly used positioning methods to be directly applied.

[0004] In the research of autonomous positioning and mapping of mobile robots in complex scenes, a large number of studies have made some achievements. The current mobile robot positioning and mapping scheme is mainly based on Kalman filtering and particle filtering, gradually turning to semantic understanding, lightweight and high efficiency, and improving the robustness of the system. Influenced by the development level of sensor hardware, early research is mostly based on sonar and ultrasonic wave. Currently, the commonly used main sensors include two-dimensional and three-dimensional laser radars, monocular and binocular cameras, depth and event cameras, and GPS, inertial measurement unit, barometer and magnetometer are also used as auxiliary positioning and mapping. Based on different sensors, systems including visual SLAM (simultaneous localization and mapping), visual-inertial SLAM and thermal-inertial SLAM network, however, it is very challenging to rely solely on vision for underground positioning and mapping, because the camera is sensitive to light changes and environmental conditions. Based on time synchronization, a positioning protocol based on Kalman filtering is proposed, which synchronizes the base station of the ultra-wideband and any number of passive ultra-wideband receivers, and achieves the precision level that can be used for mobile robots based on the low-power ultra-wideband module. Based on the matching method using frame to map, the high-frequency laser radar data is used to deal with severe rotation, and the constructed grid map can be directly used for navigation.

[0005] However, the above several positioning schemes still have great limitations. First, the positioning accuracy is limited: although these schemes can improve the efficiency and stability of robot positioning to some extent, they are single laser positioning, which has poor feature discrimination ability for underground environment, inaccurate positioning, and repeated pose estimation, cannot eliminate cumulative error, and has certain limitations in large-scale mapping; second, the current UWB (ultra-wideband) positioning algorithm has large calculation amount, it is difficult to compensate the influence of multipath effect, and it is difficult to achieve ideal positioning accuracy, which cannot meet the demand of robot autonomous positioning in complex conditions. Therefore, in the face of the problems of single sensor in mapping, large pose estimation error, and large calculation amount of UWB algorithm, in order to solve the demand of large scene high-precision positioning and mapping of mobile robots in underground environment, the fusion of IMU (inertial sensor), laser radar and ultra-wideband technology is used to provide accurate positioning algorithm for robots, and it is very important to solve the problem of autonomous positioning and mapping of robots in the environment with bumpy road surface, few features and low illumination. SUMMARY

[0006] The application provides a mobile robot autonomous positioning method in a complex underground environment and a robot, to solve the technical problems that the positioning accuracy of the prior art is limited, it is difficult to achieve ideal positioning accuracy, and it cannot meet the demand of robot autonomous positioning in complex conditions.

[0007] To solve the above technical problems, the application provides the following technical scheme:

[0008] In one aspect, the present application provides a method for autonomous positioning of a mobile robot in a complex underground environment, wherein a multi-sensor positioning system is installed on the robot to be positioned, and the sensors in the multi-sensor positioning system include an odometer, a laser radar, a UWB module, and an IMU module; the method comprises:

[0009] placing the robot in a GPS signal-free underground environment and controlling it to move to various positions in the environment;

[0010] constructing an environment map based on the data collected by the multi-sensor positioning system during the movement of the robot;

[0011] obtaining the relative positions at two time instants provided by the UWB module, constructing a relative position constraint, and adding the position constraint to the pose graph optimization constraint to construct a global map without offset, thereby completing the fusion positioning.

[0012] Further, when placing the robot in a GPS signal-free underground environment and controlling it to move to various positions in the environment, each sensor is turned on synchronously, and the initial working time is kept the same; an optical encoder is installed on the wheel shaft of the robot for trajectory prediction and calculation of the running distance of the robot.

[0013] Further, the construction of the environment map based on the data collected by the multi-sensor positioning system during the movement of the robot comprises:

[0014] constructing a laser grid map using a laser SLAM algorithm based on the laser data collected by the laser radar;

[0015] when receiving the measurement data of the UWB module and the IMU module, associating the grid coordinate data in the laser grid map with the positioning coordinates output by the UWB module at this time to obtain an environment map corresponding to the environment in which the robot is currently located; wherein the IMU module is used to assist the UWB module in positioning and improve the positioning accuracy.

[0016] Further, the obtaining of the relative positions at two time instants provided by the UWB module, the construction of the relative position constraint, and the addition of the position constraint to the pose graph optimization constraint to construct a global map without offset for the purpose of fusion positioning comprise two operations; wherein,

[0017] In the first part, the relative positions at two time instants provided by the UWB module of the robot are obtained, the relative position constraint is constructed, and the position constraint provided by the UWB module is added to the back-end pose optimization of the graph framework to achieve fusion with the laser SLAM;

[0018] In the second part, the laser point cloud data acquired by the lidar is preprocessed to obtain a local sub-map and its pose. Then, the pose is optimized and the position constraints provided by the UWB module are accepted to construct a global map without offset in order to complete the fusion positioning.

[0019] Furthermore, the preprocessing includes distortion correction, point cloud filtering, and local scan matching.

[0020] Furthermore, the IMU module is used in the first part to assist the UWB module in positioning and improve positioning accuracy, and in the second part to provide matching initial values ​​for point cloud distortion correction through integration.

[0021] Furthermore, the process of obtaining the relative positions at two moments provided by the UWB module, constructing relative position constraints, and adding these constraints to the pose graph optimization constraints to construct a global map without offset, thereby completing the fusion localization, includes:

[0022] In the grid map, give the robot an initial pose and record the robot's localization information at this time as the first node;

[0023] If the robot is moving in the environment, the output data of the IMU module is read; if the robot is stationary, the measurement data of each sensor is ignored.

[0024] If measurement data provided by the UWB module is used and received, the map is built by finding the nearest grid map corresponding to the UWB module positioning coordinates stored in the map and interpolating the data; if no positioning coordinates are received from the UWB module, this step is skipped.

[0025] Based on the previous location and the distance traveled measured by the odometer, the robot's current position is obtained by matching the measurement data from the LiDAR with the grid map, and is recorded as the second node.

[0026] The laser point cloud data acquired by the lidar is processed, and the motion state of the lidar is recovered using the velocity and acceleration measured by the IMU module; the laser point cloud data corresponding to time i and time j are defined as S... i and S j The measurement data from the IMU module is integrated to provide initial matching values ​​for point cloud distortion correction; the state update from time i to time j is obtained by integrating the acceleration and angular velocity measured by the IMU module; the current position of the lidar is predicted by pre-integrating the measurement data from the IMU module, and S... j All points are remapped to S. i The coordinate system in which it is located is used to compensate for motion distortion;

[0027] The original laser point cloud data is processed using a method combining regional filtering and voxel grid filtering; wherein, when performing regional filtering, all points outside 15cm-25m are filtered out; smaller grids are divided at positions closer to the laser radar, and larger grids are divided at positions farther away from the laser radar;

[0028] The preset number of point cloud data after point cloud distortion and filtering processing is constructed into a sub-map using the concept of sub-map; wherein, n frames of laser point cloud data are represented as: {F0, F1, F2, … Fn} n-1} F i represents the i-th frame of point cloud data, i =, 1, 2, …, n-1; the n frames of laser point cloud data are transformed into a map coordinate system through coordinate system transformation, and a sub-map M i is spliced; wherein, the transformation relationship expression is as follows:

[0029]

[0030] Wherein, ξ x , ξ y , ξ θ is the best pose of a frame of laser point cloud data in the sub-map;

[0031] The states of t time and t+1 time estimated by the SLAM front-end matching are represented as: P t =[x t ; θ t ] and P t+1 =[x t+1 ; θ t+1 ], the pose transformation from t time to t+1 time is represented as

[0032]

[0033] Wherein, R(θ) represents the error function of the pose, x t+1 represents the actual pose at t+1 time, x t represents the actual pose at t time, θ t+1 represents the actual observation value of the radar scanning angle at t+1 time, θ t represents the actual observation value of the laser scanning angle at t+1 time;

[0034] According to the positioning output of the UWB module, the UWB mobile node observation from t time to t+1 time is According to the external transformation parameters of the UWB mobile node to the laser radar, the real observation in the laser radar coordinate system is obtained as The error function is formed by the prediction obtained by the scan matching and the prediction

[0035]

[0036] wherein, represents the real observation value of UWB in the laser radar coordinate system at t to t+1, represents the actual observation value of UWB in the laser radar coordinate system at t to t+1, represents the real observation value of the pose change at t to t+1, represents the real observation value of the laser radar scanning angle at t to t+1:

[0037] The to-be-estimated variables are (x t , y t , θ t ) and (x t+1 , y t+1 , θ t+1 ), and the Jacobian matrix of the error function with respect to the to-be-estimated variables is as follows:

[0038]

[0039] wherein, y t represents the actual pose in the y-axis direction at t, y t+1 represents the actual pose in the y-axis direction at t+1, and θ represents the laser radar scanning angle.

[0040] Based on the error function, the least square method is used to realize pose optimization, and a global map without offset is constructed.

[0041] In another aspect, the present application also provides a robot, wherein a multi-sensor positioning system is installed on the robot, and the sensors in the multi-sensor positioning system include an odometer, a laser radar, a UWB module and an IMU module; the robot includes a communication interface, a main control board, at least one processor and a memory; wherein the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory; when the processor executes the computer program stored in the memory, the above-mentioned mobile robot autonomous positioning method in the underground complex environment is realized.

[0042] In another aspect, the present application also provides a computer readable storage medium, wherein at least one instruction is stored in the storage medium, and the instruction is loaded and executed by a processor to realize the above-mentioned method.

[0043] The technical scheme provided by the present application has at least the following beneficial effects:

[0044] The present application aims at the problem of obvious error in robot positioning caused by unstructured terrain in underground environment without GPS signal. The position constraint provided by the UWB module is added to the pose graph optimization constraint using the framework based on graph optimization, to provide reliable initial estimate for scan matching. Through the raw point cloud preprocessing methods such as point cloud de-distortion, radius filtering and voxel grid filtering, combined with the optimized pose, accurate pose estimation and mapping are carried out, and the fusion positioning is completed. Thus the accuracy and stability of positioning can be effectively improved. BRIEF DESCRIPTION OF DRAWINGS

[0045] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the embodiment description will be briefly introduced. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative labor.

[0046] Figure 1 is the execution flow diagram of the mobile robot autonomous positioning method in the underground complex environment provided by the embodiment of the present application;

[0047] Figure 2 is the detailed execution flow diagram of the mobile robot autonomous positioning method in the underground complex environment provided by the embodiment of the present application;

[0048] Figure 3 is the robot structure model diagram provided by the embodiment of the present application;

[0049] Figure 4 is the optimization method diagram provided by the embodiment of the present application;

[0050] Figure 5 is the optimization diagram of multiple sensor observation data for each positioning node provided by the embodiment of the present application;

[0051] Figure 6 is the comparison diagram of point cloud before and after filtering provided by the embodiment of the present application. DETAILED DESCRIPTION

[0052] In order to make the purpose, technical scheme and advantages of the present application more clear, the embodiments of the present application will be further described in detail below with reference to the drawings.

[0053] The embodiment is aimed at solving the problem of robot positioning error caused by unstructured terrain in underground environment without GPS signal. The embodiment provides a method for autonomous positioning of a mobile robot in a complex underground environment, wherein a multi-sensor positioning system is installed on the robot to be positioned, the sensors in the multi-sensor positioning system include an odometer, a laser radar, a UWB module and an IMU module; the method adds the position constraint provided by the UWB module to the pose graph optimization constraint based on the framework of graph optimization, and provides a reliable initial estimate for scan matching. Through the raw point cloud preprocessing methods such as point cloud de-distortion, radius filtering and voxel grid filtering, accurate pose estimation and mapping are performed in combination with the optimized pose, and fusion positioning is completed in the underground non-feature environment. Specifically, the method execution process is as shown in Figure 1 and Figure 2 , which includes the following steps:

[0054] S1, placing the robot in an underground environment without GPS signal and controlling it to move to each position in the environment;

[0055] It should be noted that when the robot is placed in an underground environment without GPS signal and controlled to move to each position in the environment, each sensor is turned on synchronously, and the initial working time is kept the same. The optical encoder needs to be installed on the robot wheel shaft for trajectory prediction and calculation of the running distance of the robot.

[0056] S2, constructing an environment map based on the data collected by the multi-sensor positioning system during the movement of the robot;

[0057] Specifically, in the embodiment, the implementation process of S2 is as follows:

[0058] S21, constructing a laser grid map based on the laser data collected by the laser radar using a laser SLAM (Simultaneous Localization and Mapping) algorithm; wherein the laser grid map is a 2D or 3D laser grid map;

[0059] S22, when the measurement data of the UWB module and the IMU module are received, associating the grid coordinate data in the laser grid map with the positioning coordinates output by the UWB module at this time to obtain an environment map corresponding to the environment where the robot is currently located; wherein the IMU module is used to assist the UWB module in positioning and improve the positioning accuracy.

[0060] S3, obtaining the relative positions of two time instants provided by the UWB module, constructing a relative position constraint, adding the position constraint to the pose graph optimization constraint, and constructing a global map without offset to complete fusion positioning.

[0061] Specifically, in the present embodiment, the implementation process of S3 is as follows: at the time of positioning, based on the position of the robot, the positioning data of multiple sensors is read, the walking distance recorded by the odometer is obtained, the current time position and the position at the beginning of walking are detected by UWB detection, in the first part, based on the UWB module of the robot, the relative positions of the two time points provided by the UWB module are added to the back-end pose optimization of the graph framework through the construction of relative position constraints, and fusion with laser SLAM is realized. Another part processes the laser point cloud data, removes distortion, filters the point cloud, and matches the local scan to obtain the local submap and its pose. The back-end optimizes the pose and accepts the UWB pose constraint to construct a global map without offset. Among them, the IMU assists the UWB module to generate accurate positioning output in the first part, and provides matching initial value for point cloud distortion removal by integration in the second part. Then a certain number of point cloud data after point cloud distortion removal and filtering processing are constructed into a submap.

[0062] Further, S3 includes the following steps:

[0063] S31, as shown in Figure 5 , an initial pose is given to the robot in the grid map, and the positioning information of the robot at this time is recorded as the first node;

[0064] S32, if the robot moves in the environment, the output data of the IMU module is read; if the robot is stationary, the measurement data of each sensor is ignored;

[0065] S33, if the measurement data provided by the UWB module is used and received, the UWB module provides the relative positions of the two time points, and the nearest UWB coordinates corresponding to the grid map are found in the UWB coordinates stored in the map during mapping. If no positioning coordinates output by the UWB module are received, skip this step;

[0066] S34, based on the positioning at the last time and the driving distance measured by the odometer, the measurement data of the laser radar and the grid map are used for matching to obtain the position of the robot at this time, which is recorded as the second node;

[0067] S35, the laser point cloud data obtained by the laser radar is processed, and the coordinate system {W} and the UWB mobile node coordinate system {U} are defined. The UWB mobile node, the IMU and the laser radar are installed on the robot platform, and the external parameters of the IMU and the UWB mobile node and the laser radar are obtained through the previous step. The motion state of the laser radar is recovered by using the speed and acceleration measured by the high-frequency IMU module;

[0068] S36, the laser point cloud data corresponding to time i and time j is defined as s i and S j, the measurement data of the IMU module is integrated to provide a matching initial value for point cloud distortion correction; the update of the state from time i to time j can be obtained by integrating the acceleration and angular velocity measured by the IMU module; the position of the current laser radar is predicted by pre-integrating the measurement data of the IMU module, and S j All points are re-mapped to the coordinate system where S i is located to compensate for motion distortion.

[0069] S37, the original laser point cloud data is processed using a method combining regional filtering and voxel grid filtering, and then provided to subsequent algorithms. Among them, all points outside 15cm-25m are filtered out when regional filtering is performed; smaller grids are divided in positions closer to the laser radar, and larger grids are divided in positions farther away from the laser radar;

[0070] S38, using the concept of sub-map, a certain number of point cloud data after point cloud distortion correction and filtering processing is constructed into a sub-map; wherein n frames of laser point cloud data can be represented as: {F0, F1, F2, …Fn} n-1 , F i represents the i-th frame of point cloud data, i = 0, 1, 2, …, n-1; the transformation relationship between each frame of data is: {T0, T1, T2, …T n-1 , through coordinate system transformation, n frames of laser point cloud data are transformed into a map coordinate system, and a sub-map M i is obtained by splicing; wherein the transformation relationship can be according to the following:

[0071]

[0072] Wherein, ξ = (ξ x , ξ y , ξ θ ) is the best pose of a frame of laser point cloud data in the sub-map; based on the ultra-wideband positioning system

[0073] S39, the states of time t and t+1 estimated by the SLAM front-end matching are represented as: P t = [x t ; θ t ] and P t+ 1 ＝ = [x t+1 ; θ t+1 ], then the pose transformation from time t to time t+1 can be represented as

[0074]

[0075] Wherein, R(θ) represents the error function of the pose, x t+1 represents the actual pose at time t+1, xt θ represents the actual pose at time t. t+1 θ represents the actual observed value of the radar scanning angle at time t+1. t This represents the actual observed value of the laser scanning angle at time t+1.

[0076] S310, based on the positioning output of the UWB module, the UWB mobile node observations from time t to time t+1 are... Based on the external transformation parameters from the UWB mobile node to the lidar, the actual observation in the lidar coordinate system can be obtained as follows: The predictions obtained by matching the above scans constitute the error function as follows:

[0077]

[0078] in, This represents the actual observation value of UWB in the lidar coordinate system from time t to t+1. This represents the actual observation value of UWB in the lidar coordinate system from time t to t+1. This represents the actual observed value of the pose change from time t to t+1. This represents the actual observed value of the lidar scanning angle from time t to t+1.

[0079] S311, The variable to be estimated is: (x t y t θ t ) and (x t+1 y t+1 θ t+1 The Jacobian matrix of the error function relative to the variable to be estimated is as follows:

[0080]

[0081] Where, x t y t Let x and y represent the actual poses along the x-axis and y-axis at time t, respectively. t+1 y t+1 θ represents the actual pose along the x-axis and y-axis at time t+1, respectively, and θ represents the scanning angle of the lidar.

[0082] Based on the error function, pose optimization is achieved using the least squares method to construct a global map without offset.

[0083] In another aspect, the embodiment also provides a robot, wherein a multi-sensor positioning system is installed on the robot, and the sensors in the multi-sensor positioning system include an odometer, a laser radar, a UWB module and an IMU module; the robot includes a communication interface, a main control board, at least one processor and a memory; the memory is configured to store a computer program, and the processor is configured to execute the computer program stored in the memory, so as to realize the above-mentioned autonomous positioning method of the mobile robot in the complex underground environment.

[0084] Specifically, as shown in the figure, Figure 3 The main control module in the robot calculates and processes the data collected by each sensor, and sends the calculated results to the upper computer, and sends a PWM signal to the motor to control the motor, wherein an STM32F205 chip is selected as the main control chip in the embodiment. The motor driving module receives the PWM signal sent by the main control module, and changes the rotating speed of the motor by adjusting the driving voltage pulse width. In order to reduce the noise generated during the operation of the robot and improve the service life of the motor, a brushless motor is used for the direct-current speed reduction motor. The environment perception module is composed of solid-state laser radar, UWB, IMU, photoelectric encoder and other sensors, and is mainly used to complete the construction of the surrounding environment of the robot and the positioning function, and the constructed environment map will serve as the basis for the subsequent navigation module. The navigation module uses a mobile phone to realize the navigation function, and the main control board of the robot sends the map information and the position coordinates of the source point and the target point of navigation to the mobile phone, and the mobile phone program is used to realize the path planning of the robot.

[0085] After the robot is powered on, the main control board of the robot initializes each sub-module and initializes the related parameters of each module, and then waits to receive the motion command issued by the main control board. The robot designed in the embodiment can control the motion of the robot in two ways: one is to control the movement of the robot by moving the joystick forward, backward, left and right, and the other is to use a mobile phone to transmit data through Bluetooth or Wi-Fi and issue the motion control command of the robot. In the process of moving, the robot uses the data information of the UWB module, the gyroscope, the odometer and other sensors for self-positioning, and the solid-state laser radar scans the surrounding environment to construct an environment map. After the environment map is constructed, the picture is stored in the main control board, and when navigation is needed, the main control board transmits the map data and the coordinates of the source point and the target point of navigation to the mobile phone, the mobile phone performs navigation path planning, and then issues the motion control command to the main control board to control the motion of the robot.

[0086] For solid-state laser radar, the laser radar used in the embodiment is SLi30+ pure solid-state laser radar. The detection angle of the radar in the vertical direction is about 9° up and down, and the detection angle in the horizontal direction is more than 120°. The radar can output the depth map and point cloud map of the detected object. Compared with the two-dimensional laser radar, the detection range of the radar in the vertical direction is larger, which is more conducive to the detection of obstacles during the robot's travel. The SLi30+ uses the time-of-flight method to measure the distance. The radar can emit modulated near-infrared light. The near-infrared light is reflected by the obstacle and then reaches the radar. By calculating the time difference and phase difference between the emission and reception of light, the relative distance between the obstacle and the radar can be calculated. The SLi30+ can output the depth map of the detected target and the point cloud map corresponding to the depth map in the host module. The system is installed with an embedded operating system development board to process and calculate data. The RK3288 development board is used as the embedded operating system board. The processor executes the program to realize the above positioning method.

[0087] Further, as shown in Figure 4 , the embodiment uses a graph optimization-based framework to add the position constraints provided by the UWB positioning system to the pose graph optimization constraints, to provide a reliable initial estimate for scan matching. The graph in graph optimization is a concept in graph theory. The graph is composed of lines and nodes, such a nonlinear data structure, and the data are associated. When the graph optimization framework is applied to the pose graph optimization constraints, the state pose information of the robot can constitute a "node", the relationship between the states (pose constraints) can constitute an "edge", and the source of such "edge" (pose constraint) can be the constraints between poses given by sensors such as UWB. Assuming that the robot pose at time t is x t , the robot measures data z t at x t , which satisfies z t = h(x t ), the UWB error function is e t =z t -h(x t ), min(z t -h(x t ))=||z t || is the target optimization function, x t is the optimization variable, and the optimal estimate value of x t is solved by iteration. The specific form of the measurement z t is the pose constraint given by UWB. The way to minimize the error function e t is the least squares method. The actual way to add constraints is to arrange UWB base stations in underground environments (such as tunnels, etc.), and the robot carries a UWB tag, which can alleviate the phenomenon of laser degradation.

[0088] The raw point cloud preprocessing methods such as point cloud de-distortion, radius filtering and voxel grid filtering are combined with the optimized pose to perform accurate pose estimation and mapping, and the fusion positioning is completed. Figure 5 As shown in the figure, the comparison chart before and after point cloud filtering is shown in Figure 6 As shown in the figure, the comparison chart before and after point cloud filtering is shown in Figure 6 It can be seen that the optimization method of the embodiment can remove outliers and has good filtering effect.

[0089] In summary, the embodiment provides a mobile robot autonomous positioning scheme in a complex underground environment. The mobile robot equipped with an odometer, a UWB module and a laser radar is placed in an underground environment without GPS signal, and the robot is allowed to walk for a period of time to obtain the walking distance recorded by the odometer, the current position detected by the UWB and the position at the beginning of walking. The optimization method is the least square method, which adds the relative position at the two time points to the back-end pose optimization of the graph framework. Then, the laser point cloud data is processed, the local sub-map and its pose are obtained through de-distortion, point cloud filtering and local scan matching, and the back-end pose optimization and UWB pose constraint are used to construct a global map without offset. The IMU assists the UWB positioning system in the first part to generate accurate positioning output, and provides matching initial value for point cloud de-distortion through integration in the second part. Then, a certain number of point cloud data processed through point cloud de-distortion and filtering are used to construct a sub-map. Therefore, the positioning accuracy and stability can be effectively improved.

[0090] In addition, it should be noted that the present application can be provided as a method, device or computer program product. Therefore, the embodiments of the present application can adopt a completely hardware embodiment, a completely software embodiment or an embodiment combining software and hardware aspects. Moreover, the embodiments of the present application can adopt the form of a computer program product implemented on one or more computer usable storage media containing computer usable program code.

[0091] The embodiments of the present application are described with reference to the flowcharts and / or block diagrams according to the method, terminal device (system) and computer program product of the embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of the flows and / or blocks in the flowcharts and / or block diagrams can be realized by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, embedded processor or other programmable data processing terminal device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing terminal device produce a device that implements the functions specified in the flowcharts and / or block diagrams. Figure 1 The functions specified in one flow or multiple flows and / or blocks Figure 1 The device that implements the functions specified in one block or multiple blocks.

[0092] These computer program instructions can also be stored in a computer- readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instructions which implement the Figure 1 function specified in the flow or flows and / or blocks Figure 1 These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the function specified in the flow or flows and / or blocks Figure 1 function specified in the flow or flows and / or blocks Figure 1 These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the function specified in the flow or flows and / or blocks

[0093] It is also noted that the illustrative figures can show conceptual renditions of aspects of the application. The use of such illustrations is meant to provide conceptual insight into claimed subject matter. In accordance with the disclosure, it is contemplated that the components, methods, and acts can be implemented in hardware, software, or a combination thereof. It is further noted that the claimed subject matter can also be implemented as a program product using the computer readable medium. The computer readable medium can be a machine-readable storage medium or a machine-readable transmission medium. The claimed subject matter can be implemented using a variety of machine-readable storage or transmission media. The machine-readable storage medium can be any available medium or means that can be accessed by a general purpose or special purpose computing system. By way of example, and not limitation, such computer-readable media can comprise RAM, ROM, EEPROM, CD-ROM or other optical disk storage, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to carry or store desired program code in the form of computer-readable instructions or data structures and that can be accessed by a general purpose or special purpose computing system. Also, functional computer program instructions can be loaded into a computer, other programmable data processing apparatus, or a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flow or flows and / or blocks

[0094] Finally, it is to be understood that the above description is intended to be illustrative, and not restrictive. Many other embodiments will be apparent to those of skill in the art upon reviewing the above description. The scope of the application should, therefore, be determined not with reference to the above description, but instead with reference to the appended claims, along with their full scope of equivalents.

Claims

1. An autonomous localization method for mobile robots in complex underground environments, wherein, A multi-sensor positioning system is installed on the robot to be located. The sensors in the multi-sensor positioning system include: an odometer, a lidar, a UWB module, and an IMU module; the method comprises: Place the robot in an underground environment without GPS signal and control it to move to various locations in the environment; An environmental map is constructed based on data collected by a multi-sensor localization system during robot movement. The process involves obtaining the relative positions at two moments from the UWB module, constructing relative position constraints, and adding these constraints to the pose graph optimization constraints to build a zero-offset global map for fusion localization. This includes: processing the laser point cloud data acquired by the LiDAR to compensate for motion distortion; processing the original laser point cloud data using a combination of region filtering and voxel mesh filtering; and constructing a sub-map from a predetermined number of point cloud data points that have undergone distortion correction and filtering using the concept of a sub-map. The frame of laser point cloud data is represented as follows: , This represents the point cloud data of the i-th frame, i=0,1,2,…,n-1; the coordinate system transformation is used to... Transform the laser point cloud data frames to a map coordinate system and stitch them together to obtain a sub-map. The transformation relation expression is as follows: ； in, The optimal pose of a frame of laser point cloud data in a sub-map; SLAM front-end matching estimation Time and The state at time t is represented as: and ,but Time's up The pose transformation at time t is represented as : ； in, The error function representing the pose. This represents the actual pose at time t+1. This represents the actual pose at time t. This represents the actual observed value of the radar scanning angle at time t+1. This represents the actual observed value of the laser scanning angle at time t+1; Based on the positioning output of the UWB module Time's up UWB mobile node observations at time t are Based on the external transformation parameters from the UWB mobile node to the lidar, the actual observation in the lidar coordinate system is obtained as follows: The error function formed by the prediction obtained by matching with the scan is: ； in, This represents the actual observation value of UWB in the lidar coordinate system from time t to t+1. This represents the actual observation value of UWB in the lidar coordinate system from time t to t+1. This represents the actual observed value of the pose change from time t to t+1. This represents the actual observed value of the lidar scanning angle from time t to t+1; The variable to be estimated is: and The Jacobian matrix form of the error function relative to the variable to be estimated is as follows: ； in, This represents the actual pose along the y-axis at time t. This represents the actual pose along the y-axis at time t+1. Indicates the scanning angle of the lidar; Based on the error function, pose optimization is achieved using the least squares method to construct a global map without offset.

2. The autonomous localization method for mobile robots in complex underground environments as described in claim 1, characterized in that, When the robot is placed in an underground environment without GPS signal and controlled to move to various locations in the environment, all sensors are activated synchronously to maintain the same initial working time; photoelectric encoders are installed on the robot's wheel axles for trajectory prediction and calculation of the robot's running distance.

3. The autonomous localization method for mobile robots in complex underground environments as described in claim 1, characterized in that, The environmental map is constructed based on data collected by the multi-sensor positioning system during robot movement, including: A laser grid map is constructed using laser SLAM algorithm based on laser data collected by lidar. When the measurement data from the UWB module and IMU module are received, the grid coordinate data in the laser grid map is associated with the positioning coordinates output by the UWB module at this time to obtain the environmental map corresponding to the current environment of the robot. The IMU module is used to assist the UWB module in positioning and improve positioning accuracy.

4. The autonomous localization method for mobile robots in complex underground environments as described in claim 3, characterized in that, The process of obtaining the relative positions at two moments provided by the UWB module, constructing relative position constraints, and adding these constraints to the pose graph optimization constraints to build a global map without offset, thereby completing the fusion localization, includes two parts; wherein, In the first part, the relative positions of the robot at two moments are obtained from the UWB module, relative position constraints are constructed, and the position constraints provided by the UWB module are added to the back-end pose optimization of the graph framework and fused with laser SLAM. In the second part, the laser point cloud data acquired by the lidar is preprocessed to obtain a local sub-map and its pose. Then, the pose is optimized and the position constraints provided by the UWB module are accepted to construct a global map without offset in order to complete the fusion positioning.

5. The autonomous localization method for mobile robots in complex underground environments as described in claim 4, characterized in that, The preprocessing includes distortion correction, point cloud filtering, and local scan matching.

6. The autonomous localization method for mobile robots in complex underground environments as described in claim 5, characterized in that, The IMU module is used in the first part to assist the UWB module in positioning and improve positioning accuracy, and in the second part to provide matching initial values ​​for point cloud distortion correction through integration.

7. The autonomous localization method for mobile robots in complex underground environments as described in claim 3, characterized in that, The process of obtaining the relative positions at two moments provided by the UWB module, constructing relative position constraints, and adding these constraints to the pose graph optimization constraints to construct a global map without offset, thereby completing the fusion localization, includes: In the grid map, give the robot an initial pose and record the robot's localization information at this time as the first node; If the robot is moving in the environment, the output data of the IMU module is read; if the robot is stationary, the measurement data of each sensor is ignored. If measurement data provided by the UWB module is used and received, the map is built by finding the nearest grid map corresponding to the UWB module positioning coordinates stored in the map and interpolating the data; if no positioning coordinates are received from the UWB module, this step is skipped. Based on the previous location and the distance traveled measured by the odometer, the robot's current position is obtained by matching the measurement data from the LiDAR with the grid map, and is recorded as the second node. The laser point cloud data acquired by the lidar is processed, and the motion state of the lidar is recovered using the velocity and acceleration measured by the IMU module; the laser point cloud data corresponding to time i and time j are defined as follows: and The measurement data from the IMU module is integrated to provide initial matching values ​​for point cloud distortion correction; the state update from time i to time j is obtained by integrating the acceleration and angular velocity measured by the IMU module; the current position of the lidar is predicted by pre-integrating the measurement data from the IMU module. All points are re-mapped to The coordinate system in which it is located is used to compensate for motion distortion; The raw laser point cloud data is processed using a combination of region filtering and voxel grid filtering. During region filtering, all points beyond 15cm-25m are filtered out. Smaller grids are used for locations closer to the lidar, while larger grids are used for locations farther away.

8. A robot equipped with a multi-sensor positioning system, wherein the sensors in the multi-sensor positioning system include: The robot includes an odometer, a lidar, a UWB module, and an IMU module; the robot comprises a communication interface, a main control board, at least one processor, and a memory; wherein the memory is used to store a computer program, and the processor is used to execute the computer program stored in the memory, characterized in that, when the processor executes the computer program stored in the memory, it implements the autonomous positioning method for mobile robots in complex underground environments as described in any one of claims 1 to 7.

Images