A Two-Layer Path Planning Method for Real-Time Obstacle Avoidance of Mobile Robots in Nuclear Environments

Abstract

A two-layer path planning method for real-time obstacle avoidance of a mobile robot in a nuclear environment is proposed. The grid map of the nuclear environment serves as prior information, and each grid cell contains radiation field information and obstacle information. The mobile robot is equipped with a lidar. The method is as follows: S01, establishing a mathematical model for real-time obstacle avoidance path planning; S02, employing ACO-A... * - PSO algorithm for global path planning; S03, FL-DWA-VOA algorithm for local path planning. This invention is applicable to mobile robot planning in multi-information field coupled environments with unknown dynamic or static obstacles. It first adopts ACO-A * The PSO algorithm plans a global path, selects inflection points on the global path as sub-navigation points, and then calls the FL-DWA-VOA algorithm to replan the local path to form the final path. The final path not only ensures global optimization but also has the ability to avoid unknown static or dynamic obstacles.

Description

Technical Field

[0001] This invention relates to robot path planning methods, and in particular to a two-layer path planning method for real-time obstacle avoidance of mobile robots in a nuclear environment. Background Technology

[0002] When mobile robots perform tasks such as inspection and monitoring in nuclear environments, path planning needs to be done in advance to improve work efficiency.

[0003] First, because there are radioactive sources in the nuclear environment, global path planning cannot only consider path length, but also radiation risk, which increases the difficulty of global path planning.

[0004] Secondly, dynamic changes may occur at any time in the nuclear environment, such as encountering unknown static obstacles (not marked on the map) or unpredictable dynamic obstacles (such as radioactive workers or collaborative robots). Therefore, in addition to obtaining the optimal global path, dynamic obstacle avoidance is also required.

[0005] Currently, no path planning algorithm has been found that can simultaneously satisfy both of the above conditions. It is difficult to obtain the globally optimal path while also having dynamic obstacle avoidance capabilities, meaning it is not suitable for path planning of mobile robots in complex environments. Summary of the Invention

[0006] The purpose of this invention is to overcome the shortcomings of the prior art and provide a two-layer path planning method for real-time obstacle avoidance of mobile robots in a kernel environment. It is applicable to path planning of mobile robots in complex kernel environments and solves the problem that existing path planning algorithms cannot simultaneously satisfy the requirement of obtaining the globally optimal path and having dynamic obstacle avoidance function.

[0007] The technical solution of the present invention is: a two-layer path planning method for real-time obstacle avoidance of mobile robots in a nuclear environment, wherein the grid map of the nuclear environment is prior information, each grid in the grid map contains radiation field information and obstacle information, and the mobile robot is equipped with a lidar.

[0008] The method is as follows:

[0009] S01, Establish a mathematical model for real-time obstacle avoidance path planning: The goal of path planning is to plan an optimal path from the starting point to the target point for the mobile robot; considering the three sub-objectives of path length, cumulative radiation dose and turning energy consumption, establish the objective function for global path planning of the mobile robot.

[0010] S02, using ACO-A *- The PSO algorithm plans the global path: First, it combines the objective function to construct an improved A* algorithm. Then, it uses the suboptimal path obtained by running the improved A* algorithm to process the initial pheromone distribution of the ACO algorithm. Next, it combines the objective function to construct the ACO algorithm. Then, it uses the PSO algorithm to combine and optimize the parameters in the objective function and the ACO algorithm. Finally, it runs the ACO algorithm based on the optimized parameter combination to obtain the global path from the starting point to the target point.

[0011] S03, using the FL-DWA-VOA algorithm to plan local paths: First, all inflection points in the global path are defined as sub-navigation points, thus dividing the global path into multiple sequentially connected local paths. Then, each local path is replanned in a forward-to-back order. The path replanning process is as follows: First, a DWA algorithm model is established, then fuzzy logic is introduced to achieve dynamic parameter tuning of the DWA algorithm, and an FL-DWA algorithm with static obstacle avoidance effect is established. If the mobile robot does not detect dynamic obstacles, only the FL-DWA algorithm is run for local path replanning; if the mobile robot detects dynamic obstacles, the VOA dynamic obstacle avoidance algorithm is additionally called for dynamic obstacle avoidance.

[0012] A further technical solution of the present invention is: the grid in the grid map is square, the radiation field information is recorded in each grid in a real number encoding manner, the real number is the average radiation dose rate of the grid, and the obstacle information is identified in each grid by binary codes "1" and "0" to indicate "there is an obstacle" and "there is no obstacle", respectively.

[0013] A further technical solution of the present invention is as follows: Step S01 is detailed below:

[0014] S01, Establish a mathematical model for real-time obstacle avoidance path planning:

[0015] A. Problem Description:

[0016] Based on the operational scenario of mobile robots in a nuclear environment, in order to improve the operational efficiency and ensure the safety of mobile robots, it is necessary to plan a route from the starting point S(p1(x1,y1)) to the target point E(p1,y1)). n (x n ,y n The optimal path;

[0017] A-1. Defining Passability: Different colors, excluding a specified color, are used to distinguish the average radiation dose rate across different ranges. The specified color represents "obstacles present," and all other grids represent "obstacle-free." The mobile robot uses an eight-direction search strategy for path planning, defining grid coordinates as the coordinates of the grid's center point. If a grid simultaneously meets the three conditions of "no obstacles within the grid," "average radiation dose rate within the grid is below a threshold," and "the mobile robot does not collide with any obstacles when entering the grid," then the grid is defined as passable. If a grid fails to meet any of these three requirements, it is defined as impassable. A passable path represents all grids traversed by the mobile robot as passable grids, and the set of passable grids constitutes a passable area.

[0018] A-2. Path Planning Objectives: When planning a global path for a mobile robot in a kernel environment containing obstacles not in the map, three aspects are considered: obstacle avoidance, high radiation risk avoidance, and cost minimization. Obstacle avoidance means that the mobile robot does not collide with any obstacles in the environment. High radiation risk avoidance means that the mobile robot is not allowed to pass through grids with an average radiation dose rate higher than a threshold. Cost minimization means that, considering the two hard constraints of obstacle avoidance and high radiation risk avoidance, the path with the lowest cost is pursued. The cost consists of path length, cumulative radiation dose, and turning energy consumption.

[0019] B. Mathematical Modeling:

[0020] Considering path length, cumulative radiation dose, and turning energy consumption, an objective function for planning the global path of the mobile robot in a nuclear environment containing obstacles not in the map is established, see Formula 1;

[0021] Formula 1:

[0022] In the formula, M(p) represents the objective function, ω1 is the path length weight coefficient, ω2 is the cumulative radiation dose weight coefficient, ω3 is the turning energy consumption weight coefficient, L(p) is the path length sub-objective function, R(p) is the cumulative radiation dose sub-objective function, and E(p) is the turning energy consumption sub-objective function; d(p) i ,p i+1 ) is the raster p i To grid p i+1 The distance; (x i ,y i ) is the raster p i The coordinates, (x i+1 ,y i+1 ) is the raster p i+1 The coordinates of R(p) i ,p i+1) for mobile robots from grid p i Drive to the grid p i+1 The cumulative radiation dose, For grid p i average radiation dose rate, For grid p i+1 The average radiation dose rate, l is the grid p i To grid p i+1 The distance; ε1 is the weighting coefficient of the steering angle variable, ε2 is the weighting coefficient of the number of steering turns, ψ[l(p i-1 ,p i ),l(p i ,p i+1 )] is line segment l(p i-1 ,p i ) and line segment l(p i ,p i+1 The steering angle variable, l(p) i-1 ,p i ) is the raster p i-1 The center point and grid p i The line connecting the center points, l(p) i ,p i+1 ) is the raster p i The center point and grid p i+1 The line connecting the center points of N; turn This represents the number of turns.

[0023] A further technical solution of the present invention is as follows: Step S02 is described in detail below:

[0024] S02, using ACO-A * -PSO algorithm plans global paths:

[0025] A. Construct an improved A* algorithm:

[0026] A-1. Heuristic Function:

[0027] Considering the three sub-objectives in mathematical modeling, we construct the heuristic function for the improved A* algorithm, as shown in Equation 2.

[0028] Formula 2:

[0029] In the formula, g(p) is the path length cost from the starting point to the current grid cell, and h(p) is the path length cost from the current grid cell to the target point. g(p) and h(p) are calculated using L(p) in Formula 1; R(p) 1-n R(p) represents the cumulative radiation dose cost of a mobile robot traveling from its starting point to its destination. 1-n Refer to Formula 1 for the calculation of R(p). 1-n) is R(p 1-i ) and R(p i-n The sum of R(p) 1-i R(p) represents the cumulative radiation dose cost of the mobile robot traveling from the starting point to the current grid. i-n E(p) represents the cumulative radiation dose cost of the mobile robot traveling from the current grid to the target point; 1-n E(p) represents the energy consumption cost of turning between the starting point and the target point. 1-n Refer to E(p) in Formula 1 for calculation. 1-n ) is E(p 1-i ) and E(p i-n The sum of E(p) 1-i E(p) represents the energy cost of turning between the starting point and the current grid cell. i-n (x) represents the energy cost of turning from the current grid to the target point; i ,y i ) represents the current raster p i The coordinates of (x1, y1) are the coordinates of the starting point. n ,y n () represents the coordinates of the target point;

[0030] A-2. Danger Zone Avoidance Strategies:

[0031] During the path search process, when an ant senses that the average radiation dose rate of the next grid exceeds the threshold, the average radiation dose rate of the next grid is defined as infinite, prompting the ant to search for other passable grids; thus constructing a danger zone avoidance strategy, as shown in Formula 3.

[0032] Formula 3:

[0033] Among them, R(p i ) is the raster p i The radiation dose rate is algebraic, where Inf is infinite and RI is infinite. max The radiation dose rate threshold;

[0034] B, run the improved A* algorithm to obtain a suboptimal path:

[0035] After running the improved A* algorithm and completing the calculation, the output result is: suboptimal path P. sp ;

[0036] C, The initial pheromone of the ACO algorithm is processed using suboptimal path non-uniformity:

[0037] The suboptimal path P obtained by running the improved A* algorithm spThe initial pheromone in the ACO algorithm is made non-uniform; only the initial pheromone concentration of the grids traversed by the suboptimal path is increased, while the initial pheromone concentration of other grids remains unchanged; as shown in Formula 4.

[0038] Formula 4:

[0039] Where i,j represent two adjacent edges or touching vertices of a passable grid, τ i,j For grid p i With p j The initial pheromone concentration after the non-uniformity treatment, τ is the artificially set initial pheromone concentration, and C is the initial pheromone enhancement value;

[0040] D. Construct an improved ACO algorithm:

[0041] D-1. Heuristic function:

[0042] Considering the influence of the three sub-objectives in mathematical modeling on the heuristic function of the ACO algorithm, and introducing an adaptive adjustment coefficient to balance the convergence speed and global search capability of the ACO algorithm, the heuristic function of the ACO algorithm is constructed as shown in Equation 5.

[0043] Formula 5:

[0044] Where, η t (i,j) represents the value from the grid p in the t-th iteration. i to p j The heuristic function values, i,j represent two adjacent edges or touching vertices of a passable grid, Q1 is the heuristic function enhancement coefficient, L i,j R is the distance from grid i to grid j. i,j E represents the cumulative radiation dose received by the mobile robot as it travels from grid i to grid j. i,j For mobile robots from grid p i-1 To grid p i to p j Turning energy consumption, I max I is the maximum number of iterations. c Let ξ be the current iteration number, ξ be the adaptive adjustment coefficient, and Q be the heuristic function enhancement coefficient;

[0045] D-2. Choosing a probability function:

[0046] Taking into account both pheromone concentration and heuristic function, the selection probability function of ACO algorithm is constructed as shown in Equation 6;

[0047] Formula 6:

[0048] In the formula, P t(i,j) represents the value from the grid p in the t-th iteration. i to p j The selection probability is given by τ, where i and j represent passable grids with adjacent edges or touching vertices, α is the pheromone importance factor, β is the heuristic function importance factor, and τ is the pheromone importance factor. t (i,j) represents the value from grid p when the t-th iteration is completed. i to p j pheromone concentration, τ t (i,u) represents the value from grid p when the t-th iteration is completed. i to p u pheromone concentration, η t (i,j) represents the value from the grid p in the t-th iteration. i to p j The heuristic function value, η t (i,u) represents the value from the grid p in the t-th iteration. i to p u The heuristic function value; T t Let ' be the set of grid cells that have not been visited before the t-th iteration; the denominator in Formula 6 is the set of grid cells that have not been visited before the t-th iteration. i The sum of the selection probabilities of all unvisited graticules; if the graticule p in Formula 6 j It is an unprocessed raster, and then rasterization p. i to p j The selection probability calculation, if the grid p in Formula 6 j If it is a previously visited raster, then raster p i to p j The probability of choosing is 0;

