A large regular underground circular tunnel environment comprehensive positioning and sensing method

Abstract

The present invention relates to the field of industrial robots, and in particular to a method for comprehensive positioning and perception of large-scale regular underground circular tunnel environments. The method comprises the following steps: Step 1: global two-dimensional positioning; Step 2: local posture positioning; Step 3: tunnel environment perception. The beneficial effects of the present invention are: in the absence of satellite signals and wireless communication targets, by fusing the odometer, inertial measurement unit and laser radar target position information, global precise positioning in underground regular tunnels is achieved, which facilitates remote display and operation process monitoring; by multi-line laser radar and inertial measurement unit, the lateral deviation positioning, orientation deviation positioning and roll angle of the chassis relative to the tunnel wall are obtained to guide chassis motion control and robot arm trajectory planning and operation; by using multi-line laser radar, UWB base station and millimeter wave radar, information such as tunnel radius, obstacle and personnel distance can be perceived to guide the stable and safe operation of the chassis and robot arm.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of industrial robots, in particular to a large regular ground circular tunnel environment comprehensive positioning and sensing method. BACKGROUND

[0002] The final heat sink of the nuclear power unit in operation is the seawater cooling source, which is responsible for taking away the preheating of the core at shutdown, cooling various nuclear safety equipment, and plays an important role in the operation of nuclear power. The running state of the water intake directly affects the safety operation and reliability of the power plant. The heat generated during the operation of the unit will cause marine organisms to adhere to the cooling tunnel. A mobile carrier can be used to carry a mechanical arm to clean the tunnel wall. To achieve autonomous cleaning, the carrier in the tunnel needs to be accurately positioned in real time, and the inner wall environment of the tunnel needs to be sensed in real time, so that the carrier can be stably controlled through positioning data, the mechanical arm can obtain related environmental parameters for trajectory planning, and the mobile carrier and the mechanical arm can also be safely operated through the sensing of obstacle information. The ground tunnel environment cannot receive satellite signals for positioning, and the regular-shaped tunnel does not have enough texture to support visual or laser matching positioning. In the existing tunnel positioning technology, most of them use wireless communication modules arranged in the tunnel to realize positioning in the tunnel by using carrier wave ranging technology. However, the wireless communication module has a short ranging range, and the ranging method results in low ranging accuracy of moving objects, and the single price is not cheap. In a long tunnel, a large number of communication modules need to be arranged, which is high in cost. SUMMARY

[0003] The purpose of the present application is to provide a large regular ground circular tunnel environment comprehensive positioning and sensing method, which mainly solves the positioning and sensing problem in a large regular ground tunnel environment. Different sensors are carried on the chassis to realize the functions of global two-dimensional positioning, left and right deviation and orientation deviation positioning relative to the tunnel, tunnel radius sensing and obstacle sensing.

[0004] The technical scheme of the present application is as follows: A large regular ground circular tunnel environment comprehensive positioning and sensing method, comprising the following steps:

[0005] Step 1: Global two-dimensional positioning;

[0006] Step 2: Local attitude positioning;

[0007] Step 3: Tunnel environment sensing.

[0008] The step 1 comprises the following:

[0009] Step 11: Obtain sensor data;

[0010] Step 12: Reflective plate positioning solution;

[0011] Step 13: Positioning initialization;

[0012] Step 14: Inertial solution;

[0013] Step 15: High-precision positioning using error state Kalman filter algorithm;

[0014] Step 16: Real-time recording of current pose.

[0015] The data in step 11 includes three-axis acceleration and three-axis angular velocity data of the inertial measurement unit, relative coordinate data of the laser-recognized reflector, and wheel speed (odometer) data of the vehicle wheel speedometer (odometer).

[0016] The step 12 includes fine positioning and coarse positioning,

[0017] The fine positioning is to scan a plurality of reflectors by laser to obtain the relative positions {x l1 , x l2 , …, x li}, and it can be known that the vehicle starts or just starts to move near the starting point, and the estimated vehicle position The global position of the reflector is obtained The nearest neighbor matching is performed with the reflector in the map, and the position error is calculated The least square optimization is used:

[0018]

[0019] The fine positioning is obtained;

