Unmanned agricultural machine autonomous navigation dynamic obstacle avoidance method

Abstract

The invention discloses an autonomous navigation dynamic obstacle avoidance method for an unmanned agricultural machine, and relates to the field of agricultural engineering. The method mainly comprises the following steps: 1, acquiring position information of an operation starting point and an end point of the unmanned agricultural machine, and obtaining an initial track through a path planning module; 2, acquiring obstacle information and road information through a vehicle-mounted laser radar; 3, dividing the vision field of the laser radar, and adopting different obstacle avoidance algorithms for obstacles in different areas; and 4, adjusting the initial planning trajectory by using the obstacle information, the road information and an obstacle avoidance algorithm to obtain a target trajectory. According to the method, the vision field of the laser radar is divided, and different obstacle processing methods are adopted, so that the calculation burden is reduced while effective obstacle avoidance is ensured; besides, by fusing road boundary information, multi-obstacle state estimation and dynamic constraints, a smooth trajectory is generated to adapt to the operation characteristics of the agricultural machine, and the autonomous navigation applicability of the unmanned agricultural machine in a variable farmland scene is expanded.

Description

Technical Field

[0001] The invention relates to an autonomous navigation dynamic obstacle avoidance method for an unmanned agricultural machine, and belongs to the field of agricultural engineering. Background Art

[0002] With the acceleration of global agricultural modernization, unmanned agricultural machinery, owing to its efficient, precise, and sustainable operational capabilities, has become a core component of smart agriculture. However, the farmland environment is highly complex and uncertain. Agricultural machinery may encounter a variety of obstacles during unmanned operations, including static obstacles such as ridges, rocks, and ditches, as well as dynamic obstacles such as the sudden appearance of animals, operators, mobile agricultural machinery, or transport vehicles. The lack or inadequacy of obstacle avoidance capabilities not only results in direct economic losses such as damage to agricultural machinery and crops, but can also lead to serious safety accidents and even threaten human life, highlighting the urgent need for research on obstacle avoidance technology.

[0003] In traditional obstacle avoidance research, most algorithms are designed for static obstacles, using lidar to sense their positions and combining them with path planning algorithms to avoid them. However, the presence of dynamic obstacles in real-world farmland scenarios places higher demands on the real-time perception and decision-making capabilities of unmanned agricultural machinery. Dynamic obstacles in farmland environments, such as field patrols and wild animals, can randomly enter the work area. The intersecting motion paths of multiple agricultural machinery working in coordination, as well as the sudden entry of equipment such as agricultural trucks and drones, can all pose collision risks. The random, nonlinear, and time-varying motion of these obstacles makes it difficult to predict their trajectories using traditional static environment modeling methods, posing a significant challenge to existing obstacle avoidance systems. Due to the infrequent position updates of dynamic obstacles, the system suffers from perception lag and is unable to effectively capture fast-moving targets. Avoidance strategies based on fixed safety distances are conservative in their decision-making, prone to frequent starts and stops or path oscillation in dynamic scenarios. Furthermore, the lack of accurate modeling of obstacle motion trends makes it difficult for the system to plan obstacle avoidance paths in advance.

[0004] Therefore, the research focuses on developing a control strategy that can adapt to the dynamic nature of the agricultural environment and construct a highly real-time and robust "perception-prediction-decision-making" closed loop to enable autonomous operation of unmanned agricultural machinery in complex farmland environments. In the perception phase, the integration of sensors such as lidar and inertial measurement modules improves detection accuracy and response speed, enhancing comprehensive environmental perception. In the prediction phase, model predictive control methods are used to accurately predict the future trajectory of dynamic obstacles, taking into account their motion patterns and environmental constraints. This provides forward-looking information for the decision-making phase, enabling the unmanned agricultural machinery to plan obstacle avoidance paths in advance. In the decision-making phase, based on the perception and prediction results, the unmanned agricultural machinery's operating path is dynamically adjusted while ensuring operational continuity, achieving a balance between obstacle avoidance safety and operational efficiency. Furthermore, the decision-making system is real-time and adaptable, enabling rapid adjustments to environmental changes, ensuring the unmanned agricultural machinery's autonomous operation in complex farmland environments. Summary of the Invention