[0049] D-3. Pheromone Update Rules:

[0050] By combining the positive and negative feedback mechanism with the three sub-objectives in mathematical modeling, the pheromone update rule of the ACO algorithm is constructed, as shown in Formula 7.

[0051] Formula 7:

[0052] In the formula, τ t+1 (i,j) and τ t (i,j) represent the raster p when the (t+1)th and tth iterations are completed, respectively. i With p j The pheromone concentrations between these values ​​are: ρ is the pheromone evaporation factor, Q2 is the pheromone deposition factor, and M is the pheromone concentration. t-mi Let M be the total cost of the three sub-objectives of the optimal path obtained in the t-th iteration. t-ma P represents the total sub-objective value of the worst path obtained in the t-th iteration; optimal Let P be the optimal path obtained in the (t+1)th iteration.worst This represents the worst path obtained in the (t+1)th iteration;

[0053] E. The PSO algorithm is used for parameter combination optimization:

[0054] E-1. Determine the combination of parameters to be optimized:

[0055] Based on the objective function and the formula in the ACO algorithm, the initial parameter combination to be optimized is determined to be (ω1, ω2, ω3, α, β, ρ). Since (ω1, ω2, ω3) are multi-objective weight coefficients, a single-objective standardization method is used to simplify the number of multi-objective weight coefficients. Setting ω1 = 1, we obtain the approximate ranges of ω2 and ω3, and finally determine the parameter combination to be optimized as (ω2, ω3, α, β, ρ). Subsequently, the positional quality of the particles is evaluated using the objective function.

[0056] E-2. Define the particle velocity update rule:

[0057] The particle velocity update rule in the PSO algorithm is defined as shown in Equation 8;

[0058] Formula 8:

[0059] In the formula, Gbest represents the optimal position of the i-th particle in the entire particle swarm obtained in k iterations. k ω represents the optimal position obtained by all particles in the entire particle swarm in k iterations, c1 and c2 are both learning factors, and r1 and r2 are both random numbers between [0,1]. and These are the positions of the i-th particle in the k-th and (k+1)-th iterations, respectively. and Let [-v] represent the velocities of the i-th particle in the k-th and (k+1)-th iterations, respectively. max ,v max [This refers to the velocity range of the particles;]

[0060] E-3. Obtain the optimized parameter combination:

[0061] The PSO algorithm is run, and the objective function is used as the fitness function to evaluate the quality of the particle position during the process, and finally the optimized parameter combination (ω2,ω3,α,β,ρ) is obtained.

[0062] F, Obtain the global path based on optimized parameter combinations:

[0063] The optimized parameter combination is then fed into the ACO algorithm, which is run to obtain the global path from the starting point to the target point.

[0064] 5. The real-time obstacle avoidance dual-layer path planning method for mobile robots in a nuclear environment as described in claim 4, characterized in that: step S03 is detailed as follows:

[0065] S03, using the FL-DWA-VOA algorithm to plan the local path:

[0066] A. Establish the DWA algorithm model:

[0067] A-1. Velocity Sampling Space Analysis: Taking into account kinematic constraints, acceleration constraints, and obstacle constraints, the velocity sampling space of the DWA algorithm is established.

[0068] ①. Due to the limitations of the mobile robot's own motion performance, its linear velocity and angular velocity are restricted to a certain range; the velocity sampling space V under kinematic constraints m Defined as Formula 9;

[0069] Formula 9:

[0070] In the formula, V m Let (v, ω) be the velocity sampling space under kinematic constraints, where (v, ω) is the velocity tuple, v is the linear velocity of the mobile robot, and ω is the angular velocity of the mobile robot. min Let v be the minimum linear velocity of the mobile robot. max Let ω be the maximum linear velocity of the mobile robot. max Let ω be the maximum angular velocity of the mobile robot. min This represents the minimum angular velocity of the mobile robot.

[0071] ②. The variable speed performance of a mobile robot has boundaries. Furthermore, considering that excessive acceleration or deceleration during movement will lead to motor overload and rapid wear, it is necessary to constrain the robot's acceleration; the velocity sampling space V under acceleration constraints... d Defined as Formula 10;

[0072] Formula 10:

[0073] In the formula, V d Let (v,ω) be the velocity sampling space under acceleration constraints, where v is the linear velocity of the mobile robot and ω is the angular velocity of the mobile robot. t Let ω be the linear velocity of the mobile robot at time t. t Let v be the angular velocity of the mobile robot at time t, where Δt is the sampling time interval. m ω is the maximum linear acceleration of the mobile robot. m` This represents the maximum value of the angular acceleration of the mobile robot.

[0074] ③. The mobile robot needs to maintain a safe speed range to ensure that it can brake within a safe distance when encountering dynamic or static obstacles that are not marked on the map; the speed sampling space under obstacle constraints is defined by Equation 11;

[0075] Formula 11:

[0076] In the formula, V s Let dist be the velocity sampling space under obstacle constraints, where (v, ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and dist is the velocity sampling space under obstacle constraints. min (v,ω) represents the distance from the end of the path corresponding to (v,ω) to the nearest obstacle;

[0077] A-2. Establishment of path evaluation function: First, obtain the path of the mobile robot in the next sampling period, then use the path evaluation function to evaluate all paths, select and execute the velocity tuple corresponding to the optimal path. The path evaluation function is shown in Formula 12.

[0078] Formula 12: G(v,ω)=α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω)

[0079] In the formula, G(v,ω) is the path evaluation function; heading(v,ω) is the deflection angle evaluation sub-function; dist(v,ω) is the safety factor evaluation sub-function; velocity(v,ω) is the speed evaluation sub-function; α, β, and γ are the weight coefficients of the above three sub-functions, respectively.

[0080] The three sub-functions in Formula 12 are normalized to make them have the same scale in the path evaluation function, as shown in Formula 13.

[0081] Formula 13:

[0082] In the formula, n is the path with the largest sequence number in a sampling period, i is the current path to be evaluated in a sampling period, and heading(v i ,ω i ) is the deflection angle evaluation sub-function under the current path to be evaluated, dist(v i ,ω i ) is the safety factor evaluation sub-function under the current path to be evaluated, velocity(v i ,ω i ) represents the speed evaluation sub-function under the current path to be evaluated;

[0083] B. Introduce fuzzy logic to achieve dynamic parameter tuning of the DWA algorithm:

[0084] According to the principle of the DWA algorithm, (α,β,γ) in the path evaluation function are parameters that need to be dynamically adjusted. The position of the target point affects the values ​​of α and γ, and the position of the obstacle affects the values ​​of β and γ.

[0085] B-1, the basic framework of fuzzy logic is described as follows: First, design the directional fuzzy logic controller and the safety fuzzy logic controller; the input of the directional fuzzy logic controller (L...) MT ,θ DA The output is a set of weight parameters (α1, γ1); the input of the secure fuzzy logic controller is (L... MO ,L MT The system outputs a set of weight parameters (β1, γ2); then, a fusion fuzzy logic controller is designed to dynamically fuse the two, outputting the final fusion weight coefficient δ; the inputs of the three fuzzy logic controllers include: the distance L between the mobile robot and the obstacle. MO The distance L between the mobile robot and the target sub-navigation point MT and the direction angle θ of the mobile robot DA The outputs of the three fuzzy logic controllers include: the weight coefficient α of the deflection angle evaluation subfunction, the weight coefficient β of the safety factor evaluation subfunction, the weight coefficient γ of the speed evaluation subfunction, and the fusion weight coefficient δ; α1∈α, β1∈β, γ1∈γ, γ2∈γ;

[0086] B-2, The fuzzification of input quantities in fuzzy logic (FL) is as follows: For input quantity L... MO L MT and θ DA Blurring is performed; where L MO The domain is [0, 2.5], the fuzzy set is [N, M, F], where N is near, M is medium, and F is far. When L MO When L < 0.5, the result is blurred to N; when L < 0.5, the result is blurred to N. MO When L < 1.25, it is blurred to M; when L < 1.25, it is blurred to M. MO When the value is greater than 1.25, the blurring is F; L MT The domain is [0,3], and the fuzzy set is [N,M,F], where N is near, M is medium, and F is far. When L MT When L < 0.5, the result is blurred to N; when L < 0.5, the result is blurred to N. MT When L < 1.5, it is blurred to M; when L < 1.5, it is blurred to M. MT When θ > 1.5, it is blurred to F; DA The domain is defined as [-180°, 180°], and the fuzzy set is [XS, S, M, B, XB], where XS is negative (large), S is negative, M is neutral, B is positive, and XB is positive (large). DA When -120° < θ, it is blurred to XS; when -120° < θ, it is blurred to XS. DAWhen -60° < θ, it is blurred to S; when -60° < θ DA When θ < 60°, it is blurred to M; when 60° < θ DA When θ < 120°, it is blurred to B; when θ < 120°, it is blurred to B. DA When the angle is greater than 120°, it is blurred to XB;

[0087] B-3, the fuzzification of the output quantities of fuzzy logic is as follows: The output quantities α, β, γ, and δ are fuzzified; the domains of α, β, γ, and δ are all [0,1], and the fuzzy sets of α, β, and δ are all [XS,S,M,L,XL], where XS is minimal, S is small, M is medium, L is large, and XL is maximal. When 0 < α < 0.1 or 0 < β < 0.1 or 0 < δ < 0.1, it is fuzzified to XS; when 0.1 < α < 0.3 or 0.1 < β < 0.3 or 0.1 < δ < 0.3, it is fuzzified to S; when... When 0.3 < α < 0.6, 0.3 < β < 0.6, or 0.3 < δ < 0.6, the fuzzification is M; when 0.6 < α < 0.9, 0.6 < β < 0.9, or 0.6 < δ < 0.9, the fuzzification is L; when 0.9 < α < 1, 0.9 < β < 1, or 0.9 < δ < 1, the fuzzification is XL. The fuzzy set of γ is [S, M, L], where S is small, M is medium, and L is large. When γ < 0.3, the fuzzification is S; when 0.3 < γ < 0.7, the fuzzification is M; and when γ > 0.7, the fuzzification is L.

[0088] B-4, The control rules for the directional fuzzy logic controller are as follows: There are a total of 9 control rules for the directional fuzzy logic controller;

[0089] ①. Input terminal: L MT Blurring to F, θ DA The fuzzy representation is M; the output is: α1 fuzzy represented as XL, γ1 fuzzy represented as L;

[0090] ②. Input terminal: L MT Blurring to F, θ DA Blurring to SorB; Output: α1 fuzzified to S, γ1 fuzzified to L;

[0091] ③. Input terminal: L MT Blurring to F, θ DA The result is fuzzified to XSorXB; the output is: α1 fuzzified to M, γ1 fuzzified to L;

[0092] ④. Input terminal: L MT Blurring to M, θ DA Blurring to M; Output: α1 fuzzified to M, γ1 fuzzified to M;

[0093] ⑤. Input terminal: L MT Blurring to M, θ DABlurring to SorB; Output: α1 fuzzified to L, γ1 fuzzified to M;

[0094] ⑥. Input terminal: L MT Blurring to M, θ DA Blurring to XSorXB; Output: α1 fuzzified to L, γ1 fuzzified to M;

[0095] ⑦. Input terminal: L MT Blurring to N, θ DA The fuzzy representation is M; the output is: α1 fuzzy represented as M, γ1 fuzzy represented as S;

[0096] ⑧. Input terminal: L MT Blurring to N, θ DA Blurring to SorB; Output: α1 fuzzified to L, γ1 fuzzified to S;

[0097] 9. Input terminal: L MT Blurring to N, θ DA Blurring to XSorXB; Output: α1 is blurred to XL, γ1 is blurred to S;

[0098] The centroid method was used for subsequent defuzzification.

[0099] B-5, The control rules for the secure fuzzy logic controller are as follows: There are a total of 9 control rules for the secure fuzzy logic controller;

[0100] ①. Input terminal: L MO Blurring to F,L MT The fuzzification is F; the output is: β1 is fuzzified to XS, γ2 is fuzzified to L;

[0101] ②. Input terminal: L MO Blurring to F,L MT The fuzzification is M; the output is: β1 is fuzzified to S, γ2 is fuzzified to L;

[0102] ③. Input terminal: L MO Blurring to F,L MT The fuzzy representation is N; the output is: β1 is fuzzy represented as S, γ2 is fuzzy represented as S;

[0103] ④. Input terminal: L MO Blurring to M,L MT The fuzzification is F; the output is: β1 is fuzzified to M, γ2 is fuzzified to L;

[0104] ⑤. Input terminal: L MO Blurring to M,L MT Blurring to M; Output: β1 fuzzified to M, γ2 fuzzified to M;

[0105] ⑥. Input terminal: L MO Blurring to M,L MT The fuzzy representation is N; the output is: β1 is fuzzy represented as M, and γ2 is fuzzy represented as S.

[0106] ⑦. Input terminal: L MO Blurring to N,L MT The fuzzification is F; the output is: β1 is fuzzified to XL, and γ2 is fuzzified to M.

[0107] ⑧. Input terminal: L MO Blurring to N,L MT The output is: β1 is fuzzified to L, and γ2 is fuzzified to M.

[0108] 9. Input terminal: L MO Blurring to N,L MT The result is blurred to N; the output is: β1 blurred to M, γ2 blurred to M;

[0109] The centroid method was used for subsequent defuzzification.

[0110] B-6, the control rules of the fusion fuzzy logic controller are as follows: There are a total of 8 control rules for the fusion fuzzy logic controller, among which, the defuzzification method selects the centroid method;

[0111] ①. Input terminal: L MO Blurring to F, θ DA Blur to SorB; Output: δ blur to XL;

[0112] ②. Input terminal: L MO Blurring to F, θ DA Blurring to XSorXB; Output: δ blurring to L;

[0113] ③. Input terminal: L MO Blurring to F, θ DA Blurring to M; Output: δ Blurring to M;

[0114] ④. Input terminal: L MO Blurring to M, θ DA Blurring to XSorXB; Output: δ blurring to L;

[0115] ⑤. Input terminal: L MO Blurring to M, θ DA Blurring to SorB; Output: δ Blurring to M;

[0116] ⑥. Input terminal: L MO Blurring to M, θ DA The output is fuzzyened to M; the δ-fuzzyened output is S.

[0117] ⑦. Input terminal: L MO Blurring to N, θ DA Blur the value to XS or XB; Output: δ is blurred to XS;

[0118] ⑧. Input terminal: L MO Blurring to N, θ DA Blur the output to SorMorB; Output: δ is fuzzified to S;

[0119] The centroid method was used for subsequent defuzzification.

[0120] C. Integration of VOA dynamic obstacle avoidance and FL-DWA algorithm:

[0121] The principle of VOA dynamic obstacle avoidance is as follows: In the global coordinate system XOY, the mobile robot M is located at the origin, and the mobile robot M moves at a velocity v m The robot moves forward, and the grid obstacle o moves forward at a speed of v0, where Δv is the speed of the mobile robot relative to the grid obstacle. The grid obstacle is expanded into a circular obstacle with a radius of R, where the circle is the smallest circumcircle that completely contains the grid obstacle. T1 and T2 are the tangents from the origin to the two sides of the circular obstacle, and μ is the intersection of the tangent T1 or T2 and the straight line. The angle between the velocity Δv and the straight line, γ is the angle between the velocity Δv and the straight line. The angle between the tangents, μ and γ, is positive. When the velocity Δv is between the tangents T1 and T2, the mobile robot will collide with the obstacle. The angle range of the collision is 2μ. The goal of dynamic obstacle avoidance is to keep Δv outside 2μ, even if |γ| > μ.

[0122] Based on v m The velocity vector triangle formed by v0 and Δv gives us Equations 14 and 15. Differentiating Equation 15 gives us Equation 16.

[0123] Formula 14:

[0124] Formula 15:

[0125] Formula 16:

[0126] In the formula, α is the orientation angle of the mobile robot, and β is the orientation angle of the grid obstacle. v is the velocity vector in the triangle. m The angle formed by η and Δv, where η is the angle formed by v0 and Δv in the velocity vector triangle, and θ is the angle between the positive X-axis and the straight line. The angle formed; d is the differential sign; in addition, dγ = Δγ; dv m =Δv m dα = Δα; dβ = Δβ;

[0127] According to Formula 16, given the velocity v0 of the grid obstacle, the orientation angle β of the grid obstacle, the change in the grid obstacle's velocity Δv0, and the change in the grid obstacle's orientation angle Δβ, the mobile robot's velocity v can be adjusted. m The direction angle α of the mobile robot is used to change Δγ, so that the adjusted γ satisfies: γ + Δγ > μ; Δv m Let x1 and x2 be set to Δα respectively, and establish the dynamic obstacle avoidance objective function as shown in Equation 17. Solving Equation 17 will yield the velocity tuple that satisfies dynamic obstacle avoidance. Selecting this velocity tuple in the above FL-DWA algorithm will enable dynamic obstacle avoidance.

[0128] Formula 17:

[0129] The parameter definitions in Formula 14-17 are only applicable to Formula 14-17 and cannot be applied to other formulas.

[0130] Compared with the prior art, the present invention has the following advantages:

[0131] 1. It is applicable to mobile robot planning in multi-information field coupling environments with unknown dynamic or static obstacles. It first adopts ACO-A * The PSO algorithm plans a global path, selects inflection points on the global path as sub-navigation points, and then calls the FL-DWA-VOA algorithm to replan the local path to form the final path. The final path not only ensures global optimization but also has the ability to avoid unknown static or dynamic obstacles.

[0132] 2. It establishes a two-layer hybrid path planning method that integrates global planning and local planning. The global planning path guides local obstacle avoidance, which solves the problem that the local path planning algorithm gets stuck in local optima due to the lack of global environmental information. It provides a path for mobile robots in the kernel environment that guarantees global optima and can effectively avoid unknown static or dynamic obstacles.

[0133] 3. A fuzzy logic-based adaptive local obstacle avoidance method (FL-DWA) is proposed. This algorithm dynamically adjusts the weight coefficients of the evaluation function based on environmental characteristics, improving obstacle avoidance capabilities in unknown environments. Furthermore, the Velocity Obstacle Algorithm (VOA) is integrated into FL-DWA to form a hybrid local obstacle avoidance algorithm, FL-DWA-VOA, further enhancing the ability to avoid dynamic obstacles.

[0134] The present invention will be further described below with reference to the figures and embodiments. Attached Figure Description

[0135] Figure 1 This is a schematic diagram of the path planning principle of the present invention;

[0136] Figure 2 This is the two-layer algorithm logic diagram of the present invention;

[0137] Figure 3 The diagram illustrates the technical problem described in this invention.

[0138] Figure 4 This is a schematic diagram of the velocity sampling space in the DWA algorithm;

[0139] Figure 5 This is a path graph of different velocity tuples in the DWA algorithm;

[0140] Figure 6 This is a schematic diagram of the FL-DWA algorithm.

[0141] Figure 7 This is a schematic diagram of the VOA dynamic obstacle avoidance algorithm.

[0142] Figure 8 This is a path planning graph for a dynamic kernel environment scenario with low complexity.

[0143] Figure 9 This is a path planning graph for a highly complex dynamic kernel environment scenario. Detailed Implementation

[0144] Example 1:

[0145] A two-layer path planning method for real-time obstacle avoidance of mobile robots in a nuclear environment is disclosed. The grid map of the nuclear environment serves as prior information. Each grid in the grid map is a square, and each grid contains radiation field information and obstacle information (inherent obstacles in the environment). The radiation field information is the average radiation dose rate recorded in each grid. The obstacle information is identified in each grid by binary codes "1" and "0", indicating "obstacles that the mobile robot cannot cross" and "obstacles that the mobile robot cannot cross", respectively. The mobile robot is equipped with a lidar for real-time perception of "obstacles not in the map (including dynamic and static obstacles)".

[0146] like Figure 1-7 As shown, the method is as follows:

[0147] S01, Establish a mathematical model for real-time obstacle avoidance path planning:

[0148] A. Problem Description:

[0149] Mobile robots operate in nuclear power plants in two main scenarios: Ⅰ. During routine operation, the robot moves to a specific work area to perform monitoring or maintenance tasks; Ⅱ. After a nuclear accident, the robot moves to the leak point for monitoring or emergency operations. To improve the operational efficiency and ensure the safety of the mobile robot, a route needs to be planned from the starting point S(p1(x1,y1)) to the target point E(p... n (x n ,y n For the optimal path, see [link / reference] Figure 3 To better describe this path planning problem, the following explanation is provided.

[0150] A-1. Define passability:

[0151] See Figure 3 The image shows a grid map of the indoor nuclear environment. To visually represent the radiation field information, different colors are used to distinguish the average radiation dose rate in different ranges (gradually increasing according to the rule of lake blue-beige-bright yellow-pink-dark red). To more clearly represent obstacle information (inherent obstacles in the environment), black represents "obstacles that the mobile robot cannot cross," and all other grids represent "obstacles that the mobile robot cannot cross." The mobile robot uses an eight-direction search strategy for path planning. That is, when the mobile robot moves from the current grid point to an adjacent grid, it can only move along a straight path from the center point of the current grid to the center point of the adjacent grid. The grid coordinates are defined as the coordinates of the grid center point. If a grid simultaneously meets the three conditions of "no obstacles that the mobile robot cannot cross within the grid," "the average radiation dose rate within the grid is below a threshold," and "the mobile robot does not collide with any obstacles when entering the grid," then the grid is defined as a passable grid. If a grid does not meet any of the above three requirements, then the grid is defined as an impassable grid. A passable path means that all grids traversed by the mobile robot are passable grids. The set of passable grids is a passable area. The direction in which the mobile robot points to a passable point from its current position is the passable direction.

[0152] A-2. Path planning objective:

[0153] When planning a global path for a mobile robot in a kernel environment containing obstacles not shown on the map, three aspects are considered: obstacle avoidance, high radiation risk avoidance, and cost minimization. Obstacle avoidance is a hard constraint, meaning the mobile robot must not collide with any obstacles in the environment (including inherent obstacles and obstacles not shown on the map). High radiation risk avoidance is also a hard constraint, meaning the mobile robot is not allowed to pass through grids with an average radiation dose rate exceeding a threshold (artificially set). (The threshold can be understood as the limit of radiation the mobile robot can withstand; forcibly passing through would result in radiation damage or even destruction to the mobile robot or its onboard components.) Cost minimization refers to pursuing the path with the lowest cost under the two hard constraints of obstacle avoidance and high radiation risk avoidance. Cost consists of path length, cumulative radiation dose, and turning energy consumption.

[0154] B. Mathematical Modeling:

[0155] Considering path length, cumulative radiation dose, and turning energy consumption, an objective function for planning the global path of the mobile robot in a nuclear environment containing obstacles not in the map is established (this function aims to find the minimum value), see Formula 1;

[0156] Formula 1:

[0157] In the formula, M(p) represents the objective function (aiming to minimize the value), ω1 is the path length weight coefficient, ω2 is the cumulative radiation dose weight coefficient, ω3 is the turning energy consumption weight coefficient, L(p) is the path length sub-objective function, R(p) is the cumulative radiation dose sub-objective function, and E(p) is the turning energy consumption sub-objective function; d(p i ,p i+1 ) is the raster p i To grid p i+1 Distance (when grid p) i With grid p i+1 When the edges are adjacent, d(p) i ,p i+1 That is, 1 times the grid side length; when the grid p i With grid p i+1 When the vertices are in contact, d(p) i ,p i+1 That is (x) i ,y i ) is the raster p i The coordinates, (x i+1 ,y i+1 ) is the raster p i+1 The coordinates of R(p) i ,p i+1 ) for mobile robots from grid p i Drive to the grid pi+1 The cumulative radiation dose, For grid p i The average radiation dose rate (statistical range is within the space 1m above the ground), For grid p i+1 The average radiation dose rate (statistical range is within a space 1m above the ground), l is the grid p i To grid p i+1 Distance (if grid p) i With grid p i+1 For adjacent edges, l equals 1 times the grid edge length. If grid p i With grid p i+1 If the vertices are in contact, then l equals 1. (Multiple grid side length); ε1 is the weighting coefficient of the steering angle variable, ε2 is the weighting coefficient of the number of steering turns, ψ[l(p i-1 ,p i ),l(p i ,p i+1 )] is line segment l(p i-1 ,p i ) and line segment l(p i ,p i+1 The steering angle variable, l(p) i-1 ,p i ) is the raster p i-1 The center point and grid p i The line connecting the center points, l(p) i ,p i+1 ) is the raster p i The center point and grid p i+1 The line connecting the center points of N; turn The number of turns. The average radiation dose rate of a grid refers to the average radiation dose rate within the three-dimensional space formed by the grid at a height of 1m above the ground.

[0158] S02, using ACO-A * -PSO algorithm plans global paths:

[0159] A. Construct an improved A* algorithm:

[0160] A-1. Heuristic Function:

[0161] Considering the three sub-objectives in mathematical modeling, we construct the heuristic function for the improved A* algorithm, as shown in Equation 2.

[0162] Formula 2:

[0163] In the formula, g(p) is the path length cost from the starting point to the current grid (center point) (actual value), h(p) is the path length cost from the current grid (center point) to the target point (expected value), and g(p) and h(p) are calculated using L(p) in Formula 1; R(p) 1-n R(p) represents the cumulative radiation dose cost of a mobile robot traveling from its starting point to its destination. 1-n Refer to Formula 1 for the calculation of R(p). 1-n ) is R(p 1-i ) and R(p i-n The sum of R(p) 1-i R(p) represents the cumulative radiation dose cost (actual value) of the mobile robot traveling from the starting point to the current grid (center point). i-n E(p) represents the cumulative radiation dose cost (expected value) of the mobile robot traveling from the current grid (center point) to the target point; 1-n E(p) represents the energy consumption cost of turning between the starting point and the target point. 1-n Refer to E(p) in Formula 1 for calculation. 1-n ) is E(p 1-i ) and E(p i-n The sum of E(p) 1-i E(p) represents the energy cost of turning between the starting point and the current grid (center point) (the actual value). i-n (x) represents the energy consumption cost of turning between the current grid (center point) and the target point; i ,y i ) represents the current raster p i The coordinates are (x1, y1), where (x1, y1) are the coordinates of the starting point (the grid cell it belongs to), and (x... n ,y n ) represents the coordinates of the target point (the grid cell it is located in).

[0164] A-2. Danger Zone Avoidance Strategies:

[0165] During the path search process, when the ant senses that the average radiation dose rate of the next grid exceeds the threshold (the threshold can be understood as the limit of radiation that the mobile robot can withstand; if it is forced to pass through, it will cause radiation damage or even damage to the mobile robot or its mounted components), the average radiation dose rate of the next grid is defined as infinite, prompting the ant to find other passable grids; thus constructing a danger zone avoidance strategy, as shown in Formula 3.

[0166] Formula 3:

[0167] Among them, R(p i ) is the raster p iThe radiation dose rate is algebraic, where Inf is infinite and RI is infinite. max This is the radiation dose rate threshold.

[0168] B, run the improved A* algorithm to obtain a suboptimal path:

[0169] After running the improved A* algorithm and completing the calculation, the output result is: suboptimal path P. sp .

[0170] C, The initial pheromone of the ACO algorithm is processed using suboptimal path non-uniformity:

[0171] The pheromone matrix plays a crucial role in the optimization performance of the ACO algorithm. Traditional uniform initial pheromone distribution can lead to blindness in the early stages of the ACO search, which slows down its convergence speed. Therefore, the improved A* algorithm is used to obtain the suboptimal path P. sp The initial pheromone in the ACO algorithm is made non-uniform, as shown in Formula 4. The aim is to increase the initial pheromone concentration of only the grids traversed by the suboptimal path, while keeping the initial pheromone concentration of other grids unchanged, thereby more effectively guiding the ant search path and accelerating the convergence speed of the ACO algorithm.

[0172] Formula 4:

[0173] Where i,j represent two adjacent edges or touching vertices of a passable grid, τ i,j For grid p i With p j The initial pheromone concentration after the non-uniformity treatment is τ, which is the artificially set initial pheromone concentration (recommended to be set to 0.5), and C is the initial pheromone enhancement value (set between 0.1 and 1.0, for example, it can be set to 0.2, 0.5 or 0.8).

[0174] D. Construct an improved ACO algorithm:

[0175] D-1. Heuristic function:

[0176] Considering the influence of the three sub-objectives in mathematical modeling on the heuristic function of the ACO algorithm, an adaptive adjustment coefficient is introduced to balance the convergence speed and global search capability of the ACO algorithm. The function of the heuristic function is enhanced in the earlier iterations and weakened in the later iterations, thereby realizing the construction of the heuristic function of the ACO algorithm, as shown in Formula 5.

[0177] Formula 5:

[0178] Where, η t (i,j) represents the value from the grid p in the t-th iteration. ito p j The heuristic function values, i,j represent two adjacent edges or touching vertices of a passable grid, Q1 is the heuristic function enhancement coefficient, L i,j L is the distance from grid i to grid j (if grid i and grid j are adjacent edges, then L) i,j It is 1 times the grid side length; if grid i and grid j are vertex contacts, then L i,j for (times the grid side length), R i,j The cumulative radiation dose (R) received by the mobile robot as it travels from grid i to grid j. i,j The calculation refers to the cumulative radiation dose sub-target function in Formula 1, E i,j For mobile robots from grid p i-1 To grid p i to p j Turning energy consumption (E) i-1,i,j The calculation refers to the turning energy consumption sub-objective function in Formula 1, I max I is the maximum number of iterations. c ξ is the current iteration number, ξ is the adaptive adjustment coefficient, and Q is the heuristic function enhancement coefficient (recommended to be set to 1).

[0179] D-2. Choosing a probability function:

[0180] Taking into account both pheromone concentration and heuristic function, the selection probability function of ACO algorithm is constructed as shown in Equation 6;

[0181] Formula 6:

[0182] In the formula, P t (i,j) represents the value from the grid p in the t-th iteration. i to p j The selection probability is given by τ, where i and j represent passable grids with adjacent edges or touching vertices, α is the pheromone importance factor, β is the heuristic function importance factor, and τ is the pheromone importance factor. t (i,j) represents the value from grid p when the t-th iteration is completed. i to p j pheromone concentration, τ t (i,u) represents the value from grid p when the t-th iteration is completed. i to p u pheromone concentration, η t (i,j) represents the value from the grid p in the t-th iteration. i to p j The heuristic function value, η t (i,u) represents the value from the grid p in the t-th iteration. i to p u The heuristic function value; T tLet ' be the set of grid cells that have not been visited before the t-th iteration; the denominator in Formula 6 is the set of grid cells that have not been visited before the t-th iteration. i The sum of the selection probabilities of all unvisited graticules; if the graticule p in Formula 6 j It is an unprocessed raster, and then rasterization p. i to p j The selection probability calculation, if the grid p in Formula 6 j If it is a previously visited raster, then raster p i to p j The probability of choosing it is 0.

[0183] D-3. Pheromone Update Rules:

[0184] The traditional ACO algorithm uses a single positive feedback mechanism for pheromone update rules, which has a fast convergence speed. However, too much positive feedback pheromone is concentrated on excellent paths, which limits the ant colony's ability to explore unknown paths, causing the ACO algorithm to easily get trapped in local optima. Furthermore, the traditional ACO algorithm only considers the influence of path length in its pheromone update rules, which cannot meet the requirements of mobile robots planning global paths in kernel environments containing obstacles that are not in the map. Therefore, the positive and negative feedback mechanisms are combined with the three sub-objectives in mathematical modeling to construct the pheromone update rules for the ACO algorithm, as shown in Equation 7.

[0185] Formula 7:

[0186] In the formula, τ t+1 (i,j) and τ t (i,j) represent the raster p when the (t+1)th and tth iterations are completed, respectively. i With p j The pheromone concentrations between these values ​​are: ρ is the pheromone evaporation factor, Q2 is the pheromone deposition factor, and M is the pheromone concentration. t-mi M represents the total cost value of the three sub-objectives of the optimal path obtained in the t-th iteration (the total cost value of the optimal path is minimized, calculated according to Formula 1). t-ma P represents the total cost value of the three sub-objectives of the worst path obtained in the t-th iteration (the total cost value of the worst path is the largest, calculated according to Formula 1); optimal Let P be the optimal path obtained in the (t+1)th iteration. worst This is the worst path obtained in the (t+1)th iteration.

[0187] E. The PSO algorithm is used for parameter combination optimization:

[0188] PSO (Protocol for Optimization) is a swarm optimization algorithm that simulates the foraging behavior of birds. It uses particles to simulate the process of birds searching for food to find the optimal solution to an optimization problem. When the PSO algorithm is used for parameter combinatorial optimization: the position (or coordinates) of a particle represents a specific combination of parameters, i.e., a potential solution in the problem space; the velocity of the particle is used to guide the direction and magnitude of the particle's search in the solution space, which can be seen as a mechanism for fine-tuning the current parameter combination, guiding the particle to move from its current position towards a region where it is more likely to find the optimal solution; the velocity of the particle has two dimensions: direction and step size. The direction of the velocity indicates how the parameter combination should be adjusted to approach the optimal solution, which is usually achieved by comparing the particle's current position with the individual best position and the particle's current position with the global best position. The step size of the velocity determines the magnitude of the parameter adjustment. A larger step size allows the particle to jump more in the solution space, while a smaller step size allows the particle to perform a more refined search.

[0189] E-1. Determine the combination of parameters to be optimized:

[0190] Based on the objective function of mathematical modeling and the ACO algorithm constructed above, the parameter combination to be optimized is initially determined to be (ω1,ω2,ω3,α,β,ρ). Since (ω1,ω2,ω3) are multi-objective weight coefficients, the number of multi-objective weight coefficients is simplified by using a single-objective standardization method. Let ω1 = 1 to obtain the approximate range of ω2 and ω3. Finally, the parameter combination to be optimized is determined to be (ω2,ω3,α,β,ρ). Subsequently, the position quality of the particles is evaluated by the objective function of global path planning.

[0191] E-2. Define the particle velocity update rule:

[0192] The particle velocity update rule in the PSO algorithm is defined as shown in Equation 8;

[0193] Formula 8:

[0194] In the formula, Gbest represents the optimal position of the i-th particle in the entire particle swarm obtained in k iterations. k ω represents the optimal position obtained by all particles in the entire particle swarm in k iterations, ω is the inertia weight, c1 and c2 are both learning factors, c1 represents the weight of the statistical acceleration term that pushes each particle to its individual optimal position, c2 represents the weight of the statistical acceleration term that pushes each particle to the global optimal position, and r1 and r2 are both random numbers between [0,1], r1 and r2 are used to increase the randomness of particle flight. and These are the positions of the i-th particle in the k-th and (k+1)-th iterations, respectively. and Let [-v] represent the velocities of the i-th particle in the k-th and (k+1)-th iterations, respectively. max ,v max [ ] represents the velocity range of the particles.

[0195] E-3. Obtain the optimized parameter combination:

[0196] The PSO algorithm is run, and the objective function (Formula 1) is used as the fitness function to evaluate the quality of the particle position. Finally, the optimized parameter combination (ω2,ω3,α,β,ρ) is obtained.

[0197] F, Obtain the global path based on optimized parameter combinations:

[0198] The optimized parameter combination is then fed into the ACO algorithm, which is run to obtain the global path from the starting point to the target point.

[0199] S03, using the FL-DWA-VOA algorithm to plan the local path:

[0200] First, all inflection points in the global path are defined as sub-navigation points. Then, the paths between the starting point and the first sub-navigation point, all adjacent sub-navigation points, and the last sub-navigation point and the target point are defined as local paths. Path replanning is performed on each local path in a forward-to-back order. First, the FL-DWA algorithm is run to avoid static obstacles. If the mobile robot detects dynamic obstacles through the LiDAR, the VOA dynamic obstacle avoidance algorithm is additionally called to avoid the dynamic obstacles.

[0201] A. Establish the DWA algorithm model:

[0202] The DWA algorithm includes two core processes: ①. Figure 4 As shown, safety constraints are formed based on the motion characteristics of the mobile robot and environmental obstacles, limiting the velocity sampling space to a specific range. Then, a series of discrete velocity tuples are obtained based on kinematic analysis. In the figure, v max and v min ω represents the maximum and minimum linear velocities of the mobile robot, respectively. max and ω min Let v represent the maximum and minimum angular velocities of the mobile robot, respectively. Let a represent the linear acceleration of the mobile robot, b represent the angular acceleration of the mobile robot, and v represent the angular acceleration of the mobile robot. t ω represents the linear velocity of the mobile robot at time t. t Let be the angular velocity of the mobile robot at time t. ②. For example... Figure 5As shown, the velocity tuples described above are used to predict the path in the next interval. In the figure, the red dashed line represents the collision path, while the green solid line and yellow dashed line represent candidate paths. The candidate paths are evaluated using a path evaluation function to select the optimal path (i.e., the green solid line).

[0203] A-1. Velocity Sampling Space Analysis: Taking into account kinematic constraints, acceleration constraints, and obstacle constraints, the velocity sampling space of the DWA algorithm is established.

[0204] ①. Due to the limitations of the mobile robot's own motion performance, its linear velocity and angular velocity are restricted to a certain range; the velocity sampling space V under kinematic constraints m Defined as Formula 9;

[0205] Formula 9:

[0206] In the formula, V m Let (v, ω) be the velocity sampling space under kinematic constraints, where (v, ω) is the velocity tuple, v is the linear velocity of the mobile robot, and ω is the angular velocity of the mobile robot. min Let v be the minimum linear velocity of the mobile robot. max Let ω be the maximum linear velocity of the mobile robot. max Let ω be the maximum angular velocity of the mobile robot. min This represents the minimum angular velocity of the mobile robot.

[0207] ②. The variable speed performance of a mobile robot has boundaries. Furthermore, considering that excessive acceleration or deceleration during movement will lead to motor overload and rapid wear, it is necessary to constrain the robot's acceleration; the velocity sampling space V under acceleration constraints... d Defined as Formula 10;

[0208] Formula 10:

[0209] In the formula, V d Let (v,ω) be the velocity sampling space under acceleration constraints, where v is the linear velocity of the mobile robot and ω is the angular velocity of the mobile robot. t Let ω be the linear velocity of the mobile robot at time t. t Let v be the angular velocity of the mobile robot at time t, where Δt is the sampling time interval. m ω is the maximum linear acceleration of the mobile robot. m` This represents the maximum value of the angular acceleration of the mobile robot.

