Mobile robot real-time obstacle avoidance double-layer path planning method in nuclear environment

Abstract

According to the real-time obstacle avoidance double-layer path planning method for the mobile robot in the nuclear environment, a grid map of the nuclear environment is prior information, each grid in the grid map comprises radiation field information and obstacle information, and a laser radar is carried on the mobile robot; the method comprises the following steps: S01, establishing a mathematical model for real-time obstacle avoidance path planning; s02, adopting an ACO-A *-PSO algorithm to plan a global path; and S03, an FL-DWA-VOA algorithm is adopted to plan a local path. The method is suitable for mobile robot planning in a multi-information-field coupling environment with unknown dynamic or static obstacles, firstly, an ACO-A *-PSO algorithm is adopted to plan a global path, inflection points on the global path are selected as sub-navigation points, then an FL-DWA-VOA algorithm is called to perform local path re-planning, a final path is formed, and the final path is obtained. And the final path also has the capability of avoiding unknown static or dynamic obstacles on the premise of ensuring global optimum.

Description

Technical Field

[0001] The invention relates to a robot path planning method, in particular to a real-time obstacle avoidance double-layer path planning method for a mobile robot in a nuclear environment. Background Art

[0002] When mobile robots perform inspection and monitoring tasks in a nuclear environment, they need to plan their paths in advance to improve work efficiency.

[0003] First, due to the presence of radioactive sources in the nuclear environment, when doing global path planning, one cannot only consider the path length, but also the radiation risk, which increases the difficulty of global path planning.

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

[0005] At present, no path planning algorithm has been found to meet the above two points at the same time. It is difficult to simultaneously obtain the global optimal path and have dynamic obstacle avoidance function, that is, it is not suitable for path planning of mobile robots in complex nuclear environments. Summary of the invention

[0006] The purpose of the present invention is to overcome the shortcomings of the prior art and to provide a two-layer path planning method for real-time obstacle avoidance of a mobile robot in a nuclear environment. The method is suitable for path planning of a mobile robot in a complex nuclear environment and solves the problem that the existing path planning algorithm is difficult to simultaneously obtain the global optimal path and have dynamic obstacle avoidance function.

[0007] The technical solution of the present invention is: a real-time obstacle avoidance double-layer path planning method for a mobile robot in a nuclear environment, wherein a 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 laser radar;

[0008] Here’s how:

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

[0010] S02, using ACO-A *-PSO algorithm plans the global path: first, build an improved A* algorithm based on the objective function, use the suboptimal path obtained by running the improved A* algorithm to unevenly process the initial pheromone distribution of the ACO algorithm, then build the ACO algorithm based on the objective function, then use the PSO algorithm to optimize the objective function and the parameters in the ACO algorithm, and finally run the ACO algorithm based on the parameter combination obtained to obtain the global path from the starting point to the target point;

[0011] S03, using the FL-DWA-VOA algorithm to plan the local path: first define all turning points in the global path as sub-navigation points, thereby dividing the global path into multiple local paths connected in sequence, and then replanning the path for each local path in order from front to back; the path replanning process is: first establish the DWA algorithm model, then introduce fuzzy logic to realize the dynamic parameter adjustment of the DWA algorithm, and establish the FL-DWA algorithm with static obstacle avoidance effect. If the mobile robot does not find a dynamic obstacle, only the FL-DWA algorithm is run for local path replanning; if the mobile robot finds a dynamic obstacle, the VOA dynamic obstacle avoidance algorithm is additionally called to avoid the dynamic obstacle.

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

[0013] A further technical solution of the present invention is: Step S01 is described in detail as follows:

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

[0015] A. Problem description:

[0016] Based on the operation scenario of the mobile robot in the nuclear environment, in order to improve the operation efficiency of the mobile robot and ensure its safety, it is necessary to plan a path from the starting point S(p1(x1,y1)) to the target point E(p n (x n ,y n ))'s optimal path;

[0017] A-1. Define passability: Use different colors other than the specified color to distinguish the average radiation dose rate in different numerical intervals, use the specified color to indicate "there are obstacles", and other grids except the specified color indicate "no obstacles"; when the mobile robot performs path planning, an eight-direction search strategy is used for path search, and the grid coordinates are defined as the coordinates of the center point of the grid; if a grid simultaneously meets the three conditions of "no obstacles 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 obstacles when entering the grid", the grid is defined as a passable grid; if a grid does not meet any of the above three requirements, the grid is defined as an inaccessible grid; a passable path means that all grids passed by the mobile robot are passable grids, and the set of passable grids is a passable area;

[0018] A-2. Path planning objectives: When planning a global path for a mobile robot in a nuclear 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 obstacle 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 pursuing the path with the lowest cost under the two hard constraints of obstacle avoidance and high radiation risk avoidance. The cost is composed of path length, cumulative radiation dose and turning energy consumption.

[0019] B. Mathematical modeling:

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

[0021] Formula 1:

[0022] Where 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 grid p i To grid p i+1 The distance (x i ,y i ) is the grid p i The coordinates of (x i+1 ,y i+1 ) is the grid p i+1 Coordinates of R(p i ,p i+1) is the mobile robot from the grid p i Travel to grid p i+1 The cumulative radiation dose, is the grid p i The average radiation dose rate, is the grid p i+1 The average radiation dose rate of the grid p i To grid p i+1 distance; ε1 is the weight coefficient of the steering angle variable, ε2 is the weight coefficient of the number of turns, ψ[l(p i-1 ,p i ),l(p i ,p i+1 )] is the line segment l(p i-1 ,p i ) and line segment l(p i ,p i+1 ) steering angle variable, l(p i-1 ,p i ) is the grid p i-1 The center point of the grid p i The line connecting the center points of i ,p i+1 ) is the grid p i The center point of the grid p i+1 The line connecting the center points of turn is the number of turns.

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

[0024] S02, with ACO-A * -PSO algorithm plans the global path:

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

[0026] A-1. Heuristic function:

[0027] Considering the three sub-goals in mathematical modeling, the heuristic function of the improved A* algorithm is constructed, as shown in Formula 2;

[0028] Formula 2:

[0029] In the formula, g(p) is the cost value of the path length from the starting point to the current grid, h(p) is the cost value of the path length from the current grid to the target point, g(p) and h(p) are calculated by referring to L(p) in formula 1; R(p 1-n ) is the cumulative radiation dose cost of the mobile robot from the starting point to the target point, R(p 1-n ) Refer to R(p) in Formula 1 for calculation, R(p 1-n) is R(p 1-i ) and R(p i-n ), R(p 1-i ) is the cumulative radiation dose cost of the mobile robot from the starting point to the current grid, R(p i-n ) is the cumulative radiation dose cost of the mobile robot from the current grid to the target point; E(p 1-n ) is the turning energy cost between the starting point and the target point, E(p 1-n ) Refer to E(p) in Formula 1 for calculation, E(p 1-n ) is E(p 1-i ) and E(p i-n ), E(p 1-i ) is the turning energy cost between the starting point and the current grid, E(p i-n ) is the turning energy cost between the current grid and the target point; (x i ,y i ) is the current grid p i The coordinates of the starting point are (x1, y1), n ,y n ) are the coordinates of the target point;

[0030] A-2. Dangerous area avoidance strategy:

[0031] In the path search process, when the ant perceives that the average radiation dose rate of the next grid exceeds the threshold, the average radiation dose rate cost value of the next grid is defined as infinity, prompting the ant to find other passable grids; thus constructing a dangerous area avoidance strategy, as shown in Formula 3;

[0032] Formula 3:

[0033] Among them, R(p i ) is the grid p i The radiation dose rate substitute value, Inf is infinite, RI max is the radiation dose rate threshold;

[0034] B. Run the improved A* algorithm to obtain the suboptimal path:

[0035] Run the improved A* algorithm and output the result after the calculation is completed: suboptimal path P sp ;

[0036] C, using suboptimal path non-uniformization to process the initial pheromone of the ACO algorithm:

[0037] The suboptimal path P obtained by running the improved A* algorithm spThe initial pheromone in the ACO algorithm is processed unevenly; only the initial pheromone concentration of the grid through which the suboptimal path passes is increased, and the initial pheromone concentration of other grids remains unchanged; as shown in Formula 4;

[0038] Formula 4:

[0039] Among them, i and j represent two passable grids with adjacent edges or touching vertices, τ i,j is the grid p i With p j The initial pheromone concentration after the inhomogeneous treatment between τ and C is the initial pheromone concentration set artificially, 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-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 capability of the ACO algorithm, and the heuristic function of the ACO algorithm is constructed, as shown in Formula 5.

[0043] Formula 5:

[0044] Among them, η t (i,j) is the tth iteration from grid p i to p j The heuristic function value of i, j represents the passable grid with two adjacent edges or touching vertices, Q1 is the heuristic function enhancement coefficient, L i,j is the distance from grid i to grid j, R i,j is the cumulative radiation dose received by the mobile robot when it moves from grid i to grid j, E i,j For the mobile robot to move from the grid p i-1 To grid p i to p j The turning energy consumption, I max is the maximum number of iterations, I c is the current iteration number, ξ is the adaptive adjustment coefficient, and Q is the heuristic function enhancement coefficient;

