Target tracking method and system based on cooperation of path planning and follow-up holder

Abstract

The application provides a target tracking method and system based on path planning and cooperation of a follow-up holder, relates to the technical field of holder control, and comprises the following steps: acquiring dynamic trajectory data of a target in real time, constructing a three-dimensional motion trajectory graph for path tracking, and obtaining a target tracking path; setting expected pose parameters of a follow-up holder, synchronizing the target tracking path to the follow-up holder for dynamic updating, and obtaining a parameter coupling degree; generating a cooperative control instruction set based on the parameter coupling degree, driving the follow-up holder to perform bidirectional control, and tracking and controlling the target through bidirectional control instructions. The application solves the technical problem that the target is lost or the holder cannot stably aim at the target due to fast target motion speed and complex environment, optimizes the cooperation of path planning and holder control by calculating the parameter coupling degree, and improves the accuracy of target tracking in a complex scene.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of gimbal control, and in particular to a target tracking method and system based on path planning and coordinated following gimbal. BACKGROUND

[0002] A following gimbal is a gimbal that automatically adjusts its orientation and angle according to external changes or control signals, used to support and control a video camera. Currently, target tracking usually adopts a visual-based tracking method, and combines sensors such as laser radar and ultrasonic waves to perform data fusion, to enhance the perception ability of the environment. However, these methods still have many limitations when facing complex factors such as dynamic obstacles, variable environments, path curvature, etc. When the target moves at a high speed, its position changes very quickly, causing the existing target tracking system to fail to update the target position in time, thereby losing the target. Target recognition and tracking are difficult in complex environments (such as dynamic obstacles, changes in light, occlusions, etc.), especially when obstacles quickly occlude the target or the target quickly changes direction, the existing tracking algorithm is difficult to adjust quickly, resulting in a failure to real-time align the target, thereby causing tracking interruption or precision degradation.

[0003] In summary, the prior art has the technical problem that due to the fast movement of the target and the complex environment, the target is lost or the gimbal cannot stably align the target, thereby affecting the precision of target tracking. SUMMARY

[0004] The purpose of the present application is to provide a target tracking method and system based on path planning and coordinated following gimbal, to solve the technical problem in the prior art that due to the fast movement of the target and the complex environment, the target is lost or the gimbal cannot stably align the target, thereby affecting the precision of target tracking.

[0005] In view of the above problems, the present application provides a target tracking method and system based on path planning and coordinated following gimbal.

[0006] In a first aspect, the present application provides a target tracking method based on path planning and coordinated following gimbal, which is realized by a target tracking system based on path planning and coordinated following gimbal, wherein the target tracking method based on path planning and coordinated following gimbal comprises: acquiring dynamic trajectory data of a target in real time, constructing a three-dimensional motion trajectory graph for path tracking, and obtaining a target tracking path; setting expected pose parameters of a following gimbal, synchronizing the target tracking path to the following gimbal for dynamic updating according to the expected pose parameters, and obtaining a parameter coupling degree; generating a coordinated control instruction set based on the parameter coupling degree, driving the following gimbal for bidirectional control according to the coordinated control instruction set, and tracking and controlling the target through bidirectional control instructions.

[0007] Optionally, the target is collected in real time by a multi-modal sensor group to obtain a multi-modal data set, the multi-modal data set including lidar point cloud data, target visual depth data, and angular velocity data; the lidar point cloud data, the target visual depth data, and the angular velocity data are spatio-temporally aligned to determine a three-dimensional trajectory point sequence; a target motion state is predicted according to the three-dimensional trajectory point sequence to obtain a target predicted trajectory sequence, trajectory conflict detection is performed based on the target predicted trajectory sequence to determine a plurality of predicted trajectory data; motion reverse deduction is performed according to the plurality of predicted trajectory data, and identification is performed based on a deduction result to construct the three-dimensional motion trajectory graph.

[0008] Optionally, a dynamic obstacle prediction factor is introduced to the three-dimensional motion trajectory graph to construct a four-dimensional search space; a gimbal field of view constraint is extracted from a follow-up gimbal, and a mobile feasibility score is generated by performing mobile feasibility evaluation on the four-dimensional search space according to the gimbal field of view constraint; a plurality of executable paths are determined based on the three-dimensional motion trajectory graph identification based on the mobile feasibility score; and the target tracking path is determined by simulating and screening the plurality of executable paths.

[0009] Optionally, a mobile platform coordinate system of the target is constructed based on the three-dimensional motion trajectory graph, and a gimbal base coordinate system of the follow-up gimbal is constructed based on the four-dimensional search space; a conversion relationship matrix is constructed by performing space conversion on the mobile platform coordinate system and the gimbal base coordinate system; a plurality of path points are extracted by traversing the target tracking path, the plurality of path points are converted according to the conversion relationship matrix, and initial pose parameters of the follow-up gimbal are set; a mechanical limit constraint of the follow-up gimbal is extracted to construct a feasible pose parameter space of the follow-up gimbal, the initial pose parameters are synchronized to the feasible pose parameter space, and the desired pose parameters are determined.

[0010] Optionally, a pose analysis of the target is performed based on the mobile platform coordinate system to construct a homogeneous transformation matrix; an installation offset analysis is performed according to the gimbal base coordinate system to construct a gimbal kinematic chain; a coordinate system rotation transformation compensation is performed based on the path point velocity vector introduced by the target tracking path to construct the conversion relationship matrix.

[0011] Optionally, the target tracking path is synchronized to the follow-up gimbal according to the desired pose parameters to determine a three-dimensional waypoint sequence of the target tracking path, the three-dimensional waypoint sequence including path curvature characteristics and timestamp information; a decoupling evaluation is performed based on the path curvature characteristics according to the timestamp information to set a parameter coupling degree index; a parameter coupling calculation is performed according to the parameter coupling degree index to obtain the parameter coupling degree.

[0012] Optionally, the dynamic coupling analysis is performed based on the parameter coupling degree to determine a dynamic coupling degree index, and a coupling state feature vector is extracted according to the dynamic coupling degree index; weighted fusion is performed based on the coupling state feature vector to generate a cooperative control instruction set; and a composite control parameter is generated through bidirectional data interaction feedback of the follow-up gimbal according to the cooperative control instruction set, and dynamic gain is performed according to the composite control parameter to construct the bidirectional control instruction for tracking control of the target.

[0013] Optionally, weighted fusion is performed based on the coupling state feature vector to generate a path correction amount and a position adjustment amount; dynamic priority allocation is performed according to the path correction amount and the position adjustment amount to determine a first execution weight and a second execution weight; control analysis is performed according to the first execution weight in combination with the path correction amount to generate a mobile platform path correction instruction; control analysis is performed according to the second execution weight in combination with the position adjustment amount to generate a gimbal pose adjustment instruction; and the mobile platform path correction instruction and the gimbal pose adjustment instruction are integrated to generate a cooperative control instruction set.

[0014] Optionally, the cooperative control instruction set is analyzed in real time to separate the mobile platform path correction instruction and the gimbal pose adjustment instruction; gimbal state data is obtained by adjusting the follow-up gimbal based on the gimbal pose adjustment instruction; a double-channel control architecture is constructed, which includes a forward channel and a reverse channel; the mobile platform path correction instruction is transmitted through the forward channel, and the gimbal state data is fed back through the reverse channel to generate a composite control parameter; forward coupling analysis is performed according to the composite control parameter to generate a feedforward gain matrix; reverse coupling analysis is performed according to the composite control parameter to generate a feedback compensation vector; and control loop gain is performed based on the feedforward gain matrix and the feedback compensation vector to construct the bidirectional control instruction.

