Motion planning method based on dynamic target tracking

Abstract

The invention discloses a motion planning method based on dynamic target tracking, and the method comprises the steps: carrying out the path planning, defining the position of a local target point at a current moment, and obtaining a shortest path from an unmanned plane to the local target point through a path planning algorithm; secondly, performing smooth optimization on the shortest path by adopting a B-spline curve, and performing collision detection on an optimized curve after smooth optimization based on a Gaussian potential field of an obstacle to obtain a final track; and finally, controlling the unmanned aerial vehicle to move along the final track until the unmanned aerial vehicle reaches the position of the local target point. According to the method, local path planning is optimized by introducing local target points in the planning process, so that the search burden is greatly reduced; meanwhile, a Gaussian potential field B spline optimization method is adopted, so that the flight path of the unmanned aerial vehicle is smoother, and efficient, smooth and safe dynamic target tracking can be realized in a complex environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of path planning, and in particular relates to a motion planning method based on dynamic target tracking. Background Art

[0002] With the development of UAV technology, UAVs are increasingly used in military reconnaissance, disaster search and rescue, environmental monitoring and other fields. Dynamic target tracking is a key technology for autonomous navigation of UAVs, which requires UAVs to plan trajectories in real time in unknown or complex environments and keep tracking the target. However, existing motion planning methods still have many problems in dealing with dynamic target tracking tasks.

[0003] Motion planning consists of front-end path planning and back-end trajectory optimization. At present, common UAV path planning methods include path search methods based on graph search, random sampling methods, and methods based on artificial potential fields. For example, the A* (A-Star) algorithm can find the shortest path in a complex environment, but the computational overhead is large in a complex environment and it is difficult to adapt to real-time dynamic changes. Although Jump Point Search (JPS) improves the search efficiency, it is only applicable to discrete grid spaces and is difficult to handle continuous space optimization problems. In addition, although the path planning method based on artificial potential fields has certain advantages in real-time performance, it is easy to fall into local optimality in complex environments, resulting in path oscillation or stagnation.

[0004] In terms of trajectory optimization, existing methods mainly include trajectory optimization based on B-spline, trajectory optimization based on safe flight channel (SFC), and trajectory optimization based on reinforcement learning. B-spline curves can generate smooth and continuous trajectories, but they are prone to overfitting data points and are difficult to adapt to global optimization problems in complex environments. The optimization method based on the safe flight channel can ensure the safety of UAV flight, but its calculation amount is large, and hard constraints may cause the trajectory to be too close to obstacles, increasing the risk of collision. Reinforcement learning methods have strong adaptability, but the training process is prone to fall into local optimality, and the computing resource consumption is large in tasks with high real-time requirements. Summary of the invention

[0005] The purpose of the present invention is to provide a motion planning method based on dynamic target tracking.

[0006] The present invention provides a motion planning method based on dynamic target tracking, which comprises the following steps:

[0007] Step 1: Obtain the environmental information around the moving object and construct a grid map, and mark the current position of obstacles and moving objects in the grid map;

[0008] Step 2: Define the position of the local target point at the current moment: if the tracking target is within the field of view of the moving object, the coordinates of the tracking target are used as the position of the local target point; if the tracking target is not within the field of view of the moving object, the local target point is obtained according to the direction from the moving object to the point where the tracking target leaves the field of view; the path planning algorithm is used to obtain the shortest path from the moving object to the local target point;

[0009] Step 3: Use a spline curve to smoothly optimize the path obtained in step 2, and obtain the final trajectory according to the optimized curve after smooth optimization;

[0010] Step 4: Control the moving object to move along the final trajectory until it reaches the position of the local target point;

[0011] Step 5: Repeat steps 2 to 4 to complete the tracking of the moving object to the tracking target.

[0012] As a preferred embodiment, in the step 2, when the tracking target is out of the field of view, the local target point The method to obtain is:

[0013]

[0014] Among them, P current (t) is the position of the moving object; The unit vector of the moving object in the direction of the point where the tracking target leaves the field of view; is the weight factor.

[0015] Preferably, in the step three, a Gaussian potential field is constructed in the grid map, and after smoothing optimization, obstacle avoidance processing is performed on the optimization curve according to the Gaussian potential field of the obstacle.