[0020] The coarse positioning is that when there is a single reflector, the vehicle is in real-time positioning, and the nearest reflector position x rw in the known map is selected according to the current position, the relative position x′ l of the reflector scanned by the laser, and the laser position is:

[0021] x lidar =x rw *(x′ F ) -1

[0022] The relative transformation T l2b between the laser and the vehicle body can be obtained:

[0023] X b =T l2b *X lidar .

[0024] The step 13 is to realize initial positioning in the retroreflective sheeting map, to use accurate positioning to initialize position in the range of the starting point, to assign the initial time position, initial time speed and initial time attitude if not starting at the initial point, and to initialize the state quantity, state quantity variance, process noise and observation noise.

[0025] The step 14 is to obtain the data of the inertial sensor as an acceleration vector alpha and an angular velocity vector omega, and the error of the sensor as b α , b ω , to calculate the position, speed and attitude according to the inertial navigation model.

[0026] The step 15 is divided into two steps of prediction update and observation update.

[0027] The prediction update is mainly divided into two steps. Firstly, the change of the state quantity (position, speed, attitude, deviation and gravity) in the Kalman system within a single time is calculated without considering the error of the IMU, the nominal state of the system is updated, and then the error state and the covariance matrix are updated.

[0028] The observation update is that when the system receives the wheel speed meter (odometer) information or when the system receives the position and posture of the retroreflective sheeting positioning, the Kalman gain is calculated according to different observation equations, the error state and the covariance matrix are updated, the updated error state and the nominal state are combined, the accurate positioning information of the robot is obtained, and the error state is reset for the next prediction update process.

[0029] The step 2 includes the following steps:

[0030] Step 21: the vehicle obtains real-time laser data, extracts laser points within a certain range of tunnel radius height as a tunnel wall fitting range, and rasterizes the laser points;

[0031] Step 22: segment the two side grid points into binary image coordinates and data;

[0032] Step 23: use the RANSAC algorithm to fit straight lines respectively, and convert them into laser coordinates to output in point-slope form, the point-slope straight line equation y=kx+b;

[0033] Step 24: the relative angle of the tunnel wall to the vehicle body is obtained from the point-slope slope k: theta=arctan(k), and the relative heading angle of the vehicle body is -theta, and the distance from the vehicle body to the tunnel wall can be calculated from the left and right deviations (dx, dy) of the vehicle body center relative to the laser

[0034] The step 3 includes the following steps:

[0035] Step 31: filtering the range of laser point cloud data, and down-sampling to reduce the number of point clouds and the amount of calculation;

[0036] Step 32: fitting a cylinder to the sampled point cloud by using the RANSAC algorithm, and outputting the parameters such as the coordinates of the center of the circle, the radius, and the normal vector of the circle surface;

[0037] Step 33: after fitting the cylinder, the inliers and outliers are obtained, the k-means clustering is performed on the outliers, when the number of clustered points is greater than 20 points, it is judged that the object is an obstacle, the clustered points are wrapped with a cube, and the center point of the cube is taken as the center point of the perceived obstacle.

[0038] The application has the beneficial effects that in the case of no satellite signal and wireless communication target, the global accurate positioning in the regular tunnel under the ground is realized by fusing the odometer, the inertial measurement unit and the target position information of the laser radar, the remote display and operation process monitoring are facilitated, the lateral deviation positioning, the orientation deviation positioning and the roll angle of the chassis relative to the tunnel wall are obtained by the multi-line laser radar and the inertial measurement unit, the chassis motion control, the mechanical arm trajectory planning and the operation are guided, the tunnel radius, the obstacle and the personnel distance information can be perceived by using the multi-line laser radar, the UWB base station and the millimeter wave radar, and the chassis and the mechanical arm are guided to run stably and safely. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 It is a two-dimensional positioning schematic diagram;

[0040] Figure 2 It is a two-dimensional positioning process;

[0041] Figure 3 It is a chassis attitude positioning;

[0042] Figure 4 It is a tunnel attitude positioning and perception process. DETAILED DESCRIPTION

[0043] The application will be further described in detail below in combination with the drawings and specific embodiments.