[0210] ③. The mobile robot needs to maintain a safe speed range to ensure that it can brake within a safe distance when encountering dynamic or static obstacles that are not marked on the map; the speed sampling space under obstacle constraints is defined by Equation 11;

[0211] Formula 11:

[0212] In the formula, V s Let dist be the velocity sampling space under obstacle constraints, where (v, ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and dist is the velocity sampling space under obstacle constraints. min (v,ω) represents the distance from the end of the path corresponding to (v,ω) to the nearest obstacle.

[0213] A-2. Establishment of Path Evaluation Function: Based on the sampling combination of linear velocity and angular velocity of the mobile robot under linear motion in each sampling period and the simulation time, the path of the mobile robot in the next sampling period is obtained; the path evaluation function is used to evaluate all paths in the next sampling period, and the velocity tuple corresponding to the optimal path is selected and executed. The path evaluation function is shown in Formula 12.

[0214] Formula 12:

[0215] In the formula, G(v,ω) is the path evaluation function; heading(v,ω) is the deflection angle evaluation sub-function, used to evaluate the angle change of the mobile robot on the path; dist(v,ω) is the safety factor evaluation sub-function, used to evaluate the safety of the mobile robot on the path; velocity(v,ω) is the speed evaluation sub-function, used to evaluate the speed performance of the mobile robot on the path; α, β, and γ are the weight coefficients of the above three sub-functions, respectively; θ end θ is the heading angle of the mobile robot when it reaches the end of the path. start Let l be the heading angle of the mobile robot at the starting point of the path. pl The path length is (x). obs ,y obs (x) represents the coordinates of the mobile robot. m ,y m ) represents the coordinates of the obstacle; velocity(v) is the second-level sub-function for evaluating linear velocity, velocity(ω) is the second-level sub-function for evaluating angular velocity, and ε3 and ε4 are the weight coefficients of the above two second-level sub-functions, respectively;

