Roadway cleaning robot autonomous navigation and operation system based on multi-sensor fusion

Abstract

The invention discloses a roadway cleaning robot autonomous navigation and operation system based on multi-sensor fusion. The roadway cleaning robot autonomous navigation and operation system comprises a mechanical system, a control system and a power system. The mechanical system comprises a crawler chassis, a mechanical arm and a big arm turntable; the control system software part presentation layer comprises a user interaction and management module, the business layer comprises a task planning and decision module, and the function layer comprises a robot autonomous walking control module, a roadway deformation detection module, a mechanical arm accessory replacement control module and a mechanical arm roadway cleaning control module. The sensing layer comprises an infrared person detection module, a laser radar mapping positioning module, a mechanical arm visual positioning module and a mechanical arm state feedback calculation module; the hardware part comprises a laser radar, an IMU, a sensor and the like. Various sensor data are fused, accurate positioning, autonomous walking, mechanical arm visual positioning, roadway deformation detection, accessory automatic replacement, roadway automatic cleaning and the like of the robot are achieved, and the intelligent level and the working efficiency of the robot are improved.

Description

Technical Field

[0001] The invention belongs to the technical field of underground coal mine tunnel cleaning and repair, and relates to an autonomous navigation and operation system of a tunnel cleaning robot based on multi-sensor fusion. Background Art

[0002] With the rapid development of modern industry and technology, especially in complex environments such as mines and tunnels, the demand for tunnel cleaning robots is becoming increasingly urgent. These environments are usually characterized by complex terrain, limited space, and harsh environment, which poses great challenges to the navigation and operation of tunnel cleaning robots. Therefore, the development of a robot system that can autonomously navigate, accurately locate, and have the function of tunnel deformation detection has important practical significance and application value.

[0003] First, in terms of autonomous navigation, the robot needs to be able to accurately obtain its own position information and perform path planning and walking control according to the environment. Traditional navigation methods often rely on a single sensor, such as lidar or visual sensors, but in complex environments, a single sensor often has difficulty coping with various challenges, such as lighting changes, dynamic obstacles, etc. Therefore, fusing multiple sensor data and realizing multi-sensor fusion navigation is the key to improving the accuracy and robustness of robot navigation.

[0004] Secondly, since traditional tunnel repair equipment relies on manual replacement of actuators and cleaning of tunnel surrounding rocks, the accuracy of actuator replacement and surrounding rock repair is difficult to meet the requirements of modern mine surrounding rock repair construction. By analyzing the collaborative operation rules of the joints of the working arm in posture realization, the precise posture control and trajectory planning technology under the coordinated action of the joints of the working arm are studied, so as to achieve automatic high-precision positioning of the working arm for rapid replacement of actuators.

[0005] Finally, in terms of tunnel deformation detection, since tunnels are usually underground, they are affected by geological conditions, groundwater and other factors, and are prone to deformation and damage. Timely detection and evaluation of tunnel deformation is of great significance for preventing safety accidents and ensuring personnel safety. Traditional tunnel deformation detection methods usually rely on manual inspections, which are not only inefficient but also difficult to cover all areas. Therefore, the development of a robot system that can autonomously perform tunnel deformation detection has important practical value. Summary of the invention

[0006] In view of the shortcomings of the prior art, the purpose of the present invention is to provide an autonomous navigation and operation system for a tunnel cleaning robot based on multi-sensor fusion. The system realizes the functions of autonomous navigation, precise positioning and tunnel deformation detection of the robot by integrating multiple sensor data such as laser radar, IMU (inertial measurement unit) and visual sensor. Through the collaborative work between various modules, the system can improve the intelligence level and operation efficiency of the robot, and provide a new solution for mobile robot navigation and tunnel deformation detection in complex environments such as mines and tunnels.

[0007] In order to solve the above technical problems, the present invention adopts the following technical solutions to achieve the above problems:

[0008] Autonomous navigation and operation system of laneway cleaning robot based on multi-sensor fusion, including mechanical system, control system and power system;

[0009] The mechanical system includes a crawler chassis, a mechanical arm, and a boom turntable; the mechanical arm is installed on the crawler chassis through the boom turntable to adjust the mechanical arm's posture; the mechanical arm includes a boom, a middle arm, and a forearm, and an attachment turntable is installed on the forearm to adjust the attachment's posture; based on different construction process requirements, the three attachments of a breaker hammer, a bucket, and a milling head are switched through quick change to achieve tunnel cleaning construction;

[0010] The control system includes a hardware part and a software part;

[0011] The software part adopts a layered architecture, including the perception layer, function layer, business layer and presentation layer from the bottom to the top:

[0012] The presentation layer runs on the user terminal and includes the user interaction and management modules for user interaction and user management;

[0013] The business layer runs on the robot side and includes the task planning and decision-making module, which is used for lane clearing task planning and decision-making;

[0014] The functional layer runs on the robot side and is used to provide independent single functions for the business layer. This layer includes: robot autonomous walking control module, lane deformation detection module, robot arm attachment replacement control module, and robot arm lane cleaning control module.

[0015] The perception layer runs on the robot side and is used to process the robot's sensor data and provide necessary perception information for the functional layer. The modules of the perception layer are independent of each other, including: infrared personnel detection module, laser radar mapping and positioning module, robotic arm visual positioning module, and robotic arm state feedback calculation module.

[0016] The hardware part includes:

[0017] Wireless data substation, providing access points for users to connect to the robot via wireless network;

[0018] The main controller can run the task planning and decision-making module, the robot arm state feedback calculation module, the robot arm attachment replacement control module, and the robot arm lane cleaning control module; it can interact with the user end through the above modules; it can perform the top-level task planning and decision-making of the robot according to the user's requirements;

[0019] The robot arm driver can control the robot arm to move to a specified position and posture through a solenoid valve;

[0020] Displacement sensor, which can measure the displacement of each cylinder of the robot arm and is used for real-time posture calculation of the robot arm;