[0005] The main purpose of this invention is to develop a control strategy for unmanned agricultural machinery that adapts to dynamic agricultural environments. This strategy addresses the computational complexity, real-time nature, and adaptability issues of existing agricultural machinery obstacle avoidance methods. This invention focuses on three core areas: multi-source sensor data fusion, dynamic obstacle trajectory prediction, and real-time path planning optimization. This approach builds a highly real-time "perception-prediction-decision-making" closed-loop system and proposes a dynamic obstacle avoidance method for autonomous navigation of unmanned agricultural machinery. The specific steps are as follows:

[0006] 1. A method for autonomous navigation and dynamic obstacle avoidance of an unmanned agricultural machine, comprising the following steps:

[0007] S1: Obtain the starting and ending position information of the unmanned agricultural machinery operation, and obtain the initial trajectory through the path planning module;

[0008] S2: Obtain obstacle information and road information through vehicle-mounted lidar;

[0009] S3: Divide the LiDAR field of view and adopt different treatment methods for obstacles in different areas;

[0010] S4: Estimating the state of the unmanned agricultural machine and predicting the future state of the unmanned agricultural machine and moving obstacles;

[0011] S5: Estimate the state information of the moving obstacle;

[0012] S6: Establish obstacle avoidance constraint function;

[0013] S7: Use obstacle information, road information and obstacle avoidance algorithm to adjust the initial planned trajectory to obtain the target trajectory.

[0014] Furthermore, the step S2 includes the following steps:

[0015] S2.1. Obstacle information acquired by the lidar includes obstacle coordinates, and road information includes road boundary coordinates.

[0016] S2.2. Obstacles are classified as stationary or moving based on the distance between their initial and current positions. Obstacles that meet the following equation will be classified as moving obstacles:

[0017]

[0018] Obstacles that meet the following conditions will be classified as stationary obstacles:

[0019]

[0020] in: Represents the current position of the obstacle, represents the initial position of the obstacle, h represents the number of obstacles; B sn Represents a small positive constant used to compensate for the lidar's measurement noise and other errors.

[0021] Furthermore, the step S3 includes the following steps:

[0022] S3.1. The lidar is located at the front of the unmanned agricultural machine. Its field of view covers a 180° sector, aligned with the machine's heading. The lidar's detection radius is 30 meters.

[0023] S3.2. Divide the lidar field of view into three zones: Zone 1, located directly in front of the lidar field of view, is 30 meters wide; Zones 2 and 3, located to the left and right of the lidar field of view, are 15 meters wide each.

[0024] S3.3. For mobile obstacles in area 1, their future positions will be predicted by combining their real-time positions with model predictive control. For obstacles in areas 2 and 3, only the real-time position information detected by the lidar will be processed without predicting their future positions.

[0025] Further, the S4 includes the following steps:

[0026] S4.1. Use an inertial measurement unit to measure the vehicle's attitude and dynamically correct the current state estimate using a Kalman filter.

[0027] S4.2. In an autonomous operation environment for unmanned agricultural machinery, the predicted field of view must be long enough to avoid previously unknown obstacles. Therefore, to avoid any potential obstacles in advance, the state of the unmanned agricultural machinery must be predicted within a specified minimum field of view. This constraint can be expressed as:

[0028] L p =v×N×dt

[0029] L d -L p ≤0

[0030] Among them: K p represents the forecast field of view, v represents the speed of the unmanned agricultural machinery, N represents the total number of forecast steps, dt represents the duration of each forecast, L d Represents the minimum specified lidar field of view;

[0031] S4.3. When a moving obstacle is detected within area 1 of the lidar field of view, its future state, including its position and heading, is predicted;

[0032] S4.4. Calculate the distance between the initial position of the moving obstacle and its predicted position using the following formula:

[0033]

[0034] Where: C mobs (n,h) represents the distance between the initial position of the moving obstacle and its predicted position, represents the initial position of the moving obstacle, h represents the number of moving obstacles, Represents the predicted position of the moving obstacle, n represents the current prediction step number of the algorithm, and N represents the total number of prediction steps;

[0035] S4.5. Calculate the distance between the current position of the moving obstacle and its predicted position using the following formula:

[0036]

[0037] Where: D mobs (n,h) represents the distance between the initial position of the moving obstacle and its predicted position, represents the current position of the moving obstacle, h represents the number of obstacles, Represents the predicted position of the moving obstacle, n represents the current prediction step number of the algorithm, and N represents the total number of prediction steps.