[0216] The three sub-functions in Formula 12 are normalized so that they have the same scale in the path evaluation function, thus enabling a fair and reasonable assessment of their contribution to the path evaluation function, as shown in Formula 13.

[0217] Formula 13:

[0218] In the formula, n is the path with the largest sequence number in a sampling period, i is the current path to be evaluated in a sampling period, and heading(v i ,ω i ) is the deflection angle evaluation sub-function under the current path to be evaluated (the specific value is calculated according to formula 12), dist(v i ,ω i ) is the safety factor evaluation sub-function for the current path to be evaluated (the specific value is calculated according to formula 12), velocity(v i ,ω i ) is the speed evaluation sub-function under the current path to be evaluated (the specific value is calculated according to formula 12).

[0219] B. Introduce fuzzy logic to achieve dynamic parameter tuning of the DWA algorithm:

[0220] In the actual movement of a mobile robot, the environment is dynamically changing, and fixed weighting coefficients cannot adapt to this change. For example, in some situations, avoiding obstacles may be more important, while in others, quickly reaching the target point may be more critical. Fixed weighting coefficients may cause the DWA algorithm to perform poorly in certain scenarios. For instance, in an environment with dense obstacles, if the weighting coefficients are not properly adjusted, the mobile robot may focus too much on avoiding obstacles and neglect the need to quickly reach the target point, and vice versa. Therefore, it is essential to dynamically adjust the parameters of the DWA algorithm based on the current environmental state and the mobile robot's own motion state.

[0221] According to the principle of the DWA algorithm, (α,β,γ) in the path evaluation function are parameters that need to be dynamically adjusted. The position of the target point affects the values ​​of α and γ, and the position of the obstacle affects the values ​​of β and γ.