[0015] In a second aspect, the application also provides a target tracking system based on path planning and follow-up gimbal cooperation, which is used to execute the target tracking method based on path planning and follow-up gimbal cooperation as described in the first aspect. The target tracking system based on path planning and follow-up gimbal cooperation includes: a path tracking module, which is used to obtain dynamic trajectory data of a target in real time, construct a three-dimensional motion trajectory graph for path tracking, and obtain a target tracking path; a dynamic updating module, which is used to set expected pose parameters of a follow-up gimbal, synchronize the target tracking path to the follow-up gimbal according to the expected pose parameters for dynamic updating, and obtain a parameter coupling degree; and a bidirectional control module, which is used to generate a cooperative control instruction set based on the parameter coupling degree, drive the follow-up gimbal for bidirectional control according to the cooperative control instruction set, and perform tracking control of the target through bidirectional control instructions.

[0016] The one or more technical solutions provided in the application have at least the following beneficial effects:

[0017] By acquiring dynamic trajectory data of the target in real time, a three-dimensional motion trajectory graph is constructed for path tracking to obtain a target tracking path; expected pose parameters of a follow-up holder are set, the target tracking path is synchronized to the follow-up holder according to the expected pose parameters for dynamic updating to obtain a parameter coupling degree; a cooperative control instruction set is generated based on the parameter coupling degree, the follow-up holder is driven according to the cooperative control instruction set for bidirectional control, and the target is tracked and controlled through bidirectional control instructions. That is, by acquiring dynamic trajectory data of the target in real time, a three-dimensional motion trajectory graph is constructed, high-speed motion of the target is quickly responded, a motion mode of the target in a complex environment is determined, expected pose parameters of the holder are set and dynamically updated to maintain stable alignment to the target, path planning is performed based on the parameter coupling degree to improve the accuracy and robustness of target tracking in a complex scene, tracking error is reduced through closed-loop control, and system energy consumption is balanced.

[0018] The above description is only a summary of the technical solutions of the application. In order to more clearly understand the technical means of the application, the application can be implemented according to the content of the specification, and in order to make the above and other purposes, characteristics and advantages of the application more obvious and easy to understand, the following specific embodiments of the application are described. It should be understood that the content described in this section is not intended to identify key or important features of the embodiments of the application, nor is it intended to limit the scope of the application. Other features of the application will become apparent from the following description. BRIEF DESCRIPTION OF DRAWINGS

[0019] In order to more clearly illustrate the technical solutions in the application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or the prior art description. Obviously, the drawings in the following description are only exemplary, and other drawings can be obtained by those skilled in the art without creative labor on the basis of the provided drawings.

[0020] Figure 1 The flowchart of the target tracking method based on path planning and follow-up holder cooperation of the application.

[0021] Figure 2 The structural schematic diagram of the target tracking system based on path planning and follow-up holder cooperation of the application.

[0022] Explanation of reference signs: path tracking module 11, dynamic updating module 12, bidirectional control module 13. DETAILED DESCRIPTION

[0023] The application provides a target tracking method and system based on path planning and cooperative tracking of a gimbal, and solves the technical problem in the prior art that target loss or the gimbal cannot stably aim at the target due to fast target motion and complex environment, thereby affecting the accuracy of target tracking. By acquiring dynamic trajectory data of the target in real time, a three-dimensional motion trajectory graph is constructed, fast response to high-speed motion of the target is achieved, the motion mode of the target in a complex environment is determined, the expected pose parameters of the gimbal are set and dynamically updated, stable aiming at the target is maintained, path planning is performed based on the parameter coupling degree, and the accuracy and robustness of target tracking in a complex scene are improved. The tracking error is reduced through closed-loop control, and the system energy consumption is balanced.

[0024] Hereinafter, the technical solutions in the application will be described clearly and completely with reference to the drawings. Obviously, the described embodiments are only a part of the embodiments of the application, rather than all the embodiments of the application. It should be understood that the application is not limited by the example embodiments described herein. Based on the embodiments of the application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the scope of protection of the application. In addition, it should be noted that, for the convenience of description, only parts related to the application are shown in the drawings, rather than all.

[0025] Embodiment one, please refer to the accompanying Figure 1 The application provides a target tracking method based on path planning and cooperative tracking of a gimbal, wherein the target tracking method based on path planning and cooperative tracking of the gimbal is executed by a target tracking system based on path planning and cooperative tracking of the gimbal, and the target tracking method based on path planning and cooperative tracking of the gimbal specifically includes the following steps:

[0026] S100: Real-time acquisition of dynamic trajectory data of a target, construction of a three-dimensional motion trajectory graph for path tracking, and acquisition of a target tracking path.

[0027] Further, the S100 of the application includes:

[0028] The target is collected in real time by a multi-modal sensor group, a multi-modal data set is obtained, the multi-modal data set includes laser radar point cloud data, target visual depth data and angular velocity data, the laser radar point cloud data, the target visual depth data and the angular velocity data are time-space aligned, a three-dimensional trajectory point sequence is determined, the target motion state is predicted according to the three-dimensional trajectory point sequence, a target predicted trajectory sequence is obtained, trajectory conflict detection is performed based on the target predicted trajectory sequence, a plurality of predicted trajectory data are determined, motion is deduced in reverse according to the plurality of predicted trajectory data, the deduced result is marked, and the three-dimensional motion trajectory graph is constructed.

[0029] Specifically, the multi-modal sensor group refers to a set of sensors integrated with multiple different types of sensors for acquiring information in multiple dimensions. Different sensors have different data acquisition capabilities and can provide complementary information. The multi-modal sensor group at least includes a laser radar, a binocular vision sensor, and an inertial measurement unit. Laser radar point cloud data from the laser radar, target visual depth data from the binocular vision sensor, and angular velocity data provided by the inertial measurement unit are collected. The laser radar scans the surrounding environment through laser beams to obtain laser radar point cloud data and three-dimensional point cloud data of the object. The target visual depth data is obtained by calculating the parallax between two cameras to obtain the depth information of the target. The inertial measurement unit is a sensor for measuring dynamic information such as object acceleration and angular velocity, and provides angular velocity data of the target.

[0030] The data collected by each sensor is in different time and space dimensions, so it needs to be spatio-temporally aligned. Spatio-temporal alignment refers to synchronously processing and fusing data from different sensors in time and space dimensions, so that these data can correspond to the same spatial location at the same time point, thereby enabling accurate analysis combining data from each sensor. Based on the timestamp of each sensor, it is ensured that the samples collected by different data sources are synchronized at the same time. In terms of spatial alignment, coordinate transformation is usually performed according to the relative position and orientation of the sensors. For example, the coordinate system of the laser radar is converted to the coordinate system consistent with the vision sensor, or the coordinate system is adjusted according to the angular velocity information provided by the inertial measurement unit, to ensure that all data can be combined in the same three-dimensional spatial coordinate system.

[0031] The three-dimensional trajectory point sequence is a path sequence composed of multiple three-dimensional points, which describes the motion trajectory of the target in three-dimensional space over time. Each three-dimensional point represents the position of the target at a certain time (represented by its X, Y, Z coordinates). After spatio-temporal alignment, the data of multiple sensors is integrated into a continuous trajectory sequence, showing the moving path of the target.