[0038] Furthermore, in S5, the state information of the moving obstacle is estimated, and its position, speed, and steering angle are respectively as follows:

[0039]

[0040] in: Represents the speed estimation information of the moving obstacle, C mobs (n,h) represents the distance between the initial position of the moving obstacle and the predicted position, Dmobs (n,h) represents the distance between the initial position of the moving obstacle and the predicted position, n represents the number of prediction steps of the algorithm, dt mv =T s / g represents a sampling period T s g predictions will be performed within the time. Represents the steering angle estimation information of a moving obstacle.

[0041] Further, the S6 includes the following steps:

[0042] S6.1. Based on the estimated states of the UAV and obstacles and the LiDAR detection data, the collision risk between the UAV and obstacles can be preliminarily determined through the state processing process. Specifically, it can be expressed as:

[0043] or

[0044] or

[0045] in: represents the heading angle of the h-th obstacle, ψ represents the heading angle of the unmanned agricultural machine, and ψ up represents the upper limit of the heading angle used to determine the collision risk, ψ low represents the lower limit of the heading angle used to determine the collision risk;

[0046] S6.2. Establish an obstacle avoidance constraint function to achieve dynamic collision avoidance, and adopt a superellipse obstacle avoidance constraint to more accurately depict the outline of the obstacle and obtain a more accurate safety area. Specifically, the algorithm generates two superellipses to enclose the UAV and the obstacle respectively. If the distance between the obstacle and the UAV is less than or equal to the sum of the major semi-axes of the two superellipses, it indicates that there is a possibility of collision between the obstacle and the UAV, and obstacle avoidance measures need to be implemented. Otherwise, it means that the probability of collision between the obstacle and the UAV is low, and no obstacle avoidance measures are required.

[0047] Further, the S7 includes the following steps:

[0048] S7.1. To ensure smoothness of the obstacle avoidance path and to account for the limitations of the UAV's power and steering systems, set upper and lower limits for the UAV's steering angle, steering angular rate, and speed.

[0049] S7.2. Use superellipse constraints to transform the UAV and obstacles into a superellipse envelope. Based on the superellipse envelope, road information, UAV state estimation information, and obstacle avoidance algorithm, adjust the initial planned path in real time to obtain the target trajectory.

[0050] Through the above technical solution, compared with the prior art, the present invention has the following beneficial effects:

[0051] In order to solve the dynamic obstacle avoidance and path tracking problems of unmanned agricultural machinery in complex agricultural scenes, the present invention discloses a dynamic obstacle avoidance method for autonomous navigation of unmanned agricultural machinery; first, through the laser radar field of view partitioning strategy and dynamic obstacle classification prediction mechanism, the 180° field of view is divided into a core area in front and auxiliary areas on the left and right sides, and only the moving obstacles in the core area are predicted by model prediction control trajectory prediction, which significantly reduces the computing load and improves real-time performance; secondly, combined with the Kalman filter dynamic correction of agricultural machinery state and the super-elliptical obstacle avoidance constraint model, the collision risk is judged by the safety distance superimposed by the major semi-axis, and the contours of agricultural machinery and obstacles are accurately fitted, overcoming the redundancy problem of traditional circular / rectangular constraints and enhancing the obstacle avoidance safety in complex farmland environments; at the same time, by integrating road boundary information, multi-obstacle state estimation and steering angle / speed dynamic constraints, a smooth trajectory is generated to adapt to the soft ground operation characteristics of agricultural machinery, avoid sudden turns or speed changes, and expand the applicability of autonomous navigation of unmanned agricultural machinery in changeable farmland scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 A schematic flow chart of a method for autonomous navigation and dynamic obstacle avoidance for unmanned agricultural machinery designed by the present invention;

[0053] Figure 2 This is a schematic diagram of the dynamic obstacle avoidance of the unmanned agricultural machine of the present invention;

[0054] Figure 3 This is a schematic diagram of the laser radar field of view division described in the present invention;

[0055] Figure 4 This is a schematic diagram of the future position prediction of the unmanned agricultural machine and obstacles of the present invention;

[0056] Figure 5 Schematic diagram of the superellipse enclosure model of the present invention; (a) is model 1; (b) is model 2; DETAILED DESCRIPTION

