A robot adaptive local dynamic obstacle avoidance method for complex environments
By introducing angular acceleration limiting and fuzzy control modules into the dynamic window method, the evaluation function is optimized, and the local minimum value problem of the dynamic window method in complex environments is solved, and the robot's obstacle avoidance ability and path smoothness are improved.
Patent Information
- Application Number
- CN202310336967.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-03-31
- Publication Date
- 2024-11-12
- Estimated Expiration
- 2043-03-31
AI Technical Summary
The existing dynamic window method is prone to falling into local minimum values in path planning and difficult to adapt to complex environments. The common adaptive fuzzy algorithm only considers a single parameter, resulting in limited obstacle avoidance capabilities of robots in complex environments.
An improved fuzzy adaptive dynamic window obstacle avoidance algorithm is proposed, and the evaluation function of the optimization algorithm is optimized to limit the angular acceleration jump, and a fuzzy control module is introduced to optimize the evaluation function in real time based on obstacle distribution characteristics and robot pose. An adaptive local dynamic obstacle avoidance method for complex environments is obtained.
It realizes the smooth path planning and obstacle avoidance ability of the robot in complex environments, avoids the sudden load of angular velocity changes, and improves the obstacle avoidance performance of the robot in complex environments.
Smart Images

Figure CN116483074B_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to robot navigation and path planning technology, and in particular to an adaptive dynamic window method based on obstacle distribution. Background Art
[0002] As early as the last century, research on robot intelligent navigation control systems has begun. With the continuous iteration and update of sensor technology and computer technology, research results related to robot navigation control continue to emerge. Although the ideas followed by various methods are different, the problem they want to solve is very clear: how to make the robot reach the target point smoothly.
[0003] The navigation control system is a broad and large system, which consists of several subsystems. Most of the work is done by the following three systems: the first is the environmental perception system, the second is the planning and decision-making system, and the third is the robot's motion control system. Among them, the planning and decision-making layer plays the role of the "brain" of the entire system. Whether the navigation process can be completed normally depends largely on the work efficiency of this layer, and the core implementation of the planning and decision-making layer is the robot's path planning process.
[0004] Therefore, the research on path planning algorithms for mobile robots is of great significance, as it helps to ensure the safety and efficiency of the robot's navigation in the environment. The dynamic environment will bring a series of challenges to the robot, requiring them to perceive and respond to changes in the surrounding environment in real time. At the same time, the robot's ability to perceive and respond to environmental changes is particularly important in application environments where the robot is close to human operation, such as service robots in homes, hospitals, and offices. They must be able to navigate safely and efficiently to perform tasks without endangering human health and safety. In order to meet these challenges, robots need robust and reliable dynamic obstacle avoidance algorithms.
[0005] The dynamic window method is commonly used in this field to solve the path planning problems in advanced technologies such as self-driving cars and robots. This algorithm takes into account the kinematic constraints of the robot, and the resulting path is smooth and the distance is optimal. By adjusting the parameters, the robot's movement can be more in line with the characteristics of obstacles in the current environment. The problems of the dynamic window method that are easy to fall into local minima and difficult to adapt to complex environments have been improved. This algorithm has a lot of room for improvement.
[0006] Since the dynamic window method takes the robot's dynamic constraints into consideration during planning, its path itself is relatively smooth, resulting in the neglect of its angular velocity change rate. However, in practical applications, sudden changes in this item often cause huge fluctuations in the control signal of the robot's drive motor. At the same time, the common improved adaptive fuzzy algorithm usually only considers one type of parameter, including obstacle distribution, robot heading, and obstacle distance, and has limited adaptability to complex environments. Summary of the invention
[0007] In view of the above-mentioned problems, the present invention proposes an improved fuzzy adaptive dynamic window obstacle avoidance algorithm. The algorithm includes two stages of optimization: 1. Optimizing the evaluation function of the algorithm to limit the problem of angular acceleration jumps in the robot's navigation process and avoid sudden large loads on the robot's motor; 2. Introducing a fuzzy control module to analyze the distribution of environmental obstacles based on the spatial data collected by the robot's sensors, and using the obstacle distribution characteristics and the robot's posture as the controller input to optimize the evaluation function structure in real time, ultimately obtaining a robot adaptive local dynamic obstacle avoidance method that can be used in complex environments. The flow chart is as follows: Figure 1 As shown, the specific implementation steps are as follows:
[0008] (1) Perform initialization operations according to the actual robot operating parameters;
[0009] (2) Establish the robot CTRV motion model and infer the posture state at the next moment from the current moment;
[0010] (3) Combine the results of step (1) and step (2) to calculate the robot trajectory sampling window V r ;
[0011] (4) Using the CTRV motion model, combined with the linear velocity resolution dv and angular velocity resolution dw, the trajectory sampling window V r Discrete sampling is performed to obtain the candidate trajectory space PC.
[0012] (5) An adaptive fuzzy evaluation function based on obstacle distribution is used to screen the candidate paths in the trajectory space PC to obtain the optimal path.
[0013] (6) Using the velocity vector (v, w) corresponding to the optimal path obtained in step (5), this vector is used as the control instruction of the robot at the next moment.
[0014] (7) Use the control instructions obtained in step (6) to control the speed of the robot, complete the obstacle avoidance function and detect whether it has reached the target point. If it has reached the target point, the process ends; if it has not reached the target point, repeat steps (3)-(7).
[0015] Step (1) specifically includes:
[0016] The parameters that need to be initialized are [v max ,v min ,w max ,w min ,a vmax ,a wmax ,d v ,d w ,ΔT] where the parameters have the following meanings in order: maximum linear velocity, minimum linear velocity, maximum angular velocity, minimum angular velocity, maximum linear acceleration, maximum angular acceleration, linear velocity resolution, angular velocity resolution, and sampling time interval.
[0017] Step (2) specifically includes:
[0018] Establish a robot motion model, infer the posture state at the next moment from the current moment, and use the CTRV model, as described in formula (1):
[0019]
[0020] In formula 1, (x t ,y t ,θ t+1 ) is the robot state at time t, (x t+1 ,y t+1 ,θ t ) is the state at the next moment, w t is the current angular velocity value; (x t ,y t ) and (x t+1 ,y t+1 ) are the coordinates of the robot at time t and time t+1 in the world coordinate system, θ t and θ t+1 is the heading angle of the robot at time t and time t+1 (the angle between the heading and the X-axis of the world coordinate system)
[0021] Step (3) specifically includes:
[0022] Calculate the robot trajectory sampling window V r , as described in Formula 2
[0023] V r =V s ∩V d ∩V a (2)
[0024] In formula 2, V s 、V d 、V a They are velocity constraints, dynamic constraints and collision constraints.
[0025] In step (3), for V s,V d ,V a , the calculation methods are:
[0026] a. Speed constraint space:
[0027] V s ={(v,w)|v∈[v min ,v max ]∩w∈[w min ,w max ]} (3)
[0028] In Formula 3, (v,w) is the robot velocity vector corresponding to the trajectory, which are linear velocity and angular velocity respectively, v min and v max is the minimum and maximum linear velocity of the robot, w min and w max are the maximum and minimum angular velocities of the robot.
[0029] b. Dynamic constraint space:
[0030] V d ={(v,w)|v∈[v c -a vmax ΔT,v c +a vmax ΔT]∩w∈[w c -a wmax ΔT,w c +a wmax ΔT]} (4)
[0031] In formula 4, v c and w c is the linear velocity and angular velocity at the current moment, where a vmax and a wmax are the maximum linear acceleration and maximum angular acceleration of the robot respectively, and ΔT is the sampling time interval.
[0032] c. Collision constraint space:
[0033]
[0034] In Formula 5, dist(v,w) is the distance between the corresponding trajectory and the nearest obstacle.
[0035] Step (4) specifically includes:
[0036] Using the robot kinematic model obtained in step (2) to describe formula 1, combined with the linear velocity resolution dv and angular velocity resolution dw, the trajectory sampling window V r Discrete sampling is performed to obtain the candidate trajectory space PC.
[0037] Step (5) specifically includes:
[0038] a. Design the objective function of the dynamic window method and introduce a new function term Wacc to limit unnecessary angular velocity mutations. The calculation method is as shown in Equation 6.
[0039]
[0040] In formula 6, w dwa is the angular velocity value corresponding to the trajectory to be evaluated, w t is the angular velocity value at the current moment, a wmax is the maximum angular acceleration.
[0041] The final evaluation function structure is as shown in Formula 7
[0042] G(v,w)=αHeading(v,w)+βDist(v,w)+γVel(v,w)+δWacc(v,w) (7)
[0043] The calculation method of each item in Formula 7 is as shown in Formula 8 and Formula 9. First, each item is calculated according to Formula 8, and then normalized using Formula 9 to finally obtain the Heading item, Dist item, and Vel item.
[0044]
[0045]
[0046] In formula 8, (x g ,y g ) is the target point coordinate, (x t ,y t ,θ t ) is the current position and heading angle of the robot, v t is the current linear velocity value, min{d obs (i)} is the distance of the obstacle closest to the robot within the radar sampling range, and R is the reserved safety space. In formula 9, func(i) represents each item in formula 8, normal func(i) is the corresponding term in Equation 7. α, β, γ, δ are the corresponding weight coefficients. Reasonable setting can greatly improve the obstacle avoidance efficiency.
[0047] b. Design a two-input and three-output fuzzy controller to control the coefficients α, β, and γ in the evaluation function to improve the algorithm's obstacle avoidance capability.
[0048] The input of the fuzzy controller is d and θ, and the output is α, β, γ. The fuzzy decision of the fuzzy controller adopts Mamdain type reasoning, and the defuzzification adopts the center of gravity method.
[0049] ① The input calculation method is as shown in formula 10
[0050]
[0051] In formula 10, d gap and θ g They represent the robot's forward channel width and the angle difference between the robot and the target node respectively. The calculation methods of these two items are as shown in Equation 11
[0052]
[0053] In formula 11, (x g ,y g ) is the target point coordinate, (x t ,y t ,θ t ) is the current position and heading angle of the robot, and and for:
[0054] In the robot's forward sensing range, the coordinates of the reflection point di with the smallest distance li detected on the left and right sides in the robot's polar coordinate system are as shown in the schematic diagram Figure 3 .
[0055] The actual value range of d in the input is [0, r / R], where r is the sensor perception radius and R is the robot hardware parameter. Since r is generally much larger than R, the domains of the two are [0, 10R] and [0, 1] respectively.
[0056] ② The membership functions used to fuzzify input and defuzzify output are designed as follows: Figure 4 and Figure 5 ;
[0057] ③Fuzzy control rule design;
[0058] The ideas followed in the design of fuzzy logic rule table are as follows:
[0059] 1) When both d and Δθ are small, it means that there is an obstacle in front of the robot that is difficult to pass. At this time, it is necessary to slow down the speed and improve the ability to bypass the obstacle, that is, reduce α and γ, and increase β;
[0060] 2) When d is too small and Δθ is too large, it means that the obstacle in front of the robot cannot directly affect the robot. In this case, β should be appropriately reduced and α should be increased;
[0061] 3) When d is too large and Δθ is too small, it means that the forward channel can pass and the target point is in the forward direction. At this time, the value of α should be increased and the value of β should be reduced;
[0062] 4) When d and Δθ are too large, it means that the robot may be in the stage of bypassing obstacles. At this time, smaller α and γ and larger β should be selected.
[0063] c. Combine the Heading term, Dist term, Vel term, Wacc term in step a and the coefficients α, β, γ obtained in step b, calculate the evaluation value G(v,w) for each trajectory in the trajectory space PC, and select the trajectory with the largest G value as the optimal trajectory. The velocity vector corresponding to the trajectory is (v,w)
[0064] Step (6) specifically includes:
[0065] Using the optimal path obtained in step (5), the control parameter corresponding to the optimal path is the velocity vector (v, w) at the next moment, and this vector is used as the control instruction of the robot at the next moment.
[0066] Step (7) specifically includes:
[0067] The control command obtained in step (6) is used to control the speed of the robot, complete the obstacle avoidance function and detect whether it has reached the target point. The judgment method is: detect whether the distance d between the robot and the target is less than the threshold RS. If so, it is considered to have reached the destination; if not, repeat steps (3)-(6).
[0068] Compared with the prior art, the present invention has the following advantages and positive effects:
[0069] 1. In the evaluation function design phase, an angular acceleration limit item is introduced to make the robot's angular velocity curve smoother by screening out candidate paths with excessive angular velocity changes. This physically limits the sudden load that the controller may output to the robot to a certain extent.
[0070] 2. In the evaluation function calculation link, a fuzzy controller based on obstacle distribution is introduced to control the evaluation function weight, so that the obstacle avoidance algorithm can obtain a suitable evaluation function calculation method under different obstacle distribution conditions, thereby improving the robot's obstacle avoidance ability in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS
[0071] Figure 1 The present invention is a flow chart of a robot local obstacle avoidance algorithm based on obstacle distribution.
[0072] Figure 2 It is a structural diagram of a fuzzy controller in a robot local obstacle avoidance algorithm based on obstacle distribution in the present invention.
[0073] Figure 3 It is an obstacle space distribution analysis diagram corresponding to the fuzzy controller input in the present invention.
[0074] Figure 4 It is the membership function of the fuzzy controller input in the present invention.
[0075] Figure 5 It is the membership function output by the fuzzy controller in the present invention.
[0076] Figure 6 It is a simulation example diagram of the method of the present invention.
[0077] Figure 7 It is the speed change curve corresponding to the simulation example. DETAILED DESCRIPTION
[0078] The present invention is described in detail below with reference to the accompanying drawings and embodiments.
[0079] The technical solution adopted by the present invention is an adaptive dynamic window method based on obstacle distribution, and the implementation steps of the method are as follows:
[0080] (1) Perform initialization operations according to the actual robot operating parameters. The parameters that need to be initialized are [v max ,v min ,w max ,w min ,a vmax ,a wmax ,dv,dw,ΔT] where the parameters have the following meanings in order: maximum linear velocity, minimum linear velocity, maximum angular velocity, minimum angular velocity, maximum linear acceleration, maximum angular acceleration, linear velocity resolution, angular velocity resolution, and sampling time interval.
[0081] (2) Establish a robot motion model, infer the posture state at the next moment from the current moment, and use the CTRV model, as described in formula (1):
[0082]
[0083] In formula 1, (x t ,y t ,θ t+1 ) is the robot state at time t, (x t+1 ,y t+1 ,θ t ) is the state at the next moment, w t is the current angular velocity value
[0084] (3) Calculate the robot trajectory sampling window V r , as described in Formula 2
[0085] V r =V s ∩V d ∩V a (2)
[0086] In formula 2, V s 、V d 、V aThey are velocity constraints, dynamic constraints and collision constraints.
[0087] In step (3), for V s 、V d 、V a , the calculation methods are:
[0088] d. Speed constraint space:
[0089] V s ={(v,w)|v∈[v min ,v max ]∩w∈[w min ,w max ]} (3)
[0090] In Formula 3, (v,w) is the robot velocity vector corresponding to the trajectory, which are linear velocity and angular velocity respectively, v min and v max is the minimum and maximum linear velocity of the robot, w min and w max are the maximum and minimum angular velocities of the robot.
[0091] e. Dynamic constraint space:
[0092] V d ={(v,w)|v∈[v c -a vmax ΔT,v c +a vmax ΔT]∩w∈[w c -a wmax ΔT,w c +a wmax ΔT]} (4)
[0093] In formula 4, v c and w c is the linear velocity and angular velocity at the current moment, where a vmax and a wmax are the maximum linear acceleration and maximum angular acceleration of the robot respectively, and ΔT is the sampling time interval.
[0094] f. Collision constraint space:
[0095]
[0096] In Formula 5, dist(v,w) is the distance between the corresponding trajectory and the nearest obstacle.
[0097] (4) Using the robot kinematic model obtained in step (2) to describe formula 1, combined with the linear velocity resolution dv and the angular velocity resolution dw, the trajectory sampling window V rDiscrete sampling is performed to obtain the candidate trajectory space PC.
[0098] (5) The trajectory space is screened using an adaptive fuzzy evaluation function based on obstacle distribution to obtain the optimal path.
[0099] Step (5) includes the following steps:
[0100] a. Design the objective function of the dynamic window method and introduce a new function term Wacc to limit unnecessary angular velocity mutations. The calculation method is as shown in Equation 6.
[0101]
[0102] In formula 6, w dwa is the angular velocity value corresponding to the trajectory to be evaluated, w t is the angular velocity value at the current moment, a wmax is the maximum angular acceleration.
[0103] The final evaluation function structure is as shown in Formula 7
[0104] G(v,w)=αHeading(v,w)+βDist(v,w)+γVel(v,w)+δWacc(v,w) (7)
[0105] The calculation method of each item in Formula 7 is as shown in Formula 8 and Formula 9. First, each item is calculated according to Formula 8, and then normalized using Formula 9 to finally obtain the Heading item, Dist item, and Vel item.
[0106]
[0107]
[0108] In formula 8, (x g ,y g ) is the target point coordinate, (x t ,y t ,θ t ) is the current position and heading angle of the robot, v t is the current linear velocity value, min{d obs (i)} is the distance of the obstacle closest to the robot within the radar sampling range, and R is the reserved safety space. In formula 9, func(i) represents each term in formula 8, and nromal func(i) is the corresponding term in Equation 7. α, β, γ, δ are the corresponding weight coefficients, and reasonable setting can greatly improve the obstacle avoidance efficiency.
[0109] b. Design a two-input and three-output fuzzy controller to control the coefficients α, β, and γ in the evaluation function to improve the algorithm's obstacle avoidance capability.
[0110] The input of the fuzzy controller is d and θ, and the output is α, β, γ. The fuzzy decision of the fuzzy controller adopts Mamdain type reasoning, and the defuzzification adopts the center of gravity method.
[0111] ① The input calculation method is as shown in formula 10
[0112]
[0113] In formula 10, d gap and θ g They represent the robot's forward channel width and the angle difference between the robot and the target node respectively. The calculation methods of these two items are as shown in Equation 11
[0114]
[0115] In formula 11, (x g ,y g ) is the target point coordinate, (x t ,y t ,θ t ) is the current position and heading angle of the robot, and and for:
[0116] In the robot's forward sensing range, the distance l detected on the left and right sides i Minimum reflection point d i The corresponding coordinates in the robot polar coordinate system.
[0117] The actual value range of d in the input is [0, r / R], where r is the sensor perception radius and R is the robot hardware parameter. Since r is generally much larger than R, the domains of the two are [0, 10R] and [0, 1] respectively.
[0118] ② The membership functions used to fuzzify input and defuzzify output are designed as follows: Figure 4 and Figure 5 .
[0119] ③The fuzzy control rule design is shown in Table 1.
[0120] Table 1 Fuzzy control rules table
[0121]
[0122]
[0123] The ideas followed in the design of fuzzy logic rule table are as follows:
[0124] 1) When both d and Δθ are small, it means that there is an obstacle in front of the robot that is difficult to pass. At this time, it is necessary to slow down the speed and improve the ability to bypass the obstacle, that is, reduce α and γ, and increase β;
[0125] 2) When d is too small and Δθ is too large, it means that the obstacle in front of the robot cannot directly affect the robot. In this case, β should be appropriately reduced and α should be increased;
[0126] 3) When d is too large and Δθ is too small, it means that the forward channel can pass and the target point is in the forward direction. At this time, the value of α should be increased and the value of β should be reduced;
[0127] 4) When d and Δθ are too large, it means that the robot may be in the stage of bypassing obstacles. At this time, smaller α and γ and larger β should be selected.
[0128] c. Combine the Heading term, Dist term, Vel term, Wacc term in step a and the coefficients α, β, γ obtained in step b, calculate the evaluation value G(v,w) for each trajectory in the trajectory space PC, and select the trajectory with the largest G value as the optimal trajectory. The velocity vector corresponding to the trajectory is (v,w)
[0129] (6) Using the optimal path obtained in step (5), the control parameter corresponding to the optimal path is the velocity vector (v, w) at the next moment, and this vector is used as the control instruction of the robot at the next moment.
[0130] (7) Use the control command obtained in step (6) to control the speed of the robot, complete the obstacle avoidance function and detect whether it has reached the target point. The judgment method is: detect whether the distance d between the robot and the target is less than the threshold RS. If so, it is considered to have reached the destination; if not, repeat steps (3)-(6).
[0131] Description of the effect of obstacle avoidance method simulation example:
[0132] Depend on Figure 6 to Figure 7 It can be seen that the robot not only successfully bypassed the L-shaped local minimum point, but also passed through the narrow passage without collision and the path was smooth without distortion. At the same time, the angular velocity changed smoothly without obvious fluctuations, which means that the robot's angular acceleration changed steadily. However, the original DWA algorithm fell into the local minimum and could not bypass obstacles, and ultimately failed in planning.
[0133] The simulation comparison results show that the improved Adp_DWA algorithm has obvious advantages in obstacle avoidance compared with the previous DWA algorithm.
Claims
1. A robot adaptive local dynamic obstacle avoidance method that can be used in complex environments, characterized in that: The specific implementation steps of this method are as follows: (1) Perform initialization operations according to the actual robot operating parameters; (2) Establish the robot CTRV motion model and infer the posture state at the next moment from the current moment; (3) Combine the results of step (1) and step (2) to calculate the robot trajectory sampling window V r ; (4) Using the CTRV motion model, combined with the linear velocity resolution dv and angular velocity resolution dw, the trajectory sampling window V r Discrete sampling is performed to obtain the PC of the trajectory space to be selected; (5) Using the adaptive fuzzy evaluation function based on obstacle distribution to screen the candidate paths in the trajectory space PC, the optimal path is obtained; (6) Using the velocity vector (v, w) corresponding to the optimal path obtained in step (5), the vector becomes the control instruction of the robot at the next moment; (7) Using the control command obtained in step (6) to control the speed of the robot, complete the obstacle avoidance function and detect whether it has reached the target point. If it has reached the target point, the process ends; if it has not reached the target point, steps (3) to (7) are repeated. Step (5) specifically includes: a. Design the objective function of the dynamic window method and introduce a new function term Wacc to limit unnecessary angular velocity mutations. Its calculation method is as follows: In formula (6), w dwa is the angular velocity value corresponding to the trajectory to be evaluated, w t is the angular velocity value at the current moment, a wmax is the maximum angular acceleration; the final evaluation function structure is as follows: G(v,w)=αHeading(v,w)+βDist(v,w)+yVel(v,w)+δWacc(v,w)(7) The calculation method of each item in formula (7) is as shown in formula (8) and formula (9). First, each item is calculated according to formula (8), and then normalized using formula (9), and finally Heading item, Dist item, and Vel item are obtained; In formula (8), (x g ,y g ) is the target point coordinate, (x t ,y t ,θ t ) is the current position and heading angle of the robot, v t is the current linear velocity value, min{d obs (i)} is the distance of the obstacle closest to the robot within the radar sampling range, R is the reserved safety space; in formula (9), func(i) represents each item in formula (8), normal func(i) is the corresponding item in formula (7); α, β, γ, δ are the corresponding weight coefficients; b. Design a two-input and three-output fuzzy controller to control the coefficients α, β, and γ in the evaluation function to improve the obstacle avoidance capability of the algorithm; The input of the fuzzy controller is d and θ, and the output is α, β, γ. The fuzzy decision of the fuzzy controller adopts Mamdain type reasoning, and the defuzzification adopts the centroid method; ① The input calculation method is as follows: In formula (10), d gap and θ g They represent the robot's forward channel width and the angle difference between the robot and the target node respectively. The calculation methods of these two items are as follows: In formula (11), (x g ,y g ) is the target point coordinate, (x t ,y t ,θ t ) is the current position and heading angle of the robot, and and is: the distance l detected on the left and right sides in the robot's forward sensing range i Minimum reflection point d i The corresponding coordinates in the robot polar coordinate system; The actual value range of d in the input is [0, r / R], where r is the sensor sensing radius and R is the robot hardware parameter; ② Design of membership function for fuzzifying input and defuzzifying output; ③Fuzzy control rule design; 1) When both d and Δθ are small, it means that there is an obstacle in front of the robot that is difficult to pass. At this time, it is necessary to slow down the speed and improve the ability to bypass the obstacle, that is, reduce α and γ, and increase β; 2) When d is too small and Δθ is too large, it means that the obstacle in front of the robot cannot directly affect the robot. In this case, β should be appropriately reduced and α should be increased; 3) When d is too large and Δθ is too small, it means that the forward channel has passed and the target point is in the forward direction. At this time, the value of α should be increased and the value of β should be reduced; 4) When d and Δθ are too large, it means that the robot may be in the stage of bypassing obstacles. At this time, smaller α and γ and larger β should be selected; c. Combine the Heading item, Dist item, Vel item, Wacc item in step a and the coefficients α, β, γ obtained in step b, calculate the evaluation value G(v, w) for each trajectory in the trajectory space PC, and select the trajectory with the largest G value as the optimal trajectory. The velocity vector corresponding to the trajectory is (v, w).
2. The method of adaptive local dynamic obstacle avoidance of a robot in a complex environment according to claim 1, characterized in that: Step (1) specifically includes: The parameters that need to be initialized are [v max , v min , w max , w min , a vmax , a wmax , dv, dw, ΔT] where the parameters have the following meanings in order: maximum linear velocity, minimum linear velocity, maximum angular velocity, minimum angular velocity, maximum linear acceleration, maximum angular acceleration, linear velocity resolution, angular velocity resolution, and sampling time interval.
3. The method of adaptive local dynamic obstacle avoidance of a robot in a complex environment according to claim 1, characterized in that: Step (2) specifically includes: The robot motion model is established, and the posture state at the next moment is inferred from the current moment. The CTRV model is used, which is described as formula (1): In formula (1), (x t ,y t ,θ t ) is the robot state at time t, (x t+1 ,y t+1 ,θ t+1 ) is the state at the next moment, w t is the current angular velocity value; (x t ,y t ) and (x t+1 ,y t+1 ) are the coordinates of the robot at time t and time t+1 in the world coordinate system, θ t and θ t+1 is the heading angle of the robot at time t and time t+1.
4. The method of adaptive local dynamic obstacle avoidance of a robot in a complex environment according to claim 1, characterized in that: Step (3) specifically includes: Calculate the robot trajectory sampling window V r , described as formula (2): V r =V s ∩V d ∩V a (2) In formula (2), V s 、V d 、V a They are velocity constraints, dynamic constraints and collision constraints.
5. The method of adaptive local dynamic obstacle avoidance of a robot in a complex environment according to claim 1, characterized in that: In step (3), for V s , V d , V a , the calculation methods are: a. Speed constraint space: V s ={(v,w)|v∈[v min ,v max ]∩w∈[w min ,w max ]} (3) In formula (3), (v, w) is the robot velocity vector corresponding to the trajectory, which are linear velocity and angular velocity respectively, v min and v max is the minimum and maximum linear velocity of the robot, w min and w max is the maximum and minimum angular velocity of the robot; b. Dynamic constraint space: V d ={(v,w)|v∈[v c -a max ΔT,v c +a vmax ΔT]∩w∈[w c -a wmax ΔT,w c +a wmax ΔT]} (4) In formula (4), v c and w c is the linear velocity and angular velocity at the current moment, where a vmax and a wmax are the maximum linear acceleration and maximum angular acceleration of the robot, respectively, and ΔT is the sampling time interval; c. Collision constraint space: In formula (5), dist(v, w) is the distance between the corresponding trajectory and the nearest obstacle.
6. The method of adaptive local dynamic obstacle avoidance of a robot in a complex environment according to claim 1, characterized in that: Step (4) specifically includes: Using the robot kinematic model described by formula (1) obtained in step (2), combined with the linear velocity resolution dv and the angular velocity resolution dw, the trajectory sampling window V r Discrete sampling is performed to obtain the candidate trajectory space PC.
7. The method of adaptive local dynamic obstacle avoidance of a robot in a complex environment according to claim 1, characterized in that: Step (6) specifically includes: Using the optimal path obtained in step (5), the control parameter corresponding to the optimal path is the velocity vector (v, w) at the next moment, and this vector is used as the control instruction of the robot at the next moment.
8. The method of adaptive local dynamic obstacle avoidance of a robot in a complex environment according to claim 1, characterized in that: Step (7) specifically includes: The control command obtained in step (6) is used to control the speed of the robot, complete the obstacle avoidance function and detect whether it has reached the target point. The judgment method is: detect whether the distance d between the robot and the target is less than the threshold RS. If so, it is considered to have reached the destination; if not, repeat steps (3)-(7).
