Target tracking method without updating Euclidean symbol distance field

Abstract

The invention belongs to the field of robotics, computer vision and path planning, and discloses a target tracking method without updating an Euclidean symbol distance field, which comprises the following steps: pre-constructing the Euclidean symbol distance field without updating; detecting, segmenting and resolving a target position; predicting the future position of the target; calculating a visible area of the target, and finding out an unshielded optimal observation point sequence; and optimizing the observation point sequence to obtain a smooth tracking trajectory. According to the tracking algorithm for the unmanned aerial vehicle, the target visibility can be effectively evaluated through the pre-constructed FOV-ESDF, and shielding and collision in the tracking process are effectively avoided. The path search algorithm and the trajectory optimization algorithm provided by the invention are high in real-time performance, and the calculation amount is reduced by about 10 times due to the fact that the environment ESDF does not need to be constructed and updated.

Description

Technical Field

[0001] The invention belongs to the fields of robotics, computer vision and path planning, and relates to efficient path planning, visual field perception evaluation and motion planning optimization algorithm of a target tracking system, and is specifically an aerial target tracking method without updating a Euclidean signed distance field. Background Art

[0002] Target tracking is an important research topic in the field of mobile robots and drones. Its core task is to track targets in real time in a dynamic environment while ensuring path safety and field of vision continuity. This technology has wide application value in many fields such as security monitoring, intelligent transportation, film and television production, and post-disaster rescue.

[0003] Real-time target tracking is one of the key technologies in the field of mobile robots such as drones. The latest existing methods (visibility-aware planners, adaptive dynamic tracking methods, etc.) usually rely on the environmental Euclidean Signed Distance Field (ESDF) for target visibility assessment and path optimization. However, the online update of the environmental ESDF consumes a lot of computing resources, especially in open scenes with complex obstacles and frequent dynamic changes in the environment. This computational overhead will increase significantly and become the main bottleneck of the real-time tracking system. In addition, most of the current tracking methods (fast trackers, elastic trackers, safety-first trackers, intention-aware planners, etc.) lack specialized optimization for field of view perception, which can easily lead to problems such as the target being lost due to obstacles or the planned path being too conservative.

[0004] In order to overcome the above problems, the present invention proposes a target tracking method without updating ESDF. By designing the field-of-view ESDF (FOV-ESDF) and an efficient path search algorithm, the computational complexity is significantly reduced, while the robustness and continuity of target tracking are improved. Summary of the invention

[0005] The present invention aims to provide an efficient and low computational cost target tracking method, which can realize real-time target tracking in a dynamic open environment. The present invention can greatly increase the time that the target is in the tracking field of view, reduce the probability of occlusion, and avoid the collision between the tracker and obstacles in the environment. The path generated by the present invention meets the smoothness requirement and can ensure the stability of the tracker motion. In addition, the present invention does not need to construct and update the environment ESDF, which reduces the amount of calculation by about 10 times.

[0006] The technical solution of the present invention:

[0007] A target tracking method without updating the Euclidean signed distance field, the steps are as follows:

[0008] Step 1: Pre-construct the Euclidean signed distance field that does not need to be updated;

[0009] Measure the field of view of the tracker camera and define the optimal observation distance from the tracker camera to the target , if the horizontal field of view of the tracker camera is , vertical field of view is , then the field of view of the tracker camera is modeled by a tetrahedron, and the formula is:

[0010]

[0011] in, is the coordinate in the tracker coordinate system, represents the transpose operator, The optimal observation distance is predefined Sure;

[0012] For the field of view For a point within the field of view, the FOV-ESDF value is defined as the distance from the point to the nearest field of view boundary plane; for a point within the field of view For points outside the FOV, the FOV-ESDF value is defined as .

[0013] Step 2: Detect, segment and calculate the target position;

[0014] Use the YOLOv8 model to perform instance segmentation on the target, then solve the relative position of the target and the tracker and store it in a queue;

[0015] Step 3: Predict the future location of the target;

[0016] Based on the target position data of the nearest several frames, the motion trajectory of the target is fitted using a quadratic smoothing function, and the possible position of the target in multiple future time steps is obtained according to the fitted motion trajectory;