[0057] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0058] The following describes the embodiments of the present invention through specific examples, and those skilled in the art can easily implement the embodiments according to the contents disclosed in this specification.

[0059] The schematic flow diagram of the present invention is as follows Figure 1 As shown in the figure, the dynamic obstacle avoidance diagram of unmanned agricultural machinery is as follows: Figure 2 shown.

[0060] A method for autonomous navigation and dynamic obstacle avoidance of an unmanned agricultural machine, wherein the specific process of implementing the method is as follows:

[0061] S1: Obtain the starting and ending position information of the unmanned agricultural machinery operation, and obtain the initial trajectory through the path planning module;

[0062] S2: Obtain obstacle information and road information through vehicle-mounted lidar;

[0063] S2.1. Obstacle information acquired by the lidar includes obstacle coordinates, and road information includes road boundary coordinates.

[0064] S2.2. Obstacles are classified as stationary or moving based on the distance between their initial and current positions. Obstacles that meet the following equation will be classified as moving obstacles:

[0065]

[0066] Obstacles that meet the following conditions will be classified as stationary obstacles:

[0067]

[0068] in: Represents the current position of the obstacle, represents the initial position of the obstacle, h represents the number of obstacles; B sn Represents a small positive constant used to compensate for the lidar's measurement noise and other errors.

[0069] S3: LiDAR field of view is divided as follows Figure 3 As shown in the figure, the laser radar field of view is divided, and different methods are used to deal with obstacles in different areas;

[0070] S3.1. The lidar is located at the front of the unmanned agricultural machine. Its field of view covers a 180° sector, aligned with the machine's heading. The lidar's detection radius is 30 meters.

[0071] S3.2. Divide the lidar field of view into three zones: Zone 1, located directly in front of the lidar field of view, is 30 meters wide; Zones 2 and 3, located to the left and right of the lidar field of view, are 15 meters wide each.

[0072] S3.3. For mobile obstacles in area 1, their future positions will be predicted by combining their real-time positions with model predictive control. For obstacles in areas 2 and 3, only the real-time position information detected by the lidar will be processed without predicting their future positions.

[0073] S4: Estimating the state of the unmanned agricultural machinery and predicting the future state of the unmanned agricultural machinery and moving obstacles. The future state prediction diagram is shown in the figure below. Figure 4 As shown; where R f，iPredict trajectories for moving obstacles; R aeav，i Predicted trajectory of unmanned agricultural machinery.

[0074] S4.1. Use an inertial measurement unit to measure the vehicle's attitude and dynamically correct the current state estimate using a Kalman filter.

[0075] S4.2. In an autonomous operation environment for unmanned agricultural machinery, the predicted field of view must be long enough to avoid previously unknown obstacles. Therefore, to avoid any potential obstacles in advance, the state of the unmanned agricultural machinery must be predicted within a specified minimum field of view. This constraint can be expressed as:

[0076] L p =v×N×dt

[0077] L d -L p ≤0

[0078] Where: L p represents the forecast field of view, v represents the speed of the unmanned agricultural machinery, N represents the total number of forecast steps, dt represents the duration of each forecast, L d Represents the minimum specified lidar field of view;

[0079] S4.3. When a moving obstacle is detected within area 1 of the lidar field of view, its future state, including its position and heading, is predicted;

[0080] S4.4. Calculate the distance between the initial position of the moving obstacle and its predicted position using the following formula:

[0081]

[0082] Where: C mobs (n,h) represents the distance between the initial position of the moving obstacle and its predicted position, represents the initial position of the moving obstacle, h represents the number of moving obstacles, Represents the predicted position of the moving obstacle, n represents the current prediction step number of the algorithm, and N represents the total number of prediction steps;

[0083] S4.5. Calculate the distance between the current position of the moving obstacle and its predicted position using the following formula:

[0084]

[0085] Where: D mobs (n,h) represents the distance between the initial position of the moving obstacle and its predicted position, represents the current position of the moving obstacle, h represents the number of obstacles, Represents the predicted position of the moving obstacle, n represents the current prediction step number of the algorithm, and N represents the total number of prediction steps.

[0086] S5: Estimate the state information of the moving obstacle; its position, speed, and steering angle are as follows:

[0087]

[0088] in: Represents the speed estimation information of the moving obstacle, C mobs (n,h) represents the distance between the initial position of the moving obstacle and the predicted position, D mobs (n,h) represents the distance between the initial position of the moving obstacle and the predicted position, n represents the number of prediction steps of the algorithm, dt mv =T s / g represents a sampling period T s g predictions will be performed within the time. Represents the steering angle estimation information of a moving obstacle.

[0089] S6: Establish obstacle avoidance constraint function;

[0090] S6.1. Based on the estimated states of the UAV and obstacles and the LiDAR detection data, the collision risk between the UAV and obstacles can be preliminarily determined through the state processing process. Specifically, it can be expressed as:

[0091] or

[0092] or

[0093] in: represents the heading angle of the h-th obstacle, ψ represents the heading angle of the unmanned agricultural machine, and ψ up represents the upper limit of the heading angle used to determine the collision risk, ψ low represents the lower limit of the heading angle used to determine the collision risk;

[0094] S6.2. Establish an obstacle avoidance constraint function to achieve dynamic collision avoidance. Use a superellipse obstacle avoidance constraint to more accurately depict the outline of the obstacle and obtain a more accurate safety area. The superellipse enclosing model is as follows: Figure 5 As shown in the figure; specifically, the algorithm generates two superellipses to enclose the UAV and the obstacle respectively. If the distance between the obstacle and the UAV is less than or equal to the sum of the major semi-axes of the two superellipses, it indicates that there is a possibility of collision between the obstacle and the UAV, and obstacle avoidance measures need to be implemented; otherwise, it means that the probability of collision between the obstacle and the UAV is low, and no obstacle avoidance measures are needed.

[0095] S7: Use obstacle information, road information and obstacle avoidance algorithm to adjust the initial planned trajectory to obtain the target trajectory.

[0096] S7.1. To ensure smoothness of the obstacle avoidance path and to account for the limitations of the UAV's power and steering systems, set upper and lower limits for the UAV's steering angle, steering angular rate, and speed.

[0097] S7.2. Use superellipse constraints to transform the UAV and obstacles into a superellipse envelope. Based on the superellipse envelope, road information, UAV state estimation information, and obstacle avoidance algorithm, adjust the initial planned path in real time to obtain the target trajectory.

[0098] In addition, the present invention also provides a reference method for research in the same field, and can be further extended to other related unmanned agricultural machinery path planning and obstacle avoidance fields as a basis, with high practicality and promotion value.

[0099] The series of detailed descriptions listed above are only specific descriptions of feasible implementation methods of the present invention. They are not intended to limit the scope of protection of the present invention. Any equivalent methods or changes that do not deviate from the technology of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for autonomous navigation and dynamic obstacle avoidance of unmanned agricultural machinery, characterized in that: The following steps are involved: S1: Obtain the starting and ending position information of the unmanned agricultural machinery operation, and obtain the initial trajectory through the path planning module; S2: Obtain obstacle information and road information through vehicle-mounted lidar; S3: Divide the LiDAR field of view and adopt different treatment methods for obstacles in different areas; S4: Estimating the state of the unmanned agricultural machine and predicting the future state of the unmanned agricultural machine and moving obstacles; S5: Estimate the state information of the moving obstacle; S6: Establish obstacle avoidance constraint function; S7: Use obstacle information, road information and obstacle avoidance algorithm to adjust the initial planned trajectory to obtain the target trajectory.

2. The method according to claim 1, characterized in that The S2 comprises the following steps: S2.

1. Obstacle information acquired by the lidar includes obstacle coordinates, and road information includes road boundary coordinates. S2.

2. Obstacles are classified as stationary or moving based on the distance between their initial and current positions. Obstacles that meet the following equation will be classified as moving obstacles: Obstacles that meet the following conditions will be classified as stationary obstacles: in: Represents the current position of the obstacle, represents the initial position of the obstacle, i is the location, and h represents the number of obstacles; B sn Represents a small positive constant used to compensate for the lidar's measurement noise and other errors.

3. The method according to claim 1, characterized in that The S3 includes the following steps: S3.

1. The lidar is located at the front of the unmanned agricultural machine. Its field of view covers a 180° sector, aligned with the machine's heading. The lidar's detection radius is 30 meters. S3.