[0032] According to the sequence of three-dimensional trajectory points, the target motion state is predicted, that is, the possible future motion trajectory of the target is predicted, forming a target prediction trajectory sequence. According to the obtained sequence of three-dimensional trajectory points, the motion state of the target at each time point is calculated, and the speed, acceleration and motion direction of the target are estimated by analyzing the position change. Using the known target motion state (such as position, speed, acceleration, etc.), combined with the conventional kinematic model, the positions of the target at multiple future time points are predicted, and the prediction results are generated as a new sequence of trajectory points, i.e. a target prediction trajectory sequence. For example, assuming that the position of the target at the current time is (5.3, 2.1, 7.4), the speed is (0.2, 0.1, 0.1) m / s, and the current acceleration is (0.1, 0.05, 0.05) m / s². Through the prediction model, assuming that the target moves at a constant speed, the position that the target may reach at the next time step (t+1) is calculated as (5.5, 2.2, 7.5), and the speed at the next time step is (0.22, 0.11, 0.11) m / s. According to this prediction, the trajectory at a further time step is continuously calculated, and finally a predicted trajectory point sequence is obtained, such as: the position at T1 time (5.3, 2.1, 7.4), the position at T2 time (5.5, 2.2, 7.5), the position at T3 time (5.7, 2.3, 7.6), the position at T4 time (5.9, 2.4, 7.8), thereby outputting a target prediction trajectory sequence; assuming that the target accelerates, an acceleration trajectory is generated, and other acceleration trajectories are generated; assuming that the target suddenly turns, other turning trajectories are generated, and so on.

[0033] According to the target prediction trajectory sequence, trajectory conflict detection is performed, each prediction trajectory is compared with other trajectories, and it is checked whether spatial overlap or intersection occurs, and trajectories that may collide, intersect or overlap are identified and adjusted to avoid conflict impact. It is checked whether the spatial positions of different trajectories at the same time or different time points coincide, and if they coincide, it means that the two trajectories have conflicts and need to be avoided. It is analyzed whether multiple trajectories intersect or cross within the same time period, and whether there is a potential collision between targets. There may be a speed difference between targets of different trajectories, and when the motion speeds of targets differ greatly, the time and position of their meeting will change, which also needs to be considered. When performing conflict detection, not only the overlap of a single trajectory with other trajectories is compared, but also the overall analysis of multiple trajectory data is performed to identify all trajectories that may have conflicts. For example, if there are multiple targets or multiple prediction trajectories, all trajectory data that may have overlap or intersection are found and marked.

[0034] Once a trajectory conflict is detected, the conflicting trajectories are marked, the predicted trajectories are adjusted to change the moving direction or speed of the targets, or the path planning of the targets is corrected to avoid the intersection or overlapping area; in extreme cases, some targets are temporarily stopped or their speed is adjusted to avoid conflicts with other targets. After trajectory conflict detection, multiple predicted trajectory data are obtained. According to the multiple predicted trajectory data, motion backtracking is performed, that is, the past motion state of the target is calculated from the known predicted trajectory data, the historical trajectory of the target is understood, and the motion law of the target is better understood. Through backtracking, the motion trajectory of the target at a past time can be inferred from the future state of the target, and backtracking helps to understand the motion law of the target and predict changes in future trajectories.

[0035] After backtracking, different trajectories are identified according to the backtracking results of the historical or future trajectories of the target. By identifying these trajectories, different types of trajectories are distinguished. Based on the backtracking results and the identification, the motion trajectory of the target is drawn into a three-dimensional graph. The three-dimensional motion trajectory graph is usually composed of position points of the target on the X, Y and Z axes, and the motion path of the target is updated in real time as time progresses. The three-dimensional motion trajectory graph not only shows the motion trajectory of the target, but also clearly shows the relative position, motion speed and potential trajectory conflict area between the targets. Through this visual graph, the operator or the automatic system can intuitively understand the motion of the target.

[0036] By predicting multiple trajectories and detecting conflicts, potential collision risks are identified in advance, and corresponding control decisions are made. Combined with motion backtracking and three-dimensional trajectory graph construction, the motion behavior of the target is accurately analyzed, and the path planning is optimized to ensure stable tracking of the target.

[0037] Further, the present application further comprises the following steps:

[0038] A dynamic obstacle prediction factor is introduced to the three-dimensional motion trajectory graph to construct a four-dimensional search space; a gimbal field of view constraint is extracted according to a follow-up gimbal, and a mobile feasibility score is generated by performing a mobile feasibility evaluation on the four-dimensional search space according to the gimbal field of view constraint; a plurality of executable paths are determined based on the three-dimensional motion trajectory graph identification according to the mobile feasibility score; and the target tracking path is determined by simulating and screening the plurality of executable paths.

[0039] Specifically, in the target motion process, the presence of dynamic obstacles will affect path planning and target tracking. According to the position, speed, acceleration and other information of dynamic obstacles (such as other vehicles, pedestrians, etc.) in the target surrounding environment, the future dynamic behavior and influence of the obstacles are calculated and predicted, and a dynamic obstacle prediction factor is generated. The dynamic obstacle prediction factor is introduced into the constructed three-dimensional motion trajectory graph to construct a four-dimensional search space. The four-dimensional search space refers to the space formed by adding the time dimension to the three-dimensional space. By incorporating time into spatial planning, the four-dimensional search space can accurately reflect the changes of the target and its surrounding environment in a dynamic environment, improving the accuracy of path planning.

[0040] The gimbal has a certain field of view range and motion ability, so it is necessary to evaluate whether the target motion trajectory is within the field of view range that the gimbal can track according to the actual adjustable angle and range of the gimbal. The gimbal field of view constraint refers to the spatial region limited by the field of view range of the gimbal and the motion ability (such as rotation angle and speed) of the gimbal, which considers the rotation speed, rotation angle of the gimbal, and the capture ability of the target, to evaluate whether the target path meets these conditions.

[0041] According to the gimbal field of view constraint, the movement feasibility of each point in the four-dimensional search space is evaluated to generate a movement feasibility score. According to the movement feasibility score, the three-dimensional motion trajectory graph is labeled to determine multiple executable paths. According to the movement feasibility score, the score of each path is calculated, and the higher the score, the stronger the feasibility of the path. Multiple paths with higher scores are selected as executable paths for further screening.

[0042] All executable paths are traversed and further verified through simulation. Through simulation software, according to the dynamic trajectory data of the target, gimbal parameters (such as the rotation angle range, maximum rotation speed, energy consumption characteristics of the gimbal, etc.), environmental information (such as dynamic / static obstacles), etc., the simulation environment is set to ensure that the virtual environment can truly reflect the dynamic obstacles, gimbal response and target tracking process in complex scenarios. When simulating, some key evaluation criteria need to be defined, including path length, gimbal energy consumption, etc. Multiple executable paths are simulated, and each path is evaluated according to the key evaluation criteria, considering multiple factors such as time, energy consumption and accuracy to find the optimal balance point and get the score. Based on the simulation results, if a certain path cannot achieve a balance in energy consumption and time, try to optimize some parameters of the path (such as changing the rotation speed of the gimbal, adjusting the target prediction algorithm, etc.) until the optimal solution is found. After evaluating multiple paths, the optimal solution that meets the shortest path and balanced gimbal energy consumption is selected as the target tracking path.

[0043] By introducing a dynamic obstacle predictor, the behavior of the obstacle is accurately predicted to avoid potential conflicts; by introducing a gimbal field of view constraint, the target is prevented from being tracked due to exceeding the field of view range, thereby improving the stability and accuracy of target tracking; by screening and simulating the executable path, the optimal path is selected to ensure that the target can be stably tracked in a complex environment while avoiding obstacles and other interference.

[0044] S200: Set the expected pose parameters of the follow-up gimbal, synchronize the target tracking path to the follow-up gimbal for dynamic updating according to the expected pose parameters, and obtain the parameter coupling degree.

[0045] Further, the S200 of the present application comprises:

[0046] Based on the three-dimensional motion trajectory diagram, a mobile platform coordinate system of the target is constructed, and based on the four-dimensional search space, a gimbal base coordinate system of the follow-up gimbal is constructed; the mobile platform coordinate system and the gimbal base coordinate system are space-converted to construct a conversion relationship matrix; a plurality of path points are extracted by traversing the target tracking path, the plurality of path points are converted according to the conversion relationship matrix, and initial pose parameters of the follow-up gimbal are set; mechanical limit constraints of the follow-up gimbal are extracted to construct a feasible pose parameter space of the follow-up gimbal, the initial pose parameters are synchronized to the feasible pose parameter space, and the expected pose parameters are determined.