[0016] Preferably, the obstacle avoidance process is as follows:

[0017] Check whether the optimized curve has a collision risk. If the optimized curve does not have a collision risk, the optimized curve is used as the final trajectory. If the optimized curve has a collision risk, the average position of all points on the optimized curve with a collision risk is calculated, and the point closest to the average position is selected as the adjacent point in the minimum potential energy boundary of the Gaussian potential field of the obstacle. By moving the average point toward the adjacent point, a safe point that is out of collision risk is obtained. The safe point is added to the spline curve as a control point to re-optimize the curve, and the re-optimized curve is used as the final trajectory.

[0018] Preferably, the basis for judging whether the optimization curve has a collision risk is that if there is a point on the optimization curve whose potential energy is higher than the minimum potential energy in the Gaussian potential field of the obstacle, then the optimization curve has a collision risk.

[0019] Preferably, in step three, a B-spline curve is used to smoothly optimize the path; and the path nodes in the path obtained in step two are used as control points of the B-spline curve.

[0020] Preferably, in step 2, an A* algorithm is used to obtain the path from the moving object to the local target point; the heuristic function in the A* algorithm that measures the distance from the moving object to the local target point is The way to obtain is as follows:

[0021]

[0022] in, From the starting point to the point The actual moving cost, ; is the fixed cost of each move; From the starting point to the point the cost; is the local target point position; P current The position of the moving object.

[0023] Preferably, the moving object is a drone; the method for controlling the drone to move along the final trajectory is as follows: constructing a rigid body model of drone flight control and its state equation; optimizing the state equation by minimizing the distance error between the predicted trajectory and the final trajectory as the cost function to achieve drone motion control.

[0024] In a second aspect, the present invention provides a computer device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the memory stores the computer program; and the processor executes the above-mentioned motion planning method.

[0025] In a third aspect, the present invention provides a readable storage medium storing a computer program; the computer program is used to implement the above-mentioned motion planning method when executed by a processor.

[0026] The present invention has the following beneficial effects:

[0027] 1. The present invention introduces local target points in the planning process to optimize local path planning, thereby solving the problem that the target is out of view for some time periods when tracking dynamic targets, and the A* algorithm has a large computational overhead in complex environments and is difficult to adapt to real-time dynamic changes. Compared with global path planning, the present invention can greatly reduce the search burden; at the same time, the present invention adopts a Gaussian potential field B-spline optimization method based on soft constraints to make the flight trajectory of the UAV smoother and avoid flight instability caused by sharp turns. Compared with traditional trajectory optimization methods, the present invention avoids solving complex constraint optimization problems, making the calculation process more efficient, and ensuring that the UAV flies in a safe area by setting a dynamic constraint range.

[0028] 2. The present invention can quickly generate a safe real-time optimal trajectory by constructing a Gaussian potential field combined with a local dynamic A* algorithm, and achieve efficient, smooth and safe dynamic target tracking in a complex environment; at the same time, by adding control points, the drone is guided to avoid obstacles, the risk of collision is reduced, and the target tracking efficiency in a complex environment is improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 It is the overall flow chart of the present invention.

[0030] Figure 2 This is a flow chart of local dynamic path planning in the present invention.

[0031] Figure 3 Schematic diagram of the Gaussian potential energy B-spline trajectory optimization method in the present invention.

[0032] Figure 4 Schematic diagram of the control process of the UAV in the present invention.

[0033] Figure 5 It is a schematic diagram of real information of the simulated environmental obstacles in the present invention.

[0034] Figure 6 It is a schematic diagram of Gaussian potential field information of simulated environmental obstacles in the present invention.

[0035] Figure 7 Schematic diagram of the planning path of the simulation environment A* in the present invention.

[0036] Figure 8 Schematic diagram comparing the two-dimensional trajectory optimization results of the present invention and other trajectory optimization methods.

[0037] Fig. 9 It is a partially enlarged schematic diagram comparing the two-dimensional trajectory optimization results of the present invention with those of other trajectory optimization methods.

[0038] Fig.10 Schematic diagram comparing the smoothness of the three-dimensional trajectory curve of the present invention and other trajectory optimization methods.

[0039] Fig.11 Schematic diagram comparing the three-dimensional trajectory curve lengths of the present invention and other trajectory optimization methods.