[0021] Pressure sensor, which can measure the pressure of each link of the hydraulic system and is used for robot status monitoring;

[0022] The crawler chassis driver controls the rotation of the crawler tracks through a solenoid valve, which is used for the final execution of the vehicle's autonomous walking action;

[0023] Left / right track encoder, used to measure the rotation speed of the left and right tracks;

[0024] IMU, used to measure the vehicle's angular velocity, acceleration, and inclination, which is used to fuse with LiDAR data for mapping;

[0025] Point cloud data processor, running the laser radar mapping and positioning module, the robot autonomous walking control module, and the lane deformation detection module;

[0026] Image data processor, running infrared personnel detection module and robotic arm visual positioning module;

[0027] Switches, used to interconnect network devices;

[0028] Front / rear surveillance cameras, used to provide the user with video surveillance information of the robot's surroundings;

[0029] Front / rear infrared cameras are used to collect infrared video information of the robot's surrounding environment for personnel monitoring;

[0030] Front / top / rear laser radars are used to collect 3D point cloud information of the robot's surroundings for mapping, positioning and obstacle avoidance;

[0031] The power system includes a motor, an electromagnetic starter and a hydraulic pump station.

[0032] The present invention also includes the following technical features:

[0033] Specifically, the infrared personnel detection module processes the infrared video information collected by the infrared camera to detect the position, width and height information of the personnel in the field of view. This information is used for avoiding personnel in the environment during the obstacle avoidance process of the robot autonomous walking control module in the functional layer.

[0034] Specifically, the lidar mapping and positioning module establishes a dense 3D point cloud map of the working environment based on the point cloud data provided by the lidar carried by the robot and the acceleration and angular velocity measurements provided by the IMU, and determines the position and posture of the robot relative to the map. The 3D point cloud map and robot posture output by this module are the basis for the robot's autonomous walking control module to control the robot's walking. At the same time, the 3D point cloud map is also the input of the lane deformation detection module.

[0035] Specifically, the robot arm visual positioning module accurately gives the position and posture of the robot arm relative to the robot attachment bin through visual positioning, and provides accurate posture information for the robot arm attachment replacement control module;

[0036] The robot arm state feedback calculation module calculates the position information of each joint of the robot arm relative to the robot chassis reference coordinate system based on the measurement data of each sensor of the robot arm, and the calculation results are simultaneously transmitted to the robot arm lane cleaning control module and the robot arm accessory replacement control module for control.

[0037] Specifically, the robot's autonomous walking control module controls the robot to move to the target position based on the 3D point cloud map information and robot chassis position information provided by the lidar, as well as the target position information given by the task planning and decision-making module. During this period, the robot automatically avoids common obstacles and pedestrians in the environment based on the infrared personnel detection results and the lidar original point cloud data; if a person is found to intrude into a safe area, the robot stops moving, issues a warning, and continues moving after the person retreats out of the safe range.

[0038] Specifically, the lane deformation detection module performs deformation detection on the lanes in the specified area according to the dense 3D point cloud map provided by the lidar mapping and positioning module, identifies the position and amount to be cleaned, and sends them to the task planning and decision-making module for decision-making.

[0039] Specifically, after the robot arm accessory replacement control module receives the accessory replacement command sent by the task planning and decision-making module, it automatically puts back the original accessory and replaces it with the specified accessory according to the accessory type; during this period, the real-time posture of the robot arm movement is given by the robot arm visual positioning module and the robot arm state feedback calculation module; the robot arm visual positioning module provides high-precision real-time posture near the position where the accessory turntable is close to the accessory, and the posture of each joint provided by the robot arm state feedback calculation module is used for control decoupling during the control process.

[0040] Specifically, the robot arm lane cleaning control module controls the robot arm to clean the lane according to the requirements of the task planning and decision module and the robot arm posture information provided by the robot arm state feedback calculation module, and operates the robot arm driver according to the specified process.

[0041] Specifically, the task planning and decision-making module, according to user requirements, uses the services provided by the lane deformation detection module to detect lane deformation, uses the services provided by the robot autonomous walking control module to control the robot to move to the vicinity of the location to be cleaned, uses the services provided by the robotic arm accessory replacement control module to replace the required accessories, and finally uses the services provided by the robotic arm lane cleaning control module to clean the lane.

[0042] Specifically, the video monitoring information of the front / rear monitoring camera is directly transmitted to the user interaction and management module of the user end for real-time display; the point cloud data of the front / top / rear laser radar and the acceleration, angular velocity and posture information measured by the IMU are transmitted to the laser radar mapping and positioning module to establish a 3D point cloud map of the robot's environment in real time, and determine the robot's position and posture in the 3D point cloud map; the 3D point cloud map and the robot's posture are transmitted to the lane deformation detection module for lane deformation detection, and the area to be cleaned and the height to be cleaned are given; the 3D point cloud map and the robot's posture are transmitted to the robot's autonomous walking control module for path planning and robot motion control;

[0043] The data of the left / right crawler encoder is transmitted to the robot's autonomous walking control module for closed-loop control of crawler speed during robot motion control to ensure the dynamics of robot motion control;

[0044] The infrared video data of the front / rear infrared cameras are transmitted to the infrared personnel detection module for personnel detection, and the detection results are transmitted to the robot autonomous walking control module for use in pedestrian avoidance during the autonomous walking control process.

[0045] Compared with the prior art, the present invention has the following technical effects:

[0046] By integrating multiple sensor data, the present invention realizes accurate positioning, autonomous navigation, autonomous walking, mechanical arm visual positioning, lane deformation detection, automatic replacement of accessories and automatic lane cleaning of the lane cleaning robot, thus improving the robot's intelligence level and operating efficiency. At the same time, the system has strong versatility and scalability, and can be widely used in mobile robot navigation and lane deformation detection in complex environments such as mines and tunnels. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 This is a schematic diagram of the main structure of the tunnel cleaning robot.