2. Divide the lidar field of view into three zones: Zone 1, located directly in front of the lidar field of view, is 30 meters wide; Zones 2 and 3, located to the left and right of the lidar field of view, are 15 meters wide each. S3.

3. For mobile obstacles in area 1, their future positions will be predicted by combining their real-time positions with model predictive control. For obstacles in areas 2 and 3, only the real-time position information detected by the lidar will be processed without predicting their future positions.

4. The method according to claim 1, wherein The S4 comprises the following steps: S4.

1. Use an inertial measurement unit to measure the vehicle's attitude and dynamically correct the current state estimate using a Kalman filter. S4.

2. In an autonomous operation environment for unmanned agricultural machinery, the predicted field of view must be long enough to avoid previously unknown obstacles. Therefore, to avoid any potential obstacles in advance, the state of the unmanned agricultural machinery must be predicted within a specified minimum field of view. This constraint can be expressed as: L p =v×N×dt L d -L p ≤0 Where: L p represents the forecast field of view, v represents the speed of the unmanned agricultural machinery, N represents the total number of forecast steps, dt represents the duration of each forecast, L d Represents the minimum specified lidar field of view; S4.

3. When a moving obstacle is detected within area 1 of the lidar field of view, its future state, including its position and heading, is predicted; S4.

4. Calculate the distance between the initial position of the moving obstacle and its predicted position using the following formula: Where: C mobs (n,h) represents the distance between the initial position of the moving obstacle and its predicted position, represents the initial position of the moving obstacle, h represents the number of moving obstacles, Represents the predicted position of the moving obstacle, n represents the current prediction step number of the algorithm, and N represents the total number of prediction steps; S4.

5. Calculate the distance between the current position of the moving obstacle and its predicted position using the following formula: Where: D mobs (n,h) represents the distance between the initial position of the moving obstacle and its predicted position, represents the current position of the moving obstacle, h represents the number of obstacles, Represents the predicted position of the moving obstacle, n represents the current prediction step number of the algorithm, and N represents the total number of prediction steps.

5. The method according to claim 1, wherein In S5, the state information of the moving obstacle is estimated, and its position, speed and steering angle are respectively as follows: in: is the current position coordinate of the obstacle; Represents the speed estimation information of the moving obstacle, C mobs (n,h) represents the distance between the initial position of the moving obstacle and the predicted position, D mobs (n,h) represents the distance between the initial position of the moving obstacle and the predicted position, n represents the number of prediction steps of the algorithm, dt mv =T s / g represents a sampling period T s g predictions will be performed within the time. Represents the steering angle estimation information of a moving obstacle.

6. The method according to claim 1, characterized in that The S6 comprises the following steps: S6.

1. Based on the estimated states of the UAV and obstacles and the LiDAR detection data, the collision risk between the UAV and obstacles can be preliminarily determined through the state processing process. Specifically, it can be expressed as: or or in: represents the heading angle of the h-th obstacle, ψ represents the heading angle of the unmanned agricultural machine, and ψ up represents the upper limit of the heading angle used to determine the collision risk, ψ low represents the lower limit of the heading angle used to determine the collision risk; S6.

2. Establish an obstacle avoidance constraint function to achieve dynamic collision avoidance, and adopt a superellipse obstacle avoidance constraint to more accurately depict the outline of the obstacle and obtain a more accurate safety area. Specifically, the algorithm generates two superellipses to enclose the UAV and the obstacle respectively. If the distance between the obstacle and the UAV is less than or equal to the sum of the major semi-axes of the two superellipses, it indicates that there is a possibility of collision between the obstacle and the UAV, and obstacle avoidance measures need to be implemented. Otherwise, it means that the probability of collision between the obstacle and the UAV is low, and no obstacle avoidance measures are required.

7. The method according to claim 1, characterized in that The S7 comprises the following steps: S7.

1. To ensure smoothness of the obstacle avoidance path and to account for the limitations of the UAV's power and steering systems, set upper and lower limits for the UAV's steering angle, steering angular rate, and speed. S7.

2. Use superellipse constraints to transform the UAV and obstacles into a superellipse envelope. Based on the superellipse envelope, road information, UAV state estimation information, and obstacle avoidance algorithm, adjust the initial planned path in real time to obtain the target trajectory.

Images