Hydropower station gallery inspection robot navigation method

Abstract

The invention relates to the technical field of positioning navigation, and discloses a hydropower station corridor inspection robot navigation method, which comprises the following steps: S1, navigation system construction: constructing an inspection robot and a navigation system thereof; s2, map making: making a navigation map; s3, performing initial pose relocation optimization: performing initial pose relocation optimization on the inspection robot; and S4, routing inspection path planning: planning a routing inspection path. The problems of low navigation accuracy and the like in the prior art are solved.

Description

Technical Field

[0001] The invention relates to the technical field of positioning and navigation, and in particular to a navigation method for a hydropower station corridor inspection robot. Background Art

[0002] In recent years, positioning and navigation technologies have been widely used in the inspection of indoor scenes with rich geometric features. However, due to the feature degradation of corridor scenes and the lack of GPS signals, the existing mapping and navigation algorithms rely on a large amount of geometric feature information, which makes it extremely difficult for corridor inspection robots to build maps and navigate.

[0003] In order to realize the accurate map construction and stable navigation of the anti-bumping inspection robot in the complex pressure relief tunnel of the mine, Si Lei et al. proposed a positioning and mapping method for the anti-bumping inspection robot based on IMU-LiDAR tight coupling and a dynamic path planning method based on artificial potential field method and rapidly expanding random tree (RRT), and verified the feasibility of this method in various scenarios through experiments. In order to solve the problem of insufficient positioning accuracy in complex underground environments, Bai Shiyu et al. proposed a patrol target recognition method based on the fusion of laser radar and infrared camera, which can relatively locate multiple targets in weak light environments. In order to solve the problem of low inspection efficiency of wheeled inspection robots in factory environments, Wang et al. proposed a hybrid path planning algorithm combining A* algorithm and time elastic band (TEB) method, and used Gazebo simulation software in ROS system to simulate and verify that the inspection robot fell into the local optimal solution. For the substation scenario, Ren et al. built a patrol navigation system. It uses the gmapping algorithm to complete the two-dimensional map construction, integrates the A* algorithm and the dynamic window method (DWA) to complete the path planning and task execution, and is applied to the switch cabinet operation of indoor substations. et al. [7] A two-dimensional map-based path-following navigation method is proposed, which aims to use the memory of previous paths to travel along the same path. This method can reach the target with a low error rate in a multi-turn system, but symmetric or repeated target sequences may cause the inspection robot to detect similar patterns and make wrong decisions. Summary of the invention

[0004] In order to overcome the deficiencies of the prior art, the present invention provides a navigation method for a hydropower station corridor inspection robot to solve the problems of low navigation accuracy in the prior art.

[0005] The technical solution adopted by the present invention to solve the above problems is:

[0006] A navigation method for a hydropower station corridor inspection robot comprises the following steps:

[0007] S1, Navigation system construction: Build the inspection robot and its navigation system;

[0008] S2, map making: making navigation maps;

[0009] S3, initial posture repositioning optimization: repositioning and optimizing the initial posture of the inspection robot;

[0010] S4, inspection route planning: planning the inspection route.

[0011] As a preferred technical solution, in step S2, a 3D laser SLAM algorithm is used to create an environmental point cloud map, and then the environmental point cloud map is converted into a two-dimensional grid navigation map after point cloud filtering.

[0012] As a preferred technical solution, in step S2, making an environment point cloud map includes the following steps:

[0013] S21, data preprocessing: input inertial measurement unit data and lidar data, pre-integrate the inertial measurement unit data to perform radar point cloud compensation and obtain the relative transformation posture between the two frames;

[0014] S22, laser inertial navigation data fusion: using the iterative error extended Kalman filter algorithm, the dedistorted radar point cloud data and the inertial measurement unit pre-integrated data are fused to output the predicted position and posture of the laser inertial navigation odometer;

[0015] S23 uses a 3D laser SLAM algorithm tightly coupled with a lidar odometer to build a point cloud map. When the lidar field of view exceeds the map boundary, the incremental kd tree is used to incrementally update the point cloud map, delete the historical point cloud and insert a new frame of point cloud into the global map to maintain the dynamic balance of the tree structure.