[0017] Step 4: Calculate the visible area of ​​the target and find the optimal sequence of observation points without occlusion;

[0018] The specific implementation process is as follows:

[0019] For the target Observation location , the observation area at this location is defined as:

[0020]

[0021] in, ; represents the third-order identity matrix List, Take 1, 2, 3; represents the tracker position vector that satisfies the above conditions; Indicates the maximum observation distance error allowed in the vertical direction. Indicates the maximum observation distance error allowed in the horizontal direction;

[0022] Considering the occlusion of the target by obstacles, the occlusion area is defined as:

[0023]

[0024] in, represents a local grid map, If and only if There are no obstacles; represents the integral variable;

[0025] The calculation formula of the visible area of ​​the target is:

[0026]

[0027] For The tracker generates the candidate position of the tracker at the next moment. The formula is:

[0028]

[0029] Finally, Performing a breadth-first search for the source point will yield the position of the tracker at the next moment.

[0030] Step 5: Optimize the observation point sequence to obtain a smooth tracking trajectory.

[0031] The specific implementation process is as follows:

[0032] The position and orientation of the tracker are parameterized in time using a third-order B-spline, as follows:

[0033]

[0034]

[0035] in, Indicates the tracker is at position The corresponding yaw angle is Indicates Position control points, Indicates Angle control points;

[0036] The obstacles are removed from the field of view through occlusion penalty, and the objective function is:

[0037]

[0038] in, Indicates the value of FOV-ESDF. Indicates the number of obstacle points entering the FOV-ESDF, Calculated by the following formula:

[0039]

[0040] in, It is The world coordinates of the obstacle points, Represents the rotation matrix; the occlusion penalty produces a gradient for the position and a gradient for the yaw angle, calculated as follows:

[0041]

[0042]

[0043] in, represents the Nabla operator;

[0044] By observing the occlusion penalty, the tracker maintains an appropriate distance from the target. The objective function is:

[0045]

[0046] in, is the maximum value of FOV-ESDF;

[0047] The gradient of the occlusion penalty function is:

[0048]

[0049]

[0050] The angle penalty is used to promote the convergence of the observation occlusion penalty. The objective function is:

[0051]

[0052] The gradient of the penalty function is:

[0053]

[0054]

[0055] in, is the inverse tangent function with quadrant discrimination;

[0056] For obstacle avoidance, the robot-centered ESDF is used as RC-ESDF, and the objective function is:

[0057]

[0058] in, is the number of obstacles entering the RC-ESDF, is the evaluation value of RC-ESDF;

[0059] The kinematic feasibility of the trajectory is fully guaranteed by constraining the velocity and acceleration of the B-spline control points. The objective function is:

[0060]

[0061]

[0062] in, and Indicates the velocity control point and acceleration control point of the position B-spline curve, and Indicates the velocity control point and acceleration control point of the yaw angle B-spline curve, , , , Respectively represent the limit velocity, acceleration, angular velocity and angular acceleration of the tracker, and the function ;

[0063] Thanks to the convex hull property of B-spline, smoothness is ensured by minimizing the high-order derivatives of the control points. The objective function is:

[0064]

[0065] Finally, the L-BFGS algorithm is used to jointly optimize these objective functions to obtain the final trajectory.

[0066] Beneficial effects of the present invention:

[0067] (1) The tracking algorithm for UAV proposed in this invention can effectively evaluate the target visibility through the pre-constructed FOV-ESDF and effectively avoid occlusion and collision during the tracking process.

[0068] (2) The path search algorithm and trajectory optimization algorithm proposed in the present invention have strong real-time performance. Since there is no need to construct and update the environment ESDF, the amount of calculation is reduced by about 10 times. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] Figure 1 It is an implementation flow chart of the present invention.

[0070] Figure 2 Schematic diagram of the cross-section of the FOV-ESDF proposed in the present invention.

[0071] Figure 3 Schematic diagram of using FOV-ESDF to optimize the observation position in the present invention.

[0072] Figure 4 The figure is a schematic diagram of the tracking trajectory of a target tracked by the method of the present invention.

[0073] Figure 5 Tracking effect for fast tracker.