[0222] B-1, the basic framework of fuzzy logic (FL) is described as follows: First, design the directional fuzzy logic controller and the safety fuzzy logic controller; the input of the directional fuzzy logic controller (L... MT ,θ DA The output is a set of weight parameters (α1, γ1); the input of the secure fuzzy logic controller is (L... MO ,L MT The system first outputs a set of weight parameters (β1, γ2); then, a fusion fuzzy logic controller is designed to dynamically fuse the two, outputting the final fusion weight coefficient δ. The inputs to the three fuzzy logic controllers include: the distance L between the mobile robot and the obstacle. MO The distance L between the mobile robot and the target sub-navigation point MTand the direction angle θ of the mobile robot DA (Specifically, it refers to the angle difference between the direction the mobile robot is facing and the direction the mobile robot is facing towards the target sub-navigation point); the outputs of the three fuzzy logic controllers include: the weight coefficient α of the deflection angle evaluation sub-function, the weight coefficient β of the safety factor evaluation sub-function, the weight coefficient γ of the speed evaluation sub-function, and the fusion weight coefficient δ; α1∈α, β1∈β, γ1∈γ, γ2∈γ.

[0223] B-2, The fuzzification of input quantities in fuzzy logic (FL) is as follows: For input quantity L... MO L MT and θ DA Blurring is performed; where L MO The domain is [0, 2.5], the fuzzy set is [N, M, F], where N is near, M is medium, and F is far. When L MO When L < 0.5, the result is blurred to N; when L < 0.5, the result is blurred to N. MO When L < 1.25, it is blurred to M; when L < 1.25, it is blurred to M. MO When the value is greater than 1.25, the blurring is F; L MT The domain is [0,3], and the fuzzy set is [N,M,F], where N is near, M is medium, and F is far. When L MT When L < 0.5, the result is blurred to N; when L < 0.5, the result is blurred to N. MT When L < 1.5, it is blurred to M; when L < 1.5, it is blurred to M. MT When θ > 1.5, it is blurred to F; DA The domain is defined as [-180°, 180°], and the fuzzy set is [XS, S, M, B, XB], where XS is negative (large), S is negative, M is neutral, B is positive, and XB is positive (large). DA When -120° < θ, it is blurred to XS; when -120° < θ, it is blurred to XS. DA When -60° < θ, it is blurred to S; when -60° < θ DA When θ < 60°, it is blurred to M; when 60° < θ DA When θ < 120°, it is blurred to B; when θ < 120°, it is blurred to B. DA When the angle is greater than 120°, it is blurred to XB.

[0224] B-3, the fuzzification of the output quantities of fuzzy logic (FL) is as follows: The output quantities α, β, γ, and δ are fuzzified; the domains of α, β, γ, and δ are all [0,1], and the fuzzy sets of α, β, and δ are all [XS,S,M,L,XL], where XS is minimal, S is small, M is medium, L is large, and XL is maximal. When 0 < α < 0.1, 0 < β < 0.1, or 0 < δ < 0.1, the fuzzification is XS; when 0.1 < α < 0.3, 0.1 < β < 0.3, or 0.1 < δ < 0.3, the fuzzification is S. When 0.3 < α < 0.6 or 0.3 < β < 0.6 or 0.3 < δ < 0.6, it is blurred to M; when 0.6 < α < 0.9 or 0.6 < β < 0.9 or 0.6 < δ < 0.9, it is blurred to L; when 0.9 < α < 1 or 0.9 < β < 1 or 0.9 < δ < 1, it is blurred to XL. The fuzzy set of γ is [S, M, L], where S is small, M is medium, and L is large. When γ < 0.3, it is blurred to S; when 0.3 < γ < 0.7, it is blurred to M; and when γ > 0.7, it is blurred to L.

[0225] In summary, when using the DWA algorithm to plan local paths (defined as two adjacent sub-navigation points in the global path) for a mobile robot, it is necessary to consider directionality, safety, and speed characteristics based on changes in the environmental field. Therefore, based on the aforementioned fuzzy sets, fuzzy logic control rules can be established, specifically including control rules for the direction fuzzy logic controller, control rules for the safety fuzzy logic controller, and control rules for the fused fuzzy logic controller.

[0226] B-4, the control rules of the directional fuzzy logic controller are as follows: the directional guidance from the starting point (the starting sub-navigation point corresponding to the local path) to the ending point (the target sub-navigation point corresponding to the local path) will affect the mobile robot's driving efficiency; therefore, the input of the directional fuzzy logic controller is L. MT and θ DA The output of the directional fuzzy logic controller is to adjust the driving state of the mobile robot by adjusting the weight coefficients α and γ of heading(v,ω) and velocity(v,ω) in the influence evaluation function. Therefore, the output of the directional fuzzy logic controller is α and γ.

[0227] Summary of control rules:

[0228] ① When the mobile robot is far from the target sub-navigation point and the orientation angle θ DA When the value is small, γ should be increased to make the mobile robot move quickly toward the target point, while a suitable α should be selected to keep the mobile robot from deviating from the preset trajectory.

[0229] ②. When the mobile robot is far from the target sub-navigation point and the orientation angle θ DAWhen the value is large, it indicates that the mobile robot has deviated from the preset trajectory. α should be increased appropriately to increase the guiding force on the mobile robot. At the same time, a suitable value γ should be selected to give the mobile robot a reasonable speed range.

[0230] ③. When the mobile robot is close to the target sub-navigation point and the orientation angle θ DA When the value is small, γ should be reduced to slow down the mobile robot, and θ should be increased. DA This is to enable the mobile robot to reach the target sub-navigation point faster;

[0231] ④. When the mobile robot is close to the target sub-navigation point and the orientation angle θ DA When the value is large, it indicates that the mobile robot is about to reach the target sub-navigation point, but there is a tendency to detour. In this case, γ should be reduced to decelerate the mobile robot, and α should be increased to enable the mobile robot to reach the target point more smoothly.

[0232] Control rule description:

[0233] There are a total of 9 control rules for the directional fuzzy logic controller.

[0234] ①. Input terminal: L MT Blurring to F, θ DA The fuzzy representation is M; the output is: α1 fuzzy represented as XL, γ1 fuzzy represented as L;

[0235] ②. Input terminal: L MT Blurring to F, θ DA Blurring to SorB; Output: α1 fuzzified to S, γ1 fuzzified to L;

[0236] ③. Input terminal: L MT Blurring to F, θ DA The result is fuzzified to XSorXB; the output is: α1 fuzzified to M, γ1 fuzzified to L;

[0237] ④. Input terminal: L MT Blurring to M, θ DA Blurring to M; Output: α1 fuzzified to M, γ1 fuzzified to M;

[0238] ⑤. Input terminal: L MT Blurring to M, θ DA Blurring to SorB; Output: α1 fuzzified to L, γ1 fuzzified to M;

[0239] ⑥. Input terminal: L MT Blurring to M, θ DA Blurring to XSorXB; Output: α1 fuzzified to L, γ1 fuzzified to M;

[0240] ⑦. Input terminal: L MT Blurring to N, θ DA The fuzzy representation is M; the output is: α1 fuzzy represented as M, γ1 fuzzy represented as S;

[0241] ⑧. Input terminal: L MT Blurring to N, θ DA Blurring to SorB; Output: α1 fuzzified to L, γ1 fuzzified to S;

[0242] 9. Input terminal: L MT Blurring to N, θ DA The fuzzification is XSorXB; the output is: α1 is fuzzified to XL, and γ1 is fuzzified to S.

[0243] The centroid method was used for subsequent defuzzification.

[0244] B-5, the control rules of the safety fuzzy logic controller are as follows: The purpose of the safety fuzzy logic controller is to ensure that the mobile robot reaches the target sub-navigation point as quickly as possible while safely avoiding obstacles.

[0245] Overview of control rules:

[0246] ①. When mobile robot 2 is far from both the obstacle and the target sub-navigation point, i.e., L MO and L MT At the same time, the size is relatively large. In this case, the mobile robot does not need to execute the obstacle avoidance mechanism. Instead, it should increase its speed to quickly approach the target sub-navigation point. In this case, γ should be increased and β should be decreased.

[0247] ②. When the mobile robot approaches the obstacle but moves away from the target point, i.e., L MO Smaller, L MT Since the values ​​are relatively large, mobile robots should prioritize obstacle avoidance and reduce γ while increasing β.

[0248] ③. When the mobile robot is close to both an obstacle and a target sub-navigation point, in order to avoid the phenomenon of circling, an appropriate γ and β should be selected to achieve obstacle avoidance and the search range should be expanded;

[0249] ④. When the mobile robot moves away from the obstacle and approaches the target sub-navigation point, γ and β should be reduced simultaneously to allow the mobile robot to smoothly reach the target sub-navigation point.

[0250] Control rule description:

[0251] There are a total of 9 control rules for the safety fuzzy logic controller.

[0252] ①. Input terminal: L MO Blurring to F,L MT The fuzzification is F; the output is: β1 is fuzzified to XS, γ2 is fuzzified to L;

[0253] ②. Input terminal: L MO Blurring to F,L MT The fuzzification is M; the output is: β1 is fuzzified to S, γ2 is fuzzified to L;

[0254] ③. Input terminal: L MO Blurring to F,L MT The fuzzy representation is N; the output is: β1 is fuzzy represented as S, γ2 is fuzzy represented as S;

[0255] ④. Input terminal: L MO Blurring to M,L MT The fuzzification is F; the output is: β1 is fuzzified to M, γ2 is fuzzified to L;

[0256] ⑤. Input terminal: L MO Blurring to M,L MT Blurring to M; Output: β1 fuzzified to M, γ2 fuzzified to M;

[0257] ⑥. Input terminal: L MO Blurring to M,L MT The fuzzy representation is N; the output is: β1 is fuzzy represented as M, and γ2 is fuzzy represented as S.

[0258] ⑦. Input terminal: L MO Blurring to N,L MT The fuzzification is F; the output is: β1 is fuzzified to XL, and γ2 is fuzzified to M.

[0259] ⑧. Input terminal: L MO Blurring to N,L MT The output is: β1 is fuzzified to L, and γ2 is fuzzified to M.

[0260] 9. Input terminal: L MO Blurring to N,L MT The output is fuzzified to N; β1 is fuzzified to M, and γ2 is fuzzified to M.

[0261] The centroid method was used for subsequent defuzzification.

[0262] B-6, the control rules of the fused fuzzy logic controller are as follows: The design purpose of the fused fuzzy logic controller is to further improve the obstacle avoidance performance of the DWA algorithm in complex environments, which is based on L... MO and θ DA The input is δ, and the output is the fusion weight coefficient δ; the final weight coefficient combination of the DWA algorithm is shown in Formula 14.

[0263] Formula 14: (α,β,γ)=δ(α1,β2,γ1)+(1-δ)(α2,β1,γ2)

[0264] As shown in Formula 14, by adjusting the fusion weight coefficient, the mobile robot can determine the priority of the directional controller and the safety controller during the local path planning process.

[0265] Overview of control rules:

[0266] ①. When the mobile robot is far from the obstacle and the direction angle θ DA When the value is large, δ should be increased to enhance the priority of the directional fuzzy logic controller and drive the mobile robot to move quickly toward the target point.

[0267] ②. When the mobile robot approaches the obstacle and the direction angle θ DA When the value is large, obstacle avoidance should be prioritized, that is, δ should be reduced.

[0268] ③. When the mobile robot approaches the obstacle and the direction angle θ DA When the value is small, δ should be chosen as an appropriate value.

[0269] Control rule description:

[0270] There are a total of 8 control rules for the integrated fuzzy logic controller.

[0271] ①. Input terminal: L MO Blurring to F, θ DA Blur to SorB; Output: δ blur to XL;

[0272] ②. Input terminal: L MO Blurring to F, θ DA Blurring to XSorXB; Output: δ blurring to L;

[0273] ③. Input terminal: L MO Blurring to F, θ DA Blurring to M; Output: δ Blurring to M;

[0274] ④. Input terminal: L MO Blurring to M, θ DA Blurring to XSorXB; Output: δ blurring to L;

[0275] ⑤. Input terminal: L MO Blurring to M, θ DA Blurring to SorB; Output: δ Blurring to M;

[0276] ⑥. Input terminal: L MO Blurring to M, θ DA The output is fuzzyened to M; the δ-fuzzyened output is S.

[0277] ⑦. Input terminal: L MOBlurring to N, θ DA Blur the value to XS or XB; Output: δ is blurred to XS;

[0278] ⑧. Input terminal: L MO Blurring to N, θ DA The output is fuzzified to SorMorB; the output is δ fuzzified to S.

[0279] The centroid method was used for subsequent defuzzification.

[0280] C. Integration of VOA dynamic obstacle avoidance and FL-DWA algorithm:

[0281] Although the DWA algorithm with fuzzy logic (hereinafter referred to as FL-DWA) can improve the obstacle avoidance ability of mobile robots to unknown obstacles to a certain extent, it may still fail to avoid obstacles in complex dynamic obstacle environments. Therefore, it is proposed to combine VOA dynamic obstacle avoidance with FL-DWA to improve the dynamic obstacle avoidance safety of mobile robots.

[0282] VOA (Dynamic Obstacle Avoidance) is an algorithm that directly predicts collisions in a changing environment using velocity information. Its principle is to utilize the concept of velocity barriers, mapping the dynamic environment to the mobile robot's velocity space, and calculating a first-order approximation of the velocity at which the robot will collide with an obstacle at a given time interval. Using this representation, obstacle avoidance strategies can be easily implemented by selecting velocity tuples outside of velocity barriers.

[0283] The principle of VOA dynamic obstacle avoidance is as follows: In the global coordinate system XOY, the mobile robot M is located at the origin, and the mobile robot M moves at a velocity v m The robot moves forward, and the grid obstacle o moves forward at a speed of v0, where Δv is the speed of the mobile robot relative to the grid obstacle (or the sum of the speeds of the mobile robot and the grid obstacle). The grid obstacle is expanded into a circular obstacle of radius R (the circle being the smallest circumcircle that completely encloses the grid obstacle). T1 and T2 are the tangents from the origin to the two sides of the circular obstacle, and μ is the intersection of the tangent T1 or T2 and the straight line. The angle between the velocity Δv and the straight line, γ is the angle between the velocity Δv and the straight line. The angle between the two lines, μ and γ, are both positive. When the velocity Δv is between the tangents T1 and T2, the mobile robot will collide with the obstacle. The angle range of the collision is 2μ. The goal of dynamic obstacle avoidance is to keep Δv outside 2μ, even if γ > μ.

[0284] Based on v m The velocity vector triangle formed by v0 and Δv gives us Equations 14 and 15. Differentiating Equation 15 gives us Equation 16.

[0285] Formula 14:

[0286] Formula 15:

[0287] Formula 16:

[0288] In the formula, α is the orientation angle of the mobile robot (between the positive X-axis and the negative v-axis). m β is the orientation angle of the grid obstacle (the angle between the positive X-axis and v0). v is the velocity vector in the triangle. m The angle formed by η and Δv, where η is the angle formed by v0 and Δv in the velocity vector triangle, and θ is the angle between the positive X-axis and the straight line. The angle formed; d is the differential symbol; dγ = Δγ; dv m =Δv m ; dα=Δα; dβ=Δβ.

[0289] According to Formula 16, given the velocity v0 of the grid obstacle, the orientation angle β of the grid obstacle, the change in the grid obstacle's velocity Δv0, and the change in the grid obstacle's orientation angle Δβ, the mobile robot's velocity v can be adjusted. m The direction angle α of the mobile robot is used to change Δγ, so that the adjusted γ satisfies: γ + Δγ > μ; Δv m Let x1 and x2 be set as Δα respectively, and establish the dynamic obstacle avoidance objective function as shown in Equation 17. Solving Equation 17 will yield the velocity tuple that satisfies dynamic obstacle avoidance. Selecting this velocity tuple in the above FL-DWA algorithm will enable dynamic obstacle avoidance.

[0290] Formula 17:

[0291] The parameter definitions in Formula 14-17 are only applicable to Formula 14-17 and cannot be applied to other formulas.

[0292] The technical effects of this invention are as follows:

[0293] Two nuclear environment scenarios with different complexities were set up to verify the technical effects of the present invention. Scenario 1 (20m × 20m) had relatively lower complexity, while Scenario 2 (50m × 50m) had relatively higher complexity. Both Scenario 1 and Scenario 2 contained unknown dynamic or static obstacles. Scenario 1 contained seven radioactive sources with energies of 1.25, 1.25, 2.5, 1.25, 3.75, 1.25, and 1.25 MeV, respectively. Scenario 2 contained 20 radioactive sources, all with an energy of 1.25 MeV. All dynamic obstacles in both Scenario 1 and Scenario 2 underwent reciprocating linear motion. Information for the two scenarios is shown in Table 1.

[0294] Table 1:

[0295]

[0296]

[0297] In scenarios 1 and 2, four path planning methods (including those of this invention) are used for two-layer path planning (the two-layer path includes global paths and local paths). The global path planning algorithm is ACO-A*-PSO, the only difference being the local path planning method. The planning results are shown in Table 2, and the planned paths are shown in Table 3. Figure 8-9 .

[0298] Table 2:

[0299]

[0300] Combine Table 2 and Figure 8-9 It can be seen that only the path planning method provided by this invention can successfully plan paths in two scenarios with different complexities, while the other three path planning methods all have the problem of path planning failure in some or all scenarios.

Claims

1. A method for real-time obstacle avoidance and double-layer path planning of a mobile robot in a nuclear environment, characterized in that: The grid map of the nuclear environment is prior information, each grid in the grid map contains radiation field information and obstacle information, and the mobile robot is provided with a laser radar; The method is as follows: S01, a mathematical model of real-time obstacle avoidance path planning is established: the path planning target is to plan an optimal path for the mobile robot from the starting point to the target point; considering three sub-targets of path length, cumulative radiation dose and turning energy consumption, a target function of global path planning of the mobile robot is established; SO2, using ACO-A * -PSO algorithm to plan global path: first, the improved A* algorithm is constructed combined with the objective function, the initial pheromone distribution of ACO algorithm is processed by non-uniform treatment of the suboptimal path obtained by running the improved A* algorithm, then the ACO algorithm is constructed combined with the objective function, then the PSO algorithm is used to optimize the combination of the objective function and the parameters in the ACO algorithm, finally the ACO algorithm is run based on the parameter combination obtained by optimization, and the global path from the starting point to the target point is obtained; S03, a FL-DWA-VOA algorithm is used to plan a local path: firstly, all inflection points in the global path are defined as sub-navigation points, so as to divide the global path into a plurality of locally connected local paths, and then each local path is sequentially re-planned in the order from front to back; the path re-planning process is as follows: firstly, a DWA algorithm model is established, then fuzzy logic is introduced to realize dynamic parameter adjustment of the DWA algorithm, a FL-DWA algorithm with static obstacle avoidance effect is established, if the mobile robot does not find a dynamic obstacle, only the FL-DWA algorithm is run for local path re-planning; if the mobile robot finds a dynamic obstacle, a VOA dynamic obstacle avoidance algorithm is additionally called to avoid the dynamic obstacle; Fusion of VOA dynamic obstacle avoidance and FL-DWA algorithm: The VOA dynamic obstacle avoidance principle is as follows: in a global coordinate system XOY, a mobile robot M is located at the coordinate origin, the mobile robot M moves at a speed v m , a grid obstacle o moves at a speed v0, Δv is the speed of the mobile robot relative to the grid obstacle, the grid obstacle is inflated into a circular obstacle with a radius R, the circle is the minimum circumscribed circle completely containing the grid obstacle, T1 and T2 are tangent lines on both sides of the circular obstacle from the coordinate origin, μ is the included angle between the tangent line T1 or T2 and a straight line , γ is the included angle between the speed Δv and a straight line , and μ and γ are positive values. The VOA dynamic obstacle avoidance principle is as follows: in a global coordinate system XOY, a mobile robot M is located at the coordinate origin, the mobile robot M moves at a speed v m , a grid obstacle o moves at a speed v0, Δv is the speed of the mobile robot relative to the grid obstacle, the grid obstacle is inflated into a circular obstacle with a radius R, the circle is the minimum circumscribed circle completely containing the grid obstacle, T1 and T2 are tangent lines on both sides of the circular obstacle from the coordinate origin, μ is the included angle between the tangent line T1 or T2 and a straight line , γ is the included angle between the speed Δv and a straight line , and μ and γ are positive values. When the velocity Δv is located between the tangent T1 and the tangent T2, the mobile robot will collide with the obstacle, and the collision angle interval is 2μ; the dynamic obstacle avoidance target is to keep Δv always outside 2μ, that is, |γ|>μ; Based on v m The velocity vector triangle formed by v0 and Δv, formula 14 and formula 15 can be obtained. Equation 14: Equation 15: Equation 16: In the formula, α is the orientation angle of the mobile robot, and β is the orientation angle of the grid obstacle. v is the velocity vector in the triangle. m The angle formed by η and Δv, where η is the angle formed by v0 and Δv in the velocity vector triangle, and θ is the angle between the positive X-axis and the straight line. The angle formed; d is the differential sign; in addition, dγ = Δγ; dv m =Δv m dα = Δα; dβ = Δβ; According to formula 16, given the speed v0 of the grid obstacle, the direction angle β of the grid obstacle, the change Δv0 of the speed of the grid obstacle and the change Δβ of the direction angle of the grid obstacle, the change Δγ is changed by adjusting the speed v of the mobile robot and the direction angle α of the mobile robot, so that the adjusted γ satisfies: |γ+Δγ|>μ m and the direction angle α of the mobile robot, and Δv m and Δα are set as x1 and x2 respectively, a dynamic obstacle avoidance target function is established, as shown in formula 17, and the speed tuple satisfying the dynamic obstacle avoidance can be obtained by solving formula 17; in the FL-DWA algorithm described above, the speed tuple is selected, so that the dynamic obstacle avoidance is realized. Equation 17: Wherein, the parameters in formulas 14-17 are only used in formulas 14-17 and cannot be applied to other formulas.

2. The real-time obstacle avoidance double-layer path planning method for the mobile robot in the nuclear environment according to claim 1, wherein the grid in the grid map is a square, the radiation field information is recorded in each grid in a real number coding mode, and the real number is the average radiation dose rate of the grid; and the obstacle information is marked in each grid in a binary coding mode "1" and "0" to represent "obstacle" and "no obstacle" respectively.

3. The method of claim 2, wherein the method further comprises: determining a second path for the mobile robot to follow in the second layer based on the first path and the second layer; and determining a second set of obstacles in the second layer based on the first set of obstacles and the second layer. The S01 step is described in detail as follows: S01, a mathematical model of real-time obstacle avoidance path planning is established: A, problem description: Based on the operation scene of the mobile robot in the nuclear environment, in order to improve the operation efficiency of the mobile robot and guarantee the safety thereof, an optimal path from a starting point S(p n (x n ,y n )) to a target point E(p n (x n ,y n )) needs to be planned for the mobile robot. A-1. Definition of passability: different colors except for a specified color are used to distinguish different average radiation dose rate value intervals, and the specified color is used to represent "obstacle"; an eight-direction search strategy is used for path search when the mobile robot plans a path, and the grid coordinates are defined as the coordinates of the center points of the grids; if a grid meets the three conditions of "no obstacle in the grid", "the average radiation dose rate in the grid is lower than the threshold", and "the mobile robot does not collide with any obstacle when entering the grid", the grid is defined as a passable grid; if a grid cannot meet any of the above three requirements, the grid is defined as a non-passable grid; A passable path means that all the grids passed by the mobile robot are passable grids, and the set of passable grids is a passable area; A-2. Path planning objectives: when planning a global path for a mobile robot in a nuclear environment containing non-map obstacles, consider "obstacle avoidance", "high radiation risk avoidance", and "minimum cost minimization"; obstacle avoidance means that the mobile robot does not collide with any obstacle in the environment; high radiation risk avoidance means that the mobile robot is not allowed to pass through a grid with an average radiation dose rate higher than a threshold; cost minimization means that under the two hard constraint conditions of obstacle avoidance and high radiation risk avoidance, the path with the lowest cost is pursued, and the cost is composed of path length, cumulative radiation dose, and turning energy consumption; B. Mathematical modeling: Considering path length, cumulative radiation dose, and turning energy consumption, the objective function for planning a global path for a mobile robot in a nuclear environment containing non-map obstacles is established, as shown in Equation 1. M(p) = ω1 × L(p) + ω2 × R(p) + ω3 × E(p) Formula 1: In the formula, M(p) represents the target function, ω1 is a path length weight coefficient, ω2 is a cumulative radiation dose weight coefficient, ω3 is a turning energy consumption weight coefficient, L(p) is a path length sub-target function, R(p) is a cumulative radiation dose sub-target function, and E(p) is a turning energy consumption sub-target function; d(p i ,p i+1 ) is the distance from grid p i to grid p i+1 ; (x i ,y i ) is the coordinate of grid p i , (x i+1 ,y i+1 ) is the coordinate of grid p i+1 ; R(p i ,p i+1 ) is the cumulative radiation dose of the mobile robot from grid p i to grid p i+1 , is the average radiation dose rate of grid p i , is the average radiation dose rate of grid p i+1 , l is the distance from grid p i to grid p i+1 ; ε1 is a weight coefficient of a turning angle variable, ε2 is a weight coefficient of a turning number, ψ[l(p i-1 ,p i ), l(p i ,p i+1 )] is a turning angle variable of line segment l(p i-1 ,p i ) and line segment l(p i ,p i+1 ), l(p i-1 ,p i ) is a line connecting the center point of grid p i-1 and the center point of grid p i , and l(p i ,p i+1 ) is a line connecting the center point of grid p i and the center point of grid p i+1 ; N turn is the turning number.

4. The method of claim 3, wherein the method further comprises: determining a second path for the mobile robot to follow based on the second path being the shortest path from the second location to the goal location. S02 is described in detail as follows: SO2, using ACO-A * - PSO algorithm to plan global path: A. Construct an improved A* algorithm: A-1. Heuristic function: Considering the three sub-goals in mathematical modeling, the heuristic function of the improved A* algorithm is constructed, as shown in Equation 2. Equation 2: wherein g(p) is a path length cost value from the start point to the current grid, h(p) is a path length cost value between the current grid and the target point, g(p) and h(p) are calculated as L(p) in formula 1; R(p 1-n ) is a cumulative radiation dose cost value of the mobile robot from the start point to the target point, R(p 1-n ) is calculated as R(p) in formula 1, R(p 1-n ) is a sum of R(p 1-i ) and R(p i-n ), R(p 1-i ) is a cumulative radiation dose cost value of the mobile robot from the start point to the current grid, R(p i-n ) is a cumulative radiation dose cost value of the mobile robot from the current grid to the target point; E(p 1-n ) is a turning energy consumption cost value between the start point and the target point, E(p 1-n ) is calculated as E(p) in formula 1, E(p 1-n ) is a sum of E(p 1-i ) and E(p i-n ), E(p 1-i ) is a turning energy consumption cost value between the start point and the current grid, E(p i-n ) is a turning energy consumption cost value between the current grid and the target point; (x i ,y i ) is a coordinate of the current grid p i , (x1,y1) is a coordinate of the start point, (x n ,y n ) is a coordinate of the target point. A-2. Hazardous area avoidance strategy: During path search, when an ant senses that the average radiation dose rate of the next grid exceeds the threshold, the average radiation dose rate of the next grid is defined as infinity, prompting the ant to find other passable grids; thus, a hazardous area avoidance strategy is constructed, as shown in Equation 3. Equation 3: where R(p i ) is the radiation dose rate value of the grid p i , Inf is infinity, and Rl max is the radiation dose rate threshold value. B. Run the improved A* algorithm to obtain a sub-optimal path: Running the improved A* algorithm, output the result after the end of the calculation: suboptimal path P sp ; C. Use the sub-optimal path to non-uniformly process the initial pheromone of the ACO algorithm: P is a suboptimal path obtained by using the improved A* algorithm sp The initial pheromone in the ACO algorithm is unevenly processed; only the initial pheromone concentration of the grid through by the suboptimal path is increased, and the initial pheromone concentration of other grids is unchanged; as shown in formula 4; Equation 4: where i, j represent two edges adjacent or vertices contact passable grid, τ i,j is the initial pheromone concentration of the grid p i after the non-uniform treatment between p j , τ is the initial pheromone concentration set by man, and C is the initial pheromone enhancement value; D. Construct an improved ACO algorithm: D-1. Heuristic function: Considering the influence of the three sub-goals in mathematical modeling on the heuristic function of the ACO algorithm, an adaptive adjustment coefficient is introduced to balance the convergence speed and global search ability of the ACO algorithm, and the heuristic function of the ACO algorithm is constructed, as shown in Equation 5. Equation 5: wherein η t (i,j) is the heuristic function value from the grid p i to p j , i,j represent two edges adjacent or vertices contactable, Q1 is the heuristic function enhancement coefficient, L i,j is the distance from the grid i to the grid j, R i,j is the cumulative radiation dose received by the mobile robot when driving from the grid i to the grid j, E i,j is the turning energy consumption from the grid p i-1 to the grid p i to p j , I max is the maximum number of iterations, I c is the current number of iterations, ξ is the adaptive adjustment coefficient, and Q is the heuristic function enhancement coefficient. D-2. Selection probability function: Considering the pheromone concentration and the heuristic function, the selection probability function of the ACO algorithm is constructed, as shown in Equation 6. Equation 6: wherein P t (i,j) is the selection probability from grid p i to p j in the tth iteration, i,j represents two edges adjacent or vertices contacting the passable grid, and a is the pheromone importance factor and β is the heuristic function importance factor; τ t (i,j) represents the pheromone concentration from grid p i to p j when the tth iteration is completed, τ t (i,u) represents the pheromone concentration from grid p i to p u when the tth iteration is completed, η t (i,j) is the heuristic function value from grid p i to p j in the tth iteration, η t (i,u) is the heuristic function value from grid p i to p u in the tth iteration; T t ' is the set of grids not passed through before the tth iteration is performed; the denominator in the formula of formula 6 is the sum of the selection probabilities from grid p i to all grids not passed through in the tth iteration; if grid p j in formula 6 is a grid not passed through, the selection probability calculation of grid p i to p j is performed, and if grid p j in formula 6 is a grid already passed through, the selection probability of grid p i to p j is 0. D-3. Pheromone update rule: Combining the positive and negative feedback mechanisms with the three sub-goals in mathematical modeling, the pheromone update rule of the ACO algorithm is constructed, as shown in Equation 7. Equation 7: where τ t+1 (i,j) and τ t (i,j) represent the pheromone concentration between the grid p i and p j at the end of the t+1th and tth iteration, respectively, p is the pheromone evaporation factor, Q2 is the pheromone deposit factor, M t-mi is the total fitness of the three sub-goals of the optimal path obtained in the tth iteration, M t-ma is the total fitness of the three sub-goals of the worst path obtained in the tth iteration; P optimal is the optimal path obtained in the t+1th iteration, P worst is the worst path obtained in the t+1th iteration; E. Use the PSO algorithm for parameter combination optimization: E-1. Determine the parameter combination to be optimized: According to the objective function and the formula in the ACO algorithm, the parameter combination to be optimized is initially determined as (ω1, ω2, ω3, α, β, ρ); since (ω1, ω2, ω3) are multi-objective weight coefficients, a single-objective standardization method is used to simplify the number of multi-objective weight coefficients, set ω1 = 1, and obtain the approximate range of ω2 and ω3, finally determine the parameter combination to be optimized as (ω2, ω3, α, β, ρ); the position of the particle is evaluated by the objective function; E-2. Define the particle velocity update rule: The particle velocity update rule in the PSO algorithm is defined, as shown in Equation 8. Equation 8: wherein Gbest is the best position of the i-th particle in the entire swarm before the k-th iteration, k Gbest is the best position of all particles in the entire swarm before the k-th iteration, ω is the inertia weight, c1 and c2 are learning factors, and r1 and r2 are random numbers between 0 and 1; and are the positions of the i-th particle after the k-th and k+1-th iterations, respectively; and are the velocities of the i-th particle during the k-th and k+1-th iterations, respectively, [-v max ,v max ] is the velocity range of the particle. E-3. Obtain the optimized parameter combination: Running PSO algorithm, the target function is used as fitness function to evaluate the pros and cons of particle position during running, and finally the optimized parameter combination (ω2, ω3, α, β, ρ) is obtained; F, based on the optimized parameter combination to obtain the global path: The optimized parameter combination is brought into the ACO algorithm, and the ACO algorithm is run to obtain the global path from the starting point to the target point.

5. The method of claim 4, wherein the method further comprises: determining a second path for the mobile robot to follow based on the second set of data. S03 is described in detail as follows: S03, FL-DWA-VOA algorithm is used to plan local path: A, the DWA algorithm model is established: A-1. Velocity sampling space analysis: considering kinematic constraints, acceleration constraints and obstacle constraints, the velocity sampling space of DWA algorithm is established; ①. The linear and angular velocities of the mobile robot are limited within a certain range due to the motion performance of the robot itself; the velocity sampling space V under the kinematic constraint m is defined as Equation 9; Equation 9: where V m is the velocity sample space under kinematic constraints, (v, ω) is a velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, v min is the minimum linear velocity of the mobile robot, v max is the maximum linear velocity of the mobile robot, ω max is the maximum angular velocity of the mobile robot, ω min is the minimum angular velocity of the mobile robot; ②. The variable speed performance of the mobile robot is bounded, and considering that excessive acceleration or deceleration of the mobile robot during movement will lead to motor overload and rapid wear, the acceleration of the mobile robot needs to be constrained; the velocity sampling space V under acceleration constraint d is defined as formula 10; Equation 10: where V d is the velocity sample space under acceleration constraints, (v, ω) is a velocity tuple, v is the linear velocity of the mobile robot, and ω is the angular velocity of the mobile robot, v t is the linear velocity of the mobile robot at time t, ω t is the angular velocity of the mobile robot at time t, Δt is the sampling time interval, v m is the maximum linear acceleration of the mobile robot, and ω mˋ is the maximum angular acceleration of the mobile robot. ③. Mobile robot needs to keep in a safe speed range to ensure that it can brake within a safe distance when encountering dynamic or static obstacles not marked on the map; the velocity sampling space under obstacle constraint is defined as formula 11; Equation 11: where V s is the velocity sample space under obstacle constraints, (v, ω) is a velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, dist min (v, ω) represents the distance from the end of the path corresponding to (v, ω) to the nearest obstacle. A-2. Path evaluation function establishment: first, the path of the next sampling period of mobile robot is obtained, and then all paths are evaluated by using path evaluation function, the optimal path corresponding to the velocity element is selected and executed, and the path evaluation function is shown in formula 12; Formula 12: G(v, ω) = α·heading(v, ω) + β·dist(v, ω) + γ·velocity(v, ω) In the formula, G(v, ω) is the path evaluation function; heading(v, ω) is the deflection angle evaluation sub function; dist(v, ω) is the safety factor evaluation sub function; velocity(v, ω) is the speed evaluation sub function; α, β, γ are the weight coefficients of the above three sub functions respectively; The three sub functions in formula 12 are normalized, and the three sub functions have the same scale in the path evaluation function through normalization, as shown in formula 13; Equation 13: In the formula, n is the maximum serial number of the path of a sampling period, i is the current path to be evaluated of a sampling period, heading(v i ,ω i ) is the deflection angle evaluation sub-function under the current path to be evaluated, dist(v i ,ω i ) is the safety coefficient evaluation sub-function under the current path to be evaluated, and velocity(v i ,ω i ) is the speed evaluation sub-function under the current path to be evaluated. B, fuzzy logic is introduced to realize dynamic parameter adjustment of DWA algorithm: According to the principle of DWA algorithm, (α, β, γ) in path evaluation function is the parameter that needs to be dynamically adjusted, the position of target point affects the values of α and γ, and the position of obstacle affects the values of β and γ; B-1, the basic frame of fuzzy logic is described as follows: first, design the direction fuzzy logic controller and the safety fuzzy logic controller; the direction fuzzy logic controller inputs (L MT , θ DA ), and outputs a set of weight parameters (α1, γ1); the safety fuzzy logic controller inputs (L MO , L MT ), and outputs a set of weight parameters (β1, γ2); Then a fusion fuzzy logic controller is designed to dynamically fuse the two and output the final fusion weight coefficient δ; The input quantities of the three fuzzy logic controllers include: the distance L between the mobile robot and the obstacle MO , the distance L between the mobile robot and the target sub-navigation point MT , and the direction angle θ of the mobile robot DA ; the output quantities of the three fuzzy logic controllers include: the weight coefficient α of the deflection angle evaluation sub-function, the weight coefficient β of the safety coefficient evaluation sub-function, the weight coefficient γ of the speed evaluation sub-function, and the fusion weight coefficient δ; α1∈α, β1∈β, γ1∈γ, γ2∈γ; B-2, the input quantity of the fuzzy logic (FL) is fuzzed as follows: the input quantities L MO , L MT and θ DA are fuzzed; wherein the domain of L MO is [0, 2.5], the fuzzy set is [N, M, F], N is near, M is middle, and F is far, when L MO < 0.5, it is fuzzed as N, when 0.5 < L MO < 1.25, it is fuzzed as M, when L MO > 1.25, it is fuzzed as F; the domain of L MT is [0, 3], the fuzzy set is [N, M, F], N is near, M is middle, and F is far, when L MT < 0.5, it is fuzzed as N, when 0.5 < L MT < 1.5, it is fuzzed as M, when L MT > 1.5, it is fuzzed as F; the domain of θ DA is [-180°, 180°], the fuzzy set is [XS, S, M, B, XB], XS is negative big, S is negative, M is middle, B is positive, and XB is positive big, when θ DA < -120°, it is fuzzed as XS, when -120° < θ DA < -60°, it is fuzzed as S, when -60° < θ DA < 60°, it is fuzzed as M, when 60° < θ DA < 120°, it is fuzzed as B, when θ DA > 120°, it is fuzzed as XB. B-3, the output of the fuzzy logic is processed as follows: α, β, γ and δ are fuzzified; the definition domain of α, β, γ and δ is [0, 1], the fuzzy set of α, β and δ is [XS, S, M, L, XL], XS is minimum, S is small, M is medium, L is large, and XL is maximum, when 0 < α < 0.1 or 0 < β < 0.1 or 0 < δ < 0.1, the fuzzy set is XS, when 0.1 < α < 0.3 or 0.1 < β < 0.3 or 0.1 < δ < 0.3, the fuzzy set is S, when 0.3 < α < 0.6 or 0.3 < β < 0.6 or 0.3 < δ < 0.6, the fuzzy set is M, when 0.6 < α < 0.9 or 0.6 < β < 0.9 or 0.6 < δ < 0.9, the fuzzy set is L, and when 0.9 < α < 1 or 0.9 < β < 1 or 0.9 < δ < 1, the fuzzy set is XL; the fuzzy set of γ is [S, M, L], S is small, M is medium, and L is large, when γ < 0.3, the fuzzy set is S, when 0.3 < γ < 0.7, the fuzzy set is M, and when γ > 0.7, the fuzzy set is L; B-4, the control rules of the direction fuzzy logic controller are as follows: the control rules of the direction fuzzy logic controller are 9 in total; MT Fuzzed to F, θ DA Fuzzed to M; output: α1fuzzed to XL, γ1fuzzed to L;​ ii. Input: L MT Fuzzed to F, θ DA Fuzzed to Sor B Output: α1fuzzed to S, γ1fuzzed to L; MT Fuzzy to F, Θ DA Fuzzy to XSor XB ; output: α1fuzzy to M, γ1fuzzy to L;​ • Input: L MT • Fuzzed to M, θ DA • Fuzzed to M; Output: α1 Fuzzed to M, γ1 Fuzzed to M; • input: L MT • fuzzed to M, θ DA • fuzzed to Sor B • output: α1fuzzed to L, γ1fuzzed to M (6) input: L MT fuzzed to M, θ DA fuzzed to XSorXB; output: α1fuzzed to L, γ1fuzzed to M; • input: L MT • fuzzed to N, θ DA • fuzzed to M; output: α1 fuzzed to M, γ1 fuzzed to S; 8. Input: L MT Fuzzed to N, θ DA Fuzzed to SorB; Output: α1 fuzzed to L, γ1 fuzzed to S; 9. Input: L MT Fuzzed to N, θ DA Fuzzed to XSorXB; Output: α1fuzzed to XL, γ1fuzzed to S; The subsequent defuzzification adopts the gravity method; B-5, the control rules of the safety fuzzy logic controller are as follows: the control rules of the safety fuzzy logic controller are 9 in total, and the defuzzification method is the gravity method; MO Fuzzy to F, L MT Fuzzy to F; output: β1fuzzy to XS, γ2fuzzy to L;​ ii. Input: L MO Fuzzed to F, L MT Fuzzed to M; Output: β1 Fuzzed to S, γ2 Fuzzed to L; MO Fuzzy to F, L MT Fuzzy to N; output: β1 Fuzzy to S, γ2 Fuzzy to S;​ IV. Input: L MO Fuzzed to M, L MT Fuzzed to F; Output: β1 Fuzzed to M, γ2 Fuzzed to L; • Input: L MO • Fuzzed to M, L MT • Fuzzed to M; Output: β1 Fuzzed to M, γ2 Fuzzed to M; (6. Input: L MO Fuzzed to M, L MT Fuzzed to N; Output: β1 Fuzzed to M, γ2 Fuzzed to S; • Input: L MO • Fuzzed to N, L MT • Fuzzed to F; Output: β1 Fuzzed to XL, γ2 Fuzzed to M; 8. Input: L MO Fuzzed to N, L MT Fuzzed to M; Output: β1fuzzed to L, γ2fuzzed to M; 9. Input: L MO Fuzzed to N, L MT Fuzzed to N; Output: β1 Fuzzed to M, γ2 Fuzzed to M; The subsequent defuzzification adopts the gravity method; B-6, the control rules of the fusion fuzzy logic controller are as follows: the control rules of the fusion fuzzy logic controller are 8 in total; MO Fuzzy to F, θ DA Fuzzy to Sor B Output: δ Fuzzy to XL;​ ii. Input: L MO Fuzzed to F, θ DA Fuzzed to XSorXB; Output: δ Fuzzed to L; MO Fuzzy to F, θ DA Fuzzy to M; output: δ Fuzzy to M;​ IV. Input: L MO fuzzed to M, Θ DA fuzzed to XSor XB ; Output: δ fuzzed to L; • Input: L MO • Fuzzed to M, θ DA • Fuzzed to Sor B; Output: δ Fuzzed to M; • Input: L MO • Fuzzed to M, θ DA • Fuzzed to M; Output: δ Fuzzed to S; • Input: L MO • Fuzzed to N, θ DA • Fuzzed to XSorXB; Output: δ Fuzzed to XS; 8. Input: L MO Fuzzed to N, θ DA Fuzzed to SorMor B Output: δ Fuzzed to S; The subsequent defuzzification adopts the gravity method.

Images