[0016] As a preferred technical solution, in step S21, according to the inertial measurement unit data, the position and posture of the inspection robot at time t+Δt in the world coordinate system is obtained as follows:

[0017]

[0018] In the formula, W R t+Δt represents the rotation matrix of the inspection robot in the world coordinate system at time t+Δt, Represents the rotation matrix from the inertial measurement unit coordinate system to the world coordinate system, and e represents the exponential operator represents the angular velocity measured by the sensor in the inertial measurement unit coordinate system at time t, Indicates the gyroscope bias, represents the white noise of the gyroscope, Δt represents the time interval between two frames of data, W V t+Δt represents the speed of the inspection robot in the world coordinate system at time t+Δt,W v t represents the speed of the inspection robot in the world coordinate system at time t, W g represents the gravitational acceleration in the world coordinate system, Represents the acceleration measured by the sensor in the inertial measurement unit coordinate system at time t and angular velocity, represents the accelerometer zero bias, represents the white noise of the accelerometer, W P t+Δt represents the position of the inspection robot in the world coordinate system at time t+Δt, W P t represents the position of the inspection robot in the world coordinate system at time t, W v t Indicates the speed of the inspection robot in the world coordinate system.

[0019] As a preferred technical solution, when filtering the environment point cloud, the points that meet formula (2.12) are regarded as noise points and removed:

[0020]

[0021] in,

[0022]

[0023] In the formula, p and q represent two point cloud data respectively, d qk represents the average distance from point q to the k-neighborhood points of point q, represents the mean distance of all k-neighborhoods of the overall point cloud after downsampling, n represents the distance standard deviation parameter, δ represents the distance standard deviation of all k-neighborhoods of the overall point cloud after downsampling, N Indicates the total number of points in the point cloud. Q represents the overall point cloud retained after downsampling, Q qk represents the k-neighborhood of point q, d qp Represents the Euclidean distance between point q and point p.

[0024] As a preferred technical solution, in step S2, the initial posture repositioning and optimization of the inspection robot is performed based on the AMCL algorithm: first, a set of inspection robot posture sets represented by odometer covariance noise is generated according to the observation, and then the laser scanning and map point matching are performed for the posture of each particle, and the matching results are scored for correlation, and the posture with the highest correlation score is selected as the optimal posture of the inspection robot; if the map and the scanned frame have the highest matching degree, it is defined as the initial posture.

[0025] As a preferred technical solution, the formula for relevance scoring is:

[0026]

[0027] In the formula, S represents the correlation score, A represents the grid map, and B represents the current radar scanning frame. Represents the average value of the pixels in the raster map. represents the average occupancy state of the scan frame, m represents the x-axis coordinate of the pixel, n represents the y-axis coordinate of the pixel, A mn Indicates that the position of the grid map point in the grid map coordinate system is (m,n), B mn Indicates that the position of the current radar scan frame in the grid map coordinate system is (m,n).

[0028] As a preferred technical solution, in step S4, global path planning is performed based on the A* algorithm, and the cost function is:

[0029] f(n)=g(n)+w(n)*h(n)(3.2)

[0030] in,

[0031] Where n represents the current node, f(n) represents the cost function of global path planning, g(n) represents the actual cost value of the inspection robot from the starting position to the current node, w(n) represents the heuristic function weight factor, h(n) represents the cost function of the inspection robot from the current grid position to the target grid position, and x n Indicates the x-axis coordinate of the grid where the robot is currently located, y n Indicates the y-axis coordinate of the grid where the robot is currently located, x g Indicates the x-axis coordinate of the grid of the target node, y g Indicates the y-coordinate of the target node on the grid.

[0032] As a preferred technical solution, in step S4, local path planning is performed based on the TEB algorithm, and the formula is:

[0033]

[0034] in,