[0074] Figure 6 Tracking effect for visibility-aware planner.

[0075] Figure 7 Tracking performance of a two-stage tracker that prioritizes safety.

[0076] Figure 8 Tracking effect of the elastic tracker.

[0077] Fig. 9 This is the tracking effect of the present invention. DETAILED DESCRIPTION

[0078] The specific implementation of the present invention is further described below in conjunction with the accompanying drawings and technical solutions.

[0079] Example

[0080] A method for tracking aerial targets without updating the Euclidean signed distance field, the steps are as follows:

[0081] (1) The horizontal field of view of the tracker camera is measured to be 69.4° and the vertical field of view is 42.5°. The optimal observation distance from the camera to the target is defined as 2.2m, so the field of view of the tracker is determined. First, the distance transform algorithm is used to calculate the values ​​of most key points in the FOV-ESDF, and then cubic linear interpolation is used to obtain the FOV-ESDF value and gradient of any point. The constructed FOV-ESDF profile is shown in the figure below. Figure 2 As shown, the area closer to red has a higher FOV-ESDF value.

[0082] (2) Predictive modeling of target location and trajectory.

[0083] The camera image is input to the YOLOv8 model to obtain the instance segmentation result of the target, and the relative position of the target and the tracker is solved using the camera internal and external parameters. Then, based on the position data of the target in the most recent frames, the quadratic smoothing function is used to fit the motion trajectory of the target, thereby obtaining the position of the target in multiple time steps in the future.

[0084] (3) Define the visible area based on the target location and perform path planning.

[0085] The optimal observation range is determined based on the distance between the target and the tracker. By limiting the horizontal and vertical offsets of the target, a region containing the optimal observation distance is generated. This region ensures that the target is always within the field of view of the tracker and in the best observation position. On the other hand, in order to prevent the target from being blocked by obstacles, ray detection is used to evaluate whether there are any obstructions between the target and the tracker, and any obstacles that block the target's line of sight are considered as blocked areas. The final visible area is obtained by eliminating the blocked area from the observation area.

[0086] The present invention performs path planning based on the generated visible area. First, based on the predicted position of the target, an initial reference point is selected in the observation area, which is the shortest distance from the current position of the tracker and the optimal observation distance. Then, starting from the initial point, the path is gradually searched in the visible area. During the search process, the algorithm selects an unobstructed path to ensure that the generated reference path can maintain a clear observation of the target. Finally, a set of reference points is generated for each target position, and these points are connected into a continuous path to guide the next movement of the tracker.

[0087] (4) Perform trajectory optimization on the path planning results to obtain the final trajectory of the tracker.

[0088] To ensure that the target is always within the field of view of the tracker, during the optimization process, the present invention evaluates the visibility of the target based on the pre-built FOV-ESDF and adjusts the path. If the target is close to the edge of the field of view or is blocked by an obstacle, the optimization algorithm adjusts the position and heading angle of the tracker to reposition the target in the center area of ​​the field of view. The present invention optimizes the relative position of the tracker to ensure that obstacles in the field of view are eliminated while maintaining clear observation of the target, such as Figure 3 shown.

[0089] The tracking effect of the present invention is as follows Figure 4 As shown in the figure, green is the tracker's trajectory, blue is the tracked target's trajectory, and the red pyramid is the tracker's field of view. The target passes through a narrow passage at point A and turns quickly. The tracker can follow the target into the narrow passage and maintain visibility. The target suddenly turns along obstacles at points B and C, which can easily cause occlusion. The tracker successfully avoids occlusion caused by environmental obstacles by adjusting its own observation position. The tracking effect comparison of the present invention and other advanced methods is shown in the figure below. Figure 5-9As shown. The heat map shows the position of the target in the tracker's field of view during the entire tracking process. Compared with previous methods, this method can better keep the target in the center of the field of view and maintain a reasonable tracking distance. Real-time performance is another major feature of the present invention. Table 1 statistics the average time required for the method of the present invention to perform a path search and a trajectory optimization in a one-minute tracking task.

[0090] Table 1 Average calculation time for one planning

[0091]

[0092] Table 1 shows that, thanks to the efficient calculation method, the present invention can achieve efficient optimization in complex scenarios with extremely low calculation overhead.