[0048] Figure 2 This is a schematic diagram of the construction process flow of the tunnel cleaning robot.

[0049] Figure 3 This is a schematic diagram of the software architecture of the tunnel cleaning robot control system.

[0050] Figure 4 This is a schematic diagram of the hardware architecture of the tunnel cleaning robot control system.

[0051] Figure 5 This is the overall control strategy diagram. DETAILED DESCRIPTION

[0052] Specific embodiments of the present invention are given below. It should be noted that the present invention is not limited to the following specific embodiments, and all equivalent changes made on the basis of the technical solution of this application fall within the protection scope of the present invention.

[0053] Example:

[0054] This embodiment provides an autonomous navigation and operation system for a lane cleaning robot based on multi-sensor fusion. Figure 1 , including mechanical systems, control systems and power systems.

[0055] The mechanical system includes a crawler chassis, a mechanical arm, and a boom turntable. The mechanical arm is installed on the crawler chassis through the boom turntable to adjust the mechanical arm's posture. The mechanical arm includes a boom, a middle arm, and a forearm. The forearm is equipped with an attachment turntable to adjust the attachment's posture. Based on different construction process requirements, the three attachments of a breaker, a bucket, and a milling head can be switched through quick change to achieve tunnel cleaning construction, such as Figure 2 The main requirements are to realize the functions of three modules: Module 1: autonomous navigation and obstacle avoidance of the crawler chassis, including infrared sensor personnel recognition and laser radar mapping; Module 2: automatic switching of accessories and one-key vehicle collection function through quick-change device, including visual positioning module; Module 3: automatic positioning of the robot arm working accessories based on typical crushing application scenarios.

[0056] The control system includes hardware and software parts;

[0057] The software part adopts a layered architecture, such as Figure 3 , from bottom to top, including perception layer, function layer, business layer and presentation layer:

[0058] The presentation layer runs on the user terminal (tablet computer), including the user interaction and management module, which is mainly used for user interaction and user management; the interaction content includes: robot task deployment, robot status monitoring and work scene video monitoring;

[0059] The business layer runs on the robot side and includes a task planning and decision-making module, which is mainly used for lane cleaning task planning and decision-making. Task planning and decision-making include: when to control the robot to walk autonomously, when to perform lane deformation detection, which attachment to use for lane cleaning, and when to perform lane cleaning.

[0060] The functional layer runs on the robot side and is mainly used to provide independent single functions for the business layer. The functional modules included in this layer are: robot autonomous walking control module, lane deformation detection module, robot arm attachment replacement control module, and robot arm lane cleaning control module.

[0061] The perception layer runs on the robot side and is mainly used to process the robot's sensor data and provide necessary perception information for the functional layer. The modules of the perception layer are independent of each other, including: infrared personnel detection module, laser radar mapping and positioning module, robotic arm (forearm) visual positioning module, and robotic arm state feedback calculation module.

[0062] The hardware part is mainly concentrated on the robot side. The user-side tablet computer is connected to the wireless data substation on the robot side through a wireless network; Figure 4 , the robot-side hardware equipment and its functions include:

[0063] Wireless data substation, providing access points for users to connect to the robot via wireless network;

[0064] The main controller of the lane cleaning robot can run the task planning and decision-making module, the robot arm state feedback calculation module, the robot arm attachment replacement control module, and the robot arm lane cleaning control module; it can interact with the user end through the above modules; it can perform the top-level task planning and decision-making of the robot according to the user's requirements;

[0065] The robot arm driver can control the robot arm to move to a specified position and posture through a solenoid valve;

[0066] The displacement sensor can measure the displacement of each cylinder of the robot arm, which is used for real-time posture calculation of the robot arm and serves as the basis for controlling the robot arm;

[0067] Pressure sensors can measure the pressure of key links in the hydraulic system and are used for robot status monitoring;

[0068] The crawler chassis driver controls the rotation of the crawler tracks through a solenoid valve, which is used for the final execution of the vehicle's autonomous walking action;

[0069] Left / right track encoder, used to measure the speed of the left and right tracks, which serves as feedback information for closed-loop rotation control of the track speed;

[0070] IMU, used to measure the vehicle's angular velocity, acceleration, and inclination, which is used to fuse with LiDAR data for mapping;

[0071] Point cloud data processor, running the laser radar mapping and positioning module, the robot autonomous walking control module, and the lane deformation detection module;

[0072] Image data processor, running infrared personnel detection module and robotic arm (forearm) visual positioning module;

[0073] Switches, used to interconnect network devices;

[0074] Front / rear surveillance cameras, used to provide the user with video surveillance information of the robot's surroundings;

[0075] Front / rear infrared cameras are used to collect infrared video information of the robot's surrounding environment for personnel monitoring;

[0076] Front / top / rear laser radars are used to collect 3D point cloud information of the robot's surroundings for mapping, positioning and obstacle avoidance;

[0077] The power system includes motors, electromagnetic starters and hydraulic pump stations. Its main function is to provide the power required by the equipment. The motor is the core component of the power system and is responsible for converting electrical energy into mechanical energy to drive the entire system. The electromagnetic starter is used to control the start and stop of the motor to achieve remote control and protection of the motor. The hydraulic pump station is responsible for providing hydraulic oil pressure to drive actuators such as crawler travel motors and robotic arm cylinders to achieve various mechanical actions.

[0078] The infrared personnel detection module processes the infrared video information collected by the infrared camera and detects the position, width and height information of the personnel in the field of view. This information is used to avoid personnel in the environment during the obstacle avoidance process of the robot autonomous walking control module in the functional layer.

[0079] The lidar mapping and positioning module establishes a dense 3D point cloud map of the working environment based on the point cloud data provided by the lidar carried by the robot and the acceleration and angular velocity measurements provided by the IMU, and determines the position and posture of the robot relative to the map. The 3D point cloud map and robot posture output by this module are the basis for the robot's autonomous walking control module to control the robot's walking. At the same time, the 3D point cloud map is also the input of the lane deformation detection module.