[0035] In the formula, R * represents the optimal path of the TEB algorithm, R represents one of the constraints that limit the robot's trajectory, and γ k represents the weight value of the objective function, f(R) represents the objective function with multiple constraints, and f k (R) represents the objective function of one of the constraints.

[0036] As a preferred technical solution, in step S1, a patrol robot equipped with a laser radar, an inertial measurement unit, and an autonomous environmental perception module is constructed.

[0037] Compared with the prior art, the present invention has the following beneficial effects:

[0038] The present invention proposes a design and implementation of a navigation system for a hydropower station ground inspection robot based on ROS. The inspection robot can rely on its own three-dimensional laser radar, IMU and wheel odometer to complete the perception of the environment and the construction of a point cloud map. At the same time, for the inspection conditions of low-feature large-scene maps, the expression of the navigation map and the matching strategy of the initial point position during inspection are optimized. Experimental verification shows that the efficiency of initial posture positioning and matching can be effectively shortened. Finally, under the fusion path planning, the inspection robot can perform the inspection task reliably and stably. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] Figure 1 This is a site map of the inspection environment;

[0040] Figure 2 This is the inspection system framework diagram;

[0041] Figure 3 This is the robot odometer model diagram;

[0042] Figure 4 Diagram of the process of building SLAM maps for some corridors;

[0043] Figure 5 Repositioning and optimizing flow chart for AMCL;

[0044] Figure 6 This is a schematic diagram of initial posture positioning;

[0045] Figure 7 It is a safe path passing rule diagram;

[0046] Figure 8 This is a real prototype picture;

[0047] Fig. 9 It is the control input and odometer feedback diagram;

[0048] Fig.10 Build a map for SLAM;

[0049] Fig.11 This is a filtered image of a part of the point cloud map;

[0050] Fig.12 This is a comparison chart of some raster maps after filtering;

[0051] Fig.13 This is the initial posture positioning accuracy test diagram;

[0052] Fig.14 Provides a navigation map for barrier-free corridor scenes;

[0053] Fig.15 Obstacle avoidance map of corridor environment fused with A* and TEB. DETAILED DESCRIPTION

[0054] The present invention will be further described in detail below in conjunction with embodiments and drawings, but the embodiments of the present invention are not limited thereto.

[0055] Example 1

[0056] like Figures 1 to 15 As shown in the figure, the present invention proposes a patrol inspection robot mapping and navigation system designed for large scene corridors with narrow interiors and unclear geometric features in hydropower stations. First, we built a mapping and navigation perception hardware platform integrating LiDAR and Inertial Measurement Unit (IMU), and used the IMU-LiDAR tightly coupled laser SLAM algorithm to obtain the environmental point cloud map. Then, the generated map was filtered, denoised and dimensionally reduced, and then converted into an efficient two-dimensional grid navigation map. Subsequently, the initial pose matching strategy of the adaptive Monte Carlo localization (AMCL) algorithm was optimized, which significantly improved the initial pose repositioning speed and accuracy of the patrol robot. Finally, the integration of the safe A* and TEB algorithms enables the patrol robot to have the ability to flexibly avoid obstacles during the patrol process, thereby greatly improving the safety and efficiency of the patrol operation. In order to verify the actual application performance of the system, functional tests were carried out in laboratory simulation scenes and in the dam corridor of a large hydropower station. The test results fully demonstrated the effectiveness and reliability of the system and showed its superior performance in practical applications.

[0057] 1. Corridor inspection robot design

[0058] 1.1 Introduction to typical working conditions of hydropower station corridors

[0059] The hydropower station corridor inspected by the crawler mobile inspection robot has an overall tunnel length ranging from 600m to 1000m. It is constructed by pouring concrete walls with a 3m semicircular arch on the top and a 3.5m wide bottom.

[0060] Inside the corridor, the fixed facilities of the arch are mainly limited to lighting equipment and scaffolding support steel frames left over from the construction process. An I-type cable rack is installed on one side of the dam wall of the corridor, which is mainly used to store communication and power supply cables for power transmission and voltage distribution related equipment, and is a key basic equipment inside the corridor.

