A three-axis gimbal structure and a stabilization and target following control method thereof

Abstract

The application discloses a three-axis holder structure and a stabilizing and target following control method thereof, and relates to the technical field of three-axis holder visual control, which comprises the following steps: acquiring initial image data containing target visual features by means of an image acquisition device carried by a holder, extracting a complete contour of the target through foreground and background segmentation, and calculating the centroid coordinates and the size of the outer rectangular frame; acquiring the instantaneous values of the real-time pitch angle, roll angle and heading angle of the holder, calculating the target lateral and longitudinal pixel deviation and scale zoom proportion deviation, fusing the multiple deviations and the attitude angle data into a multi-dimensional error state vector, and generating motor driving instructions accordingly. The application optimizes target positioning parameter calculation through complete contour extraction, realizes visual and attitude data cooperative control by means of multi-dimensional error vector fusion, improves the target positioning deviation problem in a complex scene, improves the cooperativeness and accuracy of the holder stabilizing and target following control, and is suitable for three-axis holder visual following and stabilizing control scenes.

Description

Technical Field

[0001] This invention belongs to the field of three-axis gimbal vision control technology, specifically a three-axis gimbal structure and its stabilization and target following control method. Background Technology

[0002] Conventional three-axis gimbal stabilization and target following control technologies primarily rely on image acquisition devices to obtain local feature points of the target. They estimate the target's centroid using these local features and directly calculate pixel deviations using the feature point coordinates. Simultaneously, they utilize attitude sensors to acquire gimbal pitch, roll, and yaw angle data. Visual deviation data and gimbal attitude data are independently calculated to generate corresponding motor control commands for gimbal adjustment. This type of technology does not perform foreground / background segmentation of the image, relies solely on local features to calculate target parameters, and fails to integrate visual deviation and attitude angle data into a unified error vector for control calculation.

[0003] Conventional control schemes fail to extract complete target contour information in scenarios with partial target occlusion or complex backgrounds. This leads to deviations in the calculated target centroid coordinates and bounding box dimensions from the true target state, resulting in errors in target positioning parameters. Furthermore, the horizontal pixel deviation, vertical pixel deviation, scaling deviation, and gimbal attitude angle data are calculated independently at the visual level. These multi-dimensional error information cannot form a unified control benchmark, and the motor drive command generation process lacks coordinated support from these multi-dimensional errors. Consequently, the control logic for gimbal stabilization and target following exhibits a disconnect between different dimensions.

[0004] This invention aims to solve the problem of inaccurate calculation of the centroid and the outer rectangular frame size caused by the lack of a complete outline of the target object, and at the same time solve the problem that visual multi-dimensional deviation and the real-time attitude angle of the gimbal cannot be fused into a unified multi-dimensional error state vector. Summary of the Invention

[0005] This invention aims to solve at least one of the technical problems existing in the prior art;

[0006] Therefore, this invention proposes a three-axis gimbal structure and its stabilization and target following control method, including:

[0007] Receive initial image data containing visual features of the target object, the initial image data being acquired by an image acquisition device mounted on the three-axis gimbal structure;

[0008] The initial image data is segmented into foreground and background to extract the complete contour information of the target object, and the centroid coordinates and bounding box size of the target object in the current image frame are calculated based on the complete contour information.

[0009] The real-time attitude angle data of the three-axis gimbal structure is obtained from the attitude sensor array. The real-time attitude angle data includes the instantaneous values ​​of pitch angle, roll angle and yaw angle.

[0010] Based on the centroid coordinates of the target object and the preset image plane center point, calculate the horizontal and vertical pixel deviations of the target from the center of the field of view;

[0011] By combining the size of the outer rectangular frame with the preset target desired size, the scaling ratio deviation of the target scale is calculated;

[0012] The horizontal pixel deviation, vertical pixel deviation, scaling ratio deviation of the target object, and the real-time attitude angle data are fused into a multi-dimensional error state vector.

[0013] Based on the multi-dimensional error state vector, motor drive commands are generated.

[0014] Further, based on the centroid coordinates of the target object and a preset image plane center point, the lateral and vertical pixel deviations of the target from the center of the field of view are calculated, including:

[0015] Read the standard x-coordinate and standard y-coordinate values ​​of the preset image plane center point;

[0016] Read the actual x-coordinate and y-coordinate values ​​from the centroid coordinates of the target object;

[0017] The arithmetic difference between the actual horizontal coordinate value and the standard horizontal coordinate value is calculated to obtain the horizontal pixel deviation.

[0018] The arithmetic difference between the actual ordinate value and the standard ordinate value is calculated to obtain the vertical pixel deviation.

[0019] The calculated horizontal pixel deviation and the vertical pixel deviation are normalized and mapped to a continuous interval from negative one to positive one.

[0020] Furthermore, by combining the size of the outer rectangular frame with the preset target desired size, the scaling deviation of the target scale is calculated, including:

[0021] Extract the width and height pixel values ​​of the rectangle from the dimensions of the outer rectangle;

[0022] Read the desired width and desired height pixel values ​​from the preset target desired size;

[0023] Calculate the quotient of the width pixel value and the desired width pixel value, and the quotient of the height pixel value and the desired height pixel value, respectively;

[0024] The geometric mean of the two calculated quotients is used to obtain a comprehensive scale factor.

[0025] The difference between the overall scale factor and the standard value is calculated to obtain the scaling deviation.

[0026] Furthermore, based on the multi-dimensional error state vector, motor drive commands are generated, including:

[0027] The horizontal pixel deviation, vertical pixel deviation, and scaling ratio deviation in the multi-dimensional error state vector are respectively input to an independent inter-axis decoupling controller;

[0028] Each independent inter-axis decoupling controller calculates the desired angle increment to eliminate the deviation component based on the deviation component it receives;

[0029] Extract the real-time attitude angle data from the multi-dimensional error state vector;

[0030] Using the attitude inverse calculation process, the desired angle increment is fused with the real-time attitude angle data and decomposed into the original speed commands for driving the three rotary motors in the three-axis gimbal structure.

[0031] The original speed command is smoothed and rate-limited to generate the final motor drive command sent to the three rotating motors.

[0032] Furthermore, the process of using attitude inverse calculation to fuse the desired angle increment with the real-time attitude angle data and decompose it into the original speed commands for driving the three rotary motors in the three-axis gimbal structure includes:

[0033] Read the current heading angle, pitch angle, and roll angle from the real-time attitude angle data;

[0034] Read the expected angle increments of the heading axis, pitch axis, and focal length equivalent axis from the expected angle increments;

[0035] Convert the current heading angle, pitch angle and roll angle into a three-dimensional rotation matrix;

[0036] The desired angle increments of the heading axis, pitch axis, and focal length equivalent axis are used to construct the desired incremental rotation matrix.

[0037] The desired incremental rotation matrix and the current three-dimensional rotation matrix are combined using matrix multiplication to calculate the combined rotation matrix.

[0038] The inverse solution of the synthesized rotation matrix is ​​used to obtain the expected heading angle, expected pitch angle and expected roll angle at the next moment;

[0039] The instantaneous angular velocity commands of the desired heading angle and the current heading angle, the desired pitch angle and the current pitch angle, and the desired roll angle and the current roll angle are calculated to form the original rotational speed command.

[0040] Furthermore, it also includes active stabilization and vibration compensation processes:

[0041] High-frequency attitude angle change rate data is continuously acquired from the attitude sensor array. The attitude angle change rate data includes instantaneous values ​​of angular velocity in the pitch, roll and yaw axes.

[0042] The instantaneous angular velocity values ​​of each axis are subjected to high-pass filtering to extract high-frequency jitter components with frequencies higher than a set threshold.

[0043] Low-frequency attitude angle drift data is synchronously acquired from the attitude sensor array. The attitude angle drift data includes slow angular offsets in the pitch, roll and yaw axes.

[0044] The high-frequency jitter component is superimposed with the attitude angle drift data to synthesize an active stabilization control quantity;

[0045] Read linear acceleration data from the inertial measurement unit and integrate the linear displacement disturbance of the three-axis gimbal structure;

[0046] Multiply the linear displacement disturbance by the preset lever arm parameters to convert it into the additional angle compensation caused by the translational motion of the carrier.

[0047] The active stabilization control quantity and the additional angle compensation quantity are summed to obtain the total stabilization compensation command.

[0048] Further, the step of reading linear acceleration data from the inertial measurement device and integrating the linear displacement disturbance of the three-axis gimbal structure includes:

[0049] Raw linear acceleration data is read from the triaxial accelerometer of the inertial measurement device at fixed time intervals;

[0050] The original linear acceleration data is processed to remove the gravitational acceleration component, resulting in pure acceleration data of the carrier motion;

[0051] The pure acceleration data is numerically integrated along three orthogonal axes to obtain the instantaneous velocity components for each axis.

[0052] The instantaneous velocity component for each axis is then numerically integrated to obtain the instantaneous displacement component for each axis.

[0053] The instantaneous displacement components of the three axes are combined to form the linear displacement disturbance vector of the three-axis gimbal structure.

[0054] Furthermore, it also includes the process of integrating target-following instructions with active stabilization instructions:

[0055] Receive the motor drive command generated from claim 4 as the target following command of the main control loop;

[0056] Receive the total stabilization compensation command generated from claim 6 as the active stabilization command of the feedforward compensation loop;

[0057] The active stabilization command is injected into the corresponding axial component of the target following command in a vector superposition manner;

[0058] Saturation protection processing is performed on the synthetic control command for each axis after injection to ensure that the amplitude of the synthetic control command does not exceed the maximum allowable input value of the corresponding axis motor driver;

[0059] The synthesized control command, after saturation protection processing, is converted into a pulse width modulation signal, which directly drives the three rotary motors of the three-axis gimbal structure to perform actions.

[0060] Furthermore, it also includes the prediction and recapture process for lost targets:

[0061] Confidence score for monitoring the complete contour information of the target object in consecutive image frames;

[0062] When the confidence score is lower than the preset recapture threshold, the target loss processing procedure is initiated.

[0063] Record the trajectory coordinates and velocity vector of the target object in multiple consecutive frames of images before the target is lost;

[0064] Based on the motion trajectory coordinates and motion velocity vector, the position region of the target object at subsequent time moments is predicted by a linear extrapolation algorithm;

[0065] The rotating motor of the three-axis gimbal structure is controlled to drive the center of the field of view of the image acquisition device to point to the location area;

[0066] Within the specified location area, template matching search is performed using the historical visual feature templates of the target object. If the match is successful, the foreground and background segmentation process is restored; if the match fails, the full field of view inspection mode is entered.

[0067] Furthermore, the present invention also includes a three-axis gimbal structure, the structure comprising a pitch axis assembly, a roll axis assembly, a yaw axis assembly, an image acquisition device, an attitude sensor array, an inertial measurement device, and a control unit;

[0068] The pitch axis assembly includes a pitch axis motor and a pitch axis frame, wherein the pitch axis motor drives the pitch axis frame to rotate about a horizontal axis;

[0069] The roll axis assembly includes a roll axis motor and a roll axis frame, the roll axis frame being mounted on the pitch axis frame and driven by the roll axis motor to rotate about a horizontal axis perpendicular to the pitch axis.

[0070] The heading axis assembly includes a heading axis motor and a heading axis base, the heading axis base being mounted on the roll axis frame and driven by the heading axis motor to rotate about the vertical axis;

[0071] The image acquisition device is rigidly mounted on the roll axis frame, and its optical axis is orthogonal to the rotation axis of the roll axis frame.

[0072] The attitude sensor array is mounted on the roll shaft frame and is used to measure the attitude angle and angular velocity of the roll shaft frame.

[0073] The inertial measurement unit is mounted on the yaw axis base and is used to measure the linear acceleration and angular velocity of the three-axis gimbal structure.

[0074] The control unit integrates a processor and a memory. The control unit is electrically connected to the pitch axis motor, roll axis motor, yaw axis motor, image acquisition device, attitude sensor array, and inertial measurement device. The memory stores instruction codes. The processor executes the instruction codes to implement the stabilization and target following control method of a three-axis gimbal structure according to any one of claims 1 to 9.

[0075] Compared with the prior art, the beneficial effects of the present invention are:

[0076] The initial image data is segmented into foreground and background to extract the complete contour information of the target object. Based on the complete contour information, the centroid coordinates and bounding box size of the target object in the current image frame are calculated. This avoids the contour loss problem caused by local feature extraction. The calculated centroid coordinates of the target are close to the actual geometric center position of the target, and the bounding box size matches the real shape parameters of the target. This reduces the interference of complex background on the calculation of target positioning parameters. The calculation results of the target positioning parameters are highly consistent with the actual state of the target, reducing the positioning deviation caused by the loss of local target features or background interference.

[0077] The horizontal pixel deviation, vertical pixel deviation, scaling deviation of the target object, and real-time pitch, roll, and yaw angles of the three-axis gimbal structure are fused into a multi-dimensional error state vector. Motor drive commands are generated based on the multi-dimensional error state vector, which can achieve unified dimensional integration of visual deviation data and gimbal attitude data. This eliminates the problem of control logic fragmentation caused by independent calculation of each error data. The generation of motor drive commands relies on a unified error benchmark. The control response of gimbal stabilization and target following fits the coupling state of multi-dimensional errors. The motor drive commands are precisely adapted to the actual attitude of the gimbal and the target deviation state. The generation logic of control commands is coordinated with the actual state of gimbal movement and target offset. Attached Figure Description

[0078] Figure 1 This is a flowchart illustrating the steps of a three-axis gimbal structure and its stabilization and target following control method as described in this invention.

[0079] Figure 2 A flowchart for calculating the scaling deviation;

[0080] Figure 3 A multi-dimensional error state vector diagram;

[0081] Figure 4 Extracting high-frequency jitter components using high-pass filtering;

[0082] Figure 5 This is the output diagram of the active stabilization compensation command for a three-axis gimbal. Detailed Implementation

[0083] The technical solution of the present invention will be clearly and completely described below with reference to the embodiments. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0084] See Figure 1 This invention provides a three-axis gimbal structure and its stabilization and target following control method, the method comprising:

[0085] The system receives initial image data containing visual features of the target object, acquired by an image acquisition device mounted on a three-axis gimbal structure. Foreground and background segmentation is performed on the initial image data to extract the complete contour information of the target object. Based on the complete contour information, the system calculates the centroid coordinates and bounding box size of the target object in the current image frame. Simultaneously, real-time attitude angle data of the three-axis gimbal structure is acquired from an attitude sensor array. This real-time attitude angle data includes instantaneous values ​​of pitch, roll, and yaw angles. Based on the centroid coordinates of the target object and a preset image plane center point, the system calculates the lateral and longitudinal pixel deviations of the target from the center of the field of view. Combining the bounding box size with a preset desired target size, the system calculates the scaling deviation of the target scale. The system then fuses the lateral pixel deviation, longitudinal pixel deviation, scaling deviation, and real-time attitude angle data of the target object into a multi-dimensional error state vector. Finally, based on this multi-dimensional error state vector, a motor drive command is generated to drive the motors in the three-axis gimbal structure.

[0086] In one embodiment of the present invention, the process of calculating the lateral pixel deviation and the vertical pixel deviation reads the standard horizontal coordinate value and the standard vertical coordinate value of the preset image plane center point, and reads the actual horizontal coordinate value and the actual vertical coordinate value in the centroid coordinate of the target object. The arithmetic difference between the actual horizontal coordinate value and the standard horizontal coordinate value is calculated to obtain the lateral pixel deviation. The arithmetic difference between the actual vertical coordinate value and the standard vertical coordinate value is calculated to obtain the vertical pixel deviation. Then, the calculated lateral pixel deviation and the vertical pixel deviation are normalized and mapped to a continuous interval from negative one to positive one.

[0087] In specific implementations, the method for calculating lateral and longitudinal pixel deviations involves systematic processing of image plane coordinates. Each frame of initial image data acquired by the image acquisition device is defined in a two-dimensional pixel coordinate system, where the horizontal axis represents the horizontal direction of the image and the vertical axis represents the vertical direction. In specific implementations, a preset image plane center point has fixed standard horizontal and vertical coordinate values. These standard horizontal and vertical coordinate values ​​are determined from the physical center pixel position of the image sensor or the pixel position after lens optical calibration. In some embodiments, the centroid coordinates of the target object are calculated using the complete contour information extracted after foreground and background segmentation. The centroid coordinates include an actual horizontal coordinate value and an actual vertical coordinate value, representing the horizontal and vertical positions of the geometric center of the target object's contour in the image pixel coordinate system, respectively.

[0088] The process of calculating the horizontal pixel deviation involves arithmetic subtraction between the actual horizontal coordinate value and the standard horizontal coordinate value, while the process of calculating the vertical pixel deviation involves arithmetic subtraction between the actual vertical coordinate value and the standard vertical coordinate value. Optionally, when the actual horizontal coordinate value is greater than the standard horizontal coordinate value, the horizontal pixel deviation is positive, indicating that the target center is located to the right of the image plane center point; when the actual vertical coordinate value is greater than the standard vertical coordinate value, the vertical pixel deviation is positive, indicating that the target center is located below the image plane center point. It can be understood that the directly calculated horizontal and vertical pixel deviations are pixel unit values ​​with physical dimensions, and their numerical range is directly related to the resolution of the image acquisition device.

[0089] In specific implementations, the horizontal and vertical pixel deviations are normalized to map the deviation values ​​to a standard range independent of image resolution. Normalization is achieved using a scaling factor, typically determined based on the maximum number of pixels in the horizontal and vertical directions of the image sensor. Optionally, one form of normalization is to divide the horizontal pixel deviation by half the maximum number of pixels in the image width direction and the vertical pixel deviation by half the maximum number of pixels in the image height direction. It can be understood that the normalized horizontal and vertical pixel deviations are constrained to a continuous range from negative one to positive one, where negative one represents the target center being located at the leftmost or topmost edge of the image plane, positive one represents the target center being located at the rightmost or bottommost edge of the image plane, and zero represents the target center coinciding with the center point of the image plane. In some embodiments, this mapping relationship can be expressed by the following formula:

[0090]

[0091]

[0092] in: This represents the normalized lateral pixel deviation. This represents the normalized vertical pixel deviation. This represents the actual x-coordinate value of the target object. This represents the preset standard x-axis value. This represents the actual ordinate value of the target object. This represents the preset standard ordinate value. This represents the total pixel width in the horizontal direction of the image sensor. This represents the total pixel height in the vertical direction of the image sensor.