[0080] Specifically, this module uses lidar to scan the environment and obtains 3D point cloud data of the environment; coordinates are aligned by projecting the data of the front / rear lidar to the coordinate system of the top lidar; the point cloud data distortion caused by motion is corrected according to the pose sequence provided by the IMU during the lidar scanning cycle; the corrected point cloud data is projected to the map coordinate system and matched with the features in the map to obtain the precise robot pose; the precise pose is used to correct the IMU pose error; this module realizes the precise positioning and map building functions of the robot.

[0081] A robot positioning module method based on the fusion of 3D point cloud data and inertial measurement unit (IMU) data in the lane. In the three-dimensional point cloud map construction part, the collected point cloud data is converted into clustered point cloud through the angle clustering method, and the characteristics of the lane environment are further used to restore the authenticity of the environment; then the 3D-NDT (three-dimensional normal distribution transform) algorithm is used to match the point cloud between frames to improve the accuracy of the positioning system. Secondly, in order to overcome the limitations of the single-sensor positioning method, the UKF (unscented Kalman filter) algorithm is used to fuse the lidar positioning information with the IMU odometer information to further improve the positioning accuracy of the lane robot.

[0082] The general process of 3D point cloud matching using the 3D-NDT algorithm is as follows:

[0083] 3D point cloud data preprocessing: The three-dimensional point cloud data is converted into a depth map for storage. The vertical and horizontal resolutions of the lidar are used to map the point cloud data onto a two-dimensional image using the following formula. The intensity value of each pixel represents the depth information of the corresponding point.

[0084] Horizontal coordinate (u) calculation formula:

[0085] The vertical coordinate (v) calculation formula is:

[0086] Where W and H are the width and height of the depth map, respectively, and x max and may They are the maximum values ​​of the point cloud data in the x and y directions respectively.

[0087] The depth value (D) is usually directly used as the z coordinate of the point: D = z

[0088] In the original point cloud of 3D LiDAR, the ground point cloud data accounts for a large proportion, which will increase the amount of data processing operations. Remove the ground points of the roadway in the map; divide the ground area into grids according to the resolution; then project the data points within the height range occupied by the robot in the 3D map into each grid on the ground; when the number of points in the grid exceeds a given threshold, the grid is set to an occupied state; the remaining grids are set to an unoccupied state.

[0089] In the principle of the angle clustering algorithm, point A and point B are any two adjacent points in the depth map, point O is the origin of the laser radar coordinate system, OA, OB and AB are connected to form a triangle, OA is set as the longer side, and a perpendicular line to OA is drawn through point B, with point C as the foot of the perpendicular. β is the angle between OA and BA, and α is the angle corresponding to the horizontal resolution or vertical resolution of the laser radar.

[0090] According to the characteristics of the depth map, the graph-based Breadth First Search (BFS) is used to traverse the four areas of the depth map from a certain point, and the final clustering result is obtained after traversing all point clouds. Through angle-based point cloud clustering, many obvious features in the lane environment can be retained, such as the boundaries in the environment.

[0091] The 3D-NDT positioning method is used to match the clustering results with the prior point cloud map as the input point cloud to calculate the pose and realize self-positioning. The algorithm adjusts the pose transformation parameters so that the probability of the point cloud after pose transformation calculated in the grid probability density function is maximized, and outputs the optimal pose transformation at this time.

[0092] In the motion of the roadway robot, the translation and Euler angle are usually used to represent its posture transformation, which mainly includes three translation parameters and three rotation parameters on the X, Y, and Z coordinate axes. This transformation relationship can be represented by the posture transformation p, The raw data of the LiDAR point cloud is preprocessed and clustered to obtain the input point cloud Y = {y i |y1,y2,…,y m}, input point cloud Y = {y i |y1,y2,…,y m According to the pose transformation p, we get the point cloud Y'={y' i |y'1,y'2,…,y' m} and projected into the grid as shown in Eq.

[0093] y i ′=T(p,y i ) = t x，y，z +R z R y R x y i

[0094]

[0095] Where: t x，y，z Represents y i The translation transformation of R x , R y , Rz Then it means y i The rotation transformation of the point cloud is obtained by i , the transformed point cloud set y' is calculated through the probability density function f(x) i The probability S(p) under this grid. By iteratively optimizing and correcting the transformed posture p, the matching result when the probability S(p) is maximum or the number of iterations is reached can be obtained, and the current optimal posture transformation can be obtained.

[0096] LiDAR and IMU fusion positioning based on UKF: According to the UKF algorithm, the process mainly includes three steps: system state equation initialization, prediction, and update. Considering the sampling frequency difference between LiDAR and IMU, this paper uses IMU mileage estimation as the prediction part of the laneway robot system, and the observation part uses the positioning result of LiDAR based on 3D-NDT to update the prediction value, and fuses the information of the two to obtain the optimal posture. The specific contents are as follows: 1) Initialization: Based on the actual working conditions of the system and sensors, construct the state equation f(·) and observation equation h(·), combine the system state quantity and observation quantity, and correct the estimation results through prediction and update. 2) Prediction step: The prediction step includes generating sigma points, propagating sigma points through the state transfer function, and calculating the predicted mean and covariance. 3) Update step: The update step includes propagating sigma points through the observation model, calculating the observation residual and Kalman gain, and updating the state estimate and covariance matrix. 4) Iteration: At each time step, repeat the prediction and update steps.

[0097] After constructing the observation equation and prediction equation of the system, the sensor positioning information can be continuously fused through the prediction of IMU odometer information and the update of lidar 3D-NDT positioning to obtain more accurate positioning results.