[0061] The inspection road surface is scattered with obstacles such as power distribution cabinets and fire boxes, as well as temporary construction personnel walking around, which makes inspection by inspection robots challenging. In addition, some corridor sections are affected by water leakage, which inevitably leads to water accumulation on the already narrow road surface. The additional drainage pipelines will further increase the difficulty of inspection navigation.

[0062] 1.2 Inspection robot design

[0063] Considering the requirements of corridor working conditions and inspection navigation tasks, the navigation system design of the inspection robot needs to comply with the following three basic design requirements:

[0064] (1) Having stable mechanical structure and motion performance;

[0065] (2) Accurate mapping and positioning of inspection robots in satellite-denied corridor environments through autonomous environmental perception;

[0066] (3) Realize autonomous movement and obstacle avoidance in environments with moving people and some narrow corridors.

[0067] 1.3 Kinematic Analysis of Inspection Robot

[0068] Figure 3 Demonstrates the motion of the tracked inspection robot in the two-dimensional plane xoy. Figure 3 Where XOY is the global coordinate system, the linear velocity and angular velocity of the inspection robot chassis are v and w respectively, and the left and right tracks are symmetrically distributed along the central axis of the motion direction with a spacing of 2d. Assume that the inspection robot chassis position at time k is (x k ,y k ,θ k ), the pose at time k+1 is (x k+1 ,y k+1 ,θ k+1 ).

[0069] In the time interval Δt, the running trajectory of the inspection robot is approximated as a circular arc, and the radius r of the circular arc is as shown in formula (1.1):

[0070]

[0071] After the inspection robot moves Δt, the position of the inspection robot at time k+1 is as shown in formula (1.2):

[0072]

[0073] In conjunction with sensors such as wheel odometers and IMUs, the above formula can be used to obtain the position and posture of the inspection robot at any time in the map coordinate system.

[0074] 2. Inspection map production

[0075] 2.1 Laser SLAM Mapping

[0076] The inspection robot mapping module uses the 3D laser SLAM algorithm FAST-LIO2 tightly coupled with the lidar odometer to build a point cloud map, directly aligns the original point cloud and uses an incremental kd-tree (ikd-tree) data structure for dynamic management.

[0077] It is mainly divided into three parts: (1) Data preprocessing: input IMU and lidar raw data (the robot matches the input inertial measurement unit and lidar data in real time during the dynamic process, and completes the positioning and map construction process incrementally. That is, after inputting radar data and inertial measurement unit data, the output is a complete point cloud map), pre-integrate IMU data (the general usage of IMU is to integrate the acceleration and angular velocity it measures, and as the integration time increases, this process will produce large drift and huge calculation amount. The use of pre-integration is mainly to calculate its state quantity between two radar frames and input it into the SLAM process as the pose optimization constraint of radar observation), use IMU pre-integration to compensate for radar point cloud and obtain the relative transformation pose between the two frames; define the world coordinate system as {W}, the state x of the inspection robot at time t t as follows:

[0078]

[0079] In the formula, W R t ∈SO(3) represents the rotation matrix of the inspection robot in the world coordinate system, Indicates the position of the inspection robot in the world coordinate system, represents the speed of the inspection robot in the world coordinate system, Respectively represent the gyro bias and accelerometer bias of the IMU. The acceleration measured by the sensor in the IMU coordinate system at time t and angular velocity It can be expressed as:

[0080]

[0081] In the formula, a t and ω t are the actual acceleration and angular velocity, and represents the white noise of the accelerometer and the white noise of the gyroscope, Represents the rotation matrix from the world coordinate system to the IMU coordinate system.

[0082] According to the IMU measurement value, the position and posture of the inspection robot in the world coordinate system at time t+Δt can be obtained as follows:

[0083]

[0084] In formula (2.6), Represents the rotation matrix from the IMU coordinate system to the world coordinate system, setting the IMU measurement value and is a constant.

[0085] Using IMU pre-integration, the transformed pose between time i and time j can be obtained:

[0086]

[0087] (2) Laser inertial navigation data fusion: The iterative error extended Kalman filter algorithm is used to fuse the dedistorted radar point cloud data with the IMU pre-integrated data to output the predicted position and posture of the laser inertial navigation odometer.

[0088] (3) Global map construction. When the LiDAR field of view exceeds the map boundary, ikd-tree supports incremental updates of the point cloud map, deletes the historical point cloud, and inserts a new frame of point cloud into the global map to maintain the dynamic balance of the tree structure. The current SLAM algorithm can efficiently complete the construction of the point cloud map.

[0089] 2.2 Point cloud filtering

[0090] Since SLAM is a dynamic process, the LiDAR measurements will generate sparse outlier noise point clouds in addition to the dense normal point cloud map. This section will perform filtering processing on the following three aspects: (1) intercepting and filtering out point cloud data outside the radar perception range; (2) removing the trailing points of dynamic obstacles; and (3) removing redundant and isolated noise points in the point cloud.

[0091] First, the point cloud interception work is completed by using the straight-through filtering method, and then the voxel filtering divides the input point cloud data into voxel grids of a given resolution size, and the centroid is used to represent this voxel grid. Therefore, after voxel filtering, the downsampled point cloud is obtained, realizing the downsampling of the point cloud after straight-through filtering.

[0092] After the above steps, there are still a few outliers in the three-dimensional space, which are removed based on the difference in the distribution density between noise points and normal points. Outlier noise points usually appear more scattered and are often surrounded by empty areas. First, calculate the distance mean and standard deviation of all k-neighborhoods of the overall point cloud after downsampling.

[0093]

[0094] Where Q represents the overall point cloud retained after downsampling, Q qk represents the k-neighborhood of point q, d qprepresents the Euclidean distance between points q and p. The overall distance standard deviation is expressed as:

[0095]

[0096] If the k-neighborhood average distance of a local point in the point cloud is smaller than the k-neighborhood average distance of the point cloud as a whole, it is considered a noise point.

[0097]

[0098] d qk is the average distance from point q to its k neighboring points, n is the distance standard deviation parameter, and the points that meet the above formula are regarded as noise points and removed.

[0099] 2.3 Construction of two-dimensional occupancy grid map

[0100] After completing the above point cloud filtering processing, the 3D point cloud is projected into 2D. Each grid cloud represents a small area in the space, which contains a part of the point cloud. The scanned area is divided into grids on the xoy plane, and the scanned point cloud is projected onto the grid plane. The extreme difference in the z-axis direction in the grid is counted to determine whether the point in the grid is a passable ground point or an obstacle point. Finally, the construction of the 2D grid map is completed according to the defined grid size and resolution.

[0101] 3 Inspection Robot Navigation Control Algorithm

[0102] 3.1 Initial pose relocalization optimization based on AMCL algorithm

[0103] After the processing in Section 2, the current inspection robot positioning problem can be described as: when the environment map is known, the environment perception data is acquired and analyzed in real time by sensors to estimate the possible posture of the inspection robot in the map. However, in view of the failure of the AMCL positioning algorithm to match the initial posture of the inspection robot in the corridor convergence environment, this section introduces the idea of ​​limited range repositioning, matching the scanning information within the limited laser radar detection angle and distance with the grid map, and the AMCL positioning algorithm is improved as follows:

[0104] In AMCL, a set of inspection robot poses represented by odometer covariance noise is first generated based on observations, and then the laser scanning and map points are matched for the pose of each particle, and the matching results are scored for relevance.

[0105]

[0106] Where S is the correlation score, A represents the grid map, and B represents the current radar scan frame. is the average value of the pixels in the raster map, is the average occupancy state of the scanned frame (where occupied is 1 and idle is 0), and m and n represent the x and y coordinates of the pixel.

[0107] The above formula is used to select the posture with the highest correlation score as the optimal posture of the inspection robot. At the same time, the map and the scanned frame have the highest matching degree, which is reflected on the map as the initial posture with the highest fit.

[0108] 3.2 Path Planning Algorithm Fusion

[0109] The path planning of the present invention is divided into two parts: global path planning and local path planning. The global path planning is planned once before the navigation task is executed, while the local path planning is used for line patrol and obstacle avoidance.

[0110] 3.2.1 Safe Path Exploration Based on A*

[0111] In global path planning, a heuristic path search algorithm is used: A* algorithm. The heuristic function weight factor w(n) and path safety restrictions are added to it to avoid the inspection robot path searching for diagonally opposite paths so as to obtain the global optimal path in the above global static map with efficiency and safety as constraints according to the defined cost function. The cost function is expressed as:

[0112] f(n)=g(n)+w(n)*h(n)(3.2)

[0113] Among them, n represents the current node; f(n) represents the cost function of the current node; g(n) is the actual cost value of the inspection robot from the starting position to the current node; the larger the weight factor of w(n), the closer it will be to the breadth-first (BFS) algorithm, and vice versa, the closer it will be to the depth-first (Dijkstra) algorithm; h(n) represents the cost function of the inspection robot from the current node to the target node, and its function is expressed as:

[0114]

[0115] Among them, (x n ,y n ) is the grid coordinate of the current node, (x g ,y g ) is the grid coordinate of the target node.

[0116] 3.2.2 TEB local path planning

[0117] The TEB algorithm is used for local path planning. It abstracts the local path planning as an elastic rubber band that is balanced by internal and external forces. While following the global path and maintaining the distance from obstacles, it adds a discrete variable time series {ΔT i} i＝0,1,...n-1 Can be used with the inspection robot posture sequence {Si} i＝0.1,...,n Composition constraints.

[0118] The overall algorithm is to obtain the optimal path posture point set by optimizing a weighted multi-objective function in real time. The optimal path R * It is obtained by minimizing a weighted nonlinear least squares function:

[0119]

[0120] Among them, R * is the optimal path of the TEB algorithm, γ k is the weight value of the objective function, f(R) represents the objective function with multiple constraints, f k (R) is one of the constraint functions.

[0121] For the constraint objective function f k (R), there are four constraints, as shown in Table 1.

[0122] Table 1 Constraints and constraint descriptions

[0123]

[0124] The optimized A* algorithm can quickly calculate a global optimal path connecting the inspection starting point and the target point. Combined with precise local motion control, the navigation task can be completed safely and efficiently.

[0125] 4 Experiments

[0126] The experimental platform built by the present invention adopts Songling BUNKER crawler chassis, Livox Mid-360 laser radar, wheeltec N100 IMU, and thinkpad X280 industrial computer. The control part adopts Ubuntu 18.04 operating system, and performs mapping and navigation related tasks in ROS1 environment.

[0127] 4.1 Hardware and sensor related tests

[0128] After the entire inspection and mapping navigation system is built, the control command input of the inspection scene's linear speed and steering angular velocity is compared with the odometer's response feedback test results. Fig. 9 . Fig. 9 In the figure, (a) is the forward linear velocity command, (b) is the steering angular velocity command, (c) is the forward linear velocity command, and (d) is the angular velocity response of the odometer.

[0129] 4.2 SLAM Mapping Effect

[0130] The mapping operation is completed in the laboratory area surrounded by four corridors. The generated point cloud map is as follows Fig.11 As shown, the subsequent filtering and dimensionality reduction rasterization are carried out in the following experimental scenarios.

[0131] 4.3 Comparison of original point cloud and filtered effect

[0132] Through offline filtering, the point cloud map generated by SLAM perception of some corridor scenes is processed by point cloud filtering. Fig.11 Comparison of filtering effects in some corridors.

[0133] Table 2 Point cloud filtering effect table

[0134]

[0135] Compared with the original point cloud map, the point cloud after direct pass and voxel filtering has a data volume reduction of 80.85%, while the point cloud after k-neighborhood filtering has a data volume reduction of 41.85% compared with the point cloud after direct pass and voxel filtering.

[0136] Compare the grid map generated by the ROS filtering function package pcd2pgm with the filtering and grid map preparation effects used in the present invention. Fig.12 .

[0137] The outliers of the grid map generated by the present invention are effectively removed, the map outline is clearer, and the accuracy of navigation is better guaranteed.

[0138] 4.4 AMCL initial position matching optimization experiment

[0139] In the map, multiple different positions are selected to test the positioning of the improved AMCL algorithm. The car is moved to the corresponding point on the map, and then the AMCL initial pose is matched with the optimization algorithm.

[0140] Using the AMCL initial position positioning method, among the 7 test points selected by the present invention, the average Euclidean distance error is 0.541m, of which the largest error is 0.948m and the smallest error is 0.279m; the average orientation error is 0.095rad.

[0141] Through the improvement of the present invention, the average Euclidean distance error is 0.071m, the position matching error of the inspection robot on the map is reduced by 86.88%, and the position error is controlled within 8cm during initial posture matching; the average orientation error is 0.052rad, the orientation matching error of the inspection robot is reduced by 45.26%, and the angle error is controlled within 3°.

[0142] 4.5 Navigation function test

[0143] The inspection robot’s navigation function was demonstrated in an experimental corridor scenario. Fig.14 The global path planning and motion control in the corridor barrier-free scenario are demonstrated, with the actual corridor environment on the left and the rviz interface on the right.

[0144] Warning barrels were placed inside the corridor to simulate the TEB obstacle avoidance effect of the inspection robot when temporary obstacles appeared. Fig.15 , the left side is the actual corridor environment, and the right side is the rviz interface.

[0145] As described above, the present invention can be preferably implemented.

[0146] All features disclosed in all embodiments in this specification, or steps in all methods or processes implicitly disclosed, except for mutually exclusive features and / or steps, can be combined and / or expanded or replaced in any manner.

[0147] The above description is only a preferred embodiment of the present invention and does not limit the present invention in any form. According to the technical essence of the present invention, within the spirit and principles of the present invention, any simple modification, equivalent replacement and improvement made to the above embodiment still falls within the protection scope of the technical solution of the present invention.

Claims

1. A navigation method for a hydropower station corridor inspection robot, characterized in that: The following steps are involved: S1, Navigation system construction: Build the inspection robot and its navigation system; S2, map making: making navigation maps; S3, initial posture repositioning optimization: repositioning and optimizing the initial posture of the inspection robot; S4, inspection route planning: planning the inspection route.

2. A navigation method for a hydropower station corridor inspection robot according to claim 1, characterized in that: In step S2, a 3D laser SLAM algorithm is used to create an environmental point cloud map, and then the environmental point cloud map is converted into a 2D grid navigation map after point cloud filtering.

3. A navigation method for a hydropower station corridor inspection robot according to claim 2, characterized in that: In step S2, making an environment point cloud map includes the following steps: S21, data preprocessing: input inertial measurement unit data and lidar data, pre-integrate the inertial measurement unit data to perform radar point cloud compensation and obtain the relative transformation posture between the two frames; S22, laser inertial navigation data fusion: using the iterative error extended Kalman filter algorithm, the dedistorted radar point cloud data and the inertial measurement unit pre-integrated data are fused to output the predicted position and posture of the laser inertial navigation odometer; S23 uses a 3D laser SLAM algorithm tightly coupled with a lidar odometer to build a point cloud map. When the lidar field of view exceeds the map boundary, the incremental kd tree is used to incrementally update the point cloud map, delete the historical point cloud and insert a new frame of point cloud into the global map to maintain the dynamic balance of the tree structure.

4. A navigation method for a hydropower station corridor inspection robot according to claim 3, characterized in that: In step S21, according to the inertial measurement unit data, the position and posture of the inspection robot in the world coordinate system at time t+Δt is obtained as follows: In the formula, W R t+Δt represents the rotation matrix of the inspection robot in the world coordinate system at time t+Δt, Represents the rotation matrix from the inertial measurement unit coordinate system to the world coordinate system, and e represents the exponential operator represents the angular velocity measured by the sensor in the inertial measurement unit coordinate system at time t, Indicates the gyroscope bias, represents the white noise of the gyroscope, Δt represents the time interval between two frames of data, W V t+Δt represents the speed of the inspection robot in the world coordinate system at time t+Δt, W v t represents the speed of the inspection robot in the world coordinate system at time t, W g represents the gravitational acceleration in the world coordinate system, Represents the acceleration measured by the sensor in the inertial measurement unit coordinate system at time t and angular velocity, represents the accelerometer zero bias, represents the white noise of the accelerometer, W P t+Δt represents the position of the inspection robot in the world coordinate system at time t+Δt, W P t represents the position of the inspection robot in the world coordinate system at time t, W v t Indicates the speed of the inspection robot in the world coordinate system.

5. A hydropower station corridor inspection robot navigation method according to claim 2, characterized in that: When filtering the environment point cloud, the points that meet formula (2.12) are regarded as noise points and removed: in, In the formula, p and q represent two point cloud data respectively, d qk represents the average distance from point q to the k-neighborhood points of point q, represents the mean distance of all k-neighborhoods of the overall point cloud after downsampling, n represents the distance standard deviation parameter, δ represents the distance standard deviation of all k-neighborhoods of the overall point cloud after downsampling, N Indicates the total number of points in the point cloud. Q represents the overall point cloud retained after downsampling, Q qk represents the k-neighborhood of point q, d qp Represents the Euclidean distance between point q and point p.

6. A navigation method for a hydropower station corridor inspection robot according to claim 1, characterized in that: In step S2, the initial posture repositioning and optimization of the inspection robot is performed based on the AMCL algorithm: first, a set of inspection robot posture sets represented by odometer covariance noise is generated based on observations, and then the posture of each particle is laser scanned and matched with map points, and the matching results are scored for relevance, and the posture with the highest relevance score is selected as the optimal posture of the inspection robot; if the map and the scanned frame have the highest matching degree, it is defined as the initial posture.

7. A navigation method for a hydropower station corridor inspection robot according to claim 6, characterized in that: The formula for relevance score is: In the formula, S represents the correlation score, A represents the grid map, and B represents the current radar scanning frame. Represents the average value of the pixels in the raster map. represents the average occupancy state of the scan frame, m represents the x-axis coordinate of the pixel, n represents the y-axis coordinate of the pixel, A mn Indicates that the position of the grid map point in the grid map coordinate system is (m,n), B mn Indicates that the position of the current radar scan frame in the grid map coordinate system is (m,n).

8. A navigation method for a hydropower station corridor inspection robot according to claim 1, characterized in that: In step S4, global path planning is performed based on the A* algorithm, and the cost function is: f(n)=g(n)+w(n)*h(n)(3.2) in, Where n represents the current node, f(n) represents the cost function of global path planning, g(n) represents the actual cost value of the inspection robot from the starting position to the current node, w(n) represents the heuristic function weight factor, h(n) represents the cost function of the inspection robot from the current grid position to the target grid position, and x n Indicates the x-axis coordinate of the grid where the robot is currently located, y n Indicates the y-axis coordinate of the grid where the robot is currently located, x g Indicates the x-axis coordinate of the grid of the target node, y g Indicates the y-coordinate of the target node on the grid.

9. A navigation method for a hydropower station corridor inspection robot according to claim 8, characterized in that: In step S4, local path planning is performed based on the TEB algorithm, and the formula is: in, In the formula, R * represents the optimal path of the TEB algorithm, R represents one of the constraints that limit the robot's trajectory, and γ k represents the weight value of the objective function, f(R) represents the objective function with multiple constraints, and f k (R) represents the objective function of one of the constraints.

10. A navigation method for a hydropower station corridor inspection robot according to any one of claims 1 to 9, characterized in that: In step S1, a patrol robot equipped with a laser radar, an inertial measurement unit, and an autonomous environmental perception module is constructed.

Images