[0047] Further, the present application further comprises the following steps:

[0048] Based on the mobile platform coordinate system, the target is analyzed for attitude, a homogeneous transformation matrix is constructed; according to the gimbal base coordinate system, installation offset analysis is performed, a gimbal kinematics chain is constructed; based on the target tracking path, a path point velocity vector is introduced for coordinate system rotation transformation compensation, and the conversion relationship matrix is constructed.

[0049] Specifically, according to the three-dimensional motion trajectory diagram, a mobile platform coordinate system of the target is constructed, which is usually set at a fixed point of the mobile platform, and the coordinate axis is consistent with the motion direction of the platform. The mobile platform coordinate system is a coordinate system constructed based on the target motion path, the origin of which is usually located at a fixed point of the mobile platform, and the direction of the coordinate system is consistent with the motion direction of the platform, which is used to represent the spatial position and motion state of the target. Based on the four-dimensional search space, the motion range, observation angle and time dimension changes of the gimbal are considered, and a gimbal base coordinate system of the follow-up gimbal is constructed. The gimbal base coordinate system is a coordinate system closely related to the field of view and mechanical structure of the gimbal, and the origin is usually set as the center of the gimbal, and the coordinate axis points to the optical axis of the gimbal.

[0050] The target's mobile platform coordinate system is spatially converted with the gimbal's gimbal base coordinate system to construct a conversion relationship matrix, which is used to convert coordinates in the target coordinate system to coordinates in the gimbal coordinate system. The gimbal can adjust its line of sight to align according to the target's position in the platform coordinate system. Specifically, the target is analyzed in terms of its pose according to the mobile platform coordinate system. By analyzing the target's motion state in the mobile platform coordinate system, the target's pose (i.e., orientation) is represented using quaternion. The target's rotation and position changes can be expressed through a homogeneous transformation matrix, which includes the target's rotation and position changes. Quaternions can avoid the singular phenomenon that may occur when using traditional Euler angles and rotation matrices for continuous rotation. For example, if the target rotates from one coordinate system to another, the angle of rotation is represented using a quaternion, while the target's translational changes are added to the homogeneous transformation matrix in the form of a translation vector. Target pose analysis refers to analyzing the target's direction (or orientation) relative to a reference coordinate system through mathematical methods. Pose is usually represented by rotation angles or quaternions, which can help determine the target's direction in space. Quaternions are a mathematical representation method used to describe rotation in space, which can avoid the gimbal lock problem and has better computational efficiency compared to Euler angles and rotation matrices. Homogeneous transformation matrices are used to transform points from one coordinate system to another, and can represent both rotation and translation, allowing translation and rotation to be handled simultaneously through a single matrix.

[0051] According to the installation offset analysis of the gimbal base coordinate system, there may be a certain installation offset of the gimbal base coordinate system relative to the mobile platform coordinate system, so the offset needs to be analyzed, and the kinematic chain of the gimbal is constructed. The kinematic chain of the gimbal describes the spatial relationship from the base coordinate system to the end of the gimbal (such as a camera, a sensor, etc.), and understands how the gimbal moves from the center of the base coordinate system to the end of its actuator. For example, assuming that the gimbal starts from a fixed coordinate system and is installed on a mobile platform, the kinematic chain describes how the angles of each joint affect the motion of the end effector according to the motion of each joint of the gimbal. The kinematic chain is usually composed of multiple matrix products, each corresponding to a motion component of the gimbal, and through the chain rule, the spatial position of the gimbal end can be calculated from the gimbal base coordinate system.

[0052] In the target tracking path, considering the field of view offset error caused by the motion of the platform itself, the target path point is rotated and transformed to compensate. By introducing the velocity vector of the path point, the transformation matrix is updated in real time to correct the error caused by the motion of the platform, ensuring that the field of view of the gimbal always remains in the correct target position. The velocity vector of the path point refers to the direction and size of the velocity of each path point when the target moves along the path during target tracking, which is used to describe the instantaneous velocity of the target at a certain time. The coordinate system rotation transformation compensation is to eliminate the field of view offset error caused by the motion of the platform (such as the rotation or displacement of the platform), and to correct the error by updating the rotation matrix or transformation matrix in real time to ensure accurate tracking of the field of view.

[0053] According to the path point velocity vector and the motion state of the target, the transformation relationship matrix is calculated and updated in real time, which is used to convert the path point in the mobile platform coordinate system to the gimbal base coordinate system. Through this matrix, the gimbal can effectively adjust its own attitude to eliminate the field of view offset error caused by the motion of the platform. The transformation matrix parameters are updated in real time to ensure that the timeliness error of path point conversion is less than 1 millisecond.

[0054] Traverse the target tracking path and extract multiple path points from it, which represent the positions of the target at different time points and constitute the moving trajectory of the target. When extracting path points, you can discretize them according to the time step of the target motion to ensure that there is a path point at each time. Assuming that the target moves along a curve in three-dimensional space, the path point can be the position of the target at each time scale, for example, when t=0, the target position is (1,2,3), when t=1, the target position is (1.5,2.5,3.5), and so on.

[0055] The extracted path points are converted from the target coordinate system to the gimbal coordinate system through the transformation relationship matrix, and the conversion process includes rotation and displacement operations to ensure that the gimbal can accurately adjust its position and attitude according to the position of the target. According to the conversion result of the path point, set the initial pose parameters of the gimbal, including the initial position (displacement vector) and orientation (rotation matrix or quaternion) of the gimbal, which are the reference values of the follow-up gimbal and determine the attitude of the gimbal at the initial time. The initial pose parameters of the follow-up gimbal refer to the position and orientation of the gimbal at the initial time, which are usually represented by a rotation matrix or a quaternion to represent its attitude, and the displacement part is represented by a translation vector, which is a set of expected pose parameters of the center of the gimbal's visual axis.

[0056] The physical structure of the gimbal is analyzed, and the mechanical limit constraints of the gimbal motion are extracted, including the rotation angle, the pitch angle, the left-right offset and other hardware limitations, which determine the motion range of the gimbal. According to the mechanical limit constraints of the gimbal, the feasible pose parameter space of the gimbal is constructed, and the set of all legal poses that the gimbal can execute is defined. For example, the feasible pose parameter space of the gimbal is the set of rotation angles between [-90°, +90°] and pitch angles between [-45°, +45°].

[0057] The initial pose parameters are synchronized to the feasible pose parameter space of the gimbal, ensuring that the initial position and orientation of the gimbal are within its feasible range. If the initial parameters are not within the feasible range, they need to be corrected to meet the motion limitations of the gimbal. According to the extracted path points and the feasible pose parameter space, the desired pose parameters of the gimbal are determined, which are used to guide the real-time adjustment of the follow-up gimbal according to the target tracking path. The desired pose parameters refer to the final position and attitude that the gimbal should reach according to factors such as the target tracking path and the transformation matrix, representing the ideal position and direction that the gimbal should reach at a certain time.

[0058] By combining the homogeneous transformation matrix and the quaternion method, the motion state of the target can be accurately represented, and the errors caused by platform motion can be eliminated, ensuring that the gimbal can always accurately track the target, even when the platform moves significantly. By constructing the kinematic chain of the gimbal, the motion of each joint is ensured to accurately control the end of the gimbal, so that the gimbal can more flexibly adjust its position and attitude to cope with complex target tracking tasks. By converting the path points to the gimbal coordinate system and setting the initial pose parameters, the gimbal can accurately track the target, and the mechanical limit constraints of the gimbal ensure that the gimbal motion does not exceed the hardware capability range, avoiding damage or tracking failure due to hardware overload. According to the real-time position of the target and the path points, the pose parameters of the gimbal are dynamically adjusted to ensure that the gimbal can flexibly adapt to changes in the target and achieve more accurate target tracking.