[0044] The large regular ground circular tunnel environment comprehensive positioning sensing method provided by the application mainly realizes global two-dimensional positioning, local attitude positioning and tunnel environment sensing of a mobile carrier in a tunnel by fusing multi-sensor information. The global two-dimensional positioning mainly determines the two-dimensional positioning of the chassis in the plane from the starting point of the tunnel, mainly uses three sensors of IMU, odometer and multi-line laser radar, and realizes the two-dimensional positioning of the chassis in the tunnel through extended Kalman filtering; the local attitude positioning mainly refers to the lateral deviation, orientation deviation and roll angle of the chassis relative to the inner wall of the tunnel, the point cloud fitting straight line based on the laser radar coordinate system is calculated by scanning the tunnel center axis through the multi-line laser radar, the lateral deviation and orientation deviation in the laser radar coordinate system can be obtained by calculating the direction vector of the straight line and the distance from the origin to the two straight lines, and the lateral deviation positioning and orientation deviation positioning of the chassis relative to the inner wall of the tunnel can be obtained in combination with the coordinate conversion relationship between the laser radar sensor and the chassis, and the roll angle can be directly obtained through the inertial measurement unit; the tunnel environment sensing mainly senses the tunnel radius and the obstacles in the tunnel, the actual radius of the inner wall of the tunnel can be obtained by fitting a cylinder to the inner wall point cloud, the obstacles in the tunnel can be sensed by clustering the outliers of the laser radar fitting point cloud or by pasting a reflector on the obstacles, the millimeter wave radar installed in front of the chassis can sense the obstacles on the track of the chassis, and the UWB base station can sense the approach of personnel.

[0045] A large regular ground circular tunnel environment comprehensive positioning sensing method, comprising the following steps:

[0046] Step 1: Global two-dimensional positioning

[0047] In the process of vehicle travel, reflective plates need to be used for positioning calibration in some areas to achieve accurate positioning for a long time. The reflective plates need to be arranged at intervals, and the positions of the reflective plates need to be recorded. The interval positions are related to the accuracy of the inertial measurement unit and the odometer fusion dead reckoning and the accuracy requirement. Generally, two reflective plates are arranged on both sides of the middle of the tunnel wall at the starting point of the tunnel, and one reflective plate is arranged on one side of the tunnel wall every 100m in the tunnel. The vehicle is placed at the starting point and is adjusted, the laser is turned on, and the approximate coordinates can be calculated according to the return of the remaining reflective plates, the recorded positions of all the reflective plates are saved as map data.

[0048] As shown in Figure 1 and Figure 2 , the following are the specific positioning steps:

[0049] Step 11: Obtain sensor data, obtain the following data by analyzing the port data of different sensors: three-axis acceleration and three-axis angular velocity data of the inertial measurement unit, relative coordinate data of the reflective plate identified by the laser, wheel speed (odometer) data of the vehicle wheel speed meter (odometer).

[0050] Step 12: Retro-reflective panel positioning solution

[0051] Specifically, it includes fine positioning and coarse positioning.

[0052] Fine positioning: the laser radar scans the relative positions of multiple retro-reflective panels {x l1 , x l2 , …, x li}, and it is known that the vehicle starts near the starting point or just starts to move. Given the estimated vehicle position (defaulted as (0, 0) when starting at the starting point), the global positions of the retro-reflective panels are obtained where T l2b is the coordinate conversion relationship of the laser radar to the vehicle center position. The nearest neighbor matching is performed with the retro-reflective panels in the map, and the position error is calculated The least squares optimization is used:

[0053]

[0054] where x li , i = 1, 2, 3, 4 represent the positions of the laser radar retro-reflective panels to the laser radar, x l is the position of the retro-reflective panel in the laser radar coordinate system when there is only one retro-reflective panel represents the global position data of the robot dead reckoning or setting, which is initially (0, 0) x wi , i = 1, 2, 3, 4 represent the global positions of the laser radar retro-reflective panels, x w represents the global position of a single retro-reflective panel dx wi i = 1, 2, 3, 4 represent the difference between the global position of the retro-reflective panel measured by the laser radar and the actual position of the retro-reflective panel e(x) represents the sum of the position error two-norm, which is used for least squares optimization to solve T l2b is the coordinate conversion relationship between the laser radar and the vehicle center, which only considers translation transformation x lidar represents the global position of the laser radar.

[0055] The fine positioning of the vehicle body can be obtained.

