Path planning algorithm and medium of inspection robot

Abstract

The application relates to the technical field of path planning, in particular to a path planning algorithm and medium for an inspection robot, which comprises the following steps: step 1, griding a static ship cabin map, the map comprising an expanded obstacle and a terrain feature, introducing a terrain factor and calculating a terrain weight value map based on the terrain feature; step 2, acquiring a starting point and a target point, planning a global static optimal path by using an RA* algorithm based on the terrain weight value map; and step 3, screening a global key point in the global static optimal path by using a rectangular frame screening method. The application takes different terrain features as one of the influencing factors of path cost, and the path with small walking difficulty is more reasonable than the path with large walking difficulty; the evaluation index considers energy loss, and is more in line with the kinematic law; and the algorithm considers the safety distance of the robot, reduces path redundancy and frequent turning.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of path planning, in particular to a path planning algorithm for an inspection robot and a medium. BACKGROUND

[0002] The ship engine room, as the power source of the ocean-going ship, is the heart of the ship. The engine room accounts for about 1 / 3 of the entire ship body, and the structure is complex and intricate, which is related to the safe operation of the entire ship. The fault inspection inside the ship engine room is of great importance, but the harsh inspection environment of high temperature, high humidity and high noise inside the ship engine room brings great challenges to manual inspection. In recent years, with the development of robot technology, inspection robots have been gradually applied to various industrial inspection scenes, such as large-scale substations and ship engine rooms. Compared with the traditional manual inspection method, the inspection robot can overcome the extreme conditions inside the ship engine room and prevent faults from being overlooked due to personnel fatigue, thereby improving the single soldier operation capability of the engine crew.

[0003] Path planning technology is one of the key technologies of autonomous cruise technology, which plans a collision-free path from the starting position to the target position according to the given map information and the scene information obtained by various sensors. The current path planning algorithm rarely considers the complex environment of the ship engine room, such as stairs, narrow environment, and unknown disturbances during dynamic planning, which will affect the inspection path effect, so a ship engine room autonomous inspection path planning algorithm is needed according to the dynamic changes of the engine room environment. SUMMARY

[0004] In order to overcome the shortcomings of the prior art, the present application provides a path planning algorithm for an inspection robot, which can consider terrain factors when planning to select a path with smaller walking difficulty and relatively shorter length, and dynamically adjust the optimization using a fuzzy controller to cope with narrow areas and dynamic obstacles, while being able to real-time deviation correction to reduce the influence of unknown disturbances.

[0005] Specifically, the present application provides a path planning algorithm for an inspection robot, which comprises the following steps:

[0006] Step 1: grid the static ship engine room map, the map comprising an expanded obstacle and a terrain feature, based on the terrain feature, introducing a terrain factor and calculating a terrain weight map;

[0007] Step 2: obtaining a starting point and a target point, based on the terrain weight map, using the RA* algorithm to plan a globally static optimal path;

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

[0009] Step 4, connecting the screened global optimal key points in turn as a multi-segment reference path, and setting the two endpoints of each segment as a sub-start point and a sub-target point, respectively;

[0010] Step 5, acquiring environmental information by using a sensor, combining the sub-start point, the sub-target point and the local environmental information, and calculating a distance loss function Real-time deviation correction of the path direction, adaptive adjustment of the coefficients of the multi-objective optimization function according to the fuzzy control rules, and update of the optimal speed trajectory to avoid collision or falling into a local extremum;

[0011] Step 6, sequentially reaching each target point to form an optimal inspection path.

[0012] Preferably, a rectangular frame screening method is used to screen global key points in the global static optimal path, including removing redundant points in the global static optimal path, minimizing path length and turning times on the basis of ensuring safety, and the specific steps are as follows:

[0013] 1) input global static optimal path points , maximum radius of the robot and a safety distance;

[0014] 2) connect as a starting point, connect in turn, and judge whether the rectangular safety region between the two points passes through an obstacle;

[0015] 3) judge whether the rectangular safety region passes through an obstacle; if it does, it is an unsafe path, and the connection between the two points is cancelled, and the starting point and the previous point of the connected point are added to the global optimal key points, and the starting point is changed to ; if it does not pass through the obstacle, it is a safe path, and the next point of is connected in turn;

[0016] 4) repeat steps 2) and 3) until the target point is added to the global optimal key points, and the loop ends;

[0017] 5) connect the global optimal key points in turn to obtain a reference path for further local path planning calculation.