[0059] Further, the present application further comprises the following steps:

[0060] The target tracking path is synchronized to the follow-up gimbal according to the desired pose parameters, and the three-dimensional waypoint sequence of the target tracking path is determined, which contains path curvature characteristics and timestamp information. Based on the path curvature characteristics, the decoupling evaluation is performed according to the timestamp information, and the parameter coupling degree index is set. According to the parameter coupling degree index, the parameter coupling calculation is performed to obtain the parameter coupling degree.

[0061] Specifically, according to the desired pose parameters, the target tracking path is synchronized to the pan-tilt head for analysis. Each key point (i.e., waypoint) of the target path corresponds to a specific motion instruction of the pan-tilt head. The pan-tilt head calculates the adjustment parameters of the pan-tilt head according to the three-dimensional motion trajectory and the desired position of the target. The three-dimensional waypoint sequence is a collection of all key position points in the three-dimensional space in the target tracking path. Each waypoint contains not only position coordinates but also timestamp information and motion characteristics (such as speed, acceleration, etc.).

[0062] According to the multiple waypoints on the path, the curvature characteristics of the path are calculated, and each waypoint is labeled in combination with the timestamp information. The curvature is defined by the rate of change between path points, and the timestamp gives each waypoint a time attribute, describing the motion state of the target at different points on the path. The path curvature characteristic refers to the degree of bending of the target path. Curvature is a physical quantity that describes the speed of change of the path curve, usually represented by the angle change between path points. Higher curvature means a more curved path, and lower curvature indicates a more linear path. The timestamp information is a time marker associated with each path point or waypoint, indicating the time position of the path point in the target motion process.

[0063] According to the curvature characteristics and timestamp information of the path, a decoupling evaluation is performed to evaluate the synchronization degree between the target tracking path and the pan-tilt head. If the target's motion changes greatly (such as a sharp change in curvature or a rapid change in speed), the pan-tilt head tracking will be inaccurate or lagging. By setting the parameter coupling degree index, the response ability of the pan-tilt head is evaluated to see if it can keep up with the changes of the target. The parameter coupling degree index is used to quantify the relationship between the path characteristics (such as path curvature) and the response of the pan-tilt head control system, reflecting the coordination between the target path and the pan-tilt head control system. The higher the coupling degree, the more synchronized and the better the target tracking performance.

[0064] According to the set parameter coupling degree index, parameter coupling calculation is performed to evaluate the matching degree between the pan-tilt head and the target path based on the path curvature, timestamp information of the target, and response characteristics of the pan-tilt head. This indicates the optimization direction when controlling the pan-tilt head to handle the path. If the curvature of the target path is large and the interval between timestamps is short, the pan-tilt head may need to make a large angle adjustment in a short time. Parameter coupling calculation will adjust the pan-tilt head control strategy based on this information to ensure that the pan-tilt head can track the target in a timely and accurate manner. Parameter coupling calculation is based on the characteristics of the path (such as curvature, timestamp, etc.) and the response ability of the pan-tilt head control (such as rotation angle, speed, etc.) to evaluate the coordination between the pan-tilt head and the target path, thereby determining the priority and optimization strategy of the pan-tilt head control.

[0065] By analyzing the three-dimensional waypoint sequence of the target tracking path, combined with the path curvature characteristics and timestamp information, the tracking accuracy of the gimbal on the target can be improved; the uncoupling evaluation and parameter coupling degree calculation can effectively evaluate the adaptability of the gimbal to the target path, especially for complex paths or dynamic environments, the coupling degree index can be adjusted to optimize the control strategy of the gimbal and improve the stability of target tracking.

[0066] S300: generating a cooperative control instruction set based on the parameter coupling degree, driving the follow-up gimbal to perform bidirectional control according to the cooperative control instruction set, and tracking the target through the bidirectional control instruction.

[0067] Further, the S300 of the present application comprises:

[0068] Based on the parameter coupling degree, dynamic coupling analysis is performed to determine a dynamic coupling degree index, and a coupling state feature vector is extracted according to the dynamic coupling degree index; based on the coupling state feature vector, weighted fusion is performed to generate a cooperative control instruction set; the follow-up gimbal is fed back through bidirectional data interaction according to the cooperative control instruction set, a composite control parameter is generated, dynamic gain is performed according to the composite control parameter, and the bidirectional control instruction is constructed to track the target.

[0069] Specifically, according to the parameter coupling degree, dynamic coupling analysis is performed to analyze how the mutual dependence and influence degree between the pose parameters of the gimbal change due to the dynamic changes of the target motion state during the tracking process. For example, in the gimbal control system, there may be a certain coupling relationship between the angle of the gimbal and the position of the mobile platform. By analyzing the dynamic coupling degree between them, it is determined whether the control of these two parameters is closely related, and how to adjust the control strategy to cope with the changes. Through the dynamic coupling analysis result, a dynamic coupling degree index for evaluating the coupling characteristics of the gimbal is determined, which reflects the changes and interactions of the control parameters in the dynamic process. According to the dynamic coupling degree index, a coupling state feature vector is extracted from the coupling analysis result, i.e. the coupling state of each control parameter, which contains multiple feature values for reflecting the coupling characteristics of the gimbal at the current time, including the motion characteristics of the target (such as speed, acceleration, etc.) and the response characteristics of the gimbal (such as angle change, motion speed, etc.), in order to quantify the coordination between the target and the gimbal.

[0070] According to the coupling state feature vector, weighted fusion is performed, that is, according to the coupling relationship characteristics of each control parameter, the path planning of the mobile platform and the position adjustment of the follow-up holder are weighted and distributed, the coupling weight of the path planning and the holder control is adjusted in real time to generate a cooperative instruction, including multiple control targets, and the motion and attitude adjustment of each subsystem (such as the mobile platform and the holder) are coordinated. Through bidirectional data interaction feedback, control instructions and feedback information are transmitted in real time. According to the feedback result, the control strategy is adjusted to generate a composite control parameter. Bidirectional data interaction feedback refers to the bidirectional flow of control signals and feedback signals in the control system. In the forward channel, instructions are transmitted, and in the reverse channel, feedback information is transmitted to ensure that the control strategy can be adjusted according to the feedback information. Feedback information refers to the data fed back by the holder after executing the instructions.

[0071] The composite control parameter is a comprehensive control parameter obtained by fusing multiple control parameters and feedback signals, including the output results of multiple independent control systems, and is combined to optimize the overall behavior. For example, the mobile platform (such as an unmanned vehicle) adjusts the motion path according to the holder state data, and the holder adjusts the angle according to the platform feedback, and the two are coordinated through bidirectional data interaction. According to the composite control parameter, the response of the control system is optimized through dynamic gain, and the feedforward gain and feedback compensation vector are dynamically adjusted, so as to achieve the optimal target tracking control effect. If environmental disturbance is encountered during target tracking, the dynamic gain mechanism can enhance or reduce the strength of the control signal to ensure that the control accuracy is not affected. Through dynamic coupling analysis and weighted fusion, the relationship between each control parameter is accurately coordinated, and the coordination between the mobile platform and the holder is improved, that is, under the coordination of path planning and follow-up holder, stable operation in a dynamic environment is ensured. Through weighted fusion and dynamic gain, the control strategy is adjusted according to the environmental changes to ensure that stable operation effect is maintained even in the case of disturbance or unpredictable situations.

[0072] Further, the present application further comprises the following steps:

[0073] Based on the coupling state feature vector, weighted fusion is performed to generate a path correction amount and a position adjustment amount; dynamic priority distribution is performed according to the path correction amount and the position adjustment amount to determine a first execution weight and a second execution weight; control analysis is performed according to the first execution weight and the path correction amount to generate a mobile platform path correction instruction; control analysis is performed according to the second execution weight and the position adjustment amount to generate a holder pose adjustment instruction; and the mobile platform path correction instruction and the holder pose adjustment instruction are integrated to generate a cooperative control instruction set.