[0045] D-2. Select the probability function:

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

[0047] Formula 6:

[0048] Where P t(i,j) is the tth iteration from grid p i to p j The selection probability of i, j represents the passable grids with two adjacent edges or touching vertices, α is the pheromone importance factor, β is the heuristic function importance factor; τ t (i,j) indicates the number of cells from grid p when the tth iteration is completed. i to p j The pheromone concentration, τ t (i,u) indicates the number of cells from grid p when the tth iteration is completed. i to p u The pheromone concentration, η t (i,j) is the tth iteration from grid p i to p j The heuristic function value, η t (i,u) is the tth iteration from grid p i to p u The heuristic function value of t ′ is the set of grids that have not been passed before the tth iteration; the denominator in Formula 6 is the number of grids from the tth iteration i to the sum of the selection probabilities of all grids that have not been passed; if the grid p in formula 6 j It is a grid that has not been passed, and then the grid p i to p j The selection probability is calculated if the grid p in formula 6 j is a grid that has been passed, then grid p i to p j The probability of selection is 0;

[0049] D-3.Pheromone update rules:

[0050] Combining the positive and negative feedback mechanism with the three sub-goals 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 grid p at the completion of the t+1th and tth iterations respectively. i With p j ρ is the pheromone volatilization factor, Q2 is the pheromone deposition factor, M t-mi is the total cost of the three sub-goals of the optimal path obtained in the tth iteration, M t-ma is the total cost 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, Pworst is the worst path obtained in the t+1th iteration;

[0053] E. Use PSO algorithm to optimize parameter combination:

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

[0055] According to the objective function and the formula in the ACO algorithm, the parameter combination to be optimized is preliminarily determined to be (ω1, ω2, ω3, α, β, ρ). Since (ω1, ω2, ω3) are multi-objective weight coefficients, the single-objective standardization method is used to simplify the number of multi-objective weight coefficients. Set ω1 = 1 to obtain the approximate range of ω2 and ω3, and finally determine the parameter combination to be optimized as (ω2, ω3, α, β, ρ); subsequently, the quality of the particle position is evaluated through the objective function;

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

[0057] Define the update rule of particle velocity in the PSO algorithm, as shown in Formula 8;

[0058] Formula 8:

[0059] In the formula, Gbest is the optimal position of the i-th particle in the entire particle swarm in k iterations. k is the optimal position of all particles in the entire particle swarm after k iterations, ω is the inertia weight, c1 and c2 are learning factors, r1 and r2 are random numbers between [0,1]; and are the positions of the i-th particle at the k-th and k+1-th iterations respectively; and are the velocities of the i-th particle at the k-th and k+1-th iterations, respectively, [-v max ,v max ] is the velocity range of the particles;

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

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

[0062] F, obtain the global path based on the optimized parameter combination:

[0063] 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.

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

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

[0066] A. Establish DWA algorithm model:

[0067] A-1. Velocity sampling space analysis: Considering kinematic constraints, acceleration constraints and obstacle constraints, the velocity sampling space of the DWA algorithm is established;

[0068] ①. Due to the limitation of the mobile robot’s own motion performance, its linear velocity and angular velocity are limited to a certain range; the velocity sampling space V under kinematic constraints m It is defined as formula 9;

[0069] Formula 9:

[0070] Where V m is the velocity sampling space under kinematic constraints, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and 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;

[0071] ②. The speed change performance of the mobile robot is limited. In addition, considering that excessive acceleration or deceleration during the movement of the mobile robot will cause motor overload and rapid wear, it is necessary to constrain the acceleration of the mobile robot; the speed sampling space V under the acceleration constraint d It is defined as formula 10;

[0072] Formula 10:

[0073] Where V d is the velocity sampling space under acceleration constraint, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and 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 value of the linear acceleration of the mobile robot, ω m` is 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 not marked on the map; the speed sampling space under obstacle constraints is defined as formula 11;

[0075] Formula 11:

[0076] Where V s is the velocity sampling space under obstacle constraints, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and dist 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 speed tuple corresponding to the optimal path, and the path evaluation function is shown in Formula 12;

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

[0079] Where 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 so that the three sub-functions have the same scale in the path evaluation function, as shown in formula 13;

[0081] Formula 13:

[0082] Where 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 subfunction under the current path to be evaluated, dist(v i ,ω i ) is the safety factor evaluation subfunction under the current path to be evaluated, velocity(v i ,ω i ) is the speed evaluation subfunction under the current path to be evaluated;

[0083] B. Introducing fuzzy logic to achieve dynamic parameter adjustment of DWA algorithm:

[0084] According to the principle of DWA algorithm, (α, β, γ) in the path evaluation function are parameters that need to be adjusted dynamically. 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 direction fuzzy logic controller and the safety fuzzy logic controller; the direction fuzzy logic controller input (L MT ,θ DA ), output a set of weight parameters (α1,γ1); the safety fuzzy logic controller input (L MO ,L MT ), output a set of weight parameters (β1,γ2); then design a fusion fuzzy logic controller 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 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∈γ;

[0086] B-2, the fuzzy logic (FL) input fuzzification process is as follows: the input L MO , L MT and θ DA Fuzzy processing is performed; among them, L MO The domain is [0, 2.5], the fuzzy set is [N, M, F], N is near, M is medium, F is far, when L MO <0.5, the fuzzy value is N, when 0.5<L MO When <1.25, it is fuzzy to M. MO >1.25, fuzzy to F; L MT The domain is [0,3], the fuzzy set is [N,M,F], N is near, M is medium, F is far, when L MT <0.5, the fuzzy value is N, when 0.5<L MT When <1.5, it is fuzzy to M. MT When >1.5, it is blurred to F; θ DA The domain is [-180°, 180°], and the fuzzy set is [XS, S, M, B, XB], where XS is negative, S is negative, M is medium, B is positive, and XB is positive. DA <-120°, the blur is XS, when -120°<θ DA<-60°, the fuzzy value is S. When -60°<θ DA <60°, the fuzzy state is M. When 60°<θ DA When θ is less than 120°, it is blurred to B. DA When >120°, the blur becomes XB;

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

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

[0089] ①. Input terminal: L MT Fuzzy F, θ DA Fuzzy to M; Output: α1 is fuzzy to XL, γ1 is fuzzy to L;

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

[0091] ③. Input terminal: L MT Fuzzy F, θ DA Fuzzy to XSorXB; Output: α1 is fuzzy to M, γ1 is fuzzy to L;

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

[0093] ⑤. Input terminal: L MT Fuzzy to M, θ DAFuzzy to SorB; Output: α1 is fuzzy to L, γ1 is fuzzy to M;

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

[0095] ⑦. Input terminal: L MT Fuzzy N, θ DA Fuzzy to M; Output: α1 is fuzzy to M, γ1 is fuzzy to S;

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

[0097] ⑨. Input terminal: L MT Fuzzy N, θ DA Fuzzy to XSorXB; Output: α1 is fuzzy to XL, γ1 is fuzzy to S;

[0098] The subsequent defuzzification adopts the centroid method;

[0099] B-5, the control rules of the safety fuzzy logic controller are as follows: There are 9 control rules of the safety fuzzy logic controller;

[0100] ①. Input terminal: L MO Fuzzy to F,L MT Fuzzy to F; output: β1 is fuzzy to XS, γ2 is fuzzy to L;

[0101] ②. Input terminal: L MO Fuzzy to F,L MT Fuzzy to M; Output: β1 is fuzzy to S, γ2 is fuzzy to L;

[0102] ③. Input terminal: L MO Fuzzy to F,L MT Fuzzy to N; Output: β1 fuzzy to S, γ2 fuzzy to S;

[0103] ④. Input terminal: L MO Fuzzy to M,L MT Fuzzy to F; Output: β1 is fuzzy to M, γ2 is fuzzy to L;

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

[0105] ⑥. Input terminal: L MO Fuzzy to M,L MT Fuzzy to N; Output: β1 is fuzzy to M, γ2 is fuzzy to S;

[0106] ⑦. Input terminal: L MO Fuzzy to N,L MT Fuzzy to F; output: β1 is fuzzy to XL, γ2 is fuzzy to M;

[0107] ⑧. Input terminal: L MO Fuzzy to N,L MT Fuzzy to M; Output: β1 is fuzzy to L, γ2 is fuzzy to M;

[0108] ⑨. Input terminal: L MO Fuzzy to N,L MT Fuzzified to N; Output: β1 is fuzzified to M, γ2 is fuzzified to M;

[0109] The subsequent defuzzification adopts the centroid method;

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

[0111] ①. Input terminal: L MO Fuzzy F, θ DA Fuzzified to SorB; Output: δ fuzzified to XL;

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

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

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

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

[0116] ⑥. Input terminal: L MO Fuzzy to M, θ DA Fuzzy to M; Output: δ fuzzy to S;

[0117] ⑦. Input terminal: L MO Fuzzy N, θ DA Fuzzy to XSorXB; Output: δ fuzzy to XS;

[0118] ⑧. Input terminal: L MO Fuzzy N, θ DA Fuzzified to SorMorB; Output: δ fuzzified to S;

[0119] The subsequent defuzzification adopts the centroid method;

[0120] C, Fusion of VOA dynamic obstacle avoidance and FL-DWA algorithm:

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

[0122] Based on v m , v0 and Δv form a velocity vector triangle, we can get formula 14 and formula 15. By taking the derivative of formula 15, we get formula 16;

[0123] Formula 14:

[0124] Formula 15:

[0125] Formula 16:

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

[0127] According to formula 16, the speed v0 of the grid obstacle, the direction angle β of the grid obstacle, the change Δv0 of the grid obstacle speed and the change Δβ of the grid obstacle direction angle are known. By adjusting the speed v0 of the mobile robot m and the direction angle α of the mobile robot to change Δγ, so that the adjusted γ satisfies: γ+Δγ>μ; m and Δα are set to x1 and x2 respectively, and a dynamic obstacle avoidance objective function is established, as shown in Formula 17. Solving Formula 17 can obtain a speed tuple that satisfies dynamic obstacle avoidance. Selecting this speed tuple in the above FL-DWA algorithm can achieve dynamic obstacle avoidance.

[0128] Formula 17:

[0129] The parameter definitions in formula 14-17 are only applicable in 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 suitable for mobile robot planning in a multi-information field coupling environment with unknown dynamic or static obstacles. It first uses ACO-A * -The PSO algorithm plans the global path, selects the turning 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 the global optimality, but also has the ability to avoid unknown static or dynamic obstacles.

[0132] 2. It established a two-layer hybrid path planning method that integrates global planning and local planning, using the global planning path to guide local obstacle avoidance, solving the problem that the local path planning algorithm falls into local optimality due to the lack of global environmental information, and providing a mobile robot in a nuclear environment with an optimal path that guarantees global optimality and can effectively avoid unknown static or dynamic obstacles.

[0133] 3. It proposes an adaptive DWA local obstacle avoidance method (FL-DWA) based on fuzzy logic. The algorithm can dynamically adjust the weight coefficient of the evaluation function according to the environmental characteristics to improve the obstacle avoidance ability 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, which improves the ability to avoid dynamic obstacles.

[0134] The present invention is further described below in conjunction with the figures and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

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

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

[0137] Figure 3 It is a diagram describing the technical problem of the present invention;

[0138] Figure 4 Schematic diagram of the speed sampling space in the DWA algorithm;

[0139] Figure 5 It is the path diagram of different speed tuples in the DWA algorithm;

[0140] Figure 6 It is the principle diagram of FL-DWA algorithm;

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

[0142] Figure 8 It is a path planning diagram for a dynamic nuclear environment scenario with low complexity;

[0143] Fig. 9 This is a path planning diagram for a dynamic nuclear environment scenario with high complexity. DETAILED DESCRIPTION

[0144] Embodiment 1:

[0145] A two-layer path planning method for real-time obstacle avoidance of a mobile robot in a nuclear environment, wherein a grid map of the nuclear environment is prior information, the grids in the grid map are square, each grid in the grid map 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 in each grid with binary codes "1" and "0" to respectively identify "there are obstacles that the mobile robot cannot cross" and "there are no obstacles that the mobile robot cannot cross", and the mobile robot is equipped with a laser radar for real-time perception of "obstacles not in the map (including dynamic obstacles 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] There are two operating scenarios for mobile robots in nuclear power plants: Ⅰ. In daily operation, the robot moves to a specific work area to perform monitoring or maintenance tasks; Ⅱ. After a nuclear accident occurs, the robot moves to the leakage point for monitoring or emergency operations. In order to improve the operating efficiency of the mobile robot and ensure its safety, it is necessary to plan a path from the starting point S(p1(x1,y1)) to the target point E(p n (x n ,y n ))'s optimal path, see Figure 3 In order to better describe the path planning problem, the following explanation is given.

[0150] A-1. Defining Passability:

[0151] See also Figure 3 , the figure shows the grid map of the indoor nuclear environment. In order to intuitively display the radiation field information, different colors are used to distinguish the average radiation dose rate in different numerical ranges (gradually increasing according to the rule of lake blue-beige-bright yellow-pink-dark red). In order to more intuitively display the obstacle information (inherent obstacles in the environment), black is used to indicate "there are obstacles that the mobile robot cannot cross", and other grids except black indicate "there are no obstacles that the mobile robot cannot cross". When the mobile robot does path planning, it uses an eight-direction search strategy for path search, that is, when the mobile robot moves from the current grid point to the adjacent grid, it can only move along the straight line path of "current grid center point-adjacent grid center point", and the grid coordinates are defined as the coordinates of the grid center point. If a grid meets the three conditions of "there are no obstacles that the mobile robot cannot cross 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 obstacles when entering the grid", the grid is defined as a passable grid; if a grid does not meet any of the above three requirements, the grid is defined as an inaccessible grid. A traversable path means that all grids passed by the mobile robot are traversable grids, the set of traversable grids is the traversable area, and the direction from the current position of the mobile robot to the traversable point is the traversable direction.

[0152] A-2. Path planning objectives:

[0153] When planning a global path for a mobile robot in a nuclear environment containing obstacles not in the map, three aspects are considered: "obstacle avoidance", "high radiation risk avoidance" and "cost minimization". Obstacle avoidance is a hard constraint, that is, the mobile robot does not collide with any obstacle in the environment (including inherent obstacles in the environment and obstacles not in the map). High radiation risk avoidance is a hard constraint, that is, the mobile robot is not allowed to pass through a grid with an average radiation dose rate higher than a threshold (set artificially) (the threshold can be understood as the limit of the mobile robot's tolerance to radiation. If it is forced to pass through, the mobile robot or its mounted components will be damaged or even damaged by radiation). Cost minimization refers to the pursuit of the path with the lowest cost under the two hard constraints of obstacle avoidance and high radiation risk avoidance. The cost is composed of path length, cumulative radiation dose and turning energy consumption.

[0154] B. Mathematical modeling:

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

[0156] Formula 1:

[0157] Where M(p) represents the objective function (taking the minimum as the goal), ω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 grid p i To grid p i+1 The distance (when the grid p i With grid p i+1 When the edges are adjacent, d(p i ,p i+1 ) 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 ) is times the grid side length); (x i ,y i ) is the grid p i The coordinates of (x i+1 ,y i+1 ) is the grid p i+1 Coordinates of R(p i ,p i+1 ) is the mobile robot from the grid p i Travel to grid pi+1 The cumulative radiation dose, is the grid p i The average radiation dose rate (the statistical range is the space 1m above the ground), is the grid p i+1 The average radiation dose rate (the statistical range is the space 1m above the ground), l is the grid p i To grid p i+1 The distance (if the grid p i With grid p i+1 If the grid is adjacent to each other, then l is equal to 1 times the grid side length. i With grid p i+1 For vertex contact, l is equal to times the grid side length); ε1 is the weight coefficient of the steering angle variable, ε2 is the weight coefficient of the number of turns, ψ[l(p i-1 ,p i ),l(p i ,p i+1 )] is the line segment l(p i-1 ,p i ) and line segment l(p i ,p i+1 ) steering angle variable, l(p i-1 ,p i ) is the grid p i-1 The center point of the grid p i The line connecting the center points of i ,p i+1 ) is the grid p i The center point of the grid p i+1 The line connecting the center points of turn The average radiation dose rate of a grid refers to the average radiation dose rate in the three-dimensional space formed by the grid at a height of 1m above the ground.

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

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

[0160] A-1. Heuristic function:

[0161] Considering the three sub-goals in mathematical modeling, the heuristic function of the improved A* algorithm is constructed, as shown in Formula 2;

[0162] Formula 2:

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

[0164] A-2. Dangerous area avoidance strategy:

[0165] During the path search process, when the ant perceives that the average radiation dose rate of the next grid exceeds the threshold (the threshold can be understood as the limit of the mobile robot's tolerance to radiation. If it is forced to pass through, the mobile robot or its components will be damaged by radiation or even damaged), the average radiation dose rate cost value of the next grid is defined as infinity, prompting the ant to find other passable grids; thus constructing a dangerous area avoidance strategy, as shown in Formula 3;

[0166] Formula 3:

[0167] Among them, R(p i ) is the grid p iThe radiation dose rate substitute value, Inf is infinite, RI max is the radiation dose rate threshold.

[0168] B. Run the improved A* algorithm to obtain the suboptimal path:

[0169] Run the improved A* algorithm and output the result after the calculation is completed: suboptimal path P sp .

[0170] C, using suboptimal path non-uniformization to process the initial pheromone of the ACO algorithm:

[0171] The pheromone matrix plays an important role in the optimization performance of the ACO algorithm. The traditional uniform initial pheromone distribution will cause the ACO algorithm to be blind in the early search, which will reduce the convergence speed of the ACO algorithm. Therefore, the suboptimal path P obtained by running the improved A* algorithm is adopted. sp The initial pheromone in the ACO algorithm is processed unevenly, as shown in Formula 4; the purpose is to increase only the initial pheromone concentration of the grid through which the suboptimal path passes, leaving the initial pheromone concentration of other grids unchanged, thereby more effectively guiding the ants to search for paths and accelerating the convergence speed of the ACO algorithm;

[0172] Formula 4:

[0173] Among them, i and j represent two passable grids with adjacent edges or touching vertices, τ i,j is the grid p i With p j The initial pheromone concentration after the inhomogeneous treatment is between τ and 0.5, τ 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-1.0, for example, 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-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 capability of the ACO algorithm, enhance the function of the heuristic function in the relatively early iterations, and weaken the function of the heuristic function in the relatively late iterations, so as to realize the construction of the heuristic function of the ACO algorithm, as shown in Formula 5;

[0177] Formula 5:

[0178] Among them, η t (i,j) is the tth iteration from grid p ito p j The heuristic function value of i, j represents the passable grid with two adjacent edges or touching vertices, Q1 is the heuristic function enhancement coefficient, L i,j is the distance from grid i to grid j (if grid i and grid j are adjacent to each other, then L i,j is 1 times the grid side length; if grid i and grid j are in contact at the vertex, then L i,j for times the grid side length), R i,j is the cumulative radiation dose received by the mobile robot when it moves from grid i to grid j (R i,j The calculation of the cumulative radiation dose sub-objective function refers to Formula 1), E i,j For the mobile robot to move from the grid p i-1 To grid p i to p j Turning energy consumption (E i-1,i,j The calculation of the turning energy consumption sub-objective function refers to Formula 1), I max is the maximum number of iterations, I 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. Select the probability function:

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

[0181] Formula 6:

[0182] Where P t (i,j) is the tth iteration from grid p i to p j The selection probability of i, j represents the passable grids with two adjacent edges or touching vertices, α is the pheromone importance factor, β is the heuristic function importance factor; τ t (i,j) indicates the number of cells from grid p when the tth iteration is completed. i to p j The pheromone concentration, τ t (i,u) indicates the number of cells from grid p when the tth iteration is completed. i to p u The pheromone concentration, η t (i,j) is the tth iteration from grid p i to p j The heuristic function value, η t (i,u) is the tth iteration from grid p i to p u The heuristic function value of t′ is the set of grids that have not been passed before the tth iteration; the denominator in Formula 6 is the number of grids from the tth iteration i to the sum of the selection probabilities of all grids that have not been passed; if the grid p in formula 6 j It is a grid that has not been passed, and then the grid p i to p j The selection probability is calculated if the grid p in formula 6 j is a grid that has been passed, then grid p i to p j The probability of selection is 0.

[0183] D-3.Pheromone update rules:

[0184] The traditional ACO algorithm uses a single positive feedback mechanism in the pheromone update rule, which has a fast convergence speed. However, too many positive feedback pheromones are concentrated on excellent paths, which limits the ant colony's ability to explore unknown paths, causing the ACO algorithm to easily fall into a local optimal solution. In addition, the traditional ACO algorithm only considers the influence of path length in the pheromone update rule, which cannot meet the requirements of mobile robots planning global paths in a core environment containing non-map obstacles. Therefore, the positive and negative feedback mechanism is combined with the three sub-goals in mathematical modeling to construct the pheromone update rule of the ACO algorithm, as shown in Formula 7.

[0185] Formula 7:

[0186] In the formula, τ t+1 (i,j) and τ t (i, j) represent the grid p at the completion of the t+1th and tth iterations respectively. i With p j ρ is the pheromone volatilization factor, Q2 is the pheromone deposition factor, M t-mi is the total cost of the three sub-goals of the optimal path obtained in the tth iteration (the total cost of the optimal path is the smallest, calculated according to Formula 1), M t-ma is the total cost of the three sub-goals of the worst path obtained in the tth iteration (the total cost of the worst path is the largest, calculated according to formula 1); P optimal is the optimal path obtained in the t+1th iteration, P worst is the worst path obtained at the t+1th iteration.

[0187] E. Use PSO algorithm to optimize parameter combination:

[0188] PSO is a swarm intelligence optimization algorithm that simulates the foraging behavior of birds. It uses particles to simulate the process of birds looking for food to find the optimal solution to the optimization problem. When the PSO algorithm is used for parameter combination optimization: the position of the particle (or coordinate) represents a specific parameter combination, that is, a potential solution in the problem space; the particle's speed is used to guide the search direction and size of the particle in the solution space, which can be regarded as a mechanism for fine-tuning the current parameter combination, used to guide the particle from the current position to the area where the optimal solution is more likely to be found; the particle's speed includes two dimensions: direction and step size. The direction of the speed 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 optimal position and the particle's current position with the global optimal position. The step size of the speed determines the amplitude of the parameter adjustment. The larger the step size, the larger the particle's jump in the solution space, and the smaller the step size, the more refined the particle's search.

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

[0190] According to the objective function of mathematical modeling and the ACO algorithm constructed above, the parameter combination to be optimized is preliminarily determined to be (ω1, ω2, ω3, α, β, ρ). Since (ω1, ω2, ω3) are multi-objective weight coefficients, the single-objective standardization method is used to simplify the number of multi-objective weight coefficients. Set ω1 = 1 to obtain the approximate range of ω2 and ω3, and finally determine the parameter combination to be optimized as (ω2, ω3, α, β, ρ); subsequently, the quality of the particle position is evaluated through the objective function of global path planning.

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

[0192] Define the update rule of particle velocity in the PSO algorithm, as shown in Formula 8;

[0193] Formula 8:

[0194] In the formula, Gbest is the optimal position of the i-th particle in the entire particle swarm in k iterations. k is the optimal position of all particles in the entire particle swarm after 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, r1 and r2 are both random numbers between [0,1], and r1 and r2 are used to increase the randomness of particle flight; and are the positions of the i-th particle at the k-th and k+1-th iterations respectively; and are the velocities of the i-th particle at the k-th and k+1-th iterations, respectively, [-v max ,v max ] is the velocity range of the particles.

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

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

[0197] F, obtain the global path based on the optimized parameter combination:

[0198] 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.

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

[0200] First, all turning 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 two adjacent sub-navigation points, and the last sub-navigation point and the target point are defined as local paths. Each local path is replanned in order from front to back. 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 DWA algorithm model:

[0202] The DWA algorithm includes two core processes: ①. Figure 4 As shown in the figure, based on the motion characteristic constraints of the mobile robot and the safety constraints formed by environmental obstacles, the velocity sampling space is limited to a specific range, and then a series of discrete velocity tuples are obtained based on kinematic analysis. max and v min Represent the maximum linear velocity and the minimum linear velocity of the mobile robot, ω max and ω min They represent the maximum angular velocity and the minimum angular velocity of the mobile robot, respectively. a represents the linear acceleration of the mobile robot. b represents the angular acceleration of the mobile robot. v t represents the linear velocity of the mobile robot at time t, ω t is the angular velocity of the mobile robot at time t. Figure 5As shown, the above speed tuple is used to predict the path of the next interval. In the figure, the red dotted line is the collision path, the green solid line and the yellow dotted line are alternative paths, and the alternative paths are evaluated by the path evaluation function to select the optimal path (i.e., the green solid line).

[0203] A-1. Velocity sampling space analysis: The velocity sampling space of the DWA algorithm is established by comprehensively considering kinematic constraints, acceleration constraints, and obstacle constraints.

[0204] ①. Due to the limitation of the mobile robot’s own motion performance, its linear velocity and angular velocity are limited to a certain range; the velocity sampling space V under kinematic constraints m It is defined as formula 9;

[0205] Formula 9:

[0206] Where V m is the velocity sampling space under kinematic constraints, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and 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.

[0207] ②. The speed change performance of the mobile robot is limited. In addition, considering that excessive acceleration or deceleration during the movement of the mobile robot will cause motor overload and rapid wear, it is necessary to constrain the acceleration of the mobile robot; the speed sampling space V under the acceleration constraint d It is defined as formula 10;

[0208] Formula 10:

[0209] Where V d is the velocity sampling space under acceleration constraint, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and 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 value of the linear acceleration of the mobile robot, ω m` is the maximum 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 not marked on the map; the speed sampling space under obstacle constraints is defined as formula 11;