[0093] In summary, no matter the target moves quickly or the environment changes suddenly, the present invention can respond quickly and regenerate the optimal trajectory to ensure the continuity of tracking.

Claims

1. A target tracking method without updating the Euclidean signed distance field, characterized in that: Here are the steps: Step 1: Pre-construct the Euclidean signed distance field that does not need to be updated; Step 2: Detect, segment and calculate the target position; Use the YOLOv8 model to perform instance segmentation on the target, then solve the relative position of the target and the tracker and store it in a queue; Step 3: Predict the future location of the target; Based on the target position data of the nearest several frames, the target's motion trajectory is fitted using a quadratic smoothing function, and the possible position of the target in multiple future time steps is obtained according to the fitted motion trajectory; Step 4: Calculate the visible area of ​​the target and find the optimal sequence of observation points without occlusion; Step 5: Optimize the observation point sequence to obtain a smooth tracking trajectory.

2. The target tracking method without updating the Euclidean signed distance field according to claim 1, characterized in that: The specific implementation process of step 1 is as follows: Measure the field of view of the tracker camera and define the optimal observation distance from the tracker camera to the target , if the horizontal field of view of the tracker camera is , vertical field of view is , then the field of view of the tracker camera is modeled by a tetrahedron, and the formula is: in, is the coordinate in the tracker coordinate system, represents the transpose operator, The optimal observation distance is predefined Sure; For the field of view For a point within the field of view, the FOV-ESDF value is defined as the distance from the point to the nearest field of view boundary plane; for a point within the field of view For points outside the FOV, the FOV-ESDF value is defined as .

3. The target tracking method without updating the Euclidean signed distance field according to claim 1, characterized in that: The specific implementation process of step 4 is as follows: For the target Observation location , the observation area at this location is defined as: in, ; represents the third-order identity matrix List, Take 1, 2, 3; represents the tracker position vector that satisfies the above conditions; Indicates the maximum observation distance error allowed in the vertical direction. Indicates the maximum observation distance error allowed in the horizontal direction; Considering the occlusion of the target by obstacles, the occlusion area is defined as: in, represents a local grid map, If and only if There are no obstacles; represents the integral variable; The calculation formula of the visible area of ​​the target is: For the The tracker generates the candidate position of the tracker at the next moment. The formula is: Finally, Performing a breadth-first search for the source point will yield the position of the tracker at the next moment. The position and orientation of the tracker are parameterized in time using a third-order B-spline, as follows: in, Indicates the tracker is at position The corresponding yaw angle is Indicates Position control points, Indicates Angle control points; The obstacles are removed from the field of view through occlusion penalty, and the objective function is: in, Indicates the value of FOV-ESDF. Indicates the number of obstacle points entering the FOV-ESDF, Calculated by the following formula: in, It is The world coordinates of the obstacle points, Represents the rotation matrix; the occlusion penalty produces a gradient for the position and a gradient for the yaw angle, calculated as follows: in, represents the Nabla operator; By observing the occlusion penalty, the tracker maintains an appropriate distance from the target, and the objective function is: in, is the maximum value of FOV-ESDF; The gradient of the occlusion penalty function is: The angle penalty is used to promote the convergence of the observation occlusion penalty. The objective function is: The gradient of the penalty function is: in, is the inverse tangent function with quadrant discrimination; For obstacle avoidance, the robot-centered ESDF is used as RC-ESDF, and the objective function is: in, is the number of obstacles entering the RC-ESDF, is the evaluation value of RC-ESDF; The kinematic feasibility of the trajectory is fully guaranteed by constraining the velocity and acceleration of the B-spline control points. The objective function is: in, and Indicates the velocity control point and acceleration control point of the position B-spline curve, and Indicates the velocity control point and acceleration control point of the yaw angle B-spline curve, , , , Respectively represent the limit velocity, acceleration, angular velocity and angular acceleration of the tracker, and the function ; Thanks to the convex hull property of B-spline, smoothness is ensured by minimizing the high-order derivatives of the control points. The objective function is: Finally, the L-BFGS algorithm is used to jointly optimize these objective functions to obtain the final trajectory.

Images