[0018] Preferably, a starting point and a target point are acquired, a global static optimal path is planned based on a terrain weight map, and the RA* algorithm is used for planning as follows:

[0019]

[0020] ​

[0021]

[0022] wherein the terrain-based loss function represents an estimated cost function from the start point to the end point; represents an actual cost function from the start point to the point to be investigated based on terrain calculation; represents an estimated cost function from the point to be investigated to the end point based on terrain calculation; is a global static optimal path considering terrain; is a terrain factor of the current path node.

[0023] Preferably, the distance loss function is calculated to real-time correct the path direction, including the following steps:

[0024] (1) Obtain the global optimal key points connected in turn as a reference path and the current position of the robot, and the relationship between the reference path and the current position of the robot is as follows:

[0025] ;

[0026] (2) Real-time calculate the distance between the current position and the reference path, and the distance is taken as the distance loss function value at the current time to calculate the optimal speed trajectory in the multi-objective evaluation function:

[0027] ;

[0028] ;

[0029] (3) Update the speed and acceleration trajectory at the next time according to the multi-objective evaluation function formula, and the multi-objective optimization function is:

[0030] ;

[0031] wherein, is the start point, is the target point, is the current point, is each calculation result of formula (6), is the evaluation function, is the direction angle, is the minimum obstacle distance, is the speed, is the distance loss function; and α is the weight coefficient of the distance loss function .

[0032] Preferably, the multi-objective optimization function is adaptively adjusted according to the fuzzy control rule The specific method is as follows: a two-input three-output fuzzy controller is designed, the input variables include the number of obstacles in the detection range and the number of corners in the detection range, wherein the number of obstacles in the detection range corresponds to the domain [0, 8], the fuzzy set is , , the empty scene is that the number of obstacles is less than 1, , the general scene is that the number of obstacles is less than 2, , the dense scene is that the number of obstacles is greater than 2; the number of corners in the detection range corresponds to the domain [0, 2], the fuzzy set is , , the simple environment is that the number of corners is less than 1, , the complex environment is that the number of corners is greater than 1; the output variables are direction angle weight coefficient , minimum obstacle distance weight coefficient and speed weight coefficient , the corresponding domain is [0, 1], and the corresponding fuzzy set is , , .

[0033] In order to solve the above technical problems, the application also provides a computer storage medium, which comprises a hardware device for storing data and programs, and when the programs and data are called, the steps of the path planning algorithm of the inspection robot are realized.

[0034] Advantages

[0035] 1. The different terrain features are used as one of the influencing factors of the path cost, the path with small walking difficulty is more reasonable than the path with large walking difficulty, the evaluation index considers energy loss, and the kinematics law is more in line with the kinematics law;

[0036] 2. The algorithm considers the safety distance of the robot, reduces path redundancy and frequent turning;

[0037] 3. The application considers that the ship is affected by the sea wave, and realizes path correction under the interference of unknown disturbance;

[0038] 4. The application realizes dynamic parameter optimization for the narrow environment of the ship cabin and dynamic obstacles. DETAILED DESCRIPTION

[0039] Fig. 1 is the path planning algorithm flowchart of the inspection robot described in the application;

[0040] Fig. 2 is the RA*-ADWA algorithm flowchart of the application;

[0041] Fig. 3 are schematic diagrams of grid maps with different terrain value setting modes. DETAILED DESCRIPTION

[0042] In order to make the technical means, creative features, purposes and effects of the present application easy to understand, the present application is further described below in combination with specific drawings.

[0043] As shown in the drawings, a path planning algorithm of an inspection robot includes Figs. 1-2

[0044] S01, rasterizing a static ship engine room map, the map including an expanded obstacle and a terrain feature, introducing a terrain factor and calculating a terrain weight map based on the terrain feature;

[0045] S02, acquiring a starting point and a target point, planning a global static optimal path based on the terrain weight map and using an RA* algorithm;

[0046] S03, using a rectangular frame screening method to screen a global optimal key point in the global static optimal path;

[0047] S04, segmenting the path as a reference path according to the global optimal key point, and setting two endpoints of each segment as a sub-starting point and a sub-target point, respectively;

[0048] S05, acquiring environmental information using a sensor, combining the sub-starting point, the sub-target point and local environmental information, calculating a distance loss function real-time deviation of the path direction, adaptively adjusting coefficients of each item of a multi-objective optimization function according to fuzzy control rules, and updating an optimal speed trajectory to avoid collision or falling into a local extreme value;

[0049] S06, sequentially reaching each target point to form an optimal inspection path.