[0093] In one embodiment of the present invention, see [reference] Figure 2The process of calculating the scaling deviation of the target scale involves extracting the width and height pixel values ​​of the bounding rectangle from the outer rectangle size, reading the expected width and height pixel values ​​from the preset target expected size, calculating the quotient of the width pixel value and the expected width pixel value, and the quotient of the height pixel value and the expected height pixel value, respectively, performing a geometric mean calculation on the two quotients to obtain a comprehensive scale factor, and calculating the difference between the comprehensive scale factor and the standard value to obtain the scaling deviation.

[0094] In specific implementations, the method for calculating the scaling deviation of the target scale involves a quantitative comparison between the outer bounding box size and the preset target desired size. The outer bounding box size is the projection range of the complete outline information of the target object obtained from the segmentation process onto the image plane. The outer bounding box size includes the width pixel value and the height pixel value of the rectangle. The width pixel value represents the pixel span occupied by the target object in the horizontal direction of the image, and the height pixel value represents the pixel span occupied by the target object in the vertical direction of the image. In some embodiments, the preset target desired size is a preset parameter of the control system. The target desired size defines the expected visual size occupied by the target object in the image. The target desired size includes an expected width pixel value and an expected height pixel value. The setting of the expected width pixel value and the expected height pixel value is usually associated with the optimal field of view coverage requirements of subsequent recognition or tracking tasks.

[0095] The calculation process calculates the quotient of the width pixel value to the desired width pixel value, and the quotient of the height pixel value to the desired height pixel value. These two quotients reflect the proportional relationship of the target object relative to the preset desired size in the width and height dimensions, respectively. Optionally, when the width pixel value is greater than the desired width pixel value, the width dimension quotient is greater than one, indicating that the target object is larger than the desired size in the horizontal direction; when the height pixel value is less than the desired height pixel value, the height dimension quotient is less than one, indicating that the target object is smaller than the desired size in the vertical direction. It is understood that using only the quotient of a single dimension may lead to unromantic scale evaluation due to changes in the shape of the target object or partial occlusion; therefore, it is necessary to combine the quotients of both dimensions.

[0096] In practice, the two quotients are geometrically averaged to obtain a comprehensive scale factor. The geometric average calculation balances the proportional information of the width and height dimensions, reducing interference from drastic changes in a single dimension on the overall scale judgment. In some embodiments, the mathematical process for calculating the comprehensive scale factor is as follows:

[0097]

[0098] in: This represents the calculated comprehensive scale factor. This represents the width in pixels of the rectangle extracted from the size of the outer rectangle. This represents the desired width in pixels read from the preset target desired size. This represents the height in pixels of the rectangle extracted from the dimensions of the outer rectangle. This represents the desired height in pixels read from the preset target desired size. This can be understood as a comprehensive scale factor. It is a dimensionless numerical value. A value of 1 indicates that the current scale of the target object is completely consistent with the expected scale. A value greater than 1 indicates that the target object as a whole is larger than the expected scale. A value less than 1 indicates that the target object as a whole is smaller than the expected scale.

[0099] The process of calculating the scaling deviation involves performing an arithmetic subtraction operation between the comprehensive scale factor and the standard value one, where the standard value one represents the ideal state without scale deviation. Optionally, if the calculated scaling deviation is positive, the control system is instructed to drive the image acquisition device to increase the focal length or move backward to reduce the size of the target in the image; if the scaling deviation is negative, the system is instructed to decrease the focal length or move forward to enlarge the size of the target in the image. This deviation value will be sent to the subsequent controller to generate corresponding control commands.

[0100] In one embodiment of the present invention, the process of generating motor drive commands involves inputting the lateral pixel deviation, longitudinal pixel deviation, and scaling ratio deviation from the multi-dimensional error state vector to independent inter-axis decoupling controllers. Each independent inter-axis decoupling controller calculates the desired angle increment for eliminating the deviation component based on the received deviation component, extracts the real-time attitude angle data from the multi-dimensional error state vector, and fuses the desired angle increment with the real-time attitude angle data using an attitude inverse calculation process. This is then decomposed into the original speed commands for driving the three rotary motors in the three-axis gimbal structure. The original speed commands are then smoothed, filtered, and rate-limited to generate the final motor drive commands sent to the three rotary motors. The attitude inverse calculation process reads the current heading angle, pitch angle, and roll angle from the real-time attitude angle data. It reads the expected angle increments of the heading axis, pitch axis, and focal length equivalent axis from the expected angle increments. It converts the current heading angle, pitch angle, and roll angle into a three-dimensional rotation matrix. It constructs the expected incremental rotation matrix by combining the expected angle increments of the heading axis, pitch axis, and focal length equivalent axis. Through matrix multiplication, it calculates the composite rotation matrix of the expected incremental rotation matrix and the current three-dimensional rotation matrix. It performs inverse solving on the composite rotation matrix to obtain the expected heading angle, expected pitch angle, and expected roll angle at the next moment. It calculates the instantaneous angular velocity commands of the expected heading angle and the current heading angle, the expected pitch angle and the current pitch angle, and the expected roll angle and the current roll angle, which constitute the original rotation speed command.

[0101] In practical implementation, the process of generating motor drive commands begins with the decomposition of a multi-dimensional error state vector. The multi-dimensional error state vector, containing lateral pixel deviation, longitudinal pixel deviation, and scaling deviation, is input to three independent inter-axis decoupling controllers. One independent inter-axis decoupling controller receives the lateral pixel deviation, another receives the longitudinal pixel deviation, and the third receives the scaling deviation. In some embodiments, each independent inter-axis decoupling controller can be a proportional-integral-derivative controller. Each independent inter-axis decoupling controller calculates based on the received deviation components and outputs a desired angle increment to eliminate the corresponding deviation component. The desired angle increment corresponding to the lateral pixel deviation mainly affects the rotation of the yaw axis, the desired angle increment corresponding to the longitudinal pixel deviation mainly affects the rotation of the pitch axis, and the desired angle increment corresponding to the scaling deviation is mapped to a control quantity for the focus adjustment mechanism or equivalently to a virtual displacement of the gimbal along the optical axis.

