Path planning algorithm of inspection robot and medium

Abstract

The invention relates to the technical field of path planning, in particular to a path planning algorithm of an inspection robot and a medium, and the algorithm comprises the following steps: step 1, rasterizing a static marine engine room map which comprises expansion obstacles and topographic features, and based on the topographic features, introducing topographic factors and calculating a topographic weight graph; step 2, acquiring a starting point and a target point, and planning a global static optimal path by using an RA * algorithm based on the terrain weight graph; and step 3, using a rectangular frame screening method to screen out global key points in the global static optimal path. According to the method, different topographic features are used as one of the influence factors of the path cost, the path with small walking difficulty is more reasonable than the path with large walking difficulty, the evaluation index takes energy loss into consideration, and the kinematics law is better met; according to the algorithm, the safety distance of the robot is considered, and path redundancy and frequent turning are reduced.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and particularly relates to a path planning algorithm and medium for an inspection robot. Background Art

[0002] As the power source of an ocean-going ship, the ship's engine room is the heart of the ship. The engine room occupies about 1 / 3 of the entire hull, with a complex structure, which is crucial for the safe operation of the entire ship. Fault inspection inside the ship's engine room is extremely important. However, the harsh inspection environment of high temperature, high humidity, and high noise inside the ship's engine room poses a huge challenge to manual inspection. In recent years, with the development of robot technology, inspection robots have gradually been applied to various industrial inspection scenarios, such as large substations, ship engine rooms, etc. Compared with the traditional manual inspection method, the inspection robot can overcome the extreme conditions inside the ship's engine room, prevent fault neglect caused by reasons such as personnel fatigue, and improve the single-soldier operation ability of the engine room crew.

[0003] Path planning technology is one of the key technologies of autonomous cruising technology. According to the given map information and the scene information obtained by various sensors, the position of the robot is used as the starting position to plan a collision-free path to the target position. Currently, few path planning algorithms consider the complex environment of the ship's engine room, such as stairs, narrow environments, etc., and unknown interferences during dynamic planning will also affect the inspection path effect. Therefore, an autonomous inspection path planning algorithm for a ship's engine room that can dynamically change according to the engine room environment is needed. Summary of the Invention

[0004] In order to overcome the deficiencies of the prior art, the present application provides a path planning algorithm for an inspection robot, which can consider terrain factors during planning to select a path with less walking difficulty and relatively shorter length, and uses a fuzzy controller to dynamically adjust and optimize to cope with narrow areas and dynamic obstacles, and can also correct deviations in real time to reduce the impact of unknown disturbances.

[0005] Specifically, it includes a path planning algorithm for an inspection robot, which comprises the following steps:

[0006] Step 1: rasterize the static map of the ship's engine room. The map includes inflated obstacles and terrain features. Based on the terrain features, introduce a terrain factor and calculate a terrain weight map; Step 2: obtain the starting point and the target point, and based on the terrain weight map, use the RA* algorithm to plan a globally static optimal path;

[0007] Step 3: use the rectangular frame screening method to screen out the globally optimal key points in the globally static optimal path;

[0008] Step 4: Connect the selected global optimal key points in sequence to form multiple segments of reference paths, and set the two end points of each segment as the sub-starting point and the sub-target point respectively;

[0009] Step 5: Use sensors to obtain environmental information, and combine the sub-starting point, the sub-target point and the local environmental information to calculate the distance loss function Perform real-time path direction correction, and adaptively adjust the coefficients of the multi-objective optimization function according to the fuzzy control rules to update the optimal speed trajectory to avoid collisions or falling into local extrema;

[0010] Step 6: Reach each target point in sequence to form the optimal inspection path.

[0011] Preferably, use the rectangular box screening method to screen out the global key points in the global static optimal path, including removing the redundant points in the global static optimal path, and minimizing the path length and the number of turns on the basis of ensuring safety. The specific steps are as follows: 1) Input the global static optimal path points , the maximum radius of the robot and a safety distance; 2) Take as the starting point, connect in sequence, and judge whether the rectangular safety area between the two points' connection passes through obstacles; 3) Judge whether the rectangular safety area passes through obstacles; if it passes through, it is an unsafe path, cancel the connection between the two points, and add the starting point and the previous point of the connected point to the global optimal key points together, and the starting point will correspondingly become ; if it does not pass through obstacles, it is a safe path, and connect to the next point in sequence; 4) Repeat steps 2) and 3) until the target point is added to the global optimal key points and the loop ends; 5) Connect the global optimal key points in sequence to obtain a reference path for further local path planning calculation.

[0012] Preferably, obtain the starting point and the target point, and plan a global static optimal path based on the terrain weight map using the RA* algorithm as follows:

[0013]

[0014]

[0015] In the formula, the terrain-based loss function Represents the estimated cost function from the starting point to the ending point; Represents the actual cost function from the starting point to the point to be inspected calculated based on the terrain; Represents the estimated cost function from the point to be inspected to the ending point calculated based on the terrain; Is the global static optimal path considering the terrain; Is the terrain factor of the current path node.

[0016] Preferably, calculating the real-time deviation correction of the distance loss function for the path direction includes the following steps:

[0017] (1) Obtain the global optimal key points and connect them in sequence as the reference path and the current position of the robot. The relationship between the reference path and the current position of the robot is as follows: ; (2) Calculate the distance between the current position and the reference path in real time, and substitute the distance as the distance loss function value at the current moment into the multi-objective evaluation function to calculate the optimal speed trajectory: ; ; (3) Update the speed and acceleration trajectories at the next moment according to the multi-objective evaluation function formula. The multi-objective optimization function: ; In the formula, Is the starting point, Is the target point, Is the current point, Are the calculation results of each item in formula (6), Is the evaluation function, Is the direction angle, Is the minimum obstacle distance, Is the speed, Is the distance loss function; α is the weight coefficient of the distance loss function Of.

[0018] Preferably, adaptively adjust the coefficients of the multi-objective optimization function According to the fuzzy control rules. The specific method is as follows: Design a two-input and three-output fuzzy controller. The input variables include the number of obstacles within the detection range and the number of turning angles within the detection range. Among them, the number of obstacles within the detection range corresponds to the universe of discourse [0, 8], and the fuzzy set is Among them, Is an empty scene, and the number of obstacles in the empty scene is less than 1, Is a general scene, and the number of obstacles in the general scene is less than 2, For a dense scene, the number of obstacles in the dense scene is greater than 2; the number of corners within the detection range corresponds to the universe of discourse [0, 2], and the fuzzy sets are , where is a simple environment, and the number of corners in the simple environment is less than 1. is a complex environment, and the number of corners in the complex environment is greater than 1; the output variables are the direction angle weight coefficient , the minimum obstacle distance weight coefficient and the speed weight coefficient , respectively. The corresponding universes of discourse are all [0, 1], and the corresponding fuzzy sets are , , .

[0019] To solve the above technical problems, the present application also provides a computer storage medium, including a hardware device for storing data and programs. When the programs and data are called, the steps of the path planning algorithm for the above-mentioned inspection robot are implemented.

[0020] Beneficial effects

[0021] 1. The present invention takes different terrain features as one of the influencing factors of path cost. A path with less walking difficulty is more reasonable than a path with greater walking difficulty. The evaluation index takes into account energy loss and is more in line with kinematic laws.

[0022] 2. The algorithm proposed by the present invention takes into account the safety distance of the robot, reducing path redundancy and frequent turning.

[0023] 3. The present invention takes into account the influence of ship rolling caused by waves and realizes path correction under the interference of unknown disturbances.

[0024] 4. The present invention realizes dynamic parameter optimization for the narrow environment and dynamic obstacles in the ship engine room. Description of the drawings

[0025] Figure 1 is the flow chart of the path planning algorithm for the inspection robot described in the present invention; Figure 2 is the flow chart of the RA*-ADWA algorithm of the present invention; Figure 3 is the schematic diagram of the grid map of different terrain value setting methods. Specific implementation manners

[0026] In order to make the technical means, creative features, achieved purposes and effects of the present invention easy to understand, the present invention will be further described below with reference to specific drawings.

[0027] As Figures 1 to 2 shown, a path planning algorithm for an inspection robot includes

[0028] S01, rasterize the static map of the ship engine room. The map includes inflated obstacles and terrain features. Based on the terrain features, introduce terrain factors and calculate the terrain weight map;

[0029] S02, obtain the starting point and the target point. Based on the terrain weight map, use the RA* algorithm to plan a globally static optimal path;

[0030] S03, use the rectangular box screening method to screen out the globally optimal key points in the globally static optimal path;

[0031] S04, segment the path according to the globally optimal key points as the reference path, and set the two end points of each segment as the sub-starting point and the sub-target point respectively;

[0032] S05, use sensors to obtain environmental information, combine the sub-starting point, the sub-target point and the local environmental information, and calculate the distance loss function correct the path direction in real time, adaptively adjust the coefficients of the multi-objective optimization function according to the fuzzy control rules, and update the optimal speed trajectory to avoid collisions or falling into local extrema;

[0033] S06, reach each target point in turn to form an optimal inspection path.

[0034] A path planning algorithm for an inspection robot is a method that combines local path planning and global path planning. The specific operation steps of the RA*-ADWA algorithm in this embodiment are divided into two layers of work. The first layer, RA*, is for global path planning, and the purpose is to obtain global key points; the second layer, ADWA, is for local path planning to achieve real-time obstacle avoidance. The two are combined and path correction is achieved according to the distance loss function. The specific steps are as follows: (1) The improved A-star algorithm (Rectanglar safe frame A*, abbreviated as RA*) is an improvement on the A-star algorithm (A-Star Algorithm, abbreviated as A*). By introducing terrain factors to calculate the terrain weight map, it comprehensively considers the shortest path and movement efficiency to achieve optimal path search; in addition, the rectangular safety frame screening method (RectangularSafety Frame, abbreviated as RSF) is introduced to screen global key points.

[0035] (2) The optimized Adaptive Dynamic Window Approach (abbreviated as ADWA) is an optimization based on the Dynamic Window Approach (abbreviated as DWA). Aiming at the lack of foresight of the DWA algorithm in dense and narrow scenarios, an adaptive fuzzy controller is introduced to adaptively adjust the relevant parameters in the multi-objective optimization function.

[0036] (3) As a single algorithm cannot meet the needs of dynamic and complex scenarios, the RA* algorithm is used as the global planner, and ADWA performs dynamic obstacle avoidance for changing scenarios. By introducing the distance loss function , calculate the relative distance between the robot’s current position and the reference path in real time, and prevent risks such as detours and collisions caused by the robot’s deviation from the direction due to the turbulence of the ship.

[0037] Terrain features are different types of terrain, such as flat land, steps, slopes, and obstacles, e.g. Figure 3 As shown in the figure, in order to simplify the calculation, different terrains are assigned values ​​in the grid map to indicate the difficulty of terrain movement; the terrain features are different terrain types, and flat land, steps, and obstacles are set. The flat land value is set to 1, and the steps are set according to the terrain height formula (8) as follows, and the step terrain value is set to 1000; the obstacle is ∞.

[0038] (8) In the formula, is the current position height; is the maximum traversable height of the robot, Is infinite.

[0039] Among them, the flat ground is set to 1, the steps are set to 1000, and the obstacles are set to ∞ (see Table 1).

[0040] Table 1 Terrain weight table

[0041] According to formulas (1)-(3), the global static optimal path considering terrain factors (steps) is calculated:

[0042] (1) (2) (3) In the formula, the loss function based on terrain is Represents the estimated cost function from the starting point to the end point; Represents the actual cost function from the starting point to the point to be examined; Represents the estimated cost function from the point to be investigated to the end point. It is the global static optimal path considering the terrain; is the terrain factor of the current path node; for the convenience of calculation, a rectangular coordinate system is established with the lower left corner of the grid map as the origin, and each path node is represented by {(x s ,y s ),…(x n ,y n),…(x g ,y g )}; where, (x s ,y s ) represents the starting node coordinates, (x n ,y n ) represents the current path node coordinates, and (x g ,y g ) represents the target node coordinates.

[0043] (4) Based on the globally static optimal path, the globally optimal key points are screened out through the proposed Rectangular Safety Frame Screening (RSF) method. The RSF method removes redundant points in the static optimal path and minimizes the path length and the number of turns while ensuring safety. The specific steps are as follows:

[0044] Step 1: Input the globally static optimal path points , the maximum radius of the robot, and a safety distance;

[0045] Step 2: Take as the starting point and connect in sequence, and judge whether the rectangular safety area between the two points' connection passes through obstacles;

[0046] Step 3: Judge whether the rectangular safety area passes through obstacles. If it passes through, it is an unsafe path, cancel the connection between the two points, and add the starting point and the previous point of the connected point to the globally optimal key points together, and the starting point then becomes ; if it does not pass through obstacles, it is a safe path, and connect 's next point in sequence; Step 4: Repeat Step 2 and Step 3 until the target point is added to the globally optimal key points and the loop ends;

[0047] Step 5: Connect the globally optimal key points in sequence to obtain a reference path for further local path planning calculation.

[0048] (5) In this embodiment, the ADWA algorithm is used to plan the local path, aiming to obtain the local path according to the dynamic environment. The sensor collects information, connects the selected globally optimal key points in sequence as multi-segment reference paths, and sets the two end points of each segment as the sub-starting point and the sub-target point respectively;

[0049] (6) The distance loss function is adopted to overcome the unknown disturbances brought by dynamic obstacles and sea wave conditions, and realize real-time path correction. The specific steps are as follows:

[0050] Step 1: Obtain the globally optimal key points and connect them in sequence to form a reference path and the current position of the robot. The relationship between the reference path and the current position of the robot is shown in Equation (4);

[0051] Step 2: Calculate the distance between the current position and the reference path in real time, as shown in Equations (5) and (6). Substitute the distance as the value of the distance loss function into the multi-objective evaluation function to calculate the optimal velocity trajectory;

[0052] Step 3: Update the velocity and acceleration trajectories at the next moment according to the multi-objective evaluation function formula. The multi-objective optimization function is shown in Equation (7).

[0053] The specific formulas are as follows: (4) (5) (6) (7) In the formula, is the starting point, is the target point, is the current point, is a variable, is the evaluation function, is the direction angle, is the minimum obstacle distance, is the velocity, is the distance loss function.

[0054] In the evaluation function of Equation (7), , and The three weight factors affect the evaluation function to adjust the relationship among guidance, distance, and running speed. By different weight ratios, the robot can be made more suitable for the running state under different scenario logics.

[0055] The fuzzy controller converts the precise input quantity into a fuzzy linguistic variable according to the membership function, makes a decision based on fuzzy rules to obtain a fuzzy output, and finally uses the centroid method to defuzzify the fuzzy output into a precise control quantity. The fuzzy rules designed in this paper are as follows: The number of obstacles within the detection range of the input variable, with the corresponding universe of discourse being [0, 8], and the fuzzy set being , where is an empty scene, and the number of obstacles in the empty scene is less than 1, is a general scene, and the number of obstacles in the general scene is less than 2, is a dense scene, and the number of obstacles in the dense scene is greater than 2. The number of turning angles within the detection range of the input variable, with the corresponding universe of discourse [0, 2], and the fuzzy set being , where For a simple environment, the number of corners in the simple environment is less than 1. For a complex environment, the number of corners in the complex environment is greater than 1; the output variables , and , and the corresponding universes of discourse are all [0, 1]. The corresponding fuzzy sets are respectively , , . According to the dynamic window limit of the DWA algorithm, considering the mutual relationship of the weight coefficients , and in the multi-objective evaluation function, the idea of designing fuzzy rules is as follows: ① When the number of obstacles in the detection area is less than or equal to 1, and the number of turns is 0, at this time the detection area is relatively empty, and larger and values and smaller value should be selected, so that the mobile robot is more inclined to choose a trajectory with a large speed and towards the target point, quickly shortening the distance to the target point.

[0056] ② When the number of turns in the detection area is 1 or the number of obstacles in the detection area is greater than 2, at this time the detection area is an abnormal turning area or a dense area, and it is easy to fall into a local extremum. A smaller and a larger value should be selected. The value depends on the specific situation, so that the mobile robot is more inclined to choose a trajectory with a smaller speed and away from obstacles to avoid colliding with obstacles.

[0057] ③ When the number of obstacles in the detection area is equal to 2, and the number of turns is 0, at this time the detection area is a not very empty area, and moderate , , values should be selected, so that the mobile robot can balance the three evaluation indicators and select the optimal trajectory to reach the target point.

[0058] (7) A fuzzy controller is designed to dynamically adjust the optimization to cope with the working conditions of the narrow space in the cabin and avoid falling into a local optimum. The specific method is as follows: Design a two-input and three-output fuzzy controller. The input variables include the number of obstacles in the detection range and the number of turning angles in the detection range. The output variables are the direction angle weight coefficient , the minimum obstacle distance weight coefficient and the speed weight coefficient The membership functions of the input variables and output variables are set respectively. The fuzzy subsets of the input and output variables are shown in Table 2, and the fuzzy control rules of the input and output variables are shown in Table 3.

[0059] Table 2 Fuzzy Subsets of Input and Output Variables

[0060] Table 3 Fuzzy Control Rules

[0061] (8) Reach each target point in turn to form an optimal inspection path.

[0062] The specific operation steps of the RA*-ADWA algorithm in this embodiment are divided into two layers of work. The first layer of work first calculates the global static optimal path on the terrain weight map, incorporates the walking difficulty of the path into the evaluation index, and preferentially selects the path with the fewest number of steps, thus being more in line with the kinematic laws. The RA* algorithm uses eight-direction nodes as adjacent nodes, maintains an open list and a closed list, uses a minimum heap to maintain the open list, and converts the closed list into a node state to obtain the static terrain optimal path. Using the RSF method, while considering the safety of the robot, global key points are obtained to remove redundant paths.

[0063] Considering that there are many low steps in the engine room environment, these steps are passable for the robot but are terrains with a higher degree of difficulty compared to flat ground. The A* algorithm can search for the shortest path, but only considering the length of the path and ignoring the cost of the path is unrealistic. By constructing a terrain weight map and increasing the influence of terrains with high terrain weights on the path cost, the inspection efficiency can be effectively improved. The comparison between the A* and RA* paths is shown in Table 4.

[0064] Table 4 Comparison of A* and RA* Paths

[0065] The second layer of work of the RA*-ADWA algorithm in this embodiment is to plan local paths between segmented sub-starting points and sub-target points. There are many narrow areas in the ship's engine room. The DWA algorithm lacks foresight and is prone to falling into local extrema. Since it is difficult for the same set of parameters to simultaneously meet the requirements for passing through narrow and open areas, the parameters of the minimum obstacle distance, speed, and direction angle are adaptively adjusted through a fuzzy controller, which can effectively adapt to different environments and avoid dynamic obstacles.

[0066] The beating of the sea waves will cause the hull to jolt, resulting in the robot shaking and thus causing the path planning direction to deviate. By adding a distance loss function to the evaluation function , the trajectory prediction values at each moment of speed and acceleration are calculated to achieve dynamic path correction.

[0067] As described above, one or more embodiments are provided in combination with specific content, and it is not considered that the specific implementation of the present invention is limited only to these descriptions. Any approximation, similarity to the method, structure, etc. of the present invention, or several technical deductions or substitutions made under the premise of the inventive concept of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A path planning algorithm for a patrol robot, characterized in that, It includes the following steps: Step 1: rasterize the static map of the ship's engine room, which includes inflated obstacles and terrain features. Based on the terrain features, introduce terrain factors and calculate the terrain weight map; Step 2: obtain the starting point and the target point, and based on the terrain weight map, use the RA* algorithm to plan a globally static optimal path; Step 3: use the rectangular box screening method to screen out the globally optimal key points in the globally static optimal path; Step 4: connect the screened globally optimal key points in sequence as a multi-segment reference path, and set the two end points of each segment as the sub-starting point and the sub-target point respectively; Step 5: Use sensors to obtain environmental information, and combine the sub-start point, sub-goal point, and local environmental information to calculate the distance loss function Perform real-time path direction correction, and adaptively adjust the coefficients of the multi-objective optimization function according to the fuzzy control rules to update the optimal velocity trajectory to avoid collisions or falling into local extrema; Step 6: reach each target point in sequence to form an optimal inspection path.

2. The path planning algorithm of a patrol robot according to claim 1, characterized in that, Using the rectangular box screening method to screen out the globally optimal key points in the globally static optimal path includes removing redundant points in the globally static optimal path and minimizing the path length and the number of turns on the basis of ensuring safety. The specific steps are as follows: 1) Input the global static optimal path points , the maximum radius of the robot, and a safety distance; 2) Take as the starting point and connect in sequence, and determine whether the rectangular safety area between the two connected points passes through an obstacle; 3) Determine whether the rectangular safety area passes through an obstacle; if it does, it is an unsafe path, cancel the connection between the two points, and add the starting point and the previous point of the connected point to the globally optimal key points together, and the starting point will accordingly become ; if it does not pass through an obstacle, it is a safe path, and connect the next point of in sequence; to the previous point together into the globally optimal key points, and the starting point will accordingly become ; if it does not pass through an obstacle, it is a safe path, and connect in sequence the next point of ; 4) Repeat steps 2) and 3) until the target point is added to the globally optimal key points, and the loop ends; 5) Connect the globally optimal key points in sequence to obtain a reference path for further local path planning calculation.

3. The path planning algorithm for a patrol robot according to claim 1, wherein, The rules for obtaining the starting point and the target point and using the RA* algorithm to plan a globally static optimal path based on the terrain weight map are as follows: ； ； ； In the formula, the terrain-based loss function represents the estimated cost function from the starting point to the ending point; represents the actual cost function from the starting point to the point to be inspected calculated based on the terrain; represents the estimated cost function from the point to be inspected to the ending point calculated based on the terrain; is the globally static optimal path considering the terrain; is the terrain factor of the current path node; for the convenience of calculation, a rectangular coordinate system is established with the lower left corner of the grid map as the origin, and each path node is represented as {(x s , y s ), … (x n , y n ), … (x g , y g )}; where, (x s , y s ) represents the starting node coordinates, (x n , y n ) represents the current path node coordinates, and (x g , y g ) represents the target node coordinates.

4. The path planning algorithm of an inspection robot according to claim 1, wherein , Calculate the distance loss function Real-time correction of the path direction, including the following steps: (1) Obtain the globally optimal key points and connect them in sequence as the reference path and the current position of the robot. The relationship between the reference path and the current position of the robot is as follows: ； (2) Calculate the distance between the current position and the reference path in real time, and substitute the distance as the distance loss function value at the current moment into the multi-objective evaluation function to calculate the optimal speed trajectory; ； ； (3) Update the speed and acceleration trajectories at the next moment according to the multi-objective evaluation function formula. The multi-objective optimization function: ； In the formula, is the starting point, is the target point, is the current point, are the respective calculation results of formula (6), is the evaluation function, is the direction angle, is the minimum obstacle distance, is the speed, is the distance loss function; α is the weight coefficient of the distance loss function of.

5. The path planning algorithm of an inspection robot according to claim 1, characterized in that , adaptively adjust the coefficients of the multi-objective optimization function according to the fuzzy control rules The specific method is as follows: Design a two-input and three-output fuzzy controller. The input variables include the number of obstacles within the detection range and the number of turning angles within the detection range. Among them, the corresponding universe of discourse for the number of obstacles within the detection range is [0, 8], and the fuzzy sets are , where is an open scene, and the number of obstacles in the open scene is less than 1, is a general scene, and the number of obstacles in the general scene is less than 2, is a dense scene, and the number of obstacles in the dense scene is greater than 2; the corresponding universe of discourse for the number of turning angles within the detection range is [0, 2], and the fuzzy sets are , where is a simple environment, and the number of turning angles in the simple environment is less than 1, is a complex environment, and the number of turning angles in the complex environment is greater than 1; the output variables are the direction angle weight coefficient , the minimum obstacle distance weight coefficient and the speed weight coefficient , and the corresponding universes of discourse are all [0, 1]. The corresponding fuzzy sets are respectively , , .

6. A computer storage medium, characterized in that, It includes a hardware device for storing data and programs, and when the programs and data are called, it implements the steps of the path planning algorithm of the inspection robot according to any one of claims 1 to 5.

Images