[0211] Formula 11:

[0212] Where V s is the velocity sampling space under obstacle constraints, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and dist 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: According to the sampling combination and simulation time of the linear velocity and angular velocity of the mobile robot in the linear motion of each sampling period, the path of the mobile robot in the next sampling period is obtained; the path evaluation function is used to evaluate all the paths in the next sampling period, and the speed tuple corresponding to the optimal path is selected and executed. The path evaluation function is shown in Formula 12;

[0214] Formula 12:

[0215] Where G(v,ω) is the path evaluation function; heading(v,ω) is the deflection angle evaluation sub-function, which is used to evaluate the angle change of the mobile robot on the path; dist(v,ω) is the safety factor evaluation sub-function, which is used to evaluate the safety of the mobile robot on the path; velocity(v,ω) is the speed evaluation sub-function, which is 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 is the heading angle of the mobile robot at the starting point of the path, l pl is the path length; (x obs ,y obs ) is the coordinate of the mobile robot, (x m ,y m ) is the coordinate of the obstacle; velocity(v) is the secondary sub-function for linear velocity evaluation, velocity(ω) is the secondary sub-function for angular velocity evaluation, ε3 and ε4 are the weight coefficients of the above two secondary 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, so that their contribution to the path evaluation function can be fairly and reasonably evaluated, as shown in formula 13;

