Robot local path planning method and device

Abstract

The invention discloses a robot local path planning method and device, and belongs to the technical field of robot autonomous navigation, and the method comprises the steps: loading an environment map and robot initial data; circularly executing the following steps until the robot reaches the target point: calculating the distance from the robot to the target point and the distance from the robot to the nearest obstacle in real time; calculating a dynamic window and a speed combination in the window; inputting the sum into a pre-constructed fuzzy controller, and outputting a weight factor of an improved DWA algorithm evaluation function; inputting the speed combination into a pre-constructed differential motion model to predict a trajectory, evaluating the predicted trajectory according to an improved DWA algorithm evaluation function, and screening out an optimal path; and outputting a speed combination corresponding to the optimal path as control information of the next moment. The problem that an existing local path planning method cannot adapt to complex scenes and is insufficient in obstacle avoidance capacity can be solved.

Description

Technical Field

[0001] This invention belongs to the field of robot autonomous navigation technology, specifically relating to a robot local path planning method and apparatus. Background Technology

[0002] In the field of autonomous robot navigation, global path planning can obtain a path sequence to avoid static obstacles. However, in actual movement, robots also need to deal with unknown obstacles, dynamic obstacles and acute obstacles in a local area, and perform real-time obstacle avoidance and trajectory planning, which is local path planning.

[0003] While the Dynamic Window (DWA) algorithm is real-time and efficient, and suitable for local path planning and control, it also has problems such as local optima and limited environmental adaptability, which affect the robot's navigation performance in complex environments.

[0004] Traditional DWA (Driving-Avoidance-Walk) algorithms are prone to getting stuck in local optima when encountering U-shaped or concave obstacle environments. This is because the algorithm only performs local planning based on current sensor information. When the robot enters a closed area, the goal-oriented and obstacle-avoidance terms in the evaluation function generate conflicting gradient directions, causing the robot to oscillate repeatedly or stagnate near the obstacle. Especially in narrow passages or complex maze environments, the robot may be completely unable to autonomously escape the local optimum, severely impacting the completion of the navigation task.

[0005] When faced with dynamic obstacles, traditional DWA relies solely on obstacle information at the current moment for obstacle avoidance decisions, lacking the ability to predict the obstacle's motion state. This leads to two typical problems: first, when the obstacle's motion direction intersects with the robot's path, a collision may occur due to reaction delay; second, it is prone to over-avoidance or under-avoidance of obstacles with variable speeds, resulting in path redundancy or reduced safety.

[0006] When obstacles suddenly appear in the environment, the evaluation function mechanism of traditional DWA (Distance-Based Avoidance) is difficult to make an optimal response in a timely manner. Because the algorithm uses a multi-objective optimization strategy with fixed weights, it cannot adaptively adjust the obstacle avoidance strategy in emergency situations, which may lead to safety hazards such as insufficient emergency stopping distance and excessive steering.

[0007] Therefore, it is urgent to improve the traditional DWA algorithm to enhance the robot's obstacle avoidance and environmental adaptability. Summary of the Invention

[0008] The purpose of this invention is to overcome the shortcomings of the prior art and provide a robot local path planning method and apparatus to solve the problems that existing local path planning methods cannot adapt to complex scenarios and have insufficient obstacle avoidance capabilities.

[0009] To solve the above-mentioned technical problems, the present invention is implemented using the following technical solution:

[0010] In a first aspect, the present invention provides a robot local path planning method, comprising:

[0011] Load the environment map including static obstacle information and the robot's current state, and set the robot's target point coordinates and kinematic parameters;

[0012] Repeat the following steps until the target point is reached:

[0013] Real-time calculation of distance from robot to target point and distance to the nearest obstacle ;

[0014] The dynamic window is calculated based on kinematics and environmental constraints, and the velocity combination consisting of linear velocity and angular velocity is sampled within the window;

[0015] based on and The weight factors of the improved DWA algorithm evaluation function are calculated using a pre-built fuzzy controller. The improvements to the DWA algorithm evaluation function include: introducing an angular velocity evaluation mechanism to improve the velocity evaluation function, and adding a target distance function that quantifies the distance to the target point.

[0016] The velocity combination is input into a pre-built differential motion model to predict the trajectory, and the predicted trajectory is evaluated according to the improved DWA algorithm evaluation function to select the optimal path.

[0017] The speed combination corresponding to the optimal path is output as the control information for the next moment.

[0018] The aforementioned robot local path planning method, wherein the robot's current state includes: the robot's posture information and control information at time t, the posture information... ,in express The position of the Time Robot in the world coordinate system. express Heading angle at any time; setting control information ,in This represents the initial linear velocity. This represents the initial angular velocity;

[0019] Robot kinematic parameters include: minimum linear velocity Maximum linear velocity Minimum angular velocity Maximum angular velocity Maximum linear acceleration Maximum angular acceleration Linear velocity sampling frequency and angular velocity sampling frequency .

[0020] The aforementioned robot local path planning method, wherein sampling the velocity combination composed of linear velocity and angular velocity within the window includes: according to The sampling frequency is dynamically adjusted, and the sampling speed is combined within a dynamic window.

[0021] The aforementioned robot local path planning method, wherein the dynamic window calculation based on kinematics and environmental constraints includes:

[0022] Kinematic constraints include hardware limit constraints and acceleration constraints:

[0023] The hardware limit constraints The calculation formula is:

[0024] ,

[0025] In the formula, linear velocity and angular velocity The combination of speeds;

[0026] The acceleration constraint The calculation formula is:

[0027] ,

[0028] In the formula, Sampling time; The predicted linear velocity for the next moment; The predicted angular velocity for the next moment;

[0029] The environmental constraints The calculation formula is:

[0030] ,

[0031] In the formula, For the current speed combination The distance between the trajectory of the movement and the nearest obstacle;

[0032] Dynamic window The calculation formula is:

[0033] ;

[0034] According to Dynamically adjusting the sampling frequency includes:

[0035] Dynamically adjust linear velocity sampling frequency The calculation formula is:

[0036] ,

[0037] In the formula, k represents the preset adjustment parameter of the sampling frequency. ; This indicates the distance between the robot's current position and the nearest obstacle. Indicates the maximum safe distance;

[0038] Dynamically adjust angular velocity sampling frequency The calculation formula is:

[0039] .

[0040] The aforementioned robot local path planning method, wherein the improvement of the velocity evaluation function by introducing an angular velocity evaluation mechanism includes:

[0041] Introducing a dynamic angular velocity evaluation mechanism into the velocity evaluation function Constructing an improved velocity evaluation function Improved speed evaluation function The calculation formula is:

[0042] ,

[0043] In the formula, and This indicates the preset scaling factor;

[0044] ,

[0045] In the formula, This indicates the distance from the robot's current position to the nearest obstacle. This represents the distance from the predicted trajectory point to the nearest obstacle. Represents the angular velocity at time t;

[0046] The target distance function that incorporates quantization and the distance to the target point includes:

[0047] Introduce a target distance function that quantifies the distance to the target point into the evaluation function. Target distance function The calculation formula is:

[0048] ,

[0049] In the formula, Indicates the distance from the starting point to the ending point. This represents the distance from the predicted trajectory point to the target point;

[0050] Improved DWA algorithm evaluation function The calculation formula is:

[0051] ,

[0052] In the formula, Indicates the orientation angle weighting factor; This represents the orientation angle evaluation function; Indicates the distance weighting factor; Represents the safe distance evaluation function; Indicates the speed weighting factor; This represents the target weight factor.

[0053] The aforementioned robot local path planning method, the construction of the fuzzy controller includes:

[0054] Will and As input, weighting factors , , and As the output, design a fuzzy controller with two inputs and four outputs;

[0055] Determine each input variable and The scope of the discourse and each output variable , , and The scope of the discourse;

[0056] Define input membership functions for each input variable and defuzzified output membership functions for each output variable; wherein, defining defuzzified output membership functions for each output variable includes: based on weight factors and The service is for path effectiveness, while the weighting factor and For security purposes, weighting factors and Output membership function and weighting factor and The output membership function is used to differentiate the classification;

[0057] Rules for designing fuzzy controllers;

[0058] Fuzzy inference is performed using the Mamdani model;

[0059] The fuzzy output variables are defuzzified using the centroid method.

[0060] The aforementioned robot local path planning method, input variables and The fuzzy set is {JS, ZM, YL}, where JS, ZM, and YL correspond to the three fuzzy concepts of near, medium, and far distance, respectively, used to describe the different degrees of fuzziness of the input variables in the universe of discourse; the fuzzy sets of output variables α and δ are {XS, S, M, L}, and the fuzzy sets of output variables β and γ are {S, M, L}, where XS, S, M, and L correspond to the four fuzzy concepts of small, small, medium, and large, respectively, used to describe the different degrees of fuzziness of the output variables in the universe of discourse; the designed fuzzy controller rules include:

[0061] when Belongs to JS and When JS is involved, α belongs to L, β belongs to L, γ belongs to S, and δ belongs to XS; when Belongs to JS and When ZM is involved, α belongs to M, β belongs to M, γ belongs to M, and δ belongs to S; when Belongs to JS and When YL, α belongs to L, β belongs to S, γ belongs to M, and δ belongs to S; when Belongs to ZM and When JS is involved, α belongs to S, β belongs to L, γ belongs to S, and δ belongs to S; when Belongs to ZM and When ZM is involved, α belongs to M, β belongs to M, γ belongs to M, and δ belongs to M; when ZM is involved, α belongs to M, β belongs to M, γ belongs to M, and δ belongs to M. Belongs to ZM and When α belongs to YL, β belongs to S, γ belongs to L, and δ belongs to M; when Belongs to YL and When JS is involved, α belongs to XS, β belongs to L, γ belongs to S, and δ belongs to S; when Belongs to YL and When ZM is involved, α belongs to S, β belongs to M, γ belongs to L, and δ belongs to M; when Belongs to YL and When YL, α belongs to S, β belongs to S, γ belongs to L, and δ belongs to L.

[0062] The aforementioned robot local path planning method uses a pre-constructed differential motion model. include:

[0063] ,

[0064] In the formula, This represents the x-axis coordinate of the predicted attitude information in the world coordinate system at the next moment; This represents the y-axis coordinate of the predicted attitude information in the world coordinate system at the next moment; Indicates the predicted heading angle for the next moment; Indicates the predicted linear velocity at the next moment; Indicates the predicted angular velocity at the next moment; Indicates the linear velocity of the sample; Indicates the angular velocity of the sample;

[0065] The velocity combination is input into a pre-built differential motion model to predict the trajectory, and the predicted trajectory is evaluated according to the improved DWA algorithm evaluation function to obtain the optimal path, including:

[0066] Attitude information at the current moment The improved DWA algorithm evaluation function evaluates the highest speed combination. Input differential motion model The optimal predicted trajectory for the next moment is obtained as the optimal path.

[0067] The aforementioned robot local path planning method also includes designing simulation experiments to verify the local route planning effect of the robot local path planning method;

[0068] The simulation experiments include local optimum experiments, dynamic obstacle experiments, and acute obstacle experiments;

[0069] The local optimum experiment sets up U-shaped or concave obstacles, with the starting point inside the groove and the target point outside, to verify whether the robot's local path planning method will get stuck in the local optimum region or cause the trajectory to go around in circles.

[0070] The dynamic obstacle experiment sets up dynamic obstacles to verify the robot's local path planning method's real-time obstacle avoidance capability against dynamic obstacles.

[0071] The acute obstacle experiment sets up a sudden obstacle to verify the robot's local path planning method's emergency response capability to sudden obstacles.

[0072] In a second aspect, the present invention provides a robot local path planning device, including a preprocessing module, a loop module, a distance calculation module, a weight calculation module, a path filtering module, and an output control module;

[0073] The preprocessing module is used to: load an environmental map including static obstacle information and the robot's current state, and set the robot's target point coordinates and kinematic parameters;

[0074] The loop module is used to repeatedly execute the steps of the following module until the target point is reached:

[0075] The distance calculation module is used to: calculate the distance from the robot to the target point in real time. and distance to the nearest obstacle ;

[0076] The weight calculation module is used to: calculate a dynamic window based on kinematics and environmental constraints, and sample velocity combinations composed of linear velocity and angular velocity within the window; based on... and The weight factors of the improved DWA algorithm evaluation function are calculated using a pre-built fuzzy controller. The improvements to the DWA algorithm evaluation function include: introducing an angular velocity evaluation mechanism to improve the velocity evaluation function, and adding a target distance function that quantifies the distance to the target point.

[0077] The path selection module is used to: input the speed combination into a pre-built differential motion model to predict the trajectory and evaluate the predicted trajectory according to the improved DWA algorithm evaluation function to select the optimal path;

[0078] The output control module is used to output the speed combination corresponding to the optimal path as the control information for the next moment.

[0079] Compared with the prior art, the beneficial effects achieved by the present invention are as follows:

[0080] The robot local path planning method of this invention combines fuzzy control with the DWA algorithm to plan the robot's path in real time based on the distance from the robot to the target point. and distance to the nearest obstacle The weight factors of the improved DWA algorithm evaluation function are dynamically adjusted to maintain the real-time performance of path planning while improving the environmental adaptability, safety and motion quality of the robot's local path planning method. This can solve the problem that existing local path planning methods cannot adapt to complex scenarios and have insufficient obstacle avoidance capabilities.

[0081] The robot local path planning method of this invention is particularly suitable for local path planning on flat surfaces. Compared to the traditional DWA algorithm, the robot local path planning method of this invention improves the evaluation function of the traditional DWA algorithm: focusing on the velocity function... Introducing an angular velocity evaluation mechanism and defining a target distance function. To balance robot target proximity and directional consistency and improve algorithm robustness, the robot local path planning method of this invention also proposes to consider the distance to the nearest obstacle. The sampling frequency is dynamically adjusted, increasing in areas with low or far obstacles and shortening the sampling time, thus improving the search efficiency and capability of velocity sampling within a dynamic window under different environments. The robot local path planning method of this invention integrates fuzzy control theory, designing fuzzy rules to dynamically adjust the weight factors α, β, γ, and δ of the improved DWA algorithm evaluation function under different local environments, improving environmental adaptability. The weight factors of the improved DWA algorithm evaluation function are optimized in real time using a fuzzy controller, adaptively balancing path optimality and obstacle avoidance safety based on target and obstacle distances, enhancing the robot's ability to escape from complex environments such as concave obstacles and improving its response to dynamic and sudden obstacles. The robot local path planning method of this invention also designs simulation experiments for local optimality, dynamic obstacles, and acute obstacle scenarios. The results show that compared to the traditional DWA algorithm, the robot local path planning method of this invention has smoother trajectory, significantly optimized iteration count, higher motion efficiency, and higher linear velocity. and angular velocity It offers more stable control and better adaptability to dynamic and sudden obstacle environments. Attached Figure Description

[0082] Figure 1 This is a flowchart illustrating a robot local path planning method according to Embodiment 1 of the present invention;

[0083] Figure 2 This is a flowchart illustrating the usage process of the fuzzy controller in Embodiment 1 of the present invention.

[0084] Figure 3 This is a schematic diagram of the fuzzy controller performing fuzzy control in the robot local path planning method of Embodiment 1 of the present invention;

[0085] Figure 4 This is a schematic diagram of the fuzzy controller input / output design of the robot local path planning method according to Embodiment 1 of the present invention;

[0086] Figure 5 These are the input variables defined in the robot local path planning method of Embodiment 1 of the present invention. and A schematic diagram of the membership function;

[0087] Figure 6 The weighting factor is defined in the robot local path planning method of Embodiment 1 of the present invention. and A schematic diagram of the output membership function;

[0088] Figure 7 The weighting factor is defined in the robot local path planning method of Embodiment 1 of the present invention. and A schematic diagram of the output membership function;

[0089] Figure 8 This is the fuzzy controller input of the robot local path planning method in Embodiment 1 of the present invention. , The curve showing the relationship between the output weighting factor α and the output weighting factor α.

[0090] Figure 9 This is the fuzzy controller input of the robot local path planning method in Embodiment 1 of the present invention. , The curve showing the relationship between the output weighting factor β and the output weighting factor β.

[0091] Figure 10 This is the fuzzy controller input of the robot local path planning method in Embodiment 1 of the present invention. , The curve showing the relationship between the output weighting factor γ and the output weighting factor γ.

[0092] Figure 11 This is the fuzzy controller input of the robot local path planning method in Embodiment 1 of the present invention. , The curve showing the relationship between the output weighting factor δ;

[0093] Figure 12 This is a schematic diagram of the path planning process in the local optimum experiment of the robot local path planning method in Embodiment 1 of the present invention;

[0094] Figure 13 This is a schematic diagram showing the changes in weight factors in Experiment 1, Local Optimization Experiment, of the robot local path planning method in Embodiment 1 of the present invention.

[0095] Figure 14 This is a schematic diagram of the robot linear velocity variation curve in the local optimum experiment of the robot local path planning method in Embodiment 1 of the present invention.

[0096] Figure 15 This is a schematic diagram of the robot angular velocity variation curve in the local optimum experiment of the robot local path planning method in Embodiment 1 of the present invention.

[0097] Figure 16 This is a schematic diagram of the overall path planning for the dynamic obstacle experiment in Experiment 2 of the robot local path planning method of Embodiment 1 of the present invention;

[0098] Figure 17 This is a schematic diagram of the dynamic obstacle avoidance process in Experiment 2 of the robot local path planning method of Embodiment 1 of the present invention.

[0099] Figure 18This is a schematic diagram showing the weight factor changes in Experiment 2, the dynamic obstacle experiment, of the robot local path planning method according to Embodiment 1 of the present invention.

[0100] Figure 19 This is a schematic diagram of the linear velocity curve in Experiment 2 of the dynamic obstacle experiment of the robot local path planning method of Embodiment 1 of the present invention;

[0101] Figure 20 This is a schematic diagram of the angular velocity curve of the dynamic obstacle experiment in Experiment 2 of the robot local path planning method of Embodiment 1 of the present invention;

[0102] Figure 21 This is a schematic diagram of the acute obstacle avoidance process in the acute obstacle experiment of the robot local path planning method in Embodiment 1 of the present invention.

[0103] Figure 22 This is a schematic diagram showing the changes in weight factors in Experiment 3, the acute obstacle experiment, of the robot local path planning method in Embodiment 1 of the present invention.

[0104] Figure 23 This is a schematic diagram of the linear velocity curve in the acute obstacle experiment of the robot local path planning method of Embodiment 1 of the present invention;

[0105] Figure 24 This is a schematic diagram of the angular velocity curve of the acute obstacle experiment in the robot local path planning method of Embodiment 1 of the present invention. Detailed Implementation

[0106] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the embodiments and specific features in the embodiments are detailed descriptions of the technical solution of the present application, rather than limitations thereof. In the absence of conflict, the embodiments and technical features in the embodiments can be combined with each other.

[0107] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. After reading this invention, any modifications of the invention in various equivalent forms by those skilled in the art will fall within the scope defined by the appended claims.

[0108] Example 1:

[0109] Figure 1 This is a flowchart of the robot local path planning method in Embodiment 1 of the present invention. This flowchart only illustrates the logical sequence of the method described in this embodiment. Provided there are no conflicts, different methods may be used in other possible embodiments of the present invention. Figure 1 Complete the steps shown or described in the order indicated.

[0110] This invention provides a robot local path planning method, comprising:

[0111] S1: Load the environment map including static obstacle information and the robot's current state, and set the robot's target point coordinates and kinematic parameters;

[0112] S2: Real-time calculation of the distance from the robot to the target point and distance to the nearest obstacle ;

[0113] S3: Calculates a dynamic window based on kinematics and environmental constraints, and samples the velocity combination composed of linear velocity and angular velocity within the dynamic window;

[0114] based on and The weight factors of the improved DWA algorithm evaluation function are calculated using a pre-built fuzzy controller. The improvements to the DWA algorithm evaluation function include: introducing an angular velocity evaluation mechanism to improve the velocity evaluation function, and adding a target distance function that quantifies the distance to the target point.

[0115] S4: Input the velocity combination into the pre-built differential motion model to predict the trajectory and evaluate the predicted trajectory according to the improved DWA algorithm evaluation function to select the optimal path;

[0116] S5: Output the speed combination corresponding to the optimal path as the control information for the next moment;

[0117] S6: Repeat steps S2-S5 until the target point is reached.

[0118] In step S1, the robot's current state includes: the robot's posture information and control information at time t. ,in express The position of the Time Robot in the world coordinate system. Indicates heading angle; control information ,in Represents the linear velocity at time t. Indicates the angular velocity at time t; sets the initial control information. ,in This represents the initial linear velocity. This represents the initial angular velocity;

[0119] Robot kinematic parameters include: minimum linear velocity Maximum linear velocity Minimum angular velocity Maximum angular velocity Maximum linear acceleration Maximum angular acceleration Linear velocity sampling frequency and angular velocity sampling frequency .

[0120] Step S2 includes: calculating the distance from the robot's current point to the target point in real time based on the current environment, coordinates, and radar information. Return to the nearest obstacle distance Save the current time and .

[0121] In step S3, calculating the dynamic window and sampling the velocity combination consisting of linear velocity and angular velocity within the window includes: calculating the dynamic window based on kinematics and environmental constraints and according to... Dynamically adjust the sampling frequency and combine sampling speeds within a dynamic window;

[0122] The dynamic window calculated based on kinematics and environmental constraints includes:

[0123] Kinematic constraints include hardware limit constraints and acceleration constraints:

[0124] The hardware limit constraints This refers to the upper and lower limits of speed, or hardware limit constraints, that exist based on the actual situation and the robot's physical structure. The calculation formula is:

[0125] (1)

[0126] In the formula, linear velocity and angular velocity The combination of speeds;

[0127] The acceleration constraint This refers to the fact that, due to the influence of the output torque of power devices such as motors, the robot's movement speed cannot be within the sampling time range within the acceleration range. Acceleration constraints to reach target speed. The calculation formula is:

[0128] (2)

[0129] In the formula, The predicted linear velocity for the next moment; The predicted angular velocity for the next moment;

[0130] The environmental constraints In addition to the robot's own constraints, the robot's movement speed is also limited by obstacles in the surrounding environment. To ensure that the robot slows down and avoids obstacles before colliding with them, speed constraints and environmental constraints are set for movement within a safe area. The calculation formula is:

[0131] (3),

[0132] In the formula, For the current speed combination The distance between the trajectory of the movement and the nearest obstacle;

[0133] Combining the three constraints of formulas (1), (2), and (3), after various restrictions are imposed, the velocity sampling space within the dynamic time window should be the intersection of the velocity sets under each constraint. The calculation formula is:

[0134] (4).

[0135] according to Dynamically adjusting the sampling frequency includes:

[0136] Traditional DWA algorithms use a fixed sampling frequency for prediction. Data is collected, and the linear velocity sampling frequency of the robot model is set. The angular velocity sampling frequency is 0.01. for The sampling frequency relates to the number of samples needed for the predicted trajectory. When obstacles are far away or few, an excessively frequent sampling frequency is unnecessary, as this would waste computer resources. The sampling frequency is dynamically adjusted based on the obstacle distance. The formulas for calculating the dynamically adjusted linear velocity and angular velocity sampling frequencies are as follows:

[0137] (5)

[0138] (6)

[0139] In the formula, k represents the adjustment parameter of the sampling frequency, where , This indicates the distance between the robot's current position and the nearest obstacle. This represents the maximum safe distance, indicating the farthest distance a robot is allowed to approach an obstacle; beyond this distance, it is considered completely safe.

[0140] In step S3, the sampling rate combination within the sampling window includes: within a continuous dynamic window Internally, based on the number of sampling points for speed... Converted into discretized points.

[0141] In step S3, the improvement process of the DWA algorithm evaluation function includes:

[0142] The DWA algorithm is a speed-centric planning method that makes it perform well in situations with high real-time requirements, especially in the fields of indoor mobile robots and autonomous mobile robots in driverless vehicles. Therefore, the DWA algorithm is chosen for local path planning in point-to-point autonomous navigation, enabling the robot to safely reach the designated detection point while traversing dynamic environments with high uncertainty in changing measurement areas.

[0143] The Dynamic Window (DWA) algorithm evaluates and selects the optimal trajectory for the next moment using an evaluation function. The DWA algorithm evaluation function G is expressed as:

[0144] (7),

[0145] Direction angle evaluation function Let α represent the angular difference between the robot and the target point. The goal is to minimize the deviation between the robot and the target direction. In equation (7), α represents the orientation angle weighting factor, and the initial orientation angle evaluation function is... The formula is:

[0146] (8),

[0147] In the formula, This represents the angle between the line connecting the end and the endpoint of the simulated trajectory of the robot and the axis. This represents the robot's attitude angle at the end of the simulated trajectory;

[0148] Preliminary direction angle evaluation function The formula for normalization is:

[0149] (9),

[0150] In the formula, i represents the resolution at a certain v and w. The index variable for velocity combinations; n represents the total number of index variable entries for the velocity combination sequence at this resolution; The preliminary direction angle evaluation function represents the i-th velocity combination. The normalized evaluation value of the orientation angle evaluation function;

[0151] Will Replace with and , corresponding to Replace with and This allows for the evaluation of the initial safe distance function. and preliminary velocity evaluation function The normalization process yields the normalized safety distance evaluation function. Evaluation value and speed evaluation function Evaluation value;

[0152] Safe distance evaluation function Let represent the closest distance between the robot and the obstacle. The goal is to maximize the distance to the obstacle. In equation (7), β represents the distance weighting factor. The preliminary safe distance evaluation function is the set of predicted positions and distances to obstacles for the robot at the next moment, after normalization. The formula is expressed as:

[0153] (10)

[0154] Velocity evaluation function Let γ represent the robot's moving speed. The goal is to maximize the speed to improve efficiency. In equation (7), γ represents the speed weighting factor. The linear velocity of the predicted trajectory cluster is represented and normalized, forming the initial velocity evaluation function. The formula is expressed as:

[0155] (11),

[0156] The DWA algorithm selects the trajectory with the highest evaluation from the calculated evaluation function G as the attitude output for the next time step.

[0157] This embodiment improves the evaluation function G of the DWA algorithm:

[0158] (1) Function improvements:

[0159] The function mainly evaluates the movement speed of the mobile robot, but the traditional DWA algorithm only considers the selection of linear velocity and ignores the factor of robot angular velocity, which makes it difficult for the robot to perform repeated small-amplitude movements within the local optimum.

[0160] against The function incorporates angular velocity as a factor, constructing a dynamic angular velocity evaluation mechanism that considers the influence of distance. When the predicted trajectory is close to the obstacle, the angular velocity influence of that cluster is increased to guide... The function selects trajectory clusters with high angular velocities. When trapped in a local optimum, it increases the influence of angular velocity to enable the robot to quickly rotate and escape the trap area. When the predicted trajectory is far from the obstacle, it weakens the influence of angular velocity and prioritizes linear velocity. The dynamic angular velocity evaluation mechanism is defined as follows:

[0161] (12)

[0162] In the formula This indicates the distance from the robot's current position to the nearest obstacle. This represents the distance from the predicted trajectory point to the nearest obstacle. Represents the angular velocity at time t;

[0163] Improved The function is redefined as:

[0164] (13)

[0165] In the formula, and This represents the preset ratio coefficient between the velocity evaluation mechanism and the angular velocity evaluation mechanism, which optimizes the robot's motion behavior by adjusting the ratio.

[0166] Improved The function also needs to be normalized to obtain the final improved speed evaluation function. The evaluation value is used to calculate the evaluation function formula (15) of the subsequently improved DWA algorithm;

[0167] (2) Introduce the target distance function :

[0168] In traditional DWA algorithms, the robot relies on The function ensures a reasonable direction of movement, avoids frequent turns or deviations from the target point, and guarantees path smoothness. However, it does not consider the actual distance to the target, which may prevent it from effectively guiding the robot closer to the target. Therefore, a new function is introduced... This function enhances the correlation between the robot and the target point, quantifies the distance in real time, reduces inefficient turning movements, avoids unnecessary wandering, and optimizes local optima and motion efficiency; the closer the robot is to the target, the greater the target distance function for that trajectory. The higher the evaluation, the higher the target distance function. Represented as:

[0169] (14)

[0170] In the formula, Indicates the distance from the starting point to the ending point. This represents the distance from the predicted trajectory point to the target point;

[0171] Target distance function It also needs to be normalized to obtain the final target distance function evaluation value, which is used for the calculation of the evaluation function formula (15) of the subsequently improved DWA algorithm;

[0172] In summary, improving the evaluation function generally enhances the robustness of the traditional evaluation function. functions and The function balances target proximity and directional consistency issues, and incorporates an improvement by introducing an angular velocity factor. The function optimizes local optima and improves the robot's trajectory safety and obstacle avoidance capabilities. The improved DWA algorithm evaluation function is expressed as:

[0173] (15)

[0174] In the formula, This represents the target weight factor.

[0175] In step S3, the construction of the fuzzy controller includes:

[0176] Will and As input, weighting factors , , and As the output, design a fuzzy controller with two inputs and four outputs;

[0177] Determine each input variable and The scope of the discourse and each output variable , , and The scope of the discourse;

[0178] Define input membership functions for each input variable and defuzzified output membership functions for each output variable; wherein, defining defuzzified output membership functions for each output variable includes: based on weight factors and The service is for path effectiveness, while the weighting factor and For security purposes, weighting factors and Output membership function and weighting factor and The output membership function is used to differentiate the classification;

[0179] Rules for designing fuzzy controllers;

[0180] Fuzzy inference is performed using the Mamdani model;

[0181] The fuzzy output variables are defuzzified using the centroid method. The specific construction process is as follows:

[0182] like Figure 2As shown, the process of using a fuzzy controller includes: variable fuzzification, fuzzy inference, and defuzzification. Variable fuzzification involves using membership degrees and membership functions to fuzzily describe the input and output quantities and establish fuzzy sets. Fuzzy inference requires establishing fuzzy rules that correspond to the fuzzy logic judgments in the input and output fuzzy sets through some fuzzy logic operations and conditional judgment statements, and compiling these into a rule base. The Mamdani inference engine, through maxima-minima synthesis operations, transforms these rules into fuzzy output quantities. Defuzzification removes the fuzziness from the output quantities and transforms them into precise quantities. Commonly used defuzzification methods include the centroid method, maximum membership degree, and weighted average method. A fuzzy controller is designed with weight factors as the control output object.

[0183] Evaluation function based on the improved DWA algorithm As can be seen from the calculation formula (15), the weighting factor involves four coefficients: α, β, γ, and δ. Reasonable setting can greatly improve obstacle avoidance efficiency.

[0184] Considering the guidance and safety of the ground-level detection robot's motion state and behavior, such as... Figure 3 As shown, the distance to the obstacle is introduced. Distance from target point With two input variables, a fuzzy control based on safety considerations is designed to dynamically adjust the four parameters α, β, γ, and δ according to the environmental information, thereby improving the adaptability of the ground level detection robot to multiple scenarios and enabling it to safely and stably reach the designated measurement position.

[0185] Based on the improved DWA algorithm evaluation function, a safety-considered fuzzy controller is designed to measure the distance from the robot's current position to the nearest obstacle. and the straight-line distance to the target point As system inputs, the four weight factors α, β, γ, and δ of the improved DWA algorithm evaluation function are used as outputs. A two-input, four-output fuzzy controller is designed. The fuzzy controller design is as follows: Figure 4 As shown;

[0186] Variable fuzzification:

[0187] For input quantity , Fuzzification is defined as:

[0188] The fuzzy set of input quantities is designed as {JS, ZM, YL}, representing near, medium, and far distances; among them, considering the robot's safe radius and safe distance, the input quantities... The universe of discourse is set to [0,5], when When ∈[0,2.5], it is blurred to JS (near), when When ∈[1.5,3.5], it is blurred into ZM (middle). When the distance is ∈ [2.5, 5], it is fuzzified to YL (far). If the current position is more than 5 times the distance to the target point, it is set to the maximum value of 5. Considering driving safety, the input quantity is... The universe of discourse is set to [0,6], when When ∈[0,2.5], it is blurred to JS (near), when When ∈[2,4], it is blurred into ZM (middle). When the distance is greater than 3.5, it is fuzzified to YL (far). If the distance to the nearest obstacle exceeds the universe of discourse, it is set to the maximum value of 6. The membership functions of the two input variables are designed as follows: As shown in Figure 5, the horizontal axis represents the domain of discourse, and the vertical axis represents the degree of membership.

[0189] The fuzzification of the output weighting factors α, β, γ, and δ is defined as follows:

[0190] The output quantities α, β, γ and δ involved are weighting factors, and their universe of discourse is designed to be [0,1].

[0191] Direction angle weighting factor Corresponding direction angle evaluation function This is used to guide the robot toward the target point, ensuring the accuracy of the path; target weight factor Corresponding target distance function This is used to shorten the actual distance to the target point and ensure path convergence; weighting factor and Both serve the purpose of path effectiveness, which refers to quickly and accurately reaching the target point, thus requiring more refined weight adjustment. In this embodiment, the fuzzy set of design parameters α and δ based on the effectiveness considerations of path planning is defined as [XS, S, M, L], and the weight factors... and The output membership function is as follows: Figure 6 As shown, the horizontal axis represents the domain of discourse, and the vertical axis represents the degree of membership.

[0192] Distance weight factor Corresponding safety distance evaluation function Used for obstacle avoidance to ensure the minimum distance to obstacles; speed weighting factor Corresponding improved speed evaluation function Used to balance linear velocity and angular velocity to optimize motion efficiency; weighting factor and Both serve the purpose of safety, which refers to obstacle avoidance and motion stability adjustment. Therefore, rapid adjustment should be prioritized, and they are not suitable for use with weighting factors. and The same fine-tuning is applied; in this embodiment, the fuzzy sets of output quantities β and γ are defined as [S, M, L] based on path safety considerations, with weighting factors... and The output membership function is as follows: Figure 7 As shown, the horizontal axis represents the domain of discourse, and the vertical axis represents the degree of membership.

[0193] Design fuzzy rules:

[0194] The design of fuzzy rules is a core component of the controller. Analysis of the evaluation function reveals that it selects the trajectory cluster with the highest calculated value from the predicted trajectory by setting weight factors. Based on the safety considerations of the mobile robot's motion state, according to... and The input quantity is designed with fuzzy rules:

[0195] when and When the input values ​​are relatively large, it indicates that the robot has a high safety attribute. Set γ and δ to larger values ​​and α and β to smaller values ​​to quickly reduce the distance to the target point.

[0196] when and When the input values ​​are relatively small, it indicates that the robot is in danger but very close to the target. Setting α and β to larger values ​​and γ and δ to smaller values ​​makes the robot's movement safe and avoids missing the target point.

[0197] when and When the input quantities are moderate, the robot's motion environment is relatively complex and changeable. When the input is relatively large, it indicates that the robot is far from the target. Increasing the weights of γ and δ allows the robot to quickly shorten the distance to the target point; when When the input is relatively small, increase the weight of β and decrease the weight of other factors.

[0198] Input variables and The fuzzy set of the input variables is {JS, ZM, YL}, where JS, ZM, and YL correspond to the three fuzzy concepts of near, medium, and far distance, respectively, used to describe the different degrees of fuzziness of the input variables in the universe of discourse; the fuzzy sets of the output variables α and δ are {XS, S, M, L}, and the fuzzy sets of the output variables β and γ are {S, M, L}, where XS, S, M, and L correspond to the four fuzzy concepts of small, medium, and large, respectively, used to describe the different degrees of fuzziness of the output variables in the universe of discourse; for example Figures 5 to 7As shown, each fuzzy concept is defined by its corresponding membership function, which defines the degree to which the numerical values ​​in the universe belong to that fuzzy concept. Furthermore, there is a transitional region where the membership ranges of adjacent fuzzy concepts overlap.

[0199] In this embodiment, a fuzzy rule base based on the IF-THEN form is designed:

[0200] IF =JS AND =JS, THEN α=L, β=L, γ=S, δ=XS;

[0201] IF =JS AND =ZM, THEN α=M, β=M, γ=M, δ=S;

[0202] IF =JS AND =YL, THEN α=L, β=S, γ=M, δ=S;

[0203] IF =ZM AND =JS, THEN α=S, β=L, γ=S, δ=S;

[0204] IF =ZM AND =ZM, THEN α=M, β=M, γ=M, δ=M;

[0205] IF =ZM AND =YL, THEN α=XS, β=S, γ=L, δ=M;

[0206] IF =YL AND =JS, THEN α=XS, β=L, γ=S, δ=S;

[0207] IF =YL AND =ZM, THEN α=S, β=M, γ=L, δ=M;

[0208] IF =YL AND =YL, THEN α=S, β=S, γ=L, δ=L;

[0209] Based on the above rule analysis, the fuzzy rules in this embodiment are as follows: As shown.

[0210] Table 1 Fuzzy Rule Design Table

[0211]

[0212] After the fuzzy rules are designed, the input quantities of the fuzzy controller are... , The fuzzy change relationship curves between the output variables and the four weighting factors are as follows: Figures 8-11 As shown.

[0213] Based on the fuzzy rules designed above, the Mamdani inference method is used to perform fuzzy inference, solve the fuzzy relation equation to obtain the fuzzy control quantity, and convert the fuzzy control quantity obtained by inference into a definite control signal using the centroid method. The precise values ​​of α, β, γ and δ are output as the system output values.

[0214] The robot differential motion model in step S4 infers the attitude information for the next moment from the current moment. The process of building the robot differential motion model based on DWA is as follows:

[0215] The DWA algorithm needs to generate a predicted motion trajectory based on the robot's current speed. Assuming the robot is currently at... Then the robot's kinematic equations are:

[0216] (16)

[0217] (17)

[0218] (18)

[0219] Under the DWA algorithm, the robot's next-moment pose information is derived from the current-moment control information and the current-moment state. The sampling time is set to... The predicted time is ,but

[0220] Combining equations (16), (17), and (18), the attitude information matrix at the next moment is expressed as:

[0221] (19)

[0222] The control information matrix for the next moment is represented as follows:

[0223] (20)

[0224] The differential motion model can be obtained as Diff:

[0225] (twenty one)

[0226] Substituting equations (19) and (20) into equation (21), we get:

[0227] (twenty two)

[0228] In the formula, This represents the x-axis coordinate of the predicted attitude information in the world coordinate system at the next moment; This represents the y-axis coordinate of the predicted attitude information in the world coordinate system at the next moment; Indicates the predicted heading angle for the next moment; Indicates the predicted linear velocity at the next moment; Indicates the predicted angular velocity at the next moment; Indicates the linear velocity of the sample; This represents the angular velocity of the sampled data.

[0229] Predicted time Internal sampling time The improved DWA algorithm evaluates the highest-rated speed combination. :

[0230] (twenty three)

[0231] In the formula, This represents the linear velocity corresponding to the optimal path. The angular velocity corresponding to the optimal path is represented by equation (15).

[0232] Based on the robot's dynamic characteristics, the trajectory of the sampling points is predicted to generate multiple motion trajectories for the next moment.

[0233] As can be seen from equation (22), the attitude information at the current moment is... The improved DWA algorithm evaluation function evaluates the highest speed combination. By inputting the differential motion model, the optimal predicted trajectory for the next moment is obtained as the optimal path, which enables motion control of the robot.

[0234] Step S5 includes: outputting the speed combination corresponding to the optimal path. This serves as control information for the next moment.

[0235] The robot local path planning method in this embodiment improves upon the traditional DWA algorithm and has the following three advantages compared to the traditional DWA algorithm: 1. Optimizes the evaluation function... 1. To optimize the selection of robot speed control information, improve the ability to escape local optima, and avoid prolonged entanglement in trajectory calculations for escape maneuvers; 2. To improve the robustness of the algorithm, a target distance function is proposed. With direction angle function 3. To jointly guide the robot's movement, balance target proximity and directional consistency, and optimize the robot's movement efficiency when facing local optimal areas and dynamic obstacles; 4. To dynamically optimize the sampling frequency of the sampling space, increase the sampling frequency in areas with low obstacles or far from obstacles, shorten the sampling time, and improve planning efficiency.

[0236] The robot local path planning method in this embodiment further includes step S7: designing a simulation experiment to verify the local route planning effect of the robot local path planning method;

[0237] The simulation experiments include local optimum experiments, dynamic obstacle experiments, and acute obstacle experiments;

[0238] The local optimum experiment sets up U-shaped or concave obstacles, with the starting point inside the groove and the target point outside, to verify whether the robot's local path planning method will get stuck in the local optimum region or cause the trajectory to go around in circles.

[0239] The dynamic obstacle experiment sets up dynamic obstacles to verify the robot's local path planning method's real-time obstacle avoidance capability against dynamic obstacles.

[0240] The acute obstacle experiment sets up a sudden obstacle to verify the robot's local path planning method's emergency response capability to sudden obstacles.

[0241] The specific settings for the simulation experiment are as follows:

[0242] Three sets of experiments were set up for the DWA algorithm under the same motion parameters and map environment: Experiment 1 was a local optimum experiment, Experiment 2 was a dynamic obstacle experiment, and Experiment 3 was an acute obstacle experiment.

[0243] Experiment 1: Local Optimality Experiment, an experiment on local optima problems based on trap regions:

[0244] To address the issues of traditional DWA algorithms easily getting trapped in local optima and circling, the robot local path planning method in this embodiment presents path planning results and velocity and weight factor variation curves as shown below. 2- As shown in Figure 5.

[0245] like Figure 12 As shown, in Experiment 1, three black static obstacles were placed between the red starting point and the blue target point, with the middle obstacle being a concave shape, creating an environment that easily leads to local optima, such as... Figure 12 As shown in the path planning process from left to right, the robot local path planning method in this embodiment can avoid getting stuck in local optimal areas or circling around the trajectory, and smoothly travel from the starting point around the concave obstacle to the target point.

[0246] Figure 13The four subplots represent the iteration number (Interation) on the horizontal axis and the numerical value (Value) on the vertical axis. The top-left subplot represents the change of the weight factor α (Parameter α Change), the top-right subplot represents the change of the weight factor β (Parameter α Change), the bottom-left subplot represents the change of the weight factor γ (Parameter γ Change), and the top-right subplot represents the change of the weight factor δ (Parameter δ Change).

[0247] like Figure 12 The trajectory path of the left subgraph, in this embodiment of the robot local path planning method, when the path is in a locally optimal region, i.e., after iterating to nearly 100 times, such as... Figure 13 By reducing the proportions of α, γ, and δ and increasing β, obstacle avoidance and rapid escape from the area are prioritized. Therefore, the trajectory is changed at a distance from the trap area, resulting in a smoother overall path. Figure 14 The robot v changes curve and Figure 15 The Robot wchanges curve reveals that, compared to the high-frequency fluctuations and sudden stops / goes of traditional DWA algorithms, the robot local path planning method in this embodiment maintains relatively stable acceleration along the linear velocity for obstacle avoidance, and the oscillation of angular velocity is also greatly improved, with no instances of deceleration to zero. Combined with... Figure 12 and Figure 15 As shown, the robot local path planning method in this embodiment can effectively avoid local optimal regions.

[0248] Experiment 2, Dynamic Obstacle Experiment: Obstacle Avoidance Experiment Based on the Influence of Dynamic Obstacles

[0249] like Figure 16 As shown, Experiment 2 sets the starting and ending points of the simulated robot as follows: , The red trajectory is a schematic diagram of the trajectory planning of the robot's local path planning method in this embodiment under a static map, with two coordinates. , Dynamic obstacles moving back and forth at a speed of 0.5 m / s are represented by gray squares;

[0250] Based on the above conditions, the results of dynamic obstacle trajectory planning in Experiment 2 and the curves showing the changes in velocity and weight factors are as follows: Figures 17-20 As shown.

[0251] like Figure 17 Dynamic obstacle avoidance and Figure 18As can be seen from the parameter changes, when the obstacle is dynamically changing, the weight factor of the evaluation function of the improved DWA algorithm in this embodiment fluctuates significantly. When the distance to the obstacle is close, the weight factor β increases, prioritizing obstacle avoidance. Regarding linear velocity and angular velocity, the robot maintains relatively stable velocity movement commands in a dynamic obstacle environment. Therefore, the robot local path planning method in this embodiment can better complete trajectory planning tasks in dynamic environments.

[0252] Experiment 3, Acute Obstacle Experiment: Obstacle Avoidance Experiment Based on the Effects of Acute Obstacles.

[0253] like Figure 21 of As shown in the timeline, the robot is... There are no obstacles yet; the finish line is about to be reached. Figure 21 The moment As shown, a sudden, acute obstacle is introduced, causing the robot to get stuck in a local optimum. At this point, as... Figure 23 As shown in the iteration number Interation400 to 500 on the horizontal axis, the robot's linear velocity v decreases to 0, corresponding to... Figure 24 The number of iterations on the horizontal axis is shown as Interation 400 to 500. Increasing the angular velocity w adjusts the heading angle for escape; the changes in each weighting factor are shown below. Figure 22 The number of iterations on the horizontal axis, Interation 400 to 600, is shown below: First, as shown below... Figure 22 As shown in the top left sub-graph, reducing the orientation angle weight factor α prioritizes obstacle avoidance, and then... Figure 22 As shown in the upper right and lower left sub-figures, the distance weighting factor β is slightly increased and then decreases, while the speed weighting factor γ is slightly decreased and then increases. This reversed handling of β and γ ensures a sufficient safe distance from obstacles. Then, the speed weighting factor γ is rapidly increased while the distance weighting factor β is rapidly decreased. Simultaneously, the target weighting factor δ and the direction angle weighting factor α are boosted sharply and then decrease, resulting in an improved speed evaluation function. After rapidly escaping the local optimum, the medium angular velocity w accurately approaches the target point, such as... Figure 21 of As shown in the timeline, obstacles were successfully avoided; ultimately, as Figure 21 The final state shows that the target point has been reached.

[0254] Example 2:

[0255] Based on the same inventive concept as Embodiment 1, this embodiment introduces a robot local path planning device, including a preprocessing module, a loop module, a distance calculation module, a weight calculation module, a path selection module, and an output control module;

[0256] The preprocessing module is used to: load an environmental map including static obstacle information and the robot's current state, and set the robot's target point coordinates and kinematic parameters;

[0257] The loop module is used to repeatedly execute the steps of the following module until the target point is reached:

[0258] The distance calculation module is used to: calculate the distance from the robot to the target point in real time. and distance to the nearest obstacle ;

[0259] The weight calculation module is used to: calculate a dynamic window based on kinematics and environmental constraints, and sample velocity combinations composed of linear velocity and angular velocity within the window; based on... and The weight factors of the improved DWA algorithm evaluation function are calculated using a pre-built fuzzy controller. The improvements to the DWA algorithm evaluation function include: introducing an angular velocity evaluation mechanism to improve the velocity evaluation function, and adding a target distance function that quantifies the distance to the target point.

[0260] The path selection module is used to: input the speed combination into a pre-built differential motion model to predict the trajectory and evaluate the predicted trajectory according to the improved DWA algorithm evaluation function to select the optimal path;

[0261] The output control module is used to output the speed combination corresponding to the optimal path as the control information for the next moment.

[0262] The device is equipped with a human-computer interaction interface, which allows operators to input the map coordinates of the starting point and the target point, view the real-time motion status and path planning results, and the interface response time is no more than 500ms, providing a good user experience.

[0263] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0264] This application is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart... Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0265] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0266] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment to cause a series of operational steps to be performed on the computer or other programmable equipment to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0267] The above description is only a preferred embodiment of the present invention. It should be noted that for those skilled in the art, several improvements and modifications can be made without departing from the technical principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A robot local path planning method, characterized in that, include: Load the environment map including static obstacle information and the robot's current state, and set the robot's target point coordinates and kinematic parameters; Repeat the following steps until the target point is reached: Real-time calculation of distance from robot to target point and distance to the nearest obstacle ; The dynamic window is calculated based on kinematics and environmental constraints, and the velocity combination consisting of linear velocity and angular velocity is sampled within the window; based on and The weight factors of the improved DWA algorithm evaluation function are calculated using a pre-constructed fuzzy controller. Improvements to the DWA algorithm evaluation function include: introducing an angular velocity evaluation mechanism to improve the velocity evaluation function, and adding a target distance function that quantifies the distance to the target point; The velocity combination is input into a pre-built differential motion model to predict the trajectory, and the predicted trajectory is evaluated according to the improved DWA algorithm evaluation function to select the optimal path. The speed combination corresponding to the optimal path is output as the control information for the next moment.

2. The robot local path planning method according to claim 1, characterized in that, The robot's current state includes: the robot's posture information and control information at time t, where the posture information... ,in express The position of the Time Robot in the world coordinate system. express Heading angle at any time; setting control information ,in This represents the initial linear velocity. This represents the initial angular velocity; Robot kinematic parameters include: minimum linear velocity Maximum linear velocity Minimum angular velocity Maximum angular velocity Maximum linear acceleration Maximum angular acceleration Linear velocity sampling frequency and angular velocity sampling frequency .

3. The robot local path planning method according to claim 2, characterized in that, The velocity combination composed of linear velocity and angular velocity sampled within the window includes: according to The sampling frequency is dynamically adjusted, and the sampling speed is combined within a dynamic window.

4. The local path planning method for a terrain detection robot according to claim 3, characterized in that, The dynamic window calculation based on kinematics and environmental constraints includes: Kinematic constraints include hardware limit constraints and acceleration constraints: The hardware limit constraints The calculation formula is: ， In the formula, linear velocity and angular velocity The combination of speeds; The acceleration constraint The calculation formula is: ， In the formula, Sampling time; The predicted linear velocity for the next moment; The predicted angular velocity for the next moment; The environmental constraints The calculation formula is: ， In the formula, For the current speed combination The distance between the trajectory of the movement and the nearest obstacle; Dynamic window The calculation formula is: ； According to Dynamically adjusting the sampling frequency includes: Dynamically adjust linear velocity sampling frequency The calculation formula is: ， In the formula, k represents the preset adjustment parameter of the sampling frequency. ; This indicates the distance between the robot's current position and the nearest obstacle. Indicates the maximum safe distance; Dynamically adjust angular velocity sampling frequency The calculation formula is: 。 5. The local path planning method for a terrain detection robot according to claim 1, characterized in that, The improved velocity evaluation function by introducing an angular velocity evaluation mechanism includes: Introducing a dynamic angular velocity evaluation mechanism into the velocity evaluation function Constructing an improved velocity evaluation function Improved speed evaluation function The calculation formula is: ， In the formula, and This indicates the preset scaling factor; ， In the formula, This indicates the distance from the robot's current position to the nearest obstacle. This represents the distance from the predicted trajectory point to the nearest obstacle. Represents the angular velocity at time t; The target distance function that incorporates quantization and the distance to the target point includes: Introduce a target distance function that quantifies the distance to the target point into the evaluation function. Target distance function The calculation formula is: ， In the formula, Indicates the distance from the starting point to the ending point. This represents the distance from the predicted trajectory point to the target point; Improved DWA algorithm evaluation function The calculation formula is: ， In the formula, Indicates the orientation angle weighting factor; This represents the orientation angle evaluation function; Indicates the distance weighting factor; Represents the safe distance evaluation function; Indicates the speed weighting factor; This represents the target weight factor.

6. The local path planning method for a terrain detection robot according to claim 5, characterized in that, The construction of a fuzzy controller includes: Will and As input, weighting factors , , and As the output, design a fuzzy controller with two inputs and four outputs; Determine each input variable and The scope of the discourse and each output variable , , and The scope of the discourse; Define input membership functions for each input variable and defuzzified output membership functions for each output variable; wherein, defining defuzzified output membership functions for each output variable includes: based on weight factors and The service is for path effectiveness, while the weighting factor and For security purposes, weighting factors and Output membership function and weighting factor and The output membership function is used to differentiate the classification; Rules for designing fuzzy controllers; Fuzzy inference is performed using the Mamdani model; The fuzzy output variables are defuzzified using the centroid method.

7. A robot local path planning method according to claim 6, characterized in that, Input variables and The fuzzy set is {JS, ZM, YL}, where JS, ZM, and YL correspond to the three fuzzy concepts of near, medium, and far distance, respectively, used to describe the different degrees of fuzziness of the input variables in the universe of discourse; the fuzzy sets of output variables α and δ are {XS, S, M, L}, and the fuzzy sets of output variables β and γ are {S, M, L}, where XS, S, M, and L correspond to the four fuzzy concepts of small, small, medium, and large, respectively, used to describe the different degrees of fuzziness of the output variables in the universe of discourse; the designed fuzzy controller rules include: when Belongs to JS and When JS is a subset of L, β is a subset of L, γ is a subset of S, and δ is a subset of XS. when Belongs to JS and When ZM is included, α belongs to M, β belongs to M, γ belongs to M, and δ belongs to S; when Belongs to JS and When YL is a subset of L, α belongs to L, β belongs to S, γ belongs to M, and δ belongs to S. when Belongs to ZM and When JS is a subset of S, α belongs to S, β belongs to L, γ belongs to S, and δ belongs to S. when Belongs to ZM and When ZM is included, α belongs to M, β belongs to M, γ belongs to M, and δ belongs to M; when Belongs to ZM and When YL is a subset of Y, α belongs to XS, β belongs to S, γ belongs to L, and δ belongs to M. when Belongs to YL and When JS is a subset of XS, β is a subset of L, γ is a subset of S, and δ is a subset of S. when Belongs to YL and When ZM is involved, α belongs to S, β belongs to M, γ belongs to L, and δ belongs to M; when Belongs to YL and When YL, α belongs to S, β belongs to S, γ belongs to L, and δ belongs to L.

8. A robot local path planning method according to claim 6, characterized in that, Pre-built differential motion model include: ， In the formula, This represents the x-axis coordinate of the predicted attitude information in the world coordinate system at the next moment; This represents the y-axis coordinate of the predicted attitude information in the world coordinate system at the next moment; Indicates the predicted heading angle for the next moment; Indicates the predicted linear velocity at the next moment; Indicates the predicted angular velocity at the next moment; Indicates the linear velocity of the sample; Indicates the angular velocity of the sample; The velocity combination is input into a pre-built differential motion model to predict the trajectory, and the predicted trajectory is evaluated according to the improved DWA algorithm evaluation function to obtain the optimal path, including: Attitude information at the current moment The improved DWA algorithm evaluation function evaluates the highest speed combination. Input differential motion model The optimal predicted trajectory for the next moment is obtained as the optimal path.

9. A robot local path planning method according to claim 1, characterized in that, It also includes designing simulation experiments to verify the local route planning effect of the robot's local path planning method; The simulation experiments include local optimum experiments, dynamic obstacle experiments, and acute obstacle experiments; The local optimum experiment sets up U-shaped or concave obstacles, with the starting point inside the groove and the target point outside, to verify whether the robot's local path planning method will get stuck in the local optimum region or cause the trajectory to go around in circles. The dynamic obstacle experiment sets up dynamic obstacles to verify the robot's local path planning method's real-time obstacle avoidance capability against dynamic obstacles. The acute obstacle experiment sets up a sudden obstacle to verify the robot's local path planning method's emergency response capability to sudden obstacles.

10. A robot local path planning device, characterized in that, It includes a preprocessing module, a loop module, a distance calculation module, a weight calculation module, a path filtering module, and an output control module; The preprocessing module is used to: load an environmental map including static obstacle information and the robot's current state, and set the robot's target point coordinates and kinematic parameters; The loop module is used to repeatedly execute the steps of the following module until the target point is reached: The distance calculation module is used to: calculate the distance from the robot to the target point in real time. and distance to the nearest obstacle ; The weight calculation module is used to: calculate a dynamic window based on kinematics and environmental constraints, and sample velocity combinations composed of linear velocity and angular velocity within the window; based on... and The weight factors of the improved DWA algorithm evaluation function are calculated using a pre-constructed fuzzy controller. Improvements to the DWA algorithm evaluation function include: introducing an angular velocity evaluation mechanism to improve the velocity evaluation function, and adding a target distance function that quantifies the distance to the target point; The path selection module is used to: input the speed combination into a pre-built differential motion model to predict the trajectory and evaluate the predicted trajectory according to the improved DWA algorithm evaluation function to select the optimal path; The output control module is used to output the speed combination corresponding to the optimal path as the control information for the next moment.

Images