[0056] Coarse positioning: when there is only one retro-reflective panel, the vehicle is in real-time positioning, and the nearest retro-reflective panel position in the known map is selected according to the current position. The relative position of the laser scanned retro-reflective panel x l , then the laser radar position can be solved:

[0057] x lidar = x w *(x l ) -1

[0058] The relative transformation T l2bAvailable:

[0059] X b = T l2b X lidnr

[0060] Step 13: Initialization of positioning

[0061] In the retro-reflective board map, the initial position is initialized within the starting point range. If it is not started at the initial point, the initial position, the initial time speed (generally 0 at the initial static state), the initial attitude, and the state variables, state variable variances, process noise, and observation noise can be manually set. The initial variance can be theoretically set as the square of each variable noise, and in practice, it is generally intentionally set to be larger, which can accelerate the convergence speed. The process noise and observation noise are generally kept unchanged in the Kalman iteration process.

[0062] Step 14: Inertial solution

[0063] The data of the inertial sensor (IMU) sensor is the acceleration vector α and the angular velocity vector ω. The error of the sensor is b α , b ω , and the position, velocity, and attitude are calculated according to the inertial navigation kinematics model.

[0064] Step 15: High-precision positioning is achieved using the error state Kalman filter (ESKF) algorithm, which mainly includes prediction and observation update.

[0065] Prediction update: It mainly includes two steps. First, without considering the error of the IMU, the state variables (position, velocity, attitude, bias, and gravity) in the Kalman system within a single time are calculated, and the nominal state of the system is updated; then the error state and its covariance matrix are updated.

[0066] Observation update: When the system receives the wheel speed meter (odometer) information or when the system receives the retro-reflective board positioning pose, according to different observation equations, the Kalman gain is calculated, the error state and the covariance matrix are updated, the updated error state and the nominal state are combined, the precise positioning information of the robot is obtained, and the error state is reset for the next prediction update process.

[0067] Step 16: Real-time record of the current pose, when the machine is turned off and started next time, the positioning can continue.

[0068] Step 2: Local attitude positioning

[0069] For example Figure 3 and Figure 4As shown, the local pose of the chassis relative to the tunnel is mainly obtained by fitting the laser point cloud, and the roll angle of the chassis can be obtained by directly acquiring the data of the inertial measurement unit. The main steps can be described as follows:

[0070] Step 21: The vehicle acquires real-time laser data, extracts laser points within a certain range of tunnel radius height as the fitting range of the tunnel wall, and rasterizes the laser points.

[0071] Step 22: Segment the grid points on both sides and convert them to binary image coordinates and data.

[0072] Step 23: Use the RANSAC algorithm to fit straight lines respectively, and convert them to laser coordinates to output in point-slope form. The point-slope line equation is y=kx+b.

[0073] Step 24: Get the angle θ=arctan(k) of the vehicle body relative to the tunnel inner wall from the point-slope slope k, then the relative heading angle of the vehicle body is -θ. Get the relative transformation T between the laser and the vehicle body l2b Calculate the coordinate deviation (dx, dy) of the vehicle body center relative to the laser, so as to calculate the distance of the vehicle body to the tunnel wall

[0074] Step 3: Tunnel environment perception

[0075] Tunnel environment perception is mainly to perceive the radius of the tunnel and the obstacles in the tunnel in real time. The tunnel radius and the obstacles in the tunnel can be obtained by processing laser radar data, or by directly obtaining UWB data or millimeter wave radar data. The main steps of perception by laser radar are described as follows:

[0076] Step 31: Filter the range of laser point cloud data and downsample to reduce the number of point clouds and reduce the computational load.

[0077] Step 32: Fit the cylinder to the sampled point cloud using the Random Sample Consensus (RANSAC) algorithm, output the center coordinates, radius, and normal vector of the cylinder surface, etc.

[0078] Step 33: After fitting the cylinder, the inner and outer points can be obtained. The k-means clustering is performed on the outer points. When the number of clustered points is greater than 20 points, it is judged that the object is an obstacle. The clustered points are wrapped in a cube, and the center point of the cube is taken as the center point of the perceived obstacle.

Claims