[0217] Formula 13:

[0218] Where 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 subfunction 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 subfunction under 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. Introducing fuzzy logic to achieve dynamic parameter adjustment of DWA algorithm:

[0220] In the actual movement of the mobile robot, the environment changes dynamically, and fixed weight coefficients cannot adapt to such changes; for example, in some cases, avoiding obstacles may be more important, while in other cases, quickly reaching the target point may be more critical. Fixed weight coefficients may cause the DWA algorithm to perform poorly in certain specific scenarios. For example, in an environment with dense obstacles, if the weight coefficients are not adjusted properly, the mobile robot may focus too much on avoiding obstacles and ignore the need to quickly reach the target point, and vice versa. Therefore, it is very necessary to dynamically adjust the parameters of the DWA algorithm according to the current environmental state and the mobile robot's own motion state.

[0221] According to the principle of DWA algorithm, (α, β, γ) in the path evaluation function are parameters that need to be adjusted dynamically. 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, the direction fuzzy logic controller and the safety fuzzy logic controller are designed; the direction fuzzy logic controller input (L MT ,θ DA ), output a set of weight parameters (α1,γ1); the safety fuzzy logic controller input (L MO ,L MT ), output a set of weight parameters (β1,γ2); then design a fusion fuzzy logic controller 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 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 in which the mobile robot is facing the target sub-navigation point); 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 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 fuzzy logic (FL) input fuzzification process is as follows: the input L MO , L MT and θ DA Fuzzy processing is performed; among them, L MO The domain is [0, 2.5], the fuzzy set is [N, M, F], N is near, M is medium, F is far, when L MO <0.5, the fuzzy value is N, when 0.5<L MO When <1.25, it is fuzzy to M. MO >1.25, fuzzy to F; L MT The domain is [0,3], the fuzzy set is [N,M,F], N is near, M is medium, F is far, when L MT <0.5, the fuzzy value is N, when 0.5<L MT When <1.5, it is fuzzy to M. MT When >1.5, it is blurred to F; θ DA The domain is [-180°, 180°], and the fuzzy set is [XS, S, M, B, XB], where XS is negative, S is negative, M is medium, B is positive, and XB is positive. DA <-120°, the blur is XS, when -120°<θ DA <-60°, the fuzzy value is S. When -60°<θ DA <60°, the fuzzy state is M. When 60°<θ DA When θ is less than 120°, it is blurred to B. DA When >120°, the blur becomes XB.

[0224] B-3, the fuzzy processing of the output of fuzzy logic (FL) is as follows: the output α, β, γ and δ are fuzzified; the definition domains of α, β, γ and δ are all [0,1], and the fuzzy sets of α, β and δ are all [XS, S, M, L, XL], XS is extremely small, S is small, M is medium, L is large, and XL is extremely large. When 0<α<0.1 or 0<β<0.1 or 0<δ<0.1, the fuzzification is XS, and when 0.1<α<0.3 or 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 fuzzified to M; when 0.6<α<0.9 or 0.6<β<0.9 or 0.6<δ<0.9, it is fuzzified to L; when 0.9<α<1 or 0.9<β<1 or 0.9<δ<1, it is fuzzified to XL; the fuzzy set of γ is [S, M, L], S is small, M is medium, and L is large. When γ<0.3, it is fuzzified to S; when 0.3<γ<0.7, it is fuzzified to M; when γ>0.7, it is fuzzified to L.

[0225] In summary, when the DWA algorithm is used to plan the local path of the mobile robot (local path definition: two adjacent sub-navigation points in the global path), it is necessary to consider the directionality, safety and speed characteristics according to the changes in the environmental field. Therefore, according to the above fuzzy sets, fuzzy logic control rules can be established, including the control rules of the directional fuzzy logic controller, the control rules of the safety fuzzy logic controller and the control rules of the fusion 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 end point (the target sub-navigation point corresponding to the local path) will affect the driving efficiency of the mobile robot; 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 γ that affect the evaluation function heading(v,ω) and velocity(v,ω). Therefore, the output of the directional fuzzy logic controller is α and γ.

[0227] Control rules summary:

[0228] ①. When the mobile robot is far away from the target sub-navigation point and the direction angle θ DA When it is small, γ should be increased to make the mobile robot move quickly to the target point, and a suitable α should be selected to keep the mobile robot from deviating from the guidance of the preset trajectory;

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

[0230] ③. When the mobile robot is close to the target sub-navigation point and the direction angle θ DA When it is small, γ should be reduced to slow down the mobile robot and θ should be increased. DA , so that the mobile robot can reach the target sub-navigation point faster;

[0231] ④. When the mobile robot is close to the target sub-navigation point and the direction angle θ DA When it is larger, it means that the mobile robot is about to reach the target sub-navigation point, but there is a tendency to detour. At this time, γ should be reduced to achieve the deceleration of the mobile robot, and α should be increased so that the mobile robot can reach the target point more smoothly.

[0232] Control rule description:

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

[0234] ①. Input terminal: L MT Fuzzy F, θ DA Fuzzy to M; Output: α1 is fuzzy to XL, γ1 is fuzzy to L;

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

[0236] ③. Input terminal: L MT Fuzzy F, θ DA Fuzzy to XSorXB; Output: α1 is fuzzy to M, γ1 is fuzzy to L;

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

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

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

[0240] ⑦. Input terminal: L MT Fuzzy N, θ DA Fuzzy to M; Output: α1 is fuzzy to M, γ1 is fuzzy to S;

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

[0242] ⑨. Input terminal: L MT Fuzzy N, θ DA Fuzzified into XSorXB; output end: α1 is fuzzified into XL, γ1 is fuzzified into S.

[0243] The subsequent defuzzification adopts the centroid method.

[0244] B-5, the control rules of the safety fuzzy logic controller are as follows: The design 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 the mobile robot 2 is far away from both the obstacle and the target sub-navigation point, that is, L MO and L MT At the same time, the mobile robot does not need to perform the obstacle avoidance mechanism, and should speed up to quickly approach the target sub-navigation point. At this time, γ should be increased and β should be decreased;

[0247] ②. When the mobile robot is close to the obstacle but far away from the target point, that is, L MO Smaller, L MT If it is large, the mobile robot should take safe obstacle avoidance as the principle, reduce γ and increase β;

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

[0249] ④. When the mobile robot is away from the obstacle and close to the target sub-navigation point, γ and β should be reduced at the same time so that the mobile robot can reach the target sub-navigation point smoothly.

[0250] Control rule description:

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

[0252] ①. Input terminal: L MO Fuzzy to F,L MT Fuzzy to F; output: β1 is fuzzy to XS, γ2 is fuzzy to L;

[0253] ②. Input terminal: L MO Fuzzy to F,L MT Fuzzy to M; Output: β1 is fuzzy to S, γ2 is fuzzy to L;

[0254] ③. Input terminal: L MO Fuzzy to F,L MT Fuzzy to N; Output: β1 fuzzy to S, γ2 fuzzy to S;

[0255] ④. Input terminal: L MO Fuzzy to M,L MT Fuzzy to F; Output: β1 is fuzzy to M, γ2 is fuzzy to L;

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

[0257] ⑥. Input terminal: L MO Fuzzy to M,L MT Fuzzy to N; Output: β1 is fuzzy to M, γ2 is fuzzy to S;

[0258] ⑦. Input terminal: L MO Fuzzy to N,L MT Fuzzy to F; output: β1 is fuzzy to XL, γ2 is fuzzy to M;

[0259] ⑧. Input terminal: L MO Fuzzy to N,L MT Fuzzy to M; Output: β1 is fuzzy to L, γ2 is fuzzy to M;

[0260] ⑨. Input terminal: L MO Fuzzy to N,L MT Fuzzified to N; output end: β1 is fuzzified to M, γ2 is fuzzified to M.

[0261] The subsequent defuzzification adopts the centroid method.

[0262] B-6, the control rules of the fusion fuzzy logic controller are as follows: The design purpose of the fusion fuzzy logic controller is to further improve the obstacle avoidance performance of the DWA algorithm in complex environments. MO and θ DA is the input, 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] According to 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 away from the obstacle and the direction angle θ DA When it is large, δ should be increased to enhance the priority of the directional fuzzy logic controller and drive the mobile robot to move quickly to the target point;

[0267] ②. When the mobile robot approaches an obstacle and the direction angle θ DA When it is large, obstacle avoidance should be given priority, that is, δ should be reduced;

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

[0269] Control rule description:

[0270] There are 8 control rules in total for the fusion fuzzy logic controller.

[0271] ①. Input terminal: L MO Fuzzy F, θ DA Fuzzified to SorB; Output: δ fuzzified to XL;

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

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

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

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

[0276] ⑥. Input terminal: L MO Fuzzy to M, θ DA Fuzzy to M; Output: δ fuzzy to S;

[0277] ⑦. Input terminal: L MOFuzzy N, θ DA Fuzzy to XSorXB; Output: δ fuzzy to XS;

[0278] ⑧. Input terminal: L MO Fuzzy N, θ DA Fuzzified into SorMorB; output end: δ is fuzzified into S.

[0279] The subsequent defuzzification adopts the centroid method.

[0280] C, Fusion 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 the mobile robot to a certain extent, it may still fail 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 the mobile robot.

[0282] VOA dynamic obstacle avoidance is an algorithm that directly predicts collisions in a changing environment through speed information. Its principle is to use the concept of speed barriers to map the dynamic environment to the speed space of the mobile robot and calculate the first-order approximation of the speed at which the mobile robot will collide with the obstacle at a certain moment within a given time range. Using this representation, obstacle avoidance strategies can be performed simply by selecting speed tuples outside the speed barriers.

[0283] The VOA dynamic obstacle avoidance principle is as follows: In the global coordinate system XOY, the mobile robot M is located at the origin of the coordinate system and moves at a speed v m The grid obstacle o moves forward at a speed v0, Δ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 with a radius R (the circle is the minimum circumscribed circle that completely contains the grid obstacle), T1 and T2 are the tangents from the coordinate origin to the two sides of the circular obstacle, and μ is the distance between the tangent T1 or T2 and the straight line , γ is the angle between the velocity Δv and the straight line The angle between the tangent lines T1 and T2 is 2μ, and both μ and γ are positive values. When the speed Δv is between the tangent lines T1 and T2, the mobile robot will collide with the obstacle, and the collision angle interval is 2μ. The goal of dynamic obstacle avoidance is to keep Δv outside 2μ at all times, even if: γ>μ.

[0284] Based on v m , v0 and Δv form a velocity vector triangle, we can get formula 14 and formula 15. By taking the derivative of formula 15, we get formula 16;

[0285] Formula 14:

[0286] Formula 15:

[0287] Formula 16:

[0288] Where α is the direction angle of the mobile robot (the positive direction of the X-axis and v m β is the direction angle of the grid obstacle (the angle between the positive direction of the X-axis and v0), is the velocity vector v in the triangle m The angle between the positive direction of the X-axis and the straight line is θ, η is the angle between v0 and Δv in the velocity vector triangle, and θ is the angle between the positive direction of the 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, the speed v0 of the grid obstacle, the direction angle β of the grid obstacle, the change Δv0 of the grid obstacle speed and the change Δβ of the grid obstacle direction angle are known. By adjusting the speed v0 of the mobile robot m and the direction angle α of the mobile robot to change Δγ, so that the adjusted γ satisfies: γ+Δγ>μ; m and Δα are set as x1 and x2 respectively, and the dynamic obstacle avoidance objective function is established, as shown in Formula 17. Solving Formula 17 can obtain the speed tuple that satisfies dynamic obstacle avoidance; dynamic obstacle avoidance can be achieved by selecting this speed tuple in the above FL-DWA algorithm.

[0290] Formula 17:

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

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

[0293] Two nuclear environment scenes with different complexities are set to verify the technical effect of the present invention. The scene with relatively low complexity is scene 1 (20m×20m), and the scene with relatively high complexity is scene 2 (50m×50m). There are unknown dynamic or static obstacles in both scene 1 and scene 2. There are 7 radioactive sources in scene 1, with energies of 1.25, 1.25, 2.5, 1.25, 3.75, 1.25, and 1.25 MeV, respectively. There are 20 radioactive sources in scene 2, with energies of 1.25 MeV. All dynamic obstacles in scene 1 and scene 2 make reciprocating linear motions. Information of the two scenes is shown in Table 1.

[0294] Table 1:

[0295]

[0296]

[0297] In scenario 1 and scenario 2, four path planning methods (including the present invention) are used for double-layer path planning (the double-layer path includes a global path and a local path). The planning algorithm of the global path is ACO-A*-PSO. The only difference is that the planning method of the local path is different. 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] Combining Table 2 and Figure 8-9 It can be seen that only the path planning method provided by the present invention can successfully plan paths in two scenarios of different complexity, and the other three path planning methods all have the problem of failure in path planning in some or all scenarios.

Claims

1. A two-layer path planning method for real-time obstacle avoidance of a mobile robot in a nuclear environment, characterized by: 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 laser radar; Here’s how: S01, establish a mathematical model for real-time obstacle avoidance path planning: the path planning goal is to plan an optimal path from the starting point to the target point for the mobile robot; considering the three sub-goals of path length, cumulative radiation dose and turning energy consumption, establish the objective function of the global path planning of the mobile robot; S02, with ACO-A * -PSO algorithm plans the global path: first, build an improved A* algorithm based on the objective function, use the suboptimal path obtained by running the improved A* algorithm to unevenly process the initial pheromone distribution of the ACO algorithm, then build the ACO algorithm based on the objective function, then use the PSO algorithm to optimize the objective function and the parameters in the ACO algorithm, and finally run the ACO algorithm based on the parameter combination obtained to obtain the global path from the starting point to the target point; S03, using the FL-DWA-VOA algorithm to plan the local path: first define all turning points in the global path as sub-navigation points, thereby dividing the global path into multiple local paths connected in sequence, and then replanning the path for each local path in order from front to back; the path replanning process is: first establish the DWA algorithm model, then introduce fuzzy logic to realize the dynamic parameter adjustment of the DWA algorithm, and establish the FL-DWA algorithm with static obstacle avoidance effect. If the mobile robot does not find a dynamic obstacle, only the FL-DWA algorithm is run for local path replanning; if the mobile robot finds a dynamic obstacle, the VOA dynamic obstacle avoidance algorithm is additionally called to avoid the dynamic obstacle.

2. The real-time obstacle avoidance double-layer path planning method for a mobile robot in a nuclear environment as described in claim 1 is characterized in that: the grids in the grid map are 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 in each grid in binary encoding "1" and "0" to indicate "obstacle" and "no obstacle" respectively.

3. The real-time obstacle avoidance double-layer path planning method for a mobile robot in a nuclear environment as claimed in claim 2 is characterized by: The S01 step is described in detail as follows: S01, establish a mathematical model for real-time obstacle avoidance path planning: A. Problem description: Based on the operation scenario of the mobile robot in the nuclear environment, in order to improve the operation efficiency of the mobile robot and ensure its safety, it is necessary to plan a path from the starting point S(p1(x1,y1)) to the target point E(p n (x n ,y n ))'s optimal path; A-1. Define passability: Use different colors other than the specified color to distinguish the average radiation dose rate in different numerical ranges, use the specified color to indicate "there are obstacles", and other grids except the specified color indicate "no obstacles"; when the mobile robot performs path planning, the eight-direction search strategy is used for path search, and the grid coordinates are defined as the coordinates of the center point of the grid; if a grid simultaneously meets the three conditions of "no obstacles 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 obstacles when entering the grid", the grid is defined as a passable grid; if a grid does not meet any of the above three requirements, the grid is defined as an inaccessible grid; A traversable path means that all grids passed by the mobile robot are traversable grids, and the set of traversable grids is the traversable area; A-2. Path planning objectives: When planning a global path for a mobile robot in a nuclear 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 obstacle 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 pursuing the path with the lowest cost under the two hard constraints of obstacle avoidance and high radiation risk avoidance. The cost is composed of path length, cumulative radiation dose and turning energy consumption. B. Mathematical modeling: Considering the path length, cumulative radiation dose and turning energy consumption, the objective function of the mobile robot planning the global path in a nuclear environment containing obstacles not in the map is established, see formula 1; Formula 1: Where 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 grid p i To grid p i+1 The distance (x i ,y i ) is the grid p i The coordinates of (x i+1 ,y i+1 ) is the grid p i+1 Coordinates of R(p i ,p i+1 ) is the mobile robot from the grid p i Travel to grid p i+1 The cumulative radiation dose, is the grid p i The average radiation dose rate, is the grid p i+1 The average radiation dose rate of the grid p i To grid p i+1 distance; ε1 is the weight coefficient of the steering angle variable, ε2 is the weight coefficient of the number of turns, ψ[l(p i-1 ,p i ),l(p i ,p i+1 )] is the line segment l(p i-1 ,p i ) and line segment l(p i ,p i+1 ) steering angle variable, l(p i-1 ,p i ) is the grid p i-1 The center point of the grid p i The line connecting the center points of i ,p i+1 ) is the grid p i The center point of the grid p i+1 The line connecting the center points of turn is the number of turns.

4. The real-time obstacle avoidance double-layer path planning method for a mobile robot in a nuclear environment as claimed in claim 3 is characterized by: The S02 step is described in detail as follows: S02, using ACO-A*-PSO algorithm to plan the 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 Formula 2; Formula 2: In the formula, g(p) is the cost value of the path length from the starting point to the current grid, h(p) is the cost value of the path length from the current grid to the target point, g(p) and h(p) are calculated by referring to L(p) in formula 1; R(p 1-n ) is the cumulative radiation dose cost of the mobile robot from the starting point to the target point, R(p 1-n ) Refer to R(p) in Formula 1 for calculation, R(p 1-n ) is R(p 1-i ) and R(p i-n ), R(p 1-i ) is the cumulative radiation dose cost of the mobile robot from the starting point to the current grid, R(p i-n ) is the cumulative radiation dose cost of the mobile robot from the current grid to the target point; E(p 1-n ) is the turning energy cost between the starting point and the target point, E(p 1-n ) Refer to E(p) in Formula 1 for calculation, E(p 1-n ) is E(p 1-i ) and E(p i-n ), E(p 1-i ) is the turning energy cost between the starting point and the current grid, E(p i-n ) is the turning energy cost between the current grid and the target point; (x i ,y i ) is the current grid p i The coordinates of the starting point are (x1, y1), n ,y n ) are the coordinates of the target point; A-2. Dangerous area avoidance strategy: In the path search process, when the ant perceives that the average radiation dose rate of the next grid exceeds the threshold, the average radiation dose rate cost value of the next grid is defined as infinity, prompting the ant to find other passable grids; thus constructing a dangerous area avoidance strategy, as shown in Formula 3; Formula 3: Among them, R(p i ) is the grid p i The radiation dose rate substitute value, Inf is infinite, RI max is the radiation dose rate threshold; B. Run the improved A* algorithm to obtain the suboptimal path: Run the improved A* algorithm and output the result after the calculation is completed: suboptimal path P sp ; C, using suboptimal path non-uniformization to process the initial pheromone of the ACO algorithm: The suboptimal path P obtained by running the improved A* algorithm sp The initial pheromone in the ACO algorithm is processed unevenly; only the initial pheromone concentration of the grid through which the suboptimal path passes is increased, and the initial pheromone concentration of other grids remains unchanged; as shown in Formula 4; Formula 4: Among them, i and j represent two passable grids with adjacent edges or touching vertices, τ i,j is the grid p i With p j The initial pheromone concentration after the inhomogeneous treatment between τ and C is the initial pheromone concentration set artificially, 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 capability of the ACO algorithm, and the heuristic function of the ACO algorithm is constructed, as shown in Formula 5. Formula 5: Among them, η t (i,j) is the tth iteration from grid p i to p j The heuristic function value of i, j represents the passable grid with two adjacent edges or touching vertices, Q1 is the heuristic function enhancement coefficient, L i,j is the distance from grid i to grid j, R i,j is the cumulative radiation dose received by the mobile robot when it moves from grid i to grid j, E i,j For the mobile robot to move from the grid p i-1 To grid p i to p j The turning energy consumption, I max is the maximum number of iterations, I c is the current iteration number, ξ is the adaptive adjustment coefficient, and Q is the heuristic function enhancement coefficient; D-2. Select the probability function: Taking into account the pheromone concentration and heuristic function, the selection probability function of the ACO algorithm is constructed, as shown in Formula 6; Formula 6: Where P t (i,j) is the tth iteration from grid p i to p j The selection probability of i, j represents the passable grids with two adjacent edges or touching vertices, α is the pheromone importance factor, β is the heuristic function importance factor; τ t (i,j) indicates the number of cells from grid p when the tth iteration is completed. i to p j The pheromone concentration, τ t (i,u) indicates the number of cells from grid p when the tth iteration is completed. i to p u The pheromone concentration, η t (i,j) is the tth iteration from grid p i to p j The heuristic function value, η t (i,u) is the tth iteration from grid p i to p u The heuristic function value of t ′ is the set of grids that have not been passed before the tth iteration; the denominator in Formula 6 is the number of grids from the tth iteration i to the sum of the selection probabilities of all grids that have not been passed; if the grid p in formula 6 j It is a grid that has not been passed, and then the grid p i to p j The selection probability is calculated if the grid p in formula 6 j is a grid that has been passed, then grid p i to p j The probability of selection is 0; D-3.Pheromone update rules: Combining the positive and negative feedback mechanism with the three sub-goals in mathematical modeling, the pheromone update rule of the ACO algorithm is constructed, as shown in Formula 7; Formula 7: In the formula, τ t+1 (i,j) and τ t (i, j) represent the grid p at the completion of the t+1th and tth iterations respectively. i With p j ρ is the pheromone volatilization factor, Q2 is the pheromone deposition factor, M t-mi is the total cost of the three sub-goals of the optimal path obtained in the tth iteration, M t-ma is the total cost 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 PSO algorithm to optimize parameter combination: 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 preliminarily determined to be (ω1, ω2, ω3, α, β, ρ). Since (ω1, ω2, ω3) are multi-objective weight coefficients, the single-objective standardization method is used to simplify the number of multi-objective weight coefficients. Set ω1 = 1 to obtain the approximate range of ω2 and ω3, and finally determine the parameter combination to be optimized as (ω2, ω3, α, β, ρ); subsequently, the quality of the particle position is evaluated through the objective function; E-2. Define the particle velocity update rule: Define the update rule of particle velocity in the PSO algorithm, as shown in Formula 8; Formula 8: In the formula, is the optimal position of the i-th particle in the entire particle swarm before the k-th iteration, Gbest k is the optimal position of all particles in the entire particle swarm before the kth iteration, ω is the inertia weight, c1 and c2 are learning factors, r1 and r2 are random numbers between [0,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 in the k-th and k+1-th iterations, respectively, [-v max ,v max ] is the velocity range of the particles; E-3. Get the optimized parameter combination: Run the PSO algorithm. During the operation, the objective function is used as the fitness function to evaluate the quality of the particle position, and finally the optimized parameter combination (ω2, ω3, α, β, ρ) is obtained; F, obtain the global path based on the optimized parameter combination: 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 real-time obstacle avoidance double-layer path planning method for a mobile robot in a nuclear environment as claimed in claim 4 is characterized by: The S03 step is described in detail as follows: S03, using FL-DWA-VOA algorithm to plan local paths: A. Establish DWA algorithm model: A-1. Velocity sampling space analysis: Considering kinematic constraints, acceleration constraints and obstacle constraints, the velocity sampling space of the DWA algorithm is established; ①. Due to the limitation of the mobile robot’s own motion performance, its linear velocity and angular velocity are limited to a certain range; the velocity sampling space V under kinematic constraints m It is defined as formula 9; Formula 9: Where V m is the velocity sampling space under kinematic constraints, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and 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 speed change performance of the mobile robot is limited. In addition, considering that excessive acceleration or deceleration during the movement of the mobile robot will cause motor overload and rapid wear, it is necessary to constrain the acceleration of the mobile robot; the speed sampling space V under the acceleration constraint d It is defined as formula 10; Formula 10: Where V d is the velocity sampling space under acceleration constraint, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and 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 value of the linear acceleration of the mobile robot, ω m` is the maximum value of the angular acceleration of the mobile robot; ③. 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 not marked on the map; the speed sampling space under obstacle constraints is defined as formula 11; Formula 11: Where V s is the velocity sampling space under obstacle constraints, (v,ω) is the velocity tuple, v is the linear velocity of the mobile robot, ω is the angular velocity of the mobile robot, and dist min (v,ω) represents the distance from the end of the path corresponding to (v,ω) to the nearest obstacle; 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 speed tuple corresponding to the optimal path, and the path evaluation function is shown in Formula 12; Formula 12: G(v,ω)=α·heading(v,ω)+β·dist(v,ω)+γ·velocity(v,ω) Where 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; The three sub-functions in formula 12 are normalized so that the three sub-functions have the same scale in the path evaluation function, as shown in formula 13; Formula 13: Where 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 subfunction under the current path to be evaluated, dist(v i ,ω i ) is the safety factor evaluation subfunction under the current path to be evaluated, velocity(v i ,ω i ) is the speed evaluation subfunction under the current path to be evaluated; B. Introducing fuzzy logic to achieve dynamic parameter adjustment of DWA algorithm: According to the principle of DWA algorithm, (α, β, γ) in the path evaluation function are parameters that need to be adjusted dynamically. The position of the target point affects the values ​​of α and γ, and the position of the obstacle affects the values ​​of β and γ. B-1, the basic framework 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 input (L MT ,θ DA ), output a set of weight parameters (α1,γ1); the safety fuzzy logic controller input (L MO ,L MT ), output 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 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 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∈γ; B-2, the fuzzy logic (FL) input fuzzification process is as follows: the input L MO , L MT and θ DA Fuzzy processing is performed; among them, L MO The domain is [0, 2.5], the fuzzy set is [N, M, F], N is near, M is medium, F is far, when L MO <0.5, the fuzzy value is N, when 0.5<L MO When <1.25, it is fuzzy to M. MO >1.25, fuzzy to F; L MT The domain is [0,3], the fuzzy set is [N,M,F], N is near, M is medium, F is far, when L MT <0.5, the fuzzy value is N, when 0.5<L MT When <1.5, it is fuzzy to M. MT When >1.5, it is blurred to F; θ DA The domain is [-180°, 180°], and the fuzzy set is [XS, S, M, B, XB], where XS is negative, S is negative, M is medium, B is positive, and XB is positive. DA <-120°, the blur is XS, when -120°<θ DA <-60°, the fuzzy value is S. When -60°<θ DA <60°, the fuzzy state is M. When 60°<θ DA When θ is less than 120°, it is blurred to B. DA When >120°, the blur becomes XB; B-3, the fuzzy processing of the output of fuzzy logic is as follows: the output α, β, γ and δ are fuzzified; the definition domains of α, β, γ and δ are all [0,1], and the fuzzy sets of α, β and δ are all [XS,S,M,L,XL], where XS is extremely small, S is small, M is medium, L is large, and XL is extremely large. When 0<α<0.1 or 0<β<0.1 or 0<δ<0.1, the fuzzification is XS. When 0.1<α<0.3 or 0.1<β<0.3 or 0.1<δ<0.3, the fuzzification is S. When When 0.3<α<0.6 or 0.3<β<0.6 or 0.3<δ<0.6, it is fuzzified to M; when 0.6<α<0.9 or 0.6<β<0.9 or 0.6<δ<0.9, it is fuzzified to L; when 0.9<α<1 or 0.9<β<1 or 0.9<δ<1, it is fuzzified to XL; the fuzzy set of γ is [S, M, L], S is small, M is medium, and L is large. When γ<0.3, it is fuzzified to S; when 0.3<γ<0.7, it is fuzzified to M; when γ>0.7, it is fuzzified to L; B-4, the control rules of the directional fuzzy logic controller are as follows: There are 9 control rules for the directional fuzzy logic controller; ①. Input terminal: L MT Fuzzy F, θ DA Fuzzy to M; Output: α1 is fuzzy to XL, γ1 is fuzzy to L; ②. Input terminal: L MT Fuzzy F, θ DA Fuzzy to SorB; Output: α1 is fuzzy to S, γ1 is fuzzy to L; ③. Input terminal: L MT Fuzzy F, θ DA Fuzzy to XSorXB; Output: α1 is fuzzy to M, γ1 is fuzzy to L; ④. Input terminal: L MT Fuzzy to M, θ DA Fuzzy to M; Output: α1 is fuzzy to M, γ1 is fuzzy to M; ⑤. Input terminal: L MT Fuzzy to M, θ DA Fuzzy to SorB; Output: α1 is fuzzy to L, γ1 is fuzzy to M; ⑥. Input terminal: L MT Fuzzy to M, θ DA Fuzzy to XSorXB; Output: α1 is fuzzy to L, γ1 is fuzzy to M; ⑦. Input terminal: L MT Fuzzy N, θ DA Fuzzy to M; Output: α1 is fuzzy to M, γ1 is fuzzy to S; ⑧. Input terminal: L MT Fuzzy N, θ DA Fuzzy to SorB; Output: α1 is fuzzy to L, γ1 is fuzzy to S; ⑨. Input terminal: L MT Fuzzy N, θ DA Fuzzy to XSorXB; Output: α1 is fuzzy to XL, γ1 is fuzzy to S; The subsequent defuzzification adopts the centroid method; B-5, the control rules of the safety fuzzy logic controller are as follows: There are 9 control rules for the safety fuzzy logic controller, among which the defuzzification method selects the centroid method; ①. Input terminal: L MO Fuzzy to F,L MT Fuzzy to F; output: β1 is fuzzy to XS, γ2 is fuzzy to L; ②. Input terminal: L MO Fuzzy to F,L MT Fuzzy to M; Output: β1 is fuzzy to S, γ2 is fuzzy to L; ③. Input terminal: L MO Fuzzy to F,L MT Fuzzy to N; Output: β1 fuzzy to S, γ2 fuzzy to S; ④. Input terminal: L MO Fuzzy to M,L MT Fuzzy to F; Output: β1 is fuzzy to M, γ2 is fuzzy to L; ⑤. Input terminal: L MO Fuzzy to M,L MT Fuzzy to M; Output: β1 is fuzzy to M, γ2 is fuzzy to M; ⑥. Input terminal: L MO Fuzzy to M,L MT Fuzzy to N; Output: β1 is fuzzy to M, γ2 is fuzzy to S; ⑦. Input terminal: L MO Fuzzy to N,L MT Fuzzy to F; output: β1 is fuzzy to XL, γ2 is fuzzy to M; ⑧. Input terminal: L MO Fuzzy to N,L MT Fuzzy to M; Output: β1 is fuzzy to L, γ2 is fuzzy to M; ⑨. Input terminal: L MO Fuzzy to N,L MT Fuzzified to N; Output: β1 is fuzzified to M, γ2 is fuzzified to M; The subsequent defuzzification adopts the centroid method; B-6, the control rules of the fusion fuzzy logic controller are as follows: There are 8 control rules of the fusion fuzzy logic controller; ①. Input terminal: L MO Fuzzy F, θ DA Fuzzified to SorB; Output: δ fuzzified to XL; ②. Input terminal: L MO Fuzzy F, θ DA Fuzzy to XSorXB; Output: δ fuzzy to L; ③. Input terminal: L MO Fuzzy F, θ DA Fuzzy to M; Output: δ fuzzy to M; ④. Input terminal: L MO Fuzzy to M, θ DA Fuzzy to XSorXB; Output: δ fuzzy to L; ⑤. Input terminal: L MO Fuzzy to M, θ DA Fuzzified to SorB; Output: δ fuzzified to M; ⑥. Input terminal: L MO Fuzzy to M, θ DA Fuzzy to M; Output: δ fuzzy to S; ⑦. Input terminal: L MO Fuzzy N, θ DA Fuzzy to XSorXB; Output: δ fuzzy to XS; ⑧. Input terminal: L MO Fuzzy N, θ DA Fuzzified to SorMorB; Output: δ fuzzified to S; The subsequent defuzzification adopts the centroid method; C, Fusion of VOA dynamic obstacle avoidance and FL-DWA algorithm: The VOA dynamic obstacle avoidance principle is as follows: In the global coordinate system XOY, the mobile robot M is located at the origin of the coordinate system and moves at a speed v m The grid obstacle o moves forward at a speed v0, Δv is the speed of the mobile robot relative to the grid obstacle, and the grid obstacle is expanded into a circular obstacle with a radius R. The circle is the minimum circumscribed circle that completely encompasses the grid obstacle. T1 and T2 are tangents from the coordinate origin to the two sides of the circular obstacle, and μ is the distance between the tangent T1 or T2 and the straight line , γ is the angle between the velocity Δv and the straight line The angle between the tangent lines T1 and T2 is 2μ, and both μ and γ are positive values. When the speed Δv is between the tangent lines T1 and T2, the mobile robot will collide with the obstacle, and the collision angle interval is 2μ. The dynamic obstacle avoidance goal is to keep Δv outside 2μ at all times, even if: |γ|>μ. Based on v m , v0 and Δv form a velocity vector triangle, we can get formula 14 and formula 15. By taking the derivative of formula 15, we get formula 16; Formula 14: Formula 15: Formula 16: Where α is the direction angle of the mobile robot, β is the direction angle of the grid obstacle, is the velocity vector v in the triangle m The angle between the positive direction of the X-axis and the straight line is θ, η is the angle between v0 and Δv in the velocity vector triangle, and θ is the angle between the positive direction of the X-axis and the straight line. The angle formed; d is the differential symbol; in addition, dγ=Δγ; dv m =Δv m ; dα=Δα; dβ=Δβ; According to formula 16, the speed v0 of the grid obstacle, the direction angle β of the grid obstacle, the change Δv0 of the grid obstacle speed and the change Δβ of the grid obstacle direction angle are known. By adjusting the speed v0 of the mobile robot m and the direction angle α of the mobile robot to change Δγ, so that the adjusted γ satisfies: γ+Δγ>μ; m and Δα are set to x1 and x2 respectively, and a dynamic obstacle avoidance objective function is established, as shown in Formula 17. Solving Formula 17 can obtain a speed tuple that satisfies dynamic obstacle avoidance. Selecting this speed tuple in the above FL-DWA algorithm can achieve dynamic obstacle avoidance. Formula 17: The parameter definitions in formula 14-17 are only applicable in formula 14-17 and cannot be applied to other formulas.

Images