Local path planning method for forestry robots based on off-road obstacles and fuzzy parameters

Abstract

The present invention discloses a local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters, the method comprising: establishing an Ackerman motion model for the forestry robot, determining the starting point and end point of the robot, obtaining the coordinates of the impassable obstacles and the passable obstacles; introducing a target distance function and a passable obstacle distance function, and applying an improved dynamic window method to plan a local path; introducing a fuzzy logic strategy, and adaptively adjusting the weight parameters according to the distances between the two obstacles and the robot. The present invention can solve the problems of low planning success rate and non-optimal planning routes of the traditional dynamic window method in various forest obstacle environments.

Description

Technical Field

[0001] The invention relates to the field of robot navigation and path planning, in particular to a local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters. Background Art

[0002] The rapid development of technologies such as wireless communication, pattern recognition and intelligent control has promoted the integration of modern information and control technology with agricultural equipment and the intelligentization of forestry robots. Among them, path planning is the core issue in the field of forestry robots.

[0003] In path planning, it can be divided into global path planning algorithms and local path planning algorithms according to whether the operating environment information is known. The global path planning method performs path planning under known environmental information and cannot effectively plan in the forest with changeable ground environment. Among the local path planning algorithms, the dynamic window method is the most widely used. The traditional dynamic window method cannot adapt well to the Ackerman steering robot and is prone to fall into the local optimum in an environment with many obstacles. In addition, the traditional dynamic window method only considers impassable obstacles, and does not consider impassable obstacles such as tree roots, broken branches, gullies, and shrubs in forest field operations. Summary of the invention

[0004] The purpose of the invention is to propose a local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters.

[0005] The technical solution to achieve the purpose of the present invention is: a local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters, the method comprising the following steps:

[0006] S1: Establish the Ackerman motion model of forestry robot;

[0007] S2: Determine the starting point and end point of the robot through the global path, obtain the coordinates of the impassable obstacles and the passable obstacles in real time through sensors, and perform expansion processing on the obstacles;

[0008] S3: Apply the improved dynamic window method to obtain the speed corresponding to the optimal trajectory at the current moment, including: sampling the speed in the dynamic window to generate multiple predicted trajectories; applying the sub-evaluation function to score each predicted trajectory, selecting the optimal trajectory, normalizing the sub-evaluation function, adding weight parameters to each evaluation function, and selecting the trajectory with the highest total score as the optimal trajectory;

[0009] S4: Apply fuzzy parameterizer to adjust the weight of evaluation function in real time.

[0010] Compared with the prior art, the present invention has the following significant advantages: 1) the dynamic window method and the Ackerman structure robot are adapted to make the generated path more consistent with the actual operation of the robot; 2) considering the dense characteristics of obstacles in the forest, the target distance function is introduced to make the path planning success rate higher; 3) comprehensive consideration is given to the impassable obstacles and passable obstacles in the forest, and the fuzzy parameterizer is used at the same time to make the generated path more reliable. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] Figure 1 It is an overall flow chart in the embodiment.

[0012] Figure 2 Schematic diagram of robot motion model in the embodiment.

[0013] Figure 3 It is the effect diagram of the objective function in the embodiment.

[0014] Figure 4 In the embodiment and Schematic diagram.

[0015] Figure 5 This is a flow chart of improving the dynamic window method in the embodiment.

[0016] Figure 6 This is a diagram of the path planning result based on off-road obstacles and fuzzy parameters in the embodiment. DETAILED DESCRIPTION

[0017] In order to make the purpose, technical solution and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not used to limit the present application.

[0018] The present invention provides a local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters. The overall flow chart is as follows: Figure 1 As shown, the method comprises the following steps:

[0019] S1: Establishing the forestry robot motion model;

[0020] S2: Determine the starting point and end point of the robot through the global path, obtain the coordinates of the impassable obstacles and the passable obstacles in real time through sensors, and perform expansion processing on the obstacles;