[0074] Specifically, target tracking is performed autonomously by the mobile platform, based on a coupling state feature vector, which weights the motion state of the mobile platform (e.g., unmanned vehicle) and the response state of the gimbal. Multiple input information (e.g., speed, acceleration of the mobile platform, angular velocity of the gimbal, position deviation, etc.) is combined to generate path correction and position adjustment. Path correction represents the deviation between the target trajectory and the desired trajectory, while position adjustment represents the deviation between the current posture of the gimbal and the desired posture.

[0075] Based on the size of the path correction and the position adjustment, dynamic priority allocation is performed. Generally, when the path correction is larger, it means that the mobile platform deviates more from the path, and the platform needs to prioritize path correction. When the position adjustment is larger, it means that the gimbal posture has a larger deviation, and the gimbal needs to prioritize position adjustment. Through priority allocation, the most important adjustment task is ensured to be executed first. Execution weight represents the priority and importance of the control task. According to the needs of the control target, the task may have different execution weights to ensure that critical tasks can be completed first.

[0076] Based on the first execution weight and the path correction, mobile platform path correction analysis is performed to generate control instructions to adjust the speed, acceleration, and direction of the mobile platform, so that it can move along the correct path. For example, if the mobile platform deviates from the predetermined path at a certain time, the path correction is larger. According to this correction, the path of the mobile platform is adjusted, and the instruction will guide the mobile platform to move along the new path.

[0077] Based on the second execution weight and the position adjustment, gimbal position adjustment analysis is performed to generate gimbal control instructions to adjust the rotation angle, posture, or field of view direction of the gimbal, so that it can align with the target. For example, if the field of view of the gimbal deviates from the target, the position adjustment is larger. The instruction will instruct the gimbal to rotate by a certain angle to ensure that the gimbal can re-align with the target and maintain accurate tracking.

[0078] The mobile platform path correction instruction and the gimbal position adjustment instruction are integrated to generate a set of collaborative control instructions, which includes multiple control instructions that act on each part (e.g., gimbal, mobile platform, etc.) to ensure that each part works collaboratively to ensure accurate tracking of the mobile platform (e.g., unmanned vehicle). For example, if the motion path of the target changes, the path correction instruction and the gimbal position adjustment instruction will work together to ensure that the target can adjust the path and direction of travel, and the gimbal can adjust the field of view direction in real time, and together they complete the accurate tracking of the target. The set of collaborative control instructions includes multiple control instructions for simultaneously adjusting the behavior of multiple components (e.g., gimbal and target path) so that all components can work in coordination to optimize overall performance.

[0079] By the weighted fusion of path correction amount and position adjustment amount, the deviation between the target and the gimbal can be accurately evaluated and timely calibrated, the precise tracking of the target in a dynamic environment is maintained, and the path deviation and gimbal field of view error are reduced; the dynamic priority allocation mechanism ensures that the most critical task can be executed preferentially when multiple tasks are executed, the path of the mobile platform and the pose of the gimbal are quickly adjusted, and efficient and accurate tracking is ensured; the cooperative control instruction set integrates the control tasks of the mobile platform and the gimbal, ensures their cooperation, avoids conflicts or incoordination between different parts, and adjusts the gimbal and target path in real time to avoid out-of-sync or redundant control instructions.

[0080] Further, the application also includes the following steps:

[0081] The cooperative control instruction set is analyzed in real time, and the mobile platform path correction instruction and the gimbal pose adjustment instruction are separated; the gimbal is adjusted based on the gimbal pose adjustment instruction to obtain gimbal state data; a double-channel control architecture is constructed, which includes a forward channel and a reverse channel; the mobile platform path correction instruction is transmitted through the forward channel, and the gimbal state data is fed back through the reverse channel to generate a composite control parameter; forward coupling analysis is performed according to the composite control parameter to generate a feedforward gain matrix; reverse coupling analysis is performed according to the composite control parameter to generate a feedback compensation vector; and control loop gain is performed based on the feedforward gain matrix and the feedback compensation vector to construct the bidirectional control instruction.

[0082] Specifically, the cooperative control instruction set is analyzed in real time, and the mobile platform path correction instruction and the gimbal pose adjustment instruction are separated; the path correction instruction focuses on adjusting the target motion trajectory, and the gimbal pose adjustment instruction focuses on adjusting the position and attitude of the gimbal. According to the generated gimbal pose adjustment instruction, the gimbal is actually adjusted, and the state data (such as position, angle, speed, etc.) of the gimbal is recorded. For example, if the gimbal needs to adjust its angle to align with the target, the instruction will control the gimbal to rotate by a certain angle, and the state data (such as the current angle, rotation speed, etc.) of the gimbal will be recorded in real time. The gimbal pose adjustment instruction is used to adjust the position and attitude of the gimbal, and by controlling the angle, position, etc. of the gimbal, it is ensured that the gimbal can correctly align with the target.

[0083] A double-channel control architecture is constructed, including a forward channel and a reverse channel, which are used to realize the functions of control and feedback respectively. The forward channel is the channel through which the control instruction is transmitted from the controller to the actuator, and the reverse channel is the channel through which the feedback information of the actuator is transmitted to the controller. The forward channel is used to transmit the mobile platform path correction instruction, and the reverse channel is used to feed back the gimbal state data.

[0084] The mobile platform path correction instruction is transmitted through the forward channel, and the gimbal state data is fed back through the reverse channel to generate a composite control parameter, which combines the path correction instruction and the real-time state data of the gimbal to perform more accurate control. The composite control parameter is a control quantity generated by combining the control instruction of the forward channel and the state data of the reverse channel.

[0085] According to the composite control parameter, forward coupling analysis is performed to generate a feedforward gain matrix. According to the content of the composite control parameter, the gain of the control instruction is analyzed, and the feedforward gain matrix is generated. The feedforward gain matrix is used for pre-control according to the input (such as the path correction instruction). For example, if the path correction instruction requires the target to adjust the direction of travel, the feedforward gain matrix will calculate how much force the platform needs to apply and how to change the travel angle.

[0086] Based on the composite control parameter, reverse coupling analysis is performed. By analyzing the feedback data of the reverse channel, a feedback compensation vector is generated to adjust the control instruction according to the feedback data to eliminate errors or optimize control. For example, if there is a deviation between the current view angle of the gimbal and the expected view angle, the feedback compensation vector will correct the action of the gimbal by analyzing the deviation to ensure that the gimbal is adjusted to the correct position.

[0087] According to the feedforward gain matrix and the feedback compensation vector, control loop gain is performed to optimize the response speed and stability of the system by adjusting the gain value. The control loop gain is used to adjust the gain value of the feedforward and feedback signals to generate a bidirectional control instruction that combines the forward instruction and feedback compensation. It can accurately adjust the control parameters of the mobile platform and the gimbal to achieve efficient collaborative control. When performing a task, the path of the mobile platform (such as a unmanned vehicle) tracking the target and the field of view of the gimbal will be adjusted according to the bidirectional control instruction to ensure that the target can be accurately tracked.

[0088] Through the combination of the double-channel control architecture and the feedforward and feedback mechanisms, the control strategy can be adjusted in real time according to the changes of the target, enhancing the response accuracy and stability. The bidirectional data flow of the forward channel and the reverse channel ensures that the mobile platform and the gimbal can work in coordination to optimize target path tracking and field of view adjustment. Based on forward coupling analysis and reverse coupling analysis, the control gain is dynamically adjusted to generate flexible bidirectional control instructions, improving the accuracy of target tracking.

[0089] In summary, the target tracking method based on path planning and coordinated gimbal provided by the present application has the following beneficial effects:

[0090] The dynamic trajectory data of the target is acquired in real time, a three-dimensional motion trajectory graph is constructed for path tracking, and a target tracking path is obtained; expected pose parameters of a follow-up holder are set, the target tracking path is synchronized to the follow-up holder for dynamic updating according to the expected pose parameters, and a parameter coupling degree is obtained; a cooperative control instruction set is generated based on the parameter coupling degree for path planning, the follow-up holder is driven for bidirectional control according to the cooperative control instruction set, and the target is tracked and controlled through bidirectional control instructions. That is, the dynamic trajectory data of the target is acquired in real time, a three-dimensional motion trajectory graph is constructed, the high-speed motion of the target is quickly responded, the motion mode of the target in a complex environment is determined, the expected pose parameters of the holder are set and dynamically updated, the stable alignment to the target is maintained, the parameter coupling degree is used for path planning, the accuracy and robustness of target tracking in a complex scene are improved, the tracking error is reduced through closed-loop control, and the system energy consumption is balanced.

[0091] In the second embodiment, based on the same inventive concept as the target tracking method based on path planning and follow-up holder cooperation in the first embodiment, the application also provides a target tracking system based on path planning and follow-up holder cooperation. Please refer to the accompanying drawings Figure 2 The target tracking system based on path planning and follow-up holder cooperation comprises:

[0092] The path tracking module 11 is configured to acquire dynamic trajectory data of a target in real time, construct a three-dimensional motion trajectory graph for path tracking, and obtain a target tracking path; the dynamic updating module 12 is configured to set expected pose parameters of a follow-up holder, synchronize the target tracking path to the follow-up holder for dynamic updating according to the expected pose parameters, and obtain a parameter coupling degree; and the bidirectional control module 13 is configured to generate a cooperative control instruction set based on the parameter coupling degree for path planning, drive the follow-up holder for bidirectional control according to the cooperative control instruction set, and track and control the target through bidirectional control instructions.

[0093] Further, the path tracking module 11 in the target tracking system based on path planning and follow-up holder cooperation is further configured to:

[0094] The target is collected in real time by a multi-modal sensor group, a multi-modal data set is obtained, the multi-modal data set comprises laser radar point cloud data, target visual depth data and angular velocity data; the laser radar point cloud data, the target visual depth data and the angular velocity data are time-space aligned to determine a three-dimensional trajectory point sequence; the motion state of the target is predicted according to the three-dimensional trajectory point sequence to obtain a target predicted trajectory sequence, trajectory conflict detection is performed based on the target predicted trajectory sequence, and a plurality of predicted trajectory data are determined; motion is deduced in reverse based on the plurality of predicted trajectory data, an identification is performed based on the deduced result, and the three-dimensional motion trajectory graph is constructed.

[0095] Further, the path tracking module 11 in the target tracking system based on path planning and cooperation with a follow-up gimbal is further used for:

[0096] A dynamic obstacle prediction factor is introduced into the three-dimensional motion trajectory graph to construct a four-dimensional search space; a follow-up gimbal field of view constraint is extracted according to the follow-up gimbal, and a mobile feasibility score is generated by performing mobile feasibility evaluation on the four-dimensional search space according to the gimbal field of view constraint; a plurality of executable paths are identified based on the mobile feasibility score; and the plurality of executable paths are simulated and screened to determine the target tracking path.

[0097] Further, the dynamic updating module 12 in the target tracking system based on path planning and cooperation with a follow-up gimbal is further used for:

[0098] A mobile platform coordinate system of the target is constructed based on the three-dimensional motion trajectory graph, and a gimbal base coordinate system of the follow-up gimbal is constructed based on the four-dimensional search space; a conversion relationship matrix is constructed by performing space conversion on the mobile platform coordinate system and the gimbal base coordinate system; a plurality of path points are extracted by traversing the target tracking path, and the plurality of path points are converted according to the conversion relationship matrix to set initial pose parameters of the follow-up gimbal; a feasible pose parameter space of the follow-up gimbal is constructed by extracting mechanical limit constraints of the follow-up gimbal, and the initial pose parameters are synchronized to the feasible pose parameter space to determine the expected pose parameters.

[0099] Further, the dynamic updating module 12 in the target tracking system based on path planning and cooperation with a follow-up gimbal is further used for:

[0100] The target is subjected to attitude analysis based on the mobile platform coordinate system to construct a homogeneous transformation matrix; installation offset analysis is performed according to the gimbal base coordinate system to construct a gimbal kinematics chain; and the conversion relationship matrix is constructed by introducing a path point velocity vector based on the target tracking path to perform coordinate system rotation transformation compensation.

[0101] Further, the dynamic updating module 12 in the target tracking system based on path planning and cooperation with a follow-up gimbal is further used for:

[0102] The target tracking path is synchronized to the follow-up gimbal according to the expected pose parameters to determine a three-dimensional waypoint sequence of the target tracking path, and the three-dimensional waypoint sequence contains path curvature characteristics and timestamp information; a decoupling evaluation is performed based on the path curvature characteristics according to the timestamp information to set a parameter coupling degree index; a parameter coupling calculation is performed according to the parameter coupling degree index to obtain the parameter coupling degree.

[0103] Further, the bidirectional control module 13 in the target tracking system based on path planning and collaborative pan-tilt head is further used for:

[0104] Based on the dynamic coupling analysis of the parameter coupling degree, a dynamic coupling degree index is determined, and a coupling state feature vector is extracted according to the dynamic coupling degree index; based on the coupling state feature vector, a weighted fusion is performed to generate a collaborative control instruction set; according to the collaborative control instruction set, a bidirectional data interaction feedback is performed on the follow-up pan-tilt head to generate a composite control parameter, and a dynamic gain is performed according to the composite control parameter to construct the bidirectional control instruction for tracking control of the target.

[0105] Further, the bidirectional control module 13 in the target tracking system based on path planning and collaborative pan-tilt head is further used for:

[0106] Based on the coupling state feature vector, a weighted fusion is performed to generate a path correction amount and a position adjustment amount; according to the path correction amount and the position adjustment amount, a dynamic priority allocation is performed to determine a first execution weight and a second execution weight; according to the first execution weight, a control analysis is performed in combination with the path correction amount to generate a mobile platform path correction instruction; according to the second execution weight, a control analysis is performed in combination with the position adjustment amount to generate a pan-tilt pose adjustment instruction; the mobile platform path correction instruction and the pan-tilt pose adjustment instruction are integrated to generate a collaborative control instruction set.

[0107] Further, the bidirectional control module 13 in the target tracking system based on path planning and collaborative pan-tilt head is further used for:

[0108] The collaborative control instruction set is analyzed in real time to separate the mobile platform path correction instruction and the pan-tilt pose adjustment instruction; based on the pan-tilt pose adjustment instruction, an adjustment record is performed on the follow-up pan-tilt head to obtain pan-tilt state data; a double-channel control architecture is constructed, which includes a forward channel and a reverse channel; the mobile platform path correction instruction is transmitted through the forward channel, and the pan-tilt state data is fed back through the reverse channel to generate a composite control parameter; according to the composite control parameter, a forward coupling analysis is performed to generate a feedforward gain matrix; according to the composite control parameter, a reverse coupling analysis is performed to generate a feedback compensation vector; based on the feedforward gain matrix and the feedback compensation vector, a control loop gain is performed to construct the bidirectional control instruction.

[0109] Each embodiment in the specification is described in a progressive manner, and each embodiment focuses on the difference from other embodiments. The foregoing Figure 1The target tracking method based on path planning and cooperation of a follow-up holder in Embodiment One and the specific examples are also applicable to the target tracking system based on path planning and cooperation of a follow-up holder in the present embodiment. Through the foregoing detailed description of the target tracking method based on path planning and cooperation of a follow-up holder, those skilled in the art can clearly understand the target tracking system based on path planning and cooperation of a follow-up holder in the present embodiment. Therefore, for the sake of brevity of the description, no further detailed description is given herein.