[0102] In practical implementation, the attitude inverse calculation process requires real-time attitude angle data from a multi-dimensional error state vector. This real-time attitude angle data includes the current yaw angle, current pitch angle, and current roll angle. The expected angle increments are read from the expected angle increments of the yaw axis, pitch axis, and focal length equivalent axis. The core task of attitude inverse calculation is to transform the deviation control quantity based on the image plane into specific motor rotation commands in the gimbal body coordinate system. This process is achieved through rotation matrix operations; the current yaw angle, current pitch angle, and current roll angle are converted into a three-dimensional rotation matrix representing the current attitude of the gimbal. Optionally, a common conversion method uses the ZYX order Euler angle definition, corresponding to a three-dimensional rotation matrix. The calculation method is as follows:

[0103]

[0104] in: This represents the current 3D rotation matrix. Indicates the current heading angle. Indicates the current pitch angle. Indicates the current roll angle. , , These represent the elementary rotation matrices about the Z-axis, Y-axis, and X-axis, respectively.

[0105] In practice, the desired angle increment is used to construct a desired incremental rotation matrix, which describes the pose change the gimbal needs to make from the current frame to the next desired frame. In some embodiments, the desired incremental rotation matrix is ​​multiplied by the current 3D rotation matrix. A composite rotation matrix is ​​obtained by combining the desired rotations, representing the gimbal's expected attitude at the next moment. Optionally, the composite rotation matrix can be inversely solved to extract the expected yaw, pitch, and roll angles for the next moment. This inverse solution process involves specific trigonometric operations on each element of the rotation matrix. Subsequently, the difference between the expected and current yaw angles is calculated and divided by the control cycle to obtain the instantaneous yaw axis angular velocity command; the difference between the expected and current pitch angles is calculated and divided by the control cycle to obtain the instantaneous pitch axis angular velocity command; and the difference between the expected and current roll angles is calculated and divided by the control cycle to obtain the instantaneous roll axis angular velocity command. These three instantaneous angular velocity commands together constitute the original speed command for the drive motor.

[0106] See Figure 3This is a multi-dimensional error state vector diagram, showing the variation of the three key components of the multi-dimensional error state vector with the control cycle. The lateral pixel deviation exhibits a complete sinusoidal cycle, reaching a positive peak (+0.8) at 0.25s, a negative peak (-0.8) at 0.75s, and returning to zero at 1s, simulating a target moving left and right in the field of view. The vertical pixel deviation is out of phase with the lateral deviation, being +0.6 at 0s, -0.6 at 0.5s, and returning to +0.6 at 1s, simulating a target moving up and down in the field of view. The scaling deviation exhibits an exponential decay trend, rapidly converging from an initial 0.5 to near zero, simulating the process of a target approaching from a distance and stabilizing at the desired size, demonstrating the convergence characteristics of scale matching. These three deviation components together constitute the multi-dimensional error state vector, which is the input to the subsequent inter-axis decoupling controller, directly determining the direction and amplitude of the gimbal attitude adjustment.

[0107] In one embodiment of the present invention, high-frequency attitude angle change rate data is continuously acquired from the attitude sensor array. The attitude angle change rate data includes instantaneous angular velocity values ​​in the pitch, roll, and yaw axes. High-pass filtering is performed on the instantaneous angular velocity values ​​in each axis to extract high-frequency jitter components with frequencies higher than a set threshold. Low-frequency attitude angle drift data is synchronously acquired from the attitude sensor array. The attitude angle drift data includes slow angular offsets in the pitch, roll, and yaw axes. The high-frequency jitter components are superimposed with the attitude angle drift data to synthesize an active stabilization control quantity. Linear acceleration data is read from the inertial measurement unit, and the linear displacement disturbance of the three-axis gimbal structure is integrated. The linear displacement disturbance is multiplied by a preset lever arm parameter to convert it into an additional angle compensation quantity caused by the translational motion of the carrier. The active stabilization control quantity and the additional angle compensation quantity are summed to obtain a total stabilization compensation command. The process of integrating the linear displacement disturbance involves reading raw linear acceleration data from the triaxial accelerometer of the inertial measurement device at fixed time intervals, removing the gravitational acceleration component from the raw linear acceleration data to obtain pure acceleration data of the carrier motion, performing numerical integration calculations on the pure acceleration data along the three orthogonal axes to obtain the instantaneous velocity component of each axis, performing numerical integration calculations on the instantaneous velocity component of each axis again to obtain the instantaneous displacement component of each axis, and combining the instantaneous displacement components of the three axes to form the linear displacement disturbance vector of the triaxial gimbal structure.

[0108] In practical implementation, the active stabilization and vibration compensation process relies on multi-source data acquired in real time from the sensor array and processed in layers. The attitude sensor array operates continuously at a sampling rate higher than the control loop's main frequency, outputting high-frequency attitude angle change rate data. This data directly reflects the instantaneous rotational angular velocity of the gimbal around its pitch, roll, and yaw axes. In some embodiments, high-pass filtering of the instantaneous angular velocity values ​​for each axis is achieved using a digital filter. The set threshold is determined based on the gimbal's mechanical resonant frequency and the target following bandwidth. High-frequency jitter components with frequencies higher than the set threshold are separated and extracted. These components mainly include minute high-frequency angular vibrations caused by motor vibration, gear backlash, or the carrier engine. Low-frequency attitude angle drift data synchronously acquired from the attitude sensor array is obtained through low-pass filtering or by directly differentiating the attitude angle data. This drift data characterizes the slow angular deviations in the pitch, roll, and yaw axes caused by slow carrier steering, wind, etc.

[0109] In practical implementation, the synthesized active stabilization control quantity is a process of algebraically superimposing high-frequency jitter components with attitude angle drift data. The high-frequency jitter components are given a negative sign to generate a reverse compensation command to cancel vibration, while the attitude angle drift data is used to correct slow baseline shifts. The superimposed signal constitutes the active stabilization control quantity for angular disturbances. Reading linear acceleration data from the inertial measurement unit (IMU) is key to compensating for the effects of translational motion. The IMU is usually integrated with the attitude sensor array. The process of integrating the linear displacement disturbance of the three-axis gimbal structure involves reading raw linear acceleration data from the three-axis accelerometer of the IMU at fixed time intervals. The raw linear acceleration data is the vector sum of the carrier motion acceleration and gravitational acceleration. Optionally, the removal of the gravitational acceleration component depends on the attitude angle data at the current moment. By multiplying the transpose of the current three-dimensional rotation matrix with the gravity vector [0,0,g]^T, the gravity component in the current carrier coordinate system is estimated, and then subtracted from the raw linear acceleration data to obtain the pure acceleration data of the carrier motion. Numerical integration calculations of pure acceleration data along three orthogonal axes are typically performed using the trapezoidal method or Simpson's method. The first integration yields the instantaneous velocity component for each axis, and the instantaneous velocity component for each axis is then numerically integrated again to obtain the instantaneous displacement component for each axis. The instantaneous displacement components along the three axes together constitute a linear displacement disturbance vector.