[0021] S3: Apply an improved dynamic window method to obtain the speed corresponding to the optimal trajectory at the current moment. This includes: sampling the speed within the dynamic window to generate multiple predicted trajectories; applying an evaluation function to score each predicted trajectory, selecting the optimal trajectory, normalizing the evaluation function, adding weight parameters to each evaluation function, and selecting the trajectory with the highest total score as the optimal trajectory;

[0022] S4: Apply fuzzy parameterizer to adjust the weight of evaluation function in real time.

[0023] like Figure 2 As shown, 2d is the wheelbase of the robot. Assume that the unit time The inner robot moves in a straight line, and the mathematical expression of the Ackerman kinematic model established in S1 is:

[0024]

[0025]

[0026]

[0027] In the formula, is the x-axis coordinate of the robot at time t, is the y-axis coordinate of the robot at time t, is the heading angle of the robot at time t, is the linear velocity of the robot at time t, is the steering angle of the robot’s front wheels at time t, is the sampling time interval, is the wheelbase of the robot.

[0028] In step S2, the obstacle is expanded to expand the edge of the obstacle into a circle with a radius equidistant from the longitudinal axis of the robot. The impassable obstacles include trees, high slopes, and potholes, and the passable obstacles include tree roots, broken branches, gullies, and low shrubs.

[0029] S3 consists of the following steps:

[0030] S31: Establish a dynamic window space according to the current robot speed and position;

[0031] S32: sampling the speed at a fixed sampling interval in the dynamic window;

[0032] S33: applying the sub-evaluation function to score each predicted trajectory, and normalizing the sub-evaluation function;

[0033] S34: assigning weight parameters to each evaluation function and adding them together to obtain an overall evaluation function;

[0034] S35: Select the trajectory with the highest total score as the optimal trajectory.

[0035] Among them, the speed sampling dynamic window space in S31 is:

[0036]

[0037] In the formula, is the final velocity sampling space, are the minimum and maximum acceleration limits of the robot, The minimum and maximum speed limits for the robot, The robot is able to avoid obstacles that cannot be passed through with a safe braking range.

[0038]

[0039]

[0040]

[0041]

[0042] Where v is the linear velocity of the robot; is the steering angle of the robot’s front wheels; is the minimum linear velocity of the robot; is the maximum linear speed of the robot; is the minimum front wheel steering angle of the robot; is the maximum front wheel steering angle of the robot; is the maximum linear acceleration of the robot; is the maximum front wheel steering angular acceleration of the robot; is the sampling interval; For robots The speed is the distance between the corresponding trajectory and the nearest impassable obstacle.

[0043] Among them, the evaluation function in steps S33 and S34 is

[0044]

[0045] In the formula, is the overall evaluation function; Indicated in The error between the direction of the trajectory end point generated at the sampling speed and the line connecting the target points; Indicates the shortest distance between the sampling trajectory and the impassable obstacle; Indicates the speed of the sampling trajectory; Represents the distance function between the sampled trajectory and the target; Indicates the shortest distance between the sampled trajectory and the passable obstacle; , , , , Represents the weight parameters of different sub-evaluation functions.

[0046] like Figure 3 As shown in the figure, without using the objective function, the robot cannot reach the target point because the target point is too close to the obstacle. After using the objective function, the robot successfully reaches the target point. The distance function expression between the sampling trajectory and the target is:

[0047]

[0048] In the formula, is the end point of the robot sampling trajectory; goal is the target position; Indicates the distance from the end point of the robot sampling trajectory to the target position.

[0049] like Figure 4 As shown, the calculation of the shortest distance between the sampling trajectory and the passable obstacle is expressed as:

[0050]

[0051] In the formula, Indicates the shortest distance from the sampled trajectory to the edge of the obstacle that can be passed.

[0052] Furthermore, the application of the fuzzy parameterizer in S4 to adjust the weight of the evaluation function consists of the following steps:

[0053] S41: Fuzzify the input of the fuzzy parameter selector using the binned fuzzy set method;

[0054] S42: Perform fuzzy reasoning using Mamdani fuzzy rules to obtain a fuzzy output value;

[0055] S43: Use the area average method to perform defuzzification to obtain adjusted weight parameters.

[0056] The specific steps of input fuzzification in step S41 are as follows:

[0057] S411: Predict the distance from the optimal trajectory to the impassable obstacle based on the current moment The domain of discourse generates a fuzzy set, and the fuzzy set members are VN, N, M, F, and VF, which represent very close, close, moderate, far, and very far distances, respectively. The distance from the optimal trajectory to the obstacle that can be passed is predicted based on the current moment The domain of discourse generates a fuzzy set, and the fuzzy set members are VN, N, M, F, and VF, which represent very close, close, moderate, far, and very far distances respectively;

[0058] S412: Fuzzify the fuzzy set using the triangle membership function.

[0059] S413: Based on the distance of the impassable obstacle and the distance through which obstacles can be passed Construct a fuzzy rule table. The specific fuzzy rule table is as follows:

[0060]

[0061] in, for and The ratio of two parameters, the fuzzy set members are VS, S, M, L, and VL, which represent very small, small, moderate, large, and very large ratios, respectively.

[0062] Among them, the defuzzification method in S43 is

[0063]

[0064] In the formula, are the weight parameters of the two sub-evaluation functions in step 33; is the number of fuzzy sets; is the membership degree of the i-th set; is the value corresponding to the bisector of the area enclosed by the i-th fuzzy set and the coordinate axis; is the exact value obtained by defuzzification, is the ratio of two parameters.

[0065] The process of the path planning method based on off-road obstacles and fuzzy parameters is as follows Figure 5 First, we need to obtain the coordinates of the starting point and the target point, and use sensors to obtain the coordinates of the impassable obstacles and the passable obstacles in real time, and then perform expansion processing on the obstacles. After that, we need to set the initial values ​​of the dynamic window method parameters and set the robot , , , , , , sampling interval , speed sampling interval , front wheel angle sampling interval ; The initial weight of the evaluation function is set as: , , , , , simulation time , the fuzzy parameter Set within [0.4,2.5].

[0066] At the same time, we compared it with the traditional dynamic window method, where the parameters of the traditional dynamic window method are fixed as follows: , , , , .

[0067] The effect diagram of the path planning method based on off-road obstacles and fuzzy parameters is shown in the figure Figure 6 As shown. Figure 6 It can be seen that the traditional DWA method fails to consider passable obstacles and drives directly over passable obstacles, while the improved DWA method avoids non-passable obstacles while staying away from passable obstacles as much as possible to reach the destination quickly and safely.

[0068] The present invention also provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, based on the above-mentioned local path planning method for the forestry robot, local path planning for the forestry robot based on off-road obstacles and fuzzy parameters is implemented.

[0069] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements local path planning of a forestry robot based on off-road obstacles and fuzzy parameters based on the above-mentioned local path planning method for a forestry robot.

[0070] In summary, the present invention adapts the dynamic window method and the Ackerman structure robot to make the generated path more in line with the actual operation of the robot; considering the characteristics of dense obstacles in the forest, the target distance function is introduced to make the path planning success rate higher; comprehensively considering the impassable obstacles and passable obstacles in the forest, and using fuzzy parameterizers at the same time, the generated path is more reliable.

[0071] The above embodiments only express the implementation methods of the present invention, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, and these all belong to the protection scope of the present invention.

Claims

1. A local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters, characterized in that: The following steps are involved: S1: Establish the Ackerman motion model of forestry robot; S2: Determine the starting point and end point of the robot through the global path, obtain the coordinates of the impassable obstacles and the passable obstacles in real time through sensors, and perform expansion processing on the obstacles; S3: applying the improved dynamic window method to obtain the speed corresponding to the optimal trajectory at the current moment, including: sampling the speed in the dynamic window to generate multiple predicted trajectories; applying the sub-evaluation function to score each predicted trajectory, selecting the optimal trajectory, normalizing the sub-evaluation function, adding weight parameters to each evaluation function, and selecting the trajectory with the highest total score as the optimal trajectory; step S3 includes the following steps: S31: Establish a dynamic window space according to the current robot speed and position; S32: sampling the speed at a fixed sampling interval in the dynamic window; S33: applying the sub-evaluation function to score each predicted trajectory, and normalizing the sub-evaluation function; S34: Assign weight parameters to each evaluation function and add them together to obtain an overall evaluation function; the overall evaluation function is ； In the formula, is the overall evaluation function; Indicated in The error between the direction of the trajectory end point generated at the sampling speed and the target point connection line, v is the linear speed of the robot, is the steering angle of the robot’s front wheels; Indicates the shortest distance between the sampling trajectory and the impassable obstacle; Indicates the speed of the sampling trajectory; Represents the distance function between the sampled trajectory and the target; Indicates the shortest distance between the sampled trajectory and the passable obstacle; , , , , Represents the weight parameters of different sub-evaluation functions; S35: Select the trajectory with the highest total score as the optimal trajectory; S4: Apply fuzzy parameterizer to adjust the weight of evaluation function in real time.