[0098] The basic process of the laser radar mapping and positioning module algorithm is as follows: 1). Time synchronization and coordinate alignment of the data of the front / top / back three laser radars: first, adjust the three sets of data to the same time point according to the timestamp of the data, that is, time synchronization; then project the data of the front / back two laser radars to the coordinate system of the top laser radar according to the pre-calibrated relative posture between the three, that is, coordinate alignment; 2). Correct the point cloud data distortion caused by motion according to the posture sequence provided by the IMU within one scanning cycle of the laser radar; 3). Project the point cloud data to the map coordinate system according to the posture of the IMU at the end of the laser radar scan; 4). Match the point cloud data projected to the map coordinate system with the features in the map to obtain the precise posture; 5). Use the precise posture to correct the IMU posture error; 6). The algorithm's real-time posture output frequency can reach 10Hz; 7). The algorithm's real-time map output frequency is 1Hz.

[0099] The robot arm (forearm) visual positioning module accurately gives the position and posture of the robot arm (forearm) relative to the robot attachment bin through visual positioning, providing accurate posture information for the robot arm attachment replacement control module;

[0100] The robot visual positioning is planned to adopt a cooperative positioning method that uses a QR code as a feature on the forearm of the robot. The algorithm requires that four QR codes with known positions be pasted on the robot (the four QR codes are required to be on the same plane and distributed as close to the vertices of a rectangle as possible). After the positioning camera captures the four QR codes, the coordinate center of each QR code in the image is extracted, and then the pose of the QR code in the camera coordinate system is obtained according to the traditional Pnp algorithm. Then, according to the pre-calibrated relationship between the camera coordinate system and the vehicle coordinate system, as well as the coordinates of the QR code relative to the end of the forearm, the pose of the end of the forearm relative to the robot coordinate system is calculated. This module realizes the visual positioning function of the robot.

[0101] The robot arm state feedback calculation module calculates the posture information of each joint of the robot arm relative to the robot chassis reference coordinate system based on the measurement data of each sensor of the robot arm (the displacement sensor data of each cylinder of the robot arm). The above posture information is the control basis of the robot arm tunnel cleaning control module. The calculation results are simultaneously transmitted to the robot arm tunnel cleaning control module and the robot arm accessory replacement control module for control.

[0102] The robot's autonomous walking control module controls the robot to move to the target position based on the 3D point cloud map information and robot chassis position information provided by the lidar, as well as the target position information given by the task planning and decision-making module. During this period, the robot automatically avoids common obstacles and pedestrians in the environment based on the infrared personnel detection results and the lidar original point cloud data; if a person is found to intrude into the safe area, the robot stops moving and issues a warning, and then continues moving after the person retreats out of the safe range.

[0103] The robot autonomous walking control module is designed based on the open source move_base, in which the global path planner uses the A* algorithm, and the local path planner uses mature algorithms such as TEB. The map required for control is provided by the 2D map converted from the 3D point cloud map given by the laser radar mapping and positioning module. The relationship between each coordinate system is given by the relationship between the pre-calibrated coordinate systems, and the robot posture is given by the laser radar mapping and positioning module. This module realizes the autonomous walking function of the robot. The principle of the 3D to 2D algorithm is: remove the roadway ground points in the map; divide the ground area into grids according to the resolution; then project the data points within the height range occupied by the robot in the 3D map into each grid on the ground; when the number of points in the grid exceeds the given threshold, the grid is set to an occupied state; the remaining grids are set to an unoccupied state.

[0104] In the closed-loop control of track speed, the actual measurement output of the encoder is the angular increment, so it is necessary to obtain the angular velocity measurement value through numerical differentiation. Therefore, the resolution of the encoder cannot be too low, otherwise the angle quantization error will cause obvious angular velocity noise at low speed. Directly taking derivatives by differential means will result in strong angular velocity measurement noise. Filtering the angular increment and then performing differentiation will help reduce the output noise of the angular velocity measurement results, but it will cause signal delays and distortions. Therefore, the bandwidth of the low-pass filter cannot be too low. Because the angular velocity obtained by differentiation (especially under low speed conditions) has large noise. Therefore, the differential control term is not used in the controller to avoid the angular acceleration noise caused by the angular velocity differentiation from causing side effects on the control system.

[0105] The autonomous walking module plays an important role in coordinating the global path and obstacle avoidance. When obtaining obstacle information through various sensors, the target posture adjustment must be considered in the path planning process, and the posture requirements must be accurately classified.

[0106] In order to meet the requirements of different target postures within a small error, an improved A* algorithm is proposed to divide the estimation functions that respond to different scenarios. In terms of local planning, an optimized TEB algorithm is proposed by modifying the evaluation function to achieve dense dynamic obstacle avoidance, preventing the robot from falling into a "pseudo-death" state due to encountering obstacles that cannot be avoided, and realizing the robot's autonomous walking function.

[0107] The optimal global path is planned by optimizing the cost function to prevent the target posture from being adjusted at the end point and to avoid sudden rotations that may cause harm to workers. In actual global path planning, the optimal shortest path may conflict with the target posture. Therefore, the target posture scenarios can be divided into three categories:

[0108] ① At the moment of reaching the end point of the shortest path, if the X and Y directions of the moving coordinates of the roadway robot are the same as the X and Y directions of the target posture, it is considered as "same-direction posture".

[0109] ② At the moment of reaching the end of the shortest path, if only the moving coordinate X or Y in a single direction is the same as the X or Y direction of the target posture, it is considered a "neutral posture".

[0110] ③ At the moment of reaching the end point of the shortest path, if the X and Y directions of the moving coordinates of the alley robot are inconsistent with the X and Y directions of the target posture, it will be regarded as a "reverse posture".

[0111] Introducing the target state error ε into the cost function i Parameter ε i It can be expressed as the forward moving vector p i and the target posture vector q i The inconsistency degree of , β represents the target heading angle.

[0112] The cost function of the traditional A* algorithm is: f(i) = g(i) + h(i)