[0110] It is understandable that converting linear displacement disturbance into additional angle compensation requires preset lever arm parameters. These parameters are three-dimensional vectors representing the spatial offset of the optical center of the image acquisition device relative to the gimbal's rotation center. In some embodiments, the conversion process approximates the additional angle compensation caused by translational motion through a vector cross product. It can be given by the following formula:

[0111]

[0112] in: This represents the vector of additional angle compensation. This represents a proportionality coefficient related to system parameters. This represents the preset lever arm parameter vector. Represents the linear displacement disturbance vector, "" indicates a vector cross product operation. This compensation amount reflects the optical axis pointing deviation caused by the carrier translation. Finally, the active stabilization control amount and the additional angle compensation amount are summed to obtain the total stabilization compensation command acting on the three axes. To illustrate the data processing flow, refer to Table 1, which shows an example of the data for each axis within a processing cycle.

[0113] Table 1: Data for each axis within one processing cycle

[0114]

[0115] Optionally, the data in Table 1 are for illustrative purposes only, and the actual values ​​vary depending on the sensor accuracy and motion state. It can be understood that the total stabilization compensation command, as a feedforward quantity, aims to quickly counteract disturbances directly introduced by the carrier motion, and is subsequently fused with the vision-based target following command.

[0116] See Figure 4 This is a high-pass filter diagram for extracting high-frequency jitter components, demonstrating the filtering decomposition and disturbance extraction process of the attitude angular velocity signal. The original data gradually rises in the range of 0.05–0.2 rad / s, superimposed with high-frequency spikes, reflecting the composite disturbance characteristics of "slow drift + high-frequency jitter". The low-frequency components are highly consistent with the trend of the original data, smoothing out the high-frequency spikes and clearly restoring the slow attitude changes of the carrier, serving as the basis for long-term compensation. The high-frequency jitter components fluctuate violently around the zero point, with an amplitude range of approximately -0.1–0.12 rad / s, retaining only the instantaneous disturbance component, which is the core suppression target of active stabilization. The filtering threshold serves as the zero-point reference line, clearly defining the boundary of the high-pass filter and ensuring that only high-frequency disturbances exceeding the threshold are extracted. After the high-frequency jitter components and low-frequency drift components are superimposed, they are synthesized into an active stabilization control quantity, which is then combined with linear displacement disturbance compensation to finally form a total stabilization compensation command, which is injected into the motor drive circuit.

[0117] In one embodiment of the present invention, the control method includes a process of fusing a target following command with an active stabilization command, receiving the motor drive command generated by the motor drive command generation process as the target following command of the main control loop, receiving the total stabilization compensation command generated by the active stabilization and vibration compensation process as the active stabilization command of the feedforward compensation loop, injecting the active stabilization command into the corresponding axial component of the target following command in a vector superposition manner, performing saturation protection processing on the synthesized control command of each axis after injection to ensure that the amplitude of the synthesized control command does not exceed the maximum allowable input value of the corresponding axial motor driver, and converting the synthesized control command after saturation protection processing into a pulse width modulation signal to directly drive the three rotary motors of the three-axis gimbal structure to perform actions. The control method includes a prediction and recapture process for lost targets. It monitors the confidence score of the complete contour information of the target object in consecutive image frames. When the confidence score is lower than a preset recapture threshold, a target loss processing flow is initiated. The motion trajectory coordinates and velocity vectors of the target object in multiple consecutive frames before the target loss are recorded. Based on the motion trajectory coordinates and velocity vectors, a linear extrapolation algorithm is used to predict the position region of the target object at subsequent times. The rotating motor of the three-axis gimbal structure is controlled to drive the field of view center of the image acquisition device to point to the position region. Within the position region, a template matching search is performed using the historical visual feature template of the target object. If the match is successful, the foreground and background segmentation processing flow is restored; if the match fails, a full-field-of-view inspection mode is entered.

[0118] In practical implementation, the process of fusing the target following command and the active stabilization command constitutes a dual-loop composite control structure. It receives the target following command generated by the main control loop, which is a basic control signal for driving the gimbal to track the target, calculated based on visual errors. Simultaneously, it receives the total stabilization compensation command generated by the feedforward compensation loop, which is a rapid compensation signal for compensating for carrier disturbances, calculated based on inertial sensor data. In some embodiments, the fusion process is performed in a vector superposition manner, algebraically adding the pitch, roll, and yaw axis components of the active stabilization command to the corresponding axial components of the target following command. This superposition allows the control command to simultaneously include the slow-changing trend of aligning the field of view with the target and the high-frequency compensation for compensating for carrier sway. Saturation protection processing is applied to the synthesized control command for each axis after injection to ensure the physical feasibility of the command. The saturation protection processing sets an absolute upper limit; when the amplitude of the synthesized control command for a certain axis exceeds the maximum allowable input value of the corresponding axis motor driver, the synthesized control command for that axis is clamped to the maximum allowable input value or a negative maximum allowable input value.

[0119] In practical implementation, the synthesized control command after saturation protection processing needs to be converted into a pulse-width modulation (PWM) signal that can directly drive the motor. The conversion process, based on the characteristics of the motor driver, maps the voltage representing the angular velocity or torque command to a PWM waveform with a specific frequency and duty cycle. Optionally, a conversion relationship can be described by the following equation:

[0120]

[0121] in: This indicates the duty cycle of the final generated pulse width modulation signal. This represents the synthesized control command voltage value for a specific axis after saturation protection processing. This indicates the absolute value of the maximum command voltage that the axial motor driver can accept. This indicates the upper limit of the pulse width modulation duty cycle corresponding to the driver rotating at full forward speed. This indicates the intermediate duty cycle corresponding to when the motor stops. It can be understood that this pulse width modulation signal is directly sent to the drive circuits corresponding to the three rotary motors of the three-axis gimbal structure, thereby performing a combined stabilization and following action.