2. The local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters according to claim 1 is characterized in that: The mathematical expression of the Ackerman kinematic model established in step S1 is: ； ； ； In the formula, is the x-axis coordinate of the robot at time t, is the y-axis coordinate of the robot at time t, is the heading angle of the robot at time t, is the linear velocity of the robot at time t, is the steering angle of the robot’s front wheels at time t, is the sampling time interval, is the wheelbase of the robot.

3. The local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters according to claim 1 is characterized in that: In step S2, the obstacle is expanded to expand the edge of the obstacle into a circle with a radius equidistant from the longitudinal axis of the robot; the impassable obstacles include trees, high slopes, and potholes, and the impassable obstacles include tree roots, broken branches, gullies, and low shrubs.

4. The local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters according to claim 2 is characterized in that: The speed sampling dynamic window space in step S31 is: ； In the formula, is the final velocity sampling space, are the minimum and maximum acceleration limits of the robot, The minimum and maximum speed limits for the robot, The robot is able to avoid the safe braking range of impassable obstacles; ； ； ； ； In the formula, is the minimum linear velocity of the robot; is the maximum linear speed of the robot; is the minimum front wheel steering angle of the robot; is the maximum front wheel steering angle of the robot; is the maximum linear acceleration of the robot; is the maximum front wheel steering angular acceleration of the robot; is the sampling interval; For robots The speed is the distance between the corresponding trajectory and the nearest impassable obstacle.

5. The local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters according to claim 1 is characterized in that: The distance function expression between the sampling trajectory and the target is: ； In the formula, is the end point of the robot sampling trajectory; goal is the target position; Indicates the distance from the end point of the robot sampling trajectory to the target position.

6. The local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters according to claim 1 is characterized in that: Calculate the shortest distance between the sampled trajectory and the passable obstacle. The expression is: ； In the formula, Indicates the shortest distance from the sampled trajectory to the edge of the obstacle that can be passed.

7. The local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters according to claim 1 is characterized in that: In step S4, the fuzzy parameterizer is used to adjust the weight of the evaluation function, as follows: S41: Use the graded fuzzy set method to fuzzify the input of the fuzzy parameter selector, specifically: S411: Predict the distance from the optimal trajectory to the impassable obstacle based on the current moment The domain of discourse generates a fuzzy set, and the fuzzy set members are VN, N, M, F, and VF, which represent very close, close, moderate, far, and very far distances, respectively; the distance from the optimal trajectory predicted at the current moment to the obstacle that can be passed The domain of discourse generates a fuzzy set, and the fuzzy set members are VN, N, M, F, and VF, which represent very close, close, moderate, far, and very far distances respectively; S412: Fuzzify the fuzzy set using a triangle membership function; S413: Based on the distance of the impassable obstacle and the distance through which obstacles can be passed Construct fuzzy rule table; S42: Perform fuzzy reasoning using Mamdani fuzzy rules to obtain a fuzzy output value; S43: Use the area average method to perform defuzzification to obtain adjusted weight parameters.

8. The local path planning method for a forestry robot based on off-road obstacles and fuzzy parameters according to claim 7 is characterized in that: The defuzzification method in step S43 is: ； In the formula, is the number of fuzzy sets; is the membership degree of the i-th set; is the value corresponding to the bisector of the area enclosed by the i-th fuzzy set and the coordinate axis; is the exact value obtained by defuzzification.

Images