[0113] Where: g(i) is the path cost function, which calculates the cost from the current node n to the initial point; h(i) is the heuristic function, which calculates the minimum cost evaluation value from the current node n to the target point.

[0114] Add the target state error ε i After that, the heuristic function under different postures is D(i), so the updated cost function is: f(i)=g(i)+D(i).

[0115] In response to the three target situation demand scenarios, the heuristic functions under different situations can be expressed as follows:

[0116]

[0117] The global path planning algorithm enables the robot to quickly obtain an optimal path from the initial point to the target point, but new obstacles may appear during operation, so the local path planning TEB algorithm needs to be added. The advantage of the TEB algorithm is that it can effectively handle path planning problems in dynamic environments and improve the robot's motion stability and safety.

[0118] The classic “Elastic Band” consists of n robot positions x a =[x i ,y i ,θ i ] T ∈R 2 ×s 1 A sequence composed of i ,y i ) is the robot position, θ i is defined as the global orientation of the robot.

[0119] The sequence is shown in the formula:

[0120] Q = {x i} i＝0 …n, n∈N

[0121] The TEB algorithm strengthens the representation of the time elastic band by incorporating the time interval ΔT between consecutive poses. i , generates a sequence of n-1 time intervals:

[0122] τ={ΔT i} i＝0 …n-1

[0123] Each time interval represents the time required for the robot to go from its current pose to the next pose in the sequence Q. The TEB algorithm is defined as a tuple of two sequences:

[0124] B:=(Q,τ)

[0125] The key idea of ​​the optimized TEB algorithm is to use the weighted sum model to adjust and optimize the TEB algorithm. The specific method is "using a scaled multi-objective optimization method to optimize the configuration and time difference." The objective function and sequence results of the optimized TEB algorithm are shown in the formulas:

[0126]

[0127] Where: B* represents the optimized TEB algorithm sequence result; f(B) represents the global objective function.

[0128] The tunnel deformation detection module detects the deformation of the tunnels in the specified area according to the dense 3D point cloud map provided by the lidar mapping and positioning module, identifies the location and amount to be cleaned, and sends them to the task planning and decision-making module for decision-making.

[0129] The algorithm of the lane deformation detection module takes the lane 3D point cloud map (i.e. the lane 3D point cloud model) as input. The algorithm process is as follows: the 3D point cloud map is first segmented by the ground segmentation algorithm to obtain a 3D point cloud model representing the ground; the 3D point cloud model representing the ground is plane-fitted (in the process of ground plane fitting, the data used should focus on the point cloud data of the flat area under the robot crawler); the fitting residual is calculated based on the fitting result, and each area to be cleaned is segmented and output. This module realizes the lane deformation detection function.

[0130] In the tunnel deformation detection, the module takes the tunnel 3D point cloud map as input, inverts the acquired point cloud through cloth simulation filter (CSF), and then covers the inverted surface with rigid cloth. By analyzing the interaction between the cloth node and the corresponding point cloud, the position of the cloth node is obtained, and the approximate value of the ground is obtained, and the 3D point cloud model representing the ground is obtained. When cloth simulation is applied to point cloud filtering, the movement of particles is constrained in the vertical direction, and collision detection is achieved by comparing the height values ​​of particles and terrain. When the position of particles is lower than or equal to the terrain, the particles intersect with the terrain. Secondly, when the particle reaches the "correct position", that is, the ground, the particle is set to be immovable. Usually, the position of a particle is determined by the combined force of external and internal forces. In this modified cloth simulation, the displacement of the particle from the action of gravity is first calculated (the particle is set to be immovable when it reaches the ground, so the collision force can be omitted), and then the position of this particle is modified according to the internal force. The main implementation steps of this process are described as follows. First, the cloth particles and the point cloud are projected onto the same horizontal plane, and then the nearest point (called the corresponding point CP) is found for each cloth particle in this 2D plane. The intersection height value (IHV) is defined to record the height value of the CP (before projection). If the particle reaches the lowest position defined by this value, it can no longer move forward. During each iteration, the current height value (CHV) of the particle is compared with the IHV; if the CHV is equal to or lower than the IHV, move the particle back to the position of the IHV and make the particle immovable. An approximation of the real terrain is obtained through simulation, and the distance between the original point and the simulated particle is calculated. Points with a distance less than the threshold are classified as BE (roadway ground), while the remaining points are classified as (area to be cleaned).

[0131] After the robot arm accessory replacement control module receives the accessory replacement command sent by the task planning and decision-making module, it automatically puts back the original accessory and replaces it with the specified accessory according to the accessory type; during this period, the real-time posture of the robot arm movement is given by the robot arm (forearm) visual positioning module and the robot arm state feedback calculation module; among them, the robot arm (forearm) visual positioning module provides high-precision real-time posture near the position where the accessory turntable is close to the accessory, and the posture of each joint provided by the robot arm state feedback calculation module is used for control decoupling during the control process.

[0132] The robot arm lane cleaning control module controls the robot arm to clean the lane according to the requirements of the task planning and decision module and the robot arm posture information provided by the robot arm state feedback calculation module, and operates the robot arm driver according to the specified process.

[0133] According to user requirements, the task planning and decision-making module uses the services provided by the lane deformation detection module to detect lane deformation, uses the services provided by the robot autonomous walking control module to control the robot to move to the vicinity of the location to be cleaned, uses the services provided by the robotic arm accessory replacement control module to replace the required accessories, and finally uses the services provided by the robotic arm lane cleaning control module to clean the lane.

[0134] The logical relationship between the modules of the entire control system data and the distribution of each module on computing resources:

[0135] The video surveillance information from the front / rear surveillance cameras is directly transmitted to the user interaction and management module at the user end for real-time display; the point cloud data of the front / top / rear lidar and the acceleration, angular velocity and posture information measured by the IMU are transmitted to the lidar mapping and positioning module to build a 3D point cloud map of the robot's environment in real time and determine the robot's posture (position and posture) in the 3D point cloud map; the 3D point cloud map and robot posture are transmitted to the lane deformation detection module for lane deformation detection, giving the area to be cleaned and the height to be cleaned; the 3D point cloud map and robot posture are transmitted to the robot autonomous walking control module for path planning and robot motion control;

[0136] The data of the left / right crawler encoder is transmitted to the robot's autonomous walking control module for closed-loop control of crawler speed during robot motion control to ensure the dynamics of robot motion control.

[0137] The infrared video data of the front / rear infrared cameras are transmitted to the infrared personnel detection module for personnel detection (personnel's position, width and height), and the detection results are transmitted to the robot's autonomous walking control module for use in pedestrian avoidance during autonomous walking control.

[0138] The robot's autonomous walking control module controls the robot's autonomous walking according to the target posture requirements of the task planning and decision-making module, the robot's real-time posture, the 3D point cloud map and the left / right encoder data; during this period, obstacles and pedestrians are avoided based on the infrared personnel detection results and the lidar original point cloud data.

[0139] The video information collected by the positioning camera is transmitted to the robot arm (forearm) visual positioning module, and the precise position of the forearm relative to the robot body is calculated according to the QR code mark on the forearm for use by the robot arm attachment replacement control module.

[0140] The system of the present invention can realize lane deformation detection, automatic replacement of accessories and automatic cleaning of lanes:

[0141] Lane deformation detection: Automatic lane cleaning is the core function of this robot. The automatic lane cleaning function first requires obtaining information such as the deformation position and deformation amount of the lane to be cleaned, and then taking appropriate processes for cleaning. Lane deformation detection technology is a necessary prerequisite for realizing the core functions of this robot. Lane deformation detection technology requires first autonomously establishing a 3D model of the actual lane environment, then estimating the lane state before deformation based on the model, and finally segmenting the cleaning area based on the state before and after deformation.

[0142] Automatic replacement of accessories: The robot is equipped with a special accessory compartment to carry the data required for tunnel cleaning. After the robot arrives at the work site, it needs to automatically install the relevant accessories to the end of the robotic arm according to the process flow. Accessory replacement is a prerequisite for the realization of the tunnel cleaning process, and the automatic replacement technology of accessories is an important manifestation of the degree of automation of this robot. The interface between the accessories and the robotic arm is relatively precise, and even manual replacement of accessories is a very responsible and meticulous job. For the automatic replacement of accessories, it is required to accurately obtain the relative posture relationship between the end of the robotic arm and the accessories. In addition, considering the influence of operation accuracy, the accessory interface or accessory box is specially designed to have certain adaptive properties, thereby relaxing the requirements for positioning accuracy.

[0143] Automatic lane cleaning robot arm control: Automatic lane cleaning is the core function of the entire robot. At present, lane cleaning is generally done manually, and there is no mature automatic cleaning process for reference. Therefore, the automatic lane cleaning robot arm control process is one of the key technologies that must be solved in the development process of this system. The automatic lane cleaning robot arm control process involves how to determine which attachment to use for cleaning and which specific operation process to adopt for each attachment.

[0144] like Figure 5 The basic flow of the entire control strategy of the system of the present invention is as follows:

[0145] ①After the robot is started, the laser radar mapping and positioning work will start immediately;

[0146] ②Wait for command information from the user end;

[0147] ③After receiving the cleaning task command, control the robot to automatically walk to the vicinity of the designated location;

[0148] ④ Carry out tunnel deformation detection after arriving at the designated location;

[0149] ⑤ If the current attachment is not required for the current cleaning process, replace the attachment;

[0150] ⑥ Carry out cleaning work according to the predetermined process steps;

[0151] ⑦ If there are subsequent cleaning process steps, return to step ⑤;

[0152] ⑧If all cleaning process steps have been completed, return to step ②.

Claims

1. The autonomous navigation and operation system of the lane cleaning robot based on multi-sensor fusion is characterized by: Including mechanical systems, control systems and power systems; The mechanical system includes a crawler chassis, a mechanical arm, and a boom turntable; the mechanical arm is installed on the crawler chassis through the boom turntable to adjust the mechanical arm's posture; the mechanical arm includes a boom, a middle arm, and a forearm, and an attachment turntable is installed on the forearm to adjust the attachment's posture; based on different construction process requirements, the three attachments of a breaker hammer, a bucket, and a milling head are switched through quick change to achieve tunnel cleaning construction; The control system includes a hardware part and a software part; The software part adopts a layered architecture, including the perception layer, function layer, business layer and presentation layer from the bottom to the top: The presentation layer runs on the user terminal and includes the user interaction and management modules for user interaction and user management; The business layer runs on the robot side and includes the task planning and decision-making module, which is used for lane clearing task planning and decision-making; The functional layer runs on the robot side and is used to provide independent single functions for the business layer. This layer includes: robot autonomous walking control module, lane deformation detection module, robot arm attachment replacement control module, and robot arm lane cleaning control module. The perception layer runs on the robot side and is used to process the robot's sensor data and provide necessary perception information for the functional layer. The modules of the perception layer are independent of each other, including: infrared personnel detection module, laser radar mapping and positioning module, robotic arm visual positioning module, and robotic arm state feedback calculation module. The hardware part includes: Wireless data substation, providing access points for users to connect to the robot via wireless network; The main controller can run the task planning and decision-making module, the robot arm state feedback calculation module, the robot arm attachment replacement control module, and the robot arm lane cleaning control module; it can interact with the user end through the above modules; it can perform the top-level task planning and decision-making of the robot according to the user's requirements; The robot arm driver can control the robot arm to move to a specified position and posture through a solenoid valve; Displacement sensor, which can measure the displacement of each cylinder of the robot arm and is used for real-time posture calculation of the robot arm; Pressure sensor, which can measure the pressure of each link of the hydraulic system and is used for robot status monitoring; The crawler chassis driver controls the rotation of the crawler tracks through a solenoid valve, which is used for the final execution of the vehicle's autonomous walking action; Left / right track encoder, used to measure the rotation speed of the left and right tracks; IMU, used to measure the vehicle's angular velocity, acceleration, and inclination, which is used to fuse with LiDAR data for mapping; Point cloud data processor, running the laser radar mapping and positioning module, the robot autonomous walking control module, and the lane deformation detection module; Image data processor, running infrared personnel detection module and robotic arm visual positioning module; Switches, used to interconnect network devices; Front / rear surveillance cameras, used to provide the user with video surveillance information of the robot's surroundings; Front / rear infrared cameras are used to collect infrared video information of the robot's surrounding environment for personnel monitoring; Front / top / rear laser radars are used to collect 3D point cloud information of the robot's surroundings for mapping, positioning and obstacle avoidance; The power system includes a motor, an electromagnetic starter and a hydraulic pump station.

2. The autonomous navigation and operation system of the lane cleaning robot based on multi-sensor fusion as claimed in claim 1, characterized in that: The infrared personnel detection module processes the infrared video information collected by the infrared camera to detect the position, width and height information of the personnel in the field of view. This information is used for avoiding personnel in the environment during the obstacle avoidance process of the robot autonomous walking control module in the functional layer.

3. The tunnel cleaning robot autonomous navigation and operation system based on multi-sensor fusion as claimed in claim 1, characterized in that: The laser radar mapping and positioning module establishes a dense 3D point cloud map of the working environment based on the point cloud data provided by the laser radar carried by the robot and the acceleration and angular velocity measurements provided by the IMU, and determines the position and posture of the robot relative to the map. The 3D point cloud map and robot posture output by this module are the basis for the robot autonomous walking control module to control the robot's walking. At the same time, the 3D point cloud map is also the input of the lane deformation detection module.

4. The tunnel cleaning robot autonomous navigation and operation system based on multi-sensor fusion as claimed in claim 1, characterized in that: The robot arm visual positioning module accurately gives the position and posture of the robot arm relative to the robot attachment bin through visual positioning, and provides accurate posture information for the robot arm attachment replacement control module; The robot arm state feedback calculation module calculates the position information of each joint of the robot arm relative to the robot chassis reference coordinate system based on the measurement data of each sensor of the robot arm, and the calculation results are simultaneously transmitted to the robot arm lane cleaning control module and the robot arm accessory replacement control module for control.

5. The tunnel cleaning robot autonomous navigation and operation system based on multi-sensor fusion as claimed in claim 1, characterized in that: The robot autonomous walking control module controls the robot to move to the target posture according to the 3D point cloud map information and robot chassis posture information provided by the laser radar, and the target posture information provided by the task planning and decision module. During this period, the robot automatically avoids common obstacles and pedestrians in the environment according to the infrared personnel detection results and the original point cloud data of the laser radar; If a person is found to break into the safe area, the movement will be stopped and a warning will be issued. The movement will not continue until the person retreats out of the safe range.

6. The tunnel cleaning robot autonomous navigation and operation system based on multi-sensor fusion as claimed in claim 1, characterized in that: The lane deformation detection module performs deformation detection on the lanes within the specified area according to the dense 3D point cloud map provided by the lidar mapping and positioning module, identifies the location and amount to be cleaned, and sends them to the task planning and decision-making module for decision-making.

7. The tunnel cleaning robot autonomous navigation and operation system based on multi-sensor fusion as claimed in claim 1, characterized in that: After receiving the accessory replacement command from the task planning and decision module, the manipulator arm accessory replacement control module automatically puts back the original accessory and replaces it with the specified accessory according to the accessory type; during this period, the real-time position and posture of the manipulator arm movement is given by the manipulator arm visual positioning module and the manipulator arm state feedback calculation module; The robot vision positioning module provides high-precision real-time posture near the position where the attachment turntable is close to the attachment, and the posture of each joint provided by the robot state feedback calculation module is used for control decoupling during the control process.

8. The tunnel cleaning robot autonomous navigation and operation system based on multi-sensor fusion as claimed in claim 1, characterized in that: The robot arm lane cleaning control module controls the robot arm to clean the lane according to the requirements of the task planning and decision module and the robot arm posture information provided by the robot arm state feedback calculation module and operates the robot arm driver according to the specified process.

9. The tunnel cleaning robot autonomous navigation and operation system based on multi-sensor fusion as claimed in claim 1, characterized in that: The task planning and decision-making module, according to user requirements, uses the services provided by the lane deformation detection module to detect lane deformation, uses the services provided by the robot autonomous walking control module to control the robot to move to the vicinity of the location to be cleaned, uses the services provided by the robotic arm accessory replacement control module to replace the required accessories, and finally uses the services provided by the robotic arm lane cleaning control module to clean the lane.

10. The autonomous navigation and operation system of the lane cleaning robot based on multi-sensor fusion as claimed in claim 1, characterized in that: The video surveillance information of the front / rear surveillance cameras is directly transmitted to the user interaction and management module of the user end for real-time display; the point cloud data of the front / top / rear laser radars and the acceleration, angular velocity and posture information measured by the IMU are transmitted to the laser radar mapping and positioning module to establish a 3D point cloud map of the robot's environment in real time and determine the robot's position and posture in the 3D point cloud map; the 3D point cloud map and the robot's position and posture are transmitted to the lane deformation detection module for lane deformation detection, and the area and height to be cleaned are given; The 3D point cloud map and robot posture are transmitted to the robot autonomous walking control module for path planning and robot motion control; The data of the left / right crawler encoder is transmitted to the robot's autonomous walking control module for closed-loop control of crawler speed during robot motion control to ensure the dynamics of robot motion control; The infrared video data of the front / rear infrared cameras are transmitted to the infrared personnel detection module for personnel detection, and the detection results are transmitted to the robot autonomous walking control module for use in pedestrian avoidance during the autonomous walking control process.

Images