[0050] A path planning algorithm of an inspection robot is a method combining local path planning and global path planning, and the specific operation steps of the RA*-ADWA algorithm of the present embodiment are divided into two layers of work. The first layer RA* is global path planning, and the purpose is to acquire a global key point. The second layer ADWA is local path planning, and the purpose is to achieve real-time obstacle avoidance. The two are combined and path deviation is achieved according to a distance loss function. Specifically, the steps include:

[0051] ​(1) Rectanglar safe frame A* (RA* for short), which is an improved A* algorithm. The terrain weight map is calculated by introducing the terrain factor, and the shortest path and the moving efficiency are considered to achieve the optimal path search. In addition, the Rectangular Safety Frame (RSF for short) is introduced to realize the screening of global key points.

[0052] (2) The optimized Adaptive Dynamic Window Approach (ADWA for short) is an optimization of the Dynamic Window Approach (DWA for short). In view of the lack of foresight of the DWA algorithm in dense and narrow scenes, an adaptive fuzzy controller is introduced to adaptively adjust the related parameters in the multi-objective optimization function.

[0053] (3) In view of the fact that a single algorithm cannot meet the dynamic complex scene, the RA* algorithm is used as global planning, and the ADWA is used for dynamic obstacle avoidance in the changing scene. The distance loss function is introduced to calculate the relative distance between the current position of the robot and the reference path in real time, so as to prevent the risk of detour and collision caused by the deviation of the robot direction due to the ship rolling.

[0054] The terrain features are different terrain types such as flat ground, stairs, slopes and obstacles, as shown in Fig. 3 , in order to simplify the calculation, different terrains are assigned values in the grid map to represent the difficulty of terrain movement; the terrain features are different terrain types, and flat ground, stairs and obstacles are set. The flat ground terrain value is set to 1, the stairs are set according to the terrain height setting formula (8) as follows, and the stairs terrain value is set to 1000; the obstacle is ∞.

[0055] (8)

[0056] In the formula, is the current position height; is the maximum passable height of the robot, is infinite.

[0057] Among them, the flat ground is set to 1, the stairs are set to 1000, and the obstacle is ∞ (see Table 1).

[0058] Table 1 Terrain weight table

[0059]

[0060] The global static optimal path considering the terrain factor (staircase) is calculated according to formulas (1)-(3):

[0061] (1)

[0062] (2)

[0063] (3)