[0040] Fig.12 It is a schematic diagram of the trajectory tracking results of the MPC controller in the present invention.

[0041] Fig.13 It is a schematic diagram of the speed control result of the MPC controller in the present invention.

[0042] Fig.14 It is a schematic diagram of the dynamic target trajectory tracking result of the UAV in the present invention.

[0043] Fig.15 It is a schematic diagram of the dynamic target tracking speed results of the UAV in the present invention. DETAILED DESCRIPTION

[0044] The present invention will be further described below in conjunction with the accompanying drawings.

[0045] like Figure 1 As shown, a motion planning method based on dynamic target tracking includes the following steps:

[0046] Step 1: Build an environment map

[0047] The depth camera carried by the drone is used to collect environmental point cloud data to obtain the obstacle distribution information around the drone. The drone is self-positioned through the visual inertial navigation system (VINS-Fusion) to ensure that the environmental perception data accurately matches the drone position. Combined with the point cloud data, a three-dimensional grid map is constructed, and the Euclidean distance transform (EDT) is used to calculate the distance D(x, y, z) from different locations to the nearest obstacle to obtain the obstacle distribution information. The expression of the distance D(x, y, z) is:

[0048] (1)

[0049] Among them, Ω is the obstacle point set; (x', y', z') is the position of the obstacle.

[0050] The Euclidean signed distance field (ESDF) is constructed based on the obstacle distribution information to provide environmental data support for subsequent path planning.

[0051] Step 2: Front-end path planning

[0052] like Figure 2 As shown, combined with the drone camera field of view (FOV), the local target point at the current moment is defined , whose expression is:

[0053] (2)

[0054] in, is the coordinates of the tracking target; Indicates that the tracking target appears in the field of view; is the current drone position; for The unit vector from the drone to the point where the tracked target leaves the field of view at that moment; is the weight factor used to ensure the local target point Within the field of view of the depth camera.

[0055] The A* algorithm is used to search for the shortest path from the current position of the drone to the local target point. The heuristic function in the A* algorithm measures the distance from the current position of the drone to the local target point. The expression is:

[0056] (3)

[0057] in, From the starting point to the point The actual moving cost, ; is the fixed cost of each move; From the starting point to the point the cost; is the local dynamic target point position; ||·||2 is the 2-norm.

[0058] Step 3: Backend trajectory optimization.

[0059] like Figure 3 As shown, a Gaussian potential field is constructed in the ESDF environment map to obtain the environmental information of the Gaussian potential field of the three-dimensional obstacle. The acquisition method is as follows:

[0060] (4)

[0061] in, Representing coordinates Potential energy at represents the center position of the potential field; It represents the peak height coefficient, which determines the height of the peak; represents the parameter controlling the range of the Gaussian distribution, which determines the distribution range of the potential field.

[0062] In this embodiment, the red circle in the constructed three-dimensional obstacle Gaussian potential field represents the dangerous area, the green circle represents the obstacle collision area, and the blue circle represents the Gaussian potential field optimization area; the red point represents the B-spline collision point, the green point represents the B-spline safety point, and the orange point represents the optimized safety control point.

[0063] The path nodes obtained by local dynamic path planning are converted into B-spline curve control points, and the shortest path obtained in step 2 is smoothly optimized using the B-spline method to optimize the control points. To ensure the smoothness of the trajectory, the optimization curve is obtained as follows:

[0064] (5)

[0065] (6)

[0066] in, To optimize the points on the curve; is the basis function recursive equation of the B-spline curve; is the control point; u is the parameter variable of the B-spline curve, which determines the value of each basis function.

[0067] Check whether the points on the optimization curve enter the Gaussian potential field danger zone of the obstacle. If there is no point on the curve, the potential energy is lower than or equal to the minimum potential energy of the obstacle. , it is considered that the point on the optimization curve has not entered the Gaussian potential field danger zone of the obstacle, and the optimization curve is used as the final trajectory; otherwise, it is considered that the point on the optimization curve has entered the Gaussian potential field danger zone of the obstacle, and the optimization curve is regenerated. The specific process is as follows:

[0068] Calculate all potential energies on the optimization curve that are higher than the minimum potential energy C up The average position of the points , and obtain the closest average position on the minimum potential energy boundary The point as the adjacent point , and the method to obtain it is as follows:

[0069] Average position Iterate, the iteration formula is as follows:

[0070] (7)

[0071] Among them, η is the gradient descent step size, which controls the size of each step movement to ensure that the trajectory point gradually converges to the direction of minimum potential energy; G is the potential field value, which is used to describe the impact of obstacles on the potential energy of the trajectory point; n is the number of iterations; ∇ is the gradient symbol.

[0072] Average position As the initial value of the iteration (x0, y0, z0), until the convergence condition is met, the nearby point C is obtained min .

[0073] Calculate obstacle center To the nearby point Moving distance , whose expression is: (8)

[0074] in, is the safety distance weight factor; <,·,·,> represents a three-dimensional vector, representing a direction from the center point to Vector.

[0075] By dividing the average point Towards point C min Move in the right direction to get out of the danger zone. , which is expressed as: (9)

[0076] Be safe As control points, they are added to the B-spline curve and re-optimized to obtain a safe and smooth GPB (Gaussian potential field B-spline) curve as the final trajectory, which is more adaptable to complex environments and ensures safety.

[0077] Step 4: Motion Control

[0078] 4-1. Since the UAV is mainly driven by the force and torque generated by the rotating rotor blades, the kinematic-dynamic model of the UAV is constructed based on the force and torque generated by the rotor. The body coordinate system is established as and the ground coordinate system is ; Assume that the drone's body and propeller are rigid bodies, and the overall mass is evenly distributed; By defining the drone's state vector X = [ x , y , z , ϕ , θ , ψ ] T and the control input vector U = [ F 1 , F 2 , F 3 , F 4 ] T , construct the UAV flight control rigid body model and its state equation as follows:

[0079] (10)

[0080] (11)

[0081] in, is the rolling angle; is the pitch angle; is the yaw angle; The total thrust provided to the drone by its rotors; For the quality of the drone; are the moments in the XYZ directions respectively; J xx , J yy , J zz are the rotational inertia of the drone on the XYZ axis respectively; is the acceleration of gravity; F1, F2, F3, and F4 are the thrusts provided to the UAV by different rotors; f is the discretized dynamic equation of the UAV; is the sampling time.

[0082] 4-2. If Figure 4 As shown in the figure, the model predictive controller (MPC) is used to calculate the control input so that the UAV can accurately track the final trajectory obtained in step three. The final trajectory obtained in step three is taken as the desired state of the UAV; in order to ensure that the UAV quickly and stably coincides with the final trajectory, the distance error between the predicted state and the desired state is calculated, and the error minimization is used as the cost function. The optimal control is solved by minimizing the difference between the predicted state X and the desired state; in the prediction time domain [0, N], the cost function J is designed as follows:

[0083] (12)

[0084] in, and are the state weight matrix and the input weight matrix respectively, , ; is the terminal weight matrix, ; For time The predicted status of For time the desired state; For time The control inputs of X(N) and X ref (N) represents the predicted state and expected state at the end point N, respectively; ||·|| Q 、||·|| H 、||·|| P They are the binary norms of the corresponding state weight matrix Q, input weight matrix H, and terminal weight matrix P respectively.

[0085] Considering that the thrust provided by the rotor to the UAV is limited by voltage, the minimum and maximum amplitudes of the control input are set to and The cost function of MPC-based UAV control aims to optimize the shortest distance between the UAV and the GPB optimized trajectory, while also considering the limitations of UAV speed and motor voltage. The optimization problem is expressed as follows:

[0086] (13)

[0087] in, and are the maximum and minimum speeds of the drone, respectively; represent The total speed of the drone at the moment; U(k) is The control input of the drone at time v max is the maximum speed threshold

[0088] Considering that the UAV uses visual odometer information for self-positioning in the actual experimental environment, in order to prevent the UAV from flying too fast and causing visual positioning drift, the constraint and When the maximum speed of the drone exceeds In order to stabilize the flight, the maximum speed threshold is set to .

[0089] Step 5. Repeat steps 2 to 4 to complete the tracking of the target by the drone.

[0090] Step 6: Simulation

[0091] Simulate the motion planning of drones in a three-dimensional complex environment. In the simulated environment, the field of view of the simulated camera is 0.5m~3m, and a Gaussian potential field is established every 0.5m. The real information of obstacles such as Figure 5 As shown. Figure 6 In each Gaussian plane, the red area is the area occupied by the obstacle, and the green area is the area extended by the radius of the drone. If the curve enters the green area, it is judged to have collided with the obstacle.

[0092] like Figure 7 As shown in the figure, the local dynamic A* algorithm of the front-end path planning plans the shortest path to track the target in a complex environment. The blue points on the curve are the planned path nodes. However, it can be observed from the figure that there are many non-conductive turning points in this path, which will cause the speed to be discontinuous. The drone will have a large jitter phenomenon, affecting the stability and smoothness of the drone's flight.

[0093] like Figure 8 , Fig. 9 , Fig.10 and Fig.11 As shown in the figure, the motion planning is performed using the present invention and three widely used trajectory optimization algorithms MiniSnap, B-Spline and SFC-B-Spline in complex two-dimensional and three-dimensional environments; the motion planning comparison of all optimization curves can be clearly seen from the figure. Fig. 9It can be seen that the curve optimized by the new control point obtained by the Gaussian potential field calculation in the present invention has never entered the green collision area in the entire planning process, maintaining a safe distance from obstacles and effectively avoiding collisions; at the same time, the MiniSnap, B-Spline and SFC B-Spline curves show a certain jitter in the trajectory of control point optimization, which reduces the smoothness of the UAV flight process. In contrast, the present invention shows a smoother trend during the optimization process. The entire curve conforms to the kinematic-dynamic constraints of the UAV and is suitable for tracking dynamic targets in the three-dimensional complex environment of the UAV. In a three-dimensional environment, the curvature distribution results of the curves in different trajectory optimization algorithms are shown in the following figure. Fig.10 As shown, the curvature of the Minisnap curve ranges from , the curvature distribution of the B-Spline curve changes from , the curvature of the SFC-B-Spline curve changes from , and the curvature distribution of the present invention is from Among the four curves, the curvature range of the present invention is the smallest, and its average curvature is closer to zero than the other three curves. This shows that the optimized curve of the present invention is smoother than the other three curves and is more suitable for use as the flight trajectory of the UAV. Fig.11 The total length of each trajectory curve is shown in Figure 2; the length of the Minisnap curve is 36.57 meters, the B-Spline curve is 35.53 meters, the SFC-B-Spline curve is 36.78 meters, and the optimized curve of the present invention is 35.96 meters. Although the B-Spline curve is the shortest in total length, it carries the potential risk of collision with obstacles. Compared with the other three curves, the present invention not only has no collision risk, but also has a shorter trajectory length. This highlights the superiority of the present invention in trajectory optimization, providing a safer and more efficient option for drones.

[0094] like Fig.12 As shown, based on the control method of the present invention, the UAV is simulated to track a dynamic target in a complex environment. The flight trajectory of the UAV basically matches the expected trajectory, and the tracking speed is maintained at about 2 meters per second. During the turning process, the flight performance is smooth and fluent. Fig.13 The partial and total speeds of the drone in the X, Y, and Z directions are shown. The flight speed in the X direction is maintained at The speed range in the Y direction is , the speed in the Z direction is The total speed of the drone is maintained during the flight. This shows that the present invention effectively achieves accurate tracking of the planned path and maintains good flight performance in multiple directions.

[0095] Step 7: In order to realize the experiment of UAV dynamic target tracking in a three-dimensional dense and complex environment, the experimental environment is set up in an indoor corridor with a height of 2.5m. The UAV used in the experiment is based on a P230 wheelbase quadcopter. The dynamic target in the experimental environment is set as a virtual dynamic target, which can move freely in three-dimensional space. Its range of motion is limited to the X-axis. , Y axis , Z axis . Maximum speed of the target for , maximum acceleration for .

[0096] In the constructed complex environment, the drone uses the depth camera information to initialize its own position and surrounding environment information. When the drone receives dynamic target information, it provides feedback and adjustments based on the difference between its own position and the target position. Based on the surrounding obstacle information, the motion planning controller plans the shortest path to the target at the front end and optimizes the best flight trajectory at the back end. Based on the optimal trajectory information, the drone tracks the dynamic target in real time.

[0097] Fig.14 The trajectory of the drone tracking the dynamic target is shown. The flight trajectory of the drone is basically consistent with the planned trajectory, and the tracking speed is maintained at about The drone basically tracked every move of the target, and the flight trajectory was smooth. Fig.15 The partial and total speeds of the controlled drone in the X, Y, and Z directions during tracking are shown; the flight speed in the X direction is maintained at , the Y direction ranges from , and the Z direction ranges from The total speed of the drone during flight is maintained at between.

Claims

1. A motion planning method based on dynamic target tracking, characterized in that: The following steps are involved: Step 1: Obtain the environmental information around the moving object and construct a grid map, and mark the current position of obstacles and moving objects in the grid map; Step 2: Define the position of the local target point at the current moment: if the tracking target is within the field of view of the moving object, the coordinates of the tracking target are used as the position of the local target point; if the tracking target is not within the field of view of the moving object, the local target point is obtained according to the direction from the moving object to the point where the tracking target leaves the field of view; the path planning algorithm is used to obtain the shortest path from the moving object to the local target point; Step 3: Use a spline curve to smoothly optimize the path obtained in step 2, and obtain the final trajectory according to the optimized curve after smooth optimization; Step 4: Control the moving object to move along the final trajectory until it reaches the position of the local target point; Step 5: Repeat steps 2 to 4 to complete the tracking of the moving object to the tracking target.

2. A motion planning method based on dynamic target tracking according to claim 1, characterized in that: In the step 2, when the tracking target is out of view, the local target point The method to obtain is: ； Among them, P current (t) is the position of the moving object; The unit vector of the moving object in the direction of the point where the tracking target leaves the field of view; is the weight factor.

3. The motion planning method based on dynamic target tracking according to claim 1, characterized in that: In the step three, a Gaussian potential field is constructed in the grid map, and after smooth optimization, obstacle avoidance processing is performed on the optimization curve according to the Gaussian potential field of the obstacle.

4. The motion planning method based on dynamic target tracking according to claim 3, characterized in that: The process of obstacle avoidance is as follows: Check whether the optimization curve has a collision risk. If the optimization curve does not have a collision risk, the optimization curve is used as the final trajectory. If the optimization curve has a collision risk, the average position of all points on the optimization curve with a collision risk is calculated, and the point closest to the average position is selected as the adjacent point in the minimum potential energy boundary of the Gaussian potential field of the obstacle. By moving the average point toward the adjacent point, a safe point out of the risk of collision is obtained; The safe points are added as control points to the spline curve to re-optimize the curve, and the re-optimized curve is used as the final trajectory.

5. The motion planning method based on dynamic target tracking according to claim 4, characterized in that: The basis for judging whether the optimization curve has a collision risk is as follows: if there is a point on the optimization curve whose potential energy is higher than the minimum potential energy in the Gaussian potential field of the obstacle, then the optimization curve has a collision risk.

6. The motion planning method based on dynamic target tracking according to claim 1, characterized in that: In the step three, the path is smoothly optimized using a B-spline curve; the path nodes in the path obtained in step two are used as control points of the B-spline curve.

7. The motion planning method based on dynamic target tracking according to claim 1, characterized in that: In the step 2, the A* algorithm is used to obtain the path from the moving object to the local target point; the heuristic function in the A* algorithm that measures the distance from the moving object to the local target point is The way to obtain is as follows: ； in, From the starting point to the point The actual moving cost, ; is the fixed cost of each move; From the starting point to the point the cost; is the local target point position; P current The position of the moving object.

8. The motion planning method based on dynamic target tracking according to claim 1, characterized in that: The moving object is specifically a drone; the method for controlling the drone to move along the final trajectory is as follows: constructing a rigid body model of drone flight control and its state equation; optimizing the state equation by minimizing the distance error between the predicted trajectory and the final trajectory as a cost function to achieve drone motion control.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: The memory stores a computer program; the processor executes a motion planning method based on dynamic target tracking as described in any one of claims 1-8.

10. A readable storage medium storing a computer program; characterized in that: When the computer program is executed by a processor, it is used to implement a motion planning method based on dynamic target tracking as described in any one of claims 1 to 8.

Images