1. A comprehensive positioning and perception method for a large regular underground circular tunnel environment, characterized in that: The steps include: Step 1: Global two-dimensional positioning; The step 1 includes the following: Step 11: Get sensor data; Step 12: Reflector positioning solution; The step 12 includes precise positioning and rough positioning. The precise positioning is achieved by scanning the laser beam to determine the relative positions of the multiple reflective plates. l1 , x l2 ,…,x li }, we know that the vehicle is starting or just starting to move near the starting point, and the estimated vehicle position is known. Get the global position of the reflector Perform nearest neighbor matching with the reflectors in the map and calculate the position error Use least squares optimization: You can get precise positioning; When the coarse positioning is for a single reflector, the vehicle is in real-time positioning and the nearest reflector position x in the known map is selected according to the current position. rw , the relative position x′ of the reflector scanned by the laser r , the laser position is: x lidar =x rw *(x′ r )-1 The relative transformation T between the laser and the vehicle body l2b We can get: X b =T l2b *X lidar Step 13: Positioning initialization; Step 14: Inertia solution; Step 15: Use Kalman filter algorithm to achieve high-precision positioning; Step 16: Record the current posture in real time; Step 2: local posture positioning; Described step 2 comprises the following steps: Step 21: The vehicle acquires real-time laser data, extracts laser points within a certain range of tunnel radius and height as the tunnel wall fitting range, and rasterizes the laser points; Step 22: Segment the grid points on both sides and convert them into binary image coordinates and data; Step 23: Use the RANSAC algorithm to fit the straight lines respectively and convert them into laser coordinates and output them in point-slope form. The equation of the point-slope straight line is y=kx+b; Step 24: The angle between the tunnel wall and the vehicle body is obtained from the point-to-point slope k: θ = arctan(k). The vehicle body's relative heading angle is -θ. The distance from the vehicle body to the tunnel wall can be calculated from the left and right deviations (dx, dy) of the center of the vehicle body relative to the laser. Step 3: Tunnel environment perception.

2. The method for comprehensive positioning and sensing of a large regular underground circular tunnel environment according to claim 1, characterized in that: The data in step 11 include three-axis acceleration and three-axis angular velocity data of an inertial measurement unit, relative coordinate data of a reflector identified by laser, and wheel speed data of a vehicle speedometer.

3. The method for comprehensive positioning and sensing of a large regular underground circular tunnel environment according to claim 1, characterized in that: The step 13 is to realize initial positioning in the reflector map. Within the defined starting point range, the position is initialized using precise positioning. If it is not started at the initial point, the initial position, initial velocity and initial attitude are assigned; at the same time, the state quantity, state quantity variance, process noise and observation noise are initialized.

4. The method for comprehensive positioning and sensing of a large regular underground circular tunnel environment according to claim 1, characterized in that: The step 14 is to obtain the data of the inertial sensor as the acceleration vector α, the angular velocity vector ω, and the sensor error b α , b ω , the position, velocity and attitude are calculated based on the inertial navigation model.

5. The method for comprehensive positioning and sensing of a large regular underground circular tunnel environment according to claim 1, characterized in that: The step 15 is divided into two steps: prediction update and observation update; The prediction update is mainly divided into two steps. First, without considering the error of the IMU, the state changes in the Kalman system within a single time step are calculated. The state variables include position, velocity, attitude, deviation and gravity, and the nominal state of the system is updated; then the error state and its covariance matrix are updated; The observation update described: When the system receives wheel speed meter information or when the system receives the position and posture of the reflector positioning, the Kalman gain is calculated according to different observation equations, the error state and covariance matrix are updated, the updated error state and nominal state are merged, the precise positioning information of the robot is obtained, and the error state is reset at the same time to carry out the next prediction update process.

6. The method for comprehensive positioning and perception of a large regular underground circular tunnel environment according to claim 1, characterized in that: Described step 3 comprises the following steps: Step 31: Filter the laser point cloud data range and downsample to reduce the number of point clouds and reduce the amount of calculation; Step 32: Fit a cylinder to the sampled point cloud using the RANSAC algorithm, and output the center coordinates, radius, and normal vector parameters of the cylinder surface; Step 33: After fitting the cylinder, obtain the inner and outer points, and perform k-means clustering on the outer points. When the number of cluster points is greater than 20 points, the object is judged as an obstacle, and the cluster points are wrapped with a cube, with the center point of the cube as the center point of the perceived obstacle.

Images