[0064] In the formula, the terrain-based loss function represents an estimated cost function from the starting point to the end point; represents an actual cost function from the starting point to the point to be considered; represents an estimated cost function from the point to be considered to the end point. is a global static optimal path considering the terrain; is a terrain factor of a 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 )}; wherein (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.

[0065] (4) On the basis of the global static optimal path, the global optimal key points are screened out by the proposed rectangular safety frame screening (RSF) method. The RSF method removes the redundant points in the static optimal path, minimizes the path length and the number of turns on the basis of ensuring safety, and the specific steps are as follows:

[0066] Step one, input the global static optimal path points , the maximum radius of the robot, and a safety distance;

[0067] Step two, take as the starting point, connect in turn, and judge whether the rectangular safety region between the two points passes through the obstacle;

[0068] Step three, judge whether the rectangular safety region passes through the obstacle. If it does, it is an unsafe path, the connection between the two points is cancelled, and the starting point is connected to the previous point of the connected point Together they are added to the globally optimal keypoints, and the starting point becomes accordingly. If there are no obstacles, it is a safe path; connect them sequentially. The next point ;

[0069] Step four: Repeat steps two and three until the target point is added to the global optimal key points, then the loop ends;

[0070] Step 5: Connect the global optimal key points sequentially to obtain the reference path for further local path planning calculations.

[0071] (5) In this embodiment, the ADWA algorithm is used to plan the local path in order to obtain the local path based on the dynamic environment. The sensor collects information and connects the selected global optimal key points in sequence as multiple reference paths. The two endpoints of each segment are set as sub-starting point and sub-target point respectively.

[0072] (6) Use distance loss function This overcomes unknown disturbances caused by dynamic obstacles and sea conditions, enabling real-time path correction. The specific steps are as follows:

[0073] Step 1: Obtain the global optimal key points and connect them sequentially to form the 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 formula (4).

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

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

[0076] The specific formula is as follows:

[0077] (4)

[0078] (5)

[0079] (6)

[0080] (7)

[0081] In the formula, Starting point For the target point, For the current point, As variables, For the evaluation function, is a direction angle, is a minimum obstacle distance, is a speed, is a distance loss function.

[0082] In the evaluation function of formula (7), , and Three weight factors affect the evaluation function to adjust the relationship among the guide, distance and running speed. Through different weight proportions, the robot can be more suitable for the running state under different scene logic.

[0083] The fuzzy controller is to convert the precise input quantity into fuzzy language variables according to the membership function, make decisions based on fuzzy rules to get fuzzy output, and finally use the gravity method to solve the fuzzy output to the precise control quantity. The fuzzy rule design in this paper is as follows: the number of obstacles in the detection range of the input variable corresponds to the domain [0, 8], and the fuzzy set is , wherein, 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 turns in the detection range of the input variable corresponds to the domain [0, 2], and the fuzzy set is , wherein, is a simple environment, and the number of turns in the simple environment is less than 1, is a complex environment, and the number of turns in the complex environment is greater than 1; the output variable , and , correspond to the domain [0, 1], and the corresponding fuzzy sets are , , According to the dynamic window limit of the DWA algorithm, the mutual relationship of the weight coefficients ,

[0084] ① When the number of obstacles in the detection area is less than or equal to 1, and the number of turns is 0, the detection area is relatively empty at this time, and larger and values and smaller values should be selected, so that the mobile robot is more inclined to select a trajectory with a large speed and towards the target point, and quickly shorten the distance to the target point.

[0085] ② When the number of turns in the detection area the number of obstacles in the detection area when the number of obstacles in the detection area is greater than 2, the detection area abnormally enters the turning area or the dense area, which is easy to fall into local extremum, and a smaller should be selected , the value is determined according to the specific situation, so that the mobile robot is more inclined to select a trajectory with smaller speed and farther away from the obstacle, so as to avoid collision with the obstacle.

[0086] ③ when the number of obstacles in the detection area is equal to 2, and the number of turns is 0, the detection area is not a very spacious area, and a moderate , , value should be selected to enable the mobile robot to balance the three evaluation indexes and select the optimal trajectory to reach the target point.

[0087] (7) A fuzzy controller is designed to dynamically adjust the optimization, which can cope with the narrow space working condition of the cabin and avoid falling into local optimum. The specific method is as follows: a two-input three-output fuzzy controller is designed, the input variables include the number of obstacles in the detection range and the number of turns 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 input variables and output variables are set respectively, the fuzzy subsets of input and output variables are shown in Table 2, and the fuzzy control rules of input and output variables are shown in Table 3.

[0088] Table 2 Fuzzy subsets of input and output variables

[0089]

[0090] Table 3 Fuzzy control rules

[0091]

[0092] (8) The optimal inspection path is formed by reaching each target point in turn.

[0093] The specific operation steps of the RA*-ADWA algorithm of the embodiment are divided into two layers of work. The first layer of work firstly calculates a global static optimal path on a terrain weight map, and the walking difficulty of the path is taken into account in the evaluation index, so that the path containing the least number of steps is preferentially selected, and thus the path is more in line with the kinematics law. 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, so that the static terrain optimal path can be obtained. The RSF method is used to obtain the global key points to remove the redundant path while considering the safety of the robot.

[0094] Many low steps exist in the cabin environment, which are passable terrains for the robot but are more difficult than flat terrains. The A* algorithm can search for the shortest path, but it is unrealistic to only consider the path length and ignore the cost of the path. By constructing a terrain weight map, the influence of the terrain with high terrain weight on the path cost is increased, which can effectively improve the inspection efficiency. The A* and RA* path comparison is shown in Table 4.

[0095] Table 4: A* and RA* path comparison

[0096]

[0097] The second layer of work of the RA*-ADWA algorithm of the embodiment is to plan a local path between the segmented sub-start point and the sub-target point. There are many narrow areas in the ship cabin, and the DWA algorithm lacks foresight and is easy to fall into local extremum. Since the same set of parameters cannot satisfy the passage of narrow and open areas at the same time, the parameters of the minimum obstacle distance, the speed and the direction angle are adaptively adjusted by the fuzzy controller, which can effectively adapt to different environments and avoid dynamic obstacles.

[0098] The sea waves will cause the ship to roll, causing the robot to shake, and thus causing the path planning direction to deviate. By adding a distance loss function to the evaluation function, the trajectory prediction value under the speed and acceleration at each moment is calculated, and dynamic deviation correction of the path is realized.

[0099] The above is one or more embodiments provided in combination with specific content, and it is not intended that the specific implementation of the present application is limited to these descriptions. Any approximation, similarity or replacement of the method and structure of the present application, or technical deduction or substitution under the concept of the present application, should be regarded as the protection scope of the present application.

Claims

1. A path planning algorithm for a patrol robot, characterized in that, Comprising the following steps: Step 1, rasterizing a static ship engine room map, the map comprising an expanded obstacle and a terrain feature, based on the terrain feature, introducing a terrain factor and calculating a terrain weight map; Step 2, obtaining a starting point and a target point, based on the terrain weight map, using the RA* algorithm to plan a global static optimal path; Step 3, using a rectangular frame screening method to screen out global optimal key points in the global static optimal path; Step 4, connecting the screened global optimal key points in turn as a multi-segment reference path, and setting the two endpoints of each segment as a sub-starting point and a sub-target point respectively; Step 5: Use sensors to acquire environmental information, and combine this information with the sub-starting point, sub-target point, and local environmental information to calculate the distance loss function. Real-time path direction correction, adaptive adjustment of multi-objective optimization function based on fuzzy control rules. The coefficients of each variable are updated to the optimal velocity trajectory to avoid collisions or getting trapped in local extrema; Step 6, reaching each target point in turn to form an optimal inspection path; Computing a distance loss function Real-time correction of path direction, including the following steps: (1) Obtain the global optimal key points in turn to connect the reference path and the current position of the robot, and the relationship between the reference path and the current position of the robot is as follows: ； (2) Real-time calculation of the distance between the current position and the reference path, the distance as the distance loss function value at the current time is substituted into the multi-objective evaluation function to calculate the optimal speed trajectory: ； ； (3) According to the multi-objective evaluation function formula, update the speed and acceleration trajectory at the next time, multi-objective optimization function: ； wherein, is a start point, is a target point, is a current point, is an evaluation function, is a direction angle, is a minimum obstacle distance, is a velocity, is a distance loss function; a is a weight coefficient of the distance loss function . Adaptive adjustment of coefficients of multi-objective optimization function according to fuzzy control rules The specific method is as follows: a two-input three-output fuzzy controller is designed, input variables include the number of obstacles in the detection range and the number of corners in the detection range, wherein the number of obstacles in the detection range corresponds to a domain [0, 8], and a fuzzy set is , wherein, 0 represents an empty scene, the empty scene has an obstacle number less than 1, 1 represents a general scene, the general scene has an obstacle number less than 2, 2 represents a dense scene, the dense scene has an obstacle number greater than 2; the number of corners in the detection range corresponds to a domain [0, 2], and a fuzzy set is , wherein, 0 represents a simple environment, the simple environment has a corner number less than 1, 1 represents a complex environment, the complex environment has a corner number greater than 1; output variables are a direction angle weight coefficient , a minimum obstacle distance weight coefficient and a speed weight coefficient , and the corresponding domains are all [0, 1], and the corresponding fuzzy sets are , , respectively. The rectangular frame screening method is used to screen out global optimal key points in the global static optimal path, including removing redundant points in the global static optimal path, minimizing path length and turning times on the basis of safety, the specific steps are as follows: 1) input global static optimal path points , robot maximum radius and a safety distance; 2) to As a starting point, sequentially connect , determine whether the rectangular safe region between the two points passes through the obstacle; 3) Determine if the rectangular safe area passes through an obstacle; if so, it is an unsafe path. Disconnect the two points and reconnect the starting point and the connected point. The previous point Together they are added to the globally optimal keypoints, and the starting point becomes accordingly. If there are no obstacles, it is a safe path; connect them sequentially. The next point ; 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 turn to obtain the reference path for further local path planning calculation; Obtain the starting point and the target point, based on the terrain weight map, use the RA* algorithm to plan a global static optimal path planning rule as follows: ； ； ； wherein the terrain-based loss function represents an estimated cost function from the start point to the end point; represents an actual cost function from the start point to the point to be investigated based on terrain calculation; represents an estimated cost function from the point to be investigated to the end point based on terrain calculation; is a global static optimal path considering terrain; is a 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 )}; wherein (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; The terrain feature is different terrain types, set flat, ladder, obstacle, wherein the flat terrain weight is set to 1, the ladder terrain weight is set to 1000; The obstacle terrain weight is set to infinity.

2. A computer storage medium, characterized in that, A hardware device comprising stored data and programs, when the program and data are called, the steps of the path planning algorithm of the inspection robot of claim 1 are realized.

Images