[0122] In some embodiments, the prediction and recapture process for a lost target is an independent fault recovery logic. The system monitors the confidence score of the target object's complete contour information in consecutive image frames. The confidence score is calculated based on the integrity of the contour, the matching degree with historical templates, or the number of feature points. When the confidence score is lower than a preset recapture threshold, the system determines that the target is lost and initiates the target loss processing flow. It records the motion trajectory coordinates and velocity vector of the target object in multiple consecutive frames before the target loss. The motion trajectory coordinates are a historical sequence of the target's centroid position in the image plane, and the velocity vector is obtained by differencing and filtering this sequence. Based on the motion trajectory coordinates and velocity vector, a linear extrapolation algorithm predicts the target object's position region at subsequent time points. The linear extrapolation algorithm assumes that the target maintains uniform motion for a short period, and its prediction formula is:

[0123]

[0124] in: Indicates the predicted location coordinates. This indicates the target's location coordinates in the last frame before it was lost. This represents the calculated velocity vector. Indicates the number of frames predicted. Indicates the control cycle.

[0125] In practice, the rotary motor controlling the three-axis gimbal structure drives the center of the image acquisition device's field of view to point towards the location region predicted by the linear extrapolation algorithm. This action is achieved by converting the predicted position coordinates into the desired angle command of the gimbal. Within the predicted location region, the system calls upon historical visual feature templates of the target object for template matching search. These historical visual feature templates are the target's appearance features stored during the target tracking stabilization period. Optionally, if the similarity score of the template matching exceeds a preset recovery threshold, the matching is considered successful, and the system resumes the foreground and background segmentation process, re-entering a stable tracking state. If the template matching fails, the system enters a full-field-of-view inspection mode, moving the gimbal according to a preset search pattern to attempt to rediscover the target within the entire field of view.

[0126] See Figure 5 This is a diagram showing the output of active stabilization compensation commands for a three-axis gimbal, illustrating the variation of stabilization compensation amounts across the three axes over time. The yaw axis stabilization command has the largest amplitude, ranging from approximately 0.05 to 0.2 radians, exhibiting a slow fluctuation trend with a small amount of high-frequency spikes. This reflects the control characteristics of prioritizing low-frequency drift compensation and secondary high-frequency jitter suppression in the yaw direction. The pitch axis stabilization command has the second largest amplitude, ranging from approximately -0.15 to 0.1 radians, with the most intense high-frequency fluctuations and frequent alternations between positive and negative values. This reflects the rapid response characteristics of the pitch direction to instantaneous disturbances. The roll axis stabilization command amplitude is close to that of the pitch axis, ranging from approximately -0.05 to 0.12 radians, with a fluctuation frequency between the pitch and yaw axes. This reflects the compensation characteristics of the roll direction for left and right tilt disturbances, balancing stability and response speed. These three commands are the final output after superimposing the active stabilization control quantity and the linear displacement disturbance compensation quantity, and are directly injected into the target following control loop.

[0127] The above embodiments are only used to illustrate the technical methods of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical methods of the present invention without departing from the spirit and scope of the technical methods of the present invention.

Claims

1. A three-axis gimbal structure and its stabilization and target following control method, characterized in that, The control method includes: Receive initial image data containing visual features of the target object, the initial image data being acquired by an image acquisition device mounted on the three-axis gimbal structure; The initial image data is segmented into foreground and background to extract the complete contour information of the target object, and the centroid coordinates and bounding box size of the target object in the current image frame are calculated based on the complete contour information. The real-time attitude angle data of the three-axis gimbal structure is obtained from the attitude sensor array. The real-time attitude angle data includes the instantaneous values ​​of pitch angle, roll angle and yaw angle. Based on the centroid coordinates of the target object and the preset image plane center point, calculate the horizontal and vertical pixel deviations of the target from the center of the field of view; By combining the size of the outer rectangular frame with the preset target desired size, the scaling ratio deviation of the target scale is calculated; The horizontal pixel deviation, vertical pixel deviation, scaling ratio deviation of the target object, and the real-time attitude angle data are fused into a multi-dimensional error state vector. Based on the multi-dimensional error state vector, motor drive commands are generated.

2. The three-axis gimbal structure and its stabilization and target following control method according to claim 1, characterized in that, Based on the centroid coordinates of the target object and the preset image plane center point, the lateral and vertical pixel deviations of the target from the center of the field of view are calculated, including: Read the standard x-coordinate and standard y-coordinate values ​​of the preset image plane center point; Read the actual x-coordinate and y-coordinate values ​​from the centroid coordinates of the target object; The arithmetic difference between the actual horizontal coordinate value and the standard horizontal coordinate value is calculated to obtain the horizontal pixel deviation. The arithmetic difference between the actual ordinate value and the standard ordinate value is calculated to obtain the vertical pixel deviation. The calculated horizontal pixel deviation and the vertical pixel deviation are normalized and mapped to a continuous interval from negative one to positive one.

3. The three-axis gimbal structure and its stabilization and target following control method according to claim 2, characterized in that, Based on the outer rectangular frame size and the preset target desired size, the scaling deviation of the target scale is calculated, including: Extract the width and height pixel values ​​of the rectangle from the dimensions of the outer rectangle; Read the desired width and desired height pixel values ​​from the preset target desired size; Calculate the quotient of the width pixel value and the desired width pixel value, and the quotient of the height pixel value and the desired height pixel value, respectively; The geometric mean of the two calculated quotients is used to obtain a comprehensive scale factor. The difference between the overall scale factor and the standard value is calculated to obtain the scaling deviation.

4. The three-axis gimbal structure and its stabilization and target following control method according to claim 3, characterized in that, Based on the multi-dimensional error state vector, motor drive commands are generated, including: The horizontal pixel deviation, vertical pixel deviation, and scaling ratio deviation in the multi-dimensional error state vector are respectively input to an independent inter-axis decoupling controller; Each independent inter-axis decoupling controller calculates the desired angle increment to eliminate the deviation component based on the deviation component it receives; Extract the real-time attitude angle data from the multi-dimensional error state vector; Using the attitude inverse calculation process, the desired angle increment is fused with the real-time attitude angle data and decomposed into the original speed commands for driving the three rotary motors in the three-axis gimbal structure. The original speed command is smoothed and rate-limited to generate the final motor drive command sent to the three rotating motors.

5. A three-axis gimbal structure and its stabilization and target following control method according to claim 4, characterized in that, The process of inverse attitude calculation fuses the desired angle increment with the real-time attitude angle data, decomposing it into original speed commands for driving the three rotary motors in the three-axis gimbal structure, including: Read the current heading angle, pitch angle, and roll angle from the real-time attitude angle data; Read the expected angle increments of the heading axis, pitch axis, and focal length equivalent axis from the expected angle increments; Convert the current heading angle, pitch angle and roll angle into a three-dimensional rotation matrix; The desired angle increments of the heading axis, pitch axis, and focal length equivalent axis are used to construct the desired incremental rotation matrix. The desired incremental rotation matrix and the current three-dimensional rotation matrix are combined using matrix multiplication to calculate the combined rotation matrix. The inverse solution of the synthesized rotation matrix is ​​used to obtain the expected heading angle, expected pitch angle and expected roll angle at the next moment; The instantaneous angular velocity commands of the desired heading angle and the current heading angle, the desired pitch angle and the current pitch angle, and the desired roll angle and the current roll angle are calculated to form the original rotational speed command.

6. The three-axis gimbal structure and its stabilization and target following control method according to claim 5, characterized in that, It also includes active stabilization and vibration compensation processes: High-frequency attitude angle change rate data is continuously acquired from the attitude sensor array. The attitude angle change rate data includes instantaneous values ​​of angular velocity in the pitch, roll and yaw axes. The instantaneous angular velocity values ​​of each axis are subjected to high-pass filtering to extract high-frequency jitter components with frequencies higher than a set threshold. Low-frequency attitude angle drift data is synchronously acquired from the attitude sensor array. The attitude angle drift data includes slow angular offsets in the pitch, roll and yaw axes. The high-frequency jitter component is superimposed with the attitude angle drift data to synthesize an active stabilization control quantity; Read linear acceleration data from the inertial measurement unit and integrate the linear displacement disturbance of the three-axis gimbal structure; Multiply the linear displacement disturbance by the preset lever arm parameters to convert it into the additional angle compensation caused by the translational motion of the carrier. The active stabilization control quantity and the additional angle compensation quantity are summed to obtain the total stabilization compensation command.

7. A three-axis gimbal structure and its stabilization and target following control method according to claim 6, characterized in that, The process of reading linear acceleration data from the inertial measurement device and integrating the linear displacement disturbance of the three-axis gimbal structure includes: Raw linear acceleration data is read from the triaxial accelerometer of the inertial measurement device at fixed time intervals; The original linear acceleration data is processed to remove the gravitational acceleration component, resulting in pure acceleration data of the carrier motion; The pure acceleration data is numerically integrated along three orthogonal axes to obtain the instantaneous velocity components for each axis. The instantaneous velocity component for each axis is then numerically integrated to obtain the instantaneous displacement component for each axis. The instantaneous displacement components of the three axes are combined to form the linear displacement disturbance vector of the three-axis gimbal structure.

8. The three-axis gimbal structure and its stabilization and target following control method according to claim 7, characterized in that, It also includes the process of integrating target-following instructions with active stabilization instructions: Receive the motor drive command generated from claim 4 as the target following command of the main control loop; Receive the total stabilization compensation command generated from claim 6 as the active stabilization command of the feedforward compensation loop; The active stabilization command is injected into the corresponding axial component of the target following command in a vector superposition manner; Saturation protection processing is performed on the synthetic control command for each axis after injection to ensure that the amplitude of the synthetic control command does not exceed the maximum allowable input value of the corresponding axis motor driver; The synthesized control command, after saturation protection processing, is converted into a pulse width modulation signal, which directly drives the three rotary motors of the three-axis gimbal structure to perform actions.

9. A three-axis gimbal structure and its stabilization and target following control method according to claim 8, characterized in that, It also includes the prediction and recapture process for lost targets: Confidence score for monitoring the complete contour information of the target object in consecutive image frames; When the confidence score is lower than the preset recapture threshold, the target loss processing procedure is initiated. Record the trajectory coordinates and velocity vector of the target object in multiple consecutive frames of images before the target is lost; Based on the motion trajectory coordinates and motion velocity vector, the position region of the target object at subsequent time moments is predicted by a linear extrapolation algorithm; The rotating motor of the three-axis gimbal structure is controlled to drive the center of the field of view of the image acquisition device to point to the location area; Within the specified location area, template matching search is performed using the historical visual feature templates of the target object. If the match is successful, the foreground and background segmentation process is restored; if the match fails, the full field of view inspection mode is entered.

10. A three-axis gimbal structure, characterized in that, It includes pitch axis assembly, roll axis assembly, yaw axis assembly, image acquisition device, attitude sensor array, inertial measurement device and control unit; The pitch axis assembly includes a pitch axis motor and a pitch axis frame, wherein the pitch axis motor drives the pitch axis frame to rotate about a horizontal axis; The roll axis assembly includes a roll axis motor and a roll axis frame, the roll axis frame being mounted on the pitch axis frame and driven by the roll axis motor to rotate about a horizontal axis perpendicular to the pitch axis. The heading axis assembly includes a heading axis motor and a heading axis base, the heading axis base being mounted on the roll axis frame and driven by the heading axis motor to rotate about the vertical axis; The image acquisition device is rigidly mounted on the roll axis frame, and its optical axis is orthogonal to the rotation axis of the roll axis frame. The attitude sensor array is mounted on the roll shaft frame and is used to measure the attitude angle and angular velocity of the roll shaft frame. The inertial measurement unit is mounted on the yaw axis base and is used to measure the linear acceleration and angular velocity of the three-axis gimbal structure. The control unit integrates a processor and a memory. The control unit is electrically connected to the pitch axis motor, roll axis motor, yaw axis motor, image acquisition device, attitude sensor array, and inertial measurement device. The memory stores instruction codes. The processor executes the instruction codes to implement the three-axis gimbal structure and its stabilization and target following control method as described in any one of claims 1 to 9.

Images