[0110] The above description of disclosed embodiments enables one skilled in the art to make or use the application. Numerous modifications to these embodiments will be apparent to those skilled in the art, and the generic principles defined herein can be applied to other embodiments without departing from the spirit or scope of the application. Therefore, the present application is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0111] Obviously, for those skilled in the art, some improvements and modifications can be made to the present application without departing from the principles of the present application, and these improvements and modifications also fall within the protection scope of the present application.

Claims

1. A target tracking method based on path planning and cooperative pan-tilt, characterized in that, include: Real-time acquisition of dynamic trajectory data of the target, construction of a three-dimensional motion trajectory map for path tracking, and acquisition of the target tracking path; Set the desired pose parameters of the servo gimbal, and synchronize the target tracking path to the servo gimbal according to the desired pose parameters for dynamic updating, thereby obtaining the parameter coupling degree; Based on the parameter coupling degree, path planning is performed to generate a collaborative control instruction set. The follow-up gimbal is driven to perform bidirectional control according to the collaborative control instruction set, and the target is tracked and controlled through bidirectional control instructions. Its characteristic is that it acquires the target's dynamic trajectory data in real time and constructs a three-dimensional motion trajectory map, including: The target is collected in real time by a multimodal sensor array to obtain a multimodal dataset, which includes lidar point cloud data, target visual depth data, and angular velocity data. The lidar point cloud data, the target visual depth data, and the angular velocity data are spatiotemporally aligned to determine a three-dimensional trajectory point sequence; The target motion state is predicted according to the three-dimensional trajectory point sequence to obtain the target predicted trajectory sequence. Trajectory conflict detection is performed based on the target predicted trajectory sequence to determine multiple predicted trajectory data. Based on the multiple predicted trajectory data, motion backward inference is performed, and the inference results are identified to construct the three-dimensional motion trajectory map; Construct a 3D motion trajectory map for path tracking to obtain the target tracking path, including: A dynamic obstacle prediction factor is introduced into the three-dimensional motion trajectory map to construct a four-dimensional search space; Based on the gimbal's field of view constraints extracted from the servo gimbal, a movement feasibility assessment is performed on the four-dimensional search space according to the gimbal's field of view constraints, and a movement feasibility score is generated. Based on the mobility feasibility score, multiple executable paths are determined for the three-dimensional motion trajectory map identifier; The multiple executable paths are traversed and simulated for filtering to determine the target tracking path; Set the desired pose parameters of the servo gimbal, including: Based on the three-dimensional motion trajectory diagram, a mobile platform coordinate system for the target is constructed, and a gimbal base coordinate system for the servo gimbal is constructed based on the four-dimensional search space. The coordinate system of the mobile platform is spatially transformed from the coordinate system of the gimbal base to construct a transformation relationship matrix; The target tracking path is traversed to extract multiple path points, and the multiple path points are transformed according to the transformation relationship matrix to set the initial pose parameters of the servo gimbal. Extract the mechanical limit constraints of the servo gimbal to construct a feasible pose parameter space for the servo gimbal, synchronize the initial pose parameters to the feasible pose parameter space, and determine the desired pose parameters.

2. The target tracking method based on path planning and cooperative pan-tilt head according to claim 1, characterized in that, The mobile platform coordinate system and the gimbal base coordinate system are spatially transformed to construct a transformation relationship matrix, including: Based on the coordinate system of the mobile platform, the attitude of the target is analyzed, and a homogeneous transformation matrix is ​​constructed. Based on the gimbal base coordinate system, an installation offset analysis is performed to construct the gimbal kinematic chain; Based on the target tracking path, the velocity vector of the path point is introduced to perform coordinate system rotation transformation compensation, and the transformation relationship matrix is ​​constructed. 3.The target tracking method based on path planning and cooperative pan-tilt head according to claim 1, characterized in that, The target tracking path is synchronized to the servo gimbal for dynamic updating according to the desired pose parameters to obtain the parameter coupling degree, including: Synchronize the target tracking path to the follow-up holder according to the expected pose parameters, determine a three-dimensional waypoint sequence of the target tracking path, and the three-dimensional waypoint sequence contains path curvature characteristics and timestamp information; Evaluate decoupling based on the path curvature characteristics according to the timestamp information, and set a parameter coupling degree index; Perform parameter coupling calculation according to the parameter coupling degree index, and obtain the parameter coupling degree.

4. The target tracking method based on path planning and cooperative pan-tilt according to claim 1, characterized in that, Generate a cooperative control instruction set based on the parameter coupling degree, drive the follow-up holder to perform bidirectional control according to the cooperative control instruction set, perform tracking control on the target through bidirectional control instructions, including: Perform dynamic coupling analysis based on the parameter coupling degree, determine a dynamic coupling degree index, and extract a coupling state feature vector according to the dynamic coupling degree index; Perform weighted fusion based on the coupling state feature vector, and generate a cooperative control instruction set; Perform bidirectional data interaction feedback on the follow-up holder according to the cooperative control instruction set, generate a composite control parameter, perform dynamic gain according to the composite control parameter, and construct the bidirectional control instructions to perform tracking control on the target.

5. The target tracking method based on path planning and cooperative pan-tilt according to claim 4, characterized in that, Perform weighted fusion based on the coupling state feature vector, and generate a cooperative control instruction set, including: Perform weighted fusion based on the coupling state feature vector, and generate a path correction amount and a position adjustment amount; Determine a first execution weight and a second execution weight according to dynamic priority allocation of the path correction amount and the position adjustment amount; Perform control analysis according to the first execution weight combined with the path correction amount, and generate a mobile platform path correction instruction; Perform control analysis according to the second execution weight combined with the position adjustment amount, and generate a holder pose adjustment instruction; Integrate the mobile platform path correction instruction and the holder pose adjustment instruction to generate a cooperative control instruction set.

6. The target tracking method based on path planning and cooperative pan-tilt head according to claim 5, characterized in that, Perform bidirectional data interaction feedback on the follow-up holder according to the cooperative control instruction set, generate a composite control parameter, perform dynamic gain according to the composite control parameter, and construct the bidirectional control instructions to perform tracking control on the target, including: Real-time analyze the cooperative control instruction set, and separate the mobile platform path correction instruction and the holder pose adjustment instruction; Adjust the follow-up holder based on the holder pose adjustment instruction, and obtain holder state data; Construct a double-channel control architecture, and the double-channel control architecture contains a forward channel and a reverse channel; Transmit the mobile platform path correction instruction through the forward channel, and feed back the holder state data through the reverse channel to generate a composite control parameter; Perform forward coupling analysis according to the composite control parameter, and generate a feedforward gain matrix; Perform reverse coupling analysis according to the composite control parameter, and generate a feedback compensation vector; Perform control loop gain based on the feedforward gain matrix and the feedback compensation vector, and construct the bidirectional control instructions.

7. A target tracking system based on path planning and collaborative pan-tilt, characterized in that, A target tracking system based on path planning and follow-up holder cooperation for implementing the steps of the target tracking method based on path planning and follow-up holder cooperation according to any one of claims 1 to 6, the target tracking system based on path planning and follow-up holder cooperation comprising: The path tracking module is configured to acquire dynamic trajectory data of the target in real time, construct a three-dimensional motion trajectory diagram for path tracking, and obtain a target tracking path. The dynamic updating module is configured to set expected pose parameters of the follow-up holder, synchronize the target tracking path to the follow-up holder according to the expected pose parameters, and dynamically update the target tracking path to obtain a parameter coupling degree. The bidirectional control module is configured to generate a cooperative control instruction set based on the parameter coupling degree, drive the follow-up holder to perform bidirectional control according to the cooperative control instruction set, and track and control the target through bidirectional control instructions.

Images