CN104811588A

CN104811588A - Shipborne image stabilization control method based on gyroscope

Info

Publication number: CN104811588A
Application number: CN201510170010.4A
Authority: CN
Inventors: 董辉; 赖宏焕; 王全强; 黄胜; 陈慧慧
Original assignee: Ismart Video Tech Co ltd; Zhejiang University of Technology ZJUT
Current assignee: Hangzhou Chingan Technology Co ltd; Zhejiang University of Technology ZJUT
Priority date: 2015-04-10
Filing date: 2015-04-10
Publication date: 2015-07-29
Anticipated expiration: 2035-04-10
Also published as: CN104811588B

Abstract

A gyroscope-based ship-mounted image stabilization control method, using a stepping motor-based two-degree-of-freedom ship-mounted security platform with a pitch axis and an azimuth axis as an image stabilization platform, the obtained two different pitch axes The data α _x and ω _y are fused and filtered by Kalman data optimized for calculation and memory, and then the precise pitch angle θ _pitch and pitch angular velocity ω _pitch of the gimbal are obtained, and the roll angle θ _roll of the gimbal is obtained. The obtained θ _pitch is input into the improved PD controller, and the output angular velocity of the pitch axis motor controlling the gimbal is obtained, and the influence of the rolling motion of the hull on the heading angle of the image stabilization platform is solved when the image stabilization system is in an unbalanced state. The effect is compensated by controlling the azimuth angle, and finally the calculated output angular velocity ω _out is transmitted to the underlying stepper motor driver through the serial bus RS232 for execution. The invention can compensate, eliminate and suppress the movement disturbance of the ship-mounted camera system, so as to achieve the purpose of suppressing image shaking during ship-mounted camera.

Description

A Gyroscope-Based Shipborne Image Stabilization Control Method

技术领域technical field

本发明应用于自稳定的摄像控制系统领域，涉及一种适用于基于惯性传感器陀螺仪的船载稳像系统的实时控制方法。The invention is applied to the field of self-stabilized camera control systems, and relates to a real-time control method suitable for a ship-mounted image stabilization system based on an inertial sensor gyroscope.

背景技术Background technique

船载稳定摄像技术是稳像技术的一个衍生领域，涉及传感器数据采集、数据滤波融合、运动控制、电机驱动等多个相关的各类学科。Shipborne stabilized camera technology is a derivative field of image stabilization technology, involving various related disciplines such as sensor data acquisition, data filtering and fusion, motion control, and motor drive.

随着近几年安防监控行业的迅速发展，摄像机不仅在道路，楼宇等固定平台上大量使用，而且广泛的应用于船只、汽车和飞行器等多种运动的载体中。与此同时，应用于这些不稳定载体的摄像机监控都存在一个问题：由于安装的载体存在摇晃，无法保证摄像机视角持续瞄准载体以外的监控目标，并且这个问题在船舰上尤为突出。正是因为船舰监控及安全存在的问题，民用、商用的船舰对装备具有自稳定功能的监控云台的需求越来越强烈。With the rapid development of the security monitoring industry in recent years, cameras are not only widely used on fixed platforms such as roads and buildings, but also widely used in various moving carriers such as ships, automobiles and aircraft. At the same time, there is a problem in the camera surveillance applied to these unstable carriers: due to the shaking of the installed carrier, it is impossible to ensure that the camera's angle of view is continuously aimed at the surveillance target outside the carrier, and this problem is particularly prominent on ships. It is precisely because of the problems of ship monitoring and safety that civilian and commercial ships have an increasingly strong demand for self-stabilizing monitoring pan/tilts.

船舰上稳像技术实现一般有两种方式，其中一种是依靠将船载云台安装在额外添加的稳像平台上隔离船体运动干扰，以达到稳定摄像的效果。另外一种就是利用驱动摄像云台本身的电机以及对图像的处理消除船舰摇晃带来的图像摇晃问题。外加稳像平台的方式相对于直接控制云台电机的方式来说具有安装繁琐、体积庞大、增加成本等弊端。而通过处理图像来达到稳像目的的方式不适用于大角度摇晃的船舰上使用。并且处理方法复杂，对处理器要求很高，设备往往昂贵。一般采用电机运动补偿的控制处理方法太过简单，使用的稳像云台不是具有俯仰轴与方位轴的通用云台。扩展性较差。例如在授权公告号为CN203037261 U，发明名称是“一种小型陀螺仪稳像系统”的实用新型专利中，使用具有俯仰轴与翻滚轴的云台作为控制对象。并且处理方式过于简单，并没有对加速度计与陀螺仪的数据进行进一步处理，会导致精度不足。在步进电机控制方面，步进电机的运动方式是以步进角为最小运动单位θ_u运行的，也就是说，系统具有一个高频的噪声，这对于控制器的微分量u(D)是一个很强的干扰对造成系统的自激扰动。由于没有对步进电机加减速控制方式的优化，很容易造成步进电机的失步越步导致系统不稳定。There are generally two ways to implement image stabilization technology on ships. One of them is to rely on installing the ship-mounted gimbal on an additional image stabilization platform to isolate the interference of the hull motion, so as to achieve the effect of stabilizing the camera. The other is to use the motor that drives the camera head itself and the image processing to eliminate the image shaking problem caused by the shaking of the ship. Compared with the method of directly controlling the gimbal motor, the method of adding an image stabilization platform has disadvantages such as cumbersome installation, bulky size, and increased cost. The method of image stabilization through image processing is not suitable for use on ships that shake at a large angle. Moreover, the processing method is complicated, the processor is highly required, and the equipment is often expensive. Generally, the control processing method using motor motion compensation is too simple, and the image stabilization gimbal used is not a general gimbal with pitch axis and azimuth axis. Poor scalability. For example, in the utility model patent whose authorized announcement number is CN203037261U and the title of the invention is "a small gyroscope image stabilization system", a gimbal with a pitch axis and a roll axis is used as the control object. Moreover, the processing method is too simple, and the data of the accelerometer and gyroscope are not further processed, which will lead to insufficient accuracy. In terms of stepper motor control, the motion of the stepper motor operates with the step angle as the smallest motion unit θ _u , that is to say, the system has a high-frequency noise, which affects the differential value u(D) of the controller is a strong disturbance pair causing self-excited disturbances of the system. Due to the lack of optimization of the stepper motor acceleration and deceleration control mode, it is easy to cause the stepping motor to lose step and step over step, resulting in system instability.

发明内容Contents of the invention

为了克服现有的船载摄像技术的安装复杂、通用性差、精度不足、稳定性不够以及成本过高的不足，本发明提供了一种基于陀螺仪的船载稳像控制方法，在保证稳像控制的精度和系统稳定性性的同时，又具有体积小、通用性好、低成本等特点。In order to overcome the shortcomings of the existing ship-mounted camera technology, such as complicated installation, poor versatility, insufficient precision, insufficient stability and high cost, the present invention provides a gyroscope-based ship-mounted image stabilization control method, which can ensure stable image While controlling precision and system stability, it also has the characteristics of small size, good versatility, and low cost.

本发明解决上述技术问题是通过以下技术方案实现的：The present invention solves the problems of the technologies described above and is achieved through the following technical solutions:

一种基于陀螺仪的船载稳像控制方法，所述方法包括如下步骤：A gyroscope-based shipborne image stabilization control method, said method comprising the steps of:

1)使用具有俯仰轴与方位轴的基于步进电机的二自由度船载安防云台作为稳像平台；1) Use a stepper motor-based two-degree-of-freedom ship-mounted security pan-tilt with a pitch axis and an azimuth axis as an image stabilization platform;

2)利用加速度计和陀螺仪两种传感器分别获得云台的三轴加速度α_x，α_y，α_z及其三轴角速度ω_x、ω_y、ω_z；2) Obtain the three-axis acceleration α _x , α _y , α _z and its three-axis angular velocity ω _x , ω _y , ω _z of the gimbal by using two sensors, the accelerometer and the gyroscope;

3)将获得的这两种不同的俯仰轴上的数据α_x与ω_y进行对运算量以及内存优化的卡尔曼数据融合滤波，进而获得云台的精确俯仰角θ_pitch以及俯仰角速度ω_pitch，同样的方式求得云台的翻滚角θ_roll；3) The obtained data α _x and ω _y on the two different pitch axes are fused and filtered on the computational load and memory-optimized Kalman data, and then the precise pitch angle θ _pitch and pitch angular velocity ω _pitch of the gimbal are obtained, Obtain the roll angle θ _roll of the gimbal in the same way;

4)利用得到的俯仰角θ_pitch输入改进的PD控制器中，输出控制云台的俯仰轴电机的期望角速度值；4) Input the obtained pitch angle θ _pitch into the improved PD controller, and output the expected angular velocity value of the pitch axis motor controlling the gimbal;

所述改进的PD控制器中，PD算法根据稳像云台的系统特点进行如下的改进步骤：In the improved PD controller, the PD algorithm performs the following improvement steps according to the system characteristics of the image stabilization platform:

error(k)＝θ_pitch-θ_target (6)error(k) = θ _pitch - θ _target (6)

将式(6)得到的采样序号为k时刻的偏差数据error(k)代入式(7)离散PD控制器中得到控制量输出值u(k)Substituting the deviation data error(k) obtained by the formula (6) into the discrete PD controller of the formula (7) to obtain the output value u(k) of the control quantity

$u u ((k k)) = = {k k}_{p p} ((error error ((k k)))) + + {k k}_{d d} \frac{error error ((k k)) - - error error ((k k - - 11))}{T T} - - - - - - ((77))$

式中，error(k-1)是采样序号为k-1时刻的偏差数据；In the formula, error(k-1) is the deviation data at the time when the sampling number is k-1;

控制器限幅输出： $\{\begin{matrix} u (k) = Max_U (k) & u (k) > Max_U (k) \\ u (k) = Min_U (k) & u (k) < Min_U (k) \end{matrix} - - - (8)$ Controller limit output: $\{\begin{matrix} u (k) = Max_u (k) & u (k) > Max_u (k) \\ u (k) = Min_u (k) & u (k) < Min_u (k) \end{matrix} - - - (8)$

当PD控制器输出大于Max_U(k)时，系统输出Max_U(k)，同理当PD控制器输出大于Min_U(k)时，系统输出Min_U(k)；When the output of the PD controller is greater than Max_U(k), the system outputs Max_U(k). Similarly, when the output of the PD controller is greater than Min_U(k), the system outputs Min_U(k);

根据不同的error(k)对k_p参数作出改变，对k_p与error(k)建立基于指数函数的变化关系，实时改变系统的增益如下式(9)所示：Change the k _p parameter according to different error(k), establish the change relationship between k _p and error(k) based on the exponential function, and change the gain of the system in real time as shown in the following formula (9):

$\{\begin{matrix} {k k}_{p p} = = {αe αe}^{β β * * error error ((k k))} & error error ((k k)) > > 00 \\ {k k}_{p p} = = α α & error error ((k k)) = = 00 \\ {k k}_{p p} = = α α {(({e e}^{- - 11}))}^{β β * * error error ((k k))} & error error ((k k)) < < 00 \end{matrix} - - - - - - ((99))$

使用不完全微分的PD算法，在原来的微分量u_d(k)项中引入一阶低通滤波器，如式(10)所示：Using the incomplete differential PD algorithm, a first-order low-pass filter is introduced into the original differential component u _d (k), as shown in formula (10):

u_d(k)＝k_d(1-α)(error(k)-error(k-1))+αu_d(k-1) (10)u _d (k)=k _d (1-α)(error(k)-error(k-1))+αu _d (k-1) (10)

同时结合了目标角改变微分消除算法对θ_target改变时带来的微分量不代入计算，如式(11)所示：At the same time, combined with the target angle change differential elimination algorithm, the differential value brought by the change of θ _target is not substituted into the calculation, as shown in formula (11):

u_d(k)＝u_d(k)-k_d*[θ_target(k)-θ_target(k-1)] (11)u _d (k)=u _d (k)-k _d *[θ _target (k)-θ _target (k-1)] (11)

5)解算稳像系统在非平衡状态下，船体出现翻滚运动对稳像平台航向角的影响，通过控制方位角补偿该影响；如式(14)所示：5) Solve the effect of the rolling motion of the ship on the course angle of the image stabilization platform under the unbalanced state of the image stabilization system, and compensate the effect by controlling the azimuth angle; as shown in formula (14):

式中，是k时刻云台方位角，为k-1时刻云台方位角，θ_pitch(k)为k时刻的俯仰角，θ_roll(k)为k时刻的翻滚角，sin()为正弦运算，asin()为反正弦运算；In the formula, is the azimuth angle of the pan-tilt at time k, is the azimuth angle of the gimbal at time k-1, θ _pitch (k) is the pitch angle at time k, θ _roll (k) is the roll angle at time k, sin() is sine operation, asin() is arc sine operation;

6)最后将计算结果传输到底层步进电机驱动器。6) Finally, the calculation result is transmitted to the underlying stepper motor driver.

进一步，所述步骤4)中，在PD控制器的后级加上步进电机非线性加减速控制器；步进电机的功率、扭矩和转速是相关联的，具体关系为：Further, in the step 4), a stepping motor nonlinear acceleration and deceleration controller is added to the rear stage of the PD controller; the power, torque and rotating speed of the stepping motor are associated, and the specific relationship is:

P＝α*Torque*ω (12)P＝α*Torque*ω (12)

式中P为步进电机功率，α为转换系数，Torque是电机的扭矩，ω是电机转速；In the formula, P is the power of the stepping motor, α is the conversion coefficient, Torque is the torque of the motor, and ω is the speed of the motor;

当前允许最大加速度值与当前速度呈线性相关的关系：The current allowed maximum acceleration value is linearly related to the current speed:

$\{\begin{matrix} {a a}_{max max} = = α α * * ω ω + + b b \\ {U u}_{out out} = = {u u}_{PD PD} ((k k - - 11)) + + {a a}_{max max} & (({u u}_{PD PD} ((k k)) > > {u u}_{PD PD} ((k k - - 11)) + + {a a}_{max max})) \\ {U u}_{out out} = = {u u}_{PD PD} ((k k)) & (({u u}_{PD PD} ((k k)) \leq \leq {u u}_{PD PD} ((k k - - 11)) + + {a a}_{max max})) \end{matrix} - - - - - - ((1313))$

PD控制器的输出角速度结果经过与后级的步进电机加减速控制相比较，选择以后输出到步进电机上，检验过程中系统对比PD算法获得的速度与步进电机加减速控制算法计算得出的最大速度，并且始终选择两者的较小数值。The output angular velocity of the PD controller is compared with the stepping motor acceleration and deceleration control of the subsequent stage, and then output to the stepping motor after selection. During the inspection process, the system compares the speed obtained by the PD algorithm with the stepping motor acceleration and deceleration control algorithm. The maximum speed that can be obtained, and always choose the smaller value of the two.

再进一步，在步骤3)中，所述的卡尔曼融合算法过程如下：Further, in step 3), the described Kalman fusion algorithm process is as follows:

首先建立系统的状态方程：First establish the state equation of the system:

$[\begin{matrix} {angle the angle}_{k k} \\ q q__{bias bias}_{k k} \end{matrix}] = = [\begin{matrix} 11 & - - dt dt \\ 00 & 11 \end{matrix}] [\begin{matrix} {angle the angle}_{k k - - 11} \\ q q__{bias bias}_{k k - - 11} \end{matrix}] + + [\begin{matrix} gyro gyro__m m * * dt dt \\ 00 \end{matrix}] + + [\begin{matrix} w w__angle the angle \\ w w__gyro gyro \end{matrix}]$

上式中angle_k是k时刻角度值，q_bias_k是陀螺仪的偏差，dt是更新周期，gyro_m是陀螺仪的过程噪声，w_angle和w_gyro分别是是加速度计与陀螺仪的测量噪声；In the above formula, angle _k is the angle value at time k, q_bias _k is the bias of the gyroscope, dt is the update period, gyro_m is the process noise of the gyroscope, and w_angle and w_gyro are the measurement noise of the accelerometer and gyroscope respectively;

建立测量方程： ${angle}_{k} = [\begin{matrix} 1 & 0 \end{matrix}] [\begin{matrix} {angle}_{k} \\ q_{bias}_{k} \end{matrix}] + v_angle$ Create a measurement equation: ${the angle}_{k} = [\begin{matrix} 1 & 0 \end{matrix}] [\begin{matrix} {the angle}_{k} \\ q_{bias}_{k} \end{matrix}] + v_the angle$

构造过程噪声矩阵： $[\begin{matrix} Q_angle & 0 \\ 0 & Q_gyro \end{matrix}]$ Construct the process noise matrix: $[\begin{matrix} Q_the angle & 0 \\ 0 & Q_gyro \end{matrix}]$

构造测量噪声矩阵：[R_angle]Construct measurement noise matrix: [R_angle]

角度预测：Angular projections:

angle＝angle-q_bias_k*dt+gyro_m*dt＝angle+Rate*dtangle=angle-q_bias _k *dt+gyro _m *dt=angle+Rate*dt

方差预测：Variance prediction:

$\begin{matrix} {P P}_{k k | | k k - - 11} = = [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] \\ = = [\begin{matrix} 11 & - - dt dt \\ 00 & 11 \end{matrix}] [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] [\begin{matrix} 11 & - - dt dt \\ 00 & 11 \end{matrix}] + + [\begin{matrix} Q Q__angle the angle & 00 \\ 00 & Q Q__gyro gyro \end{matrix}] \end{matrix} - - - - - - ((11))$

角度误差更新：angle＝incAngle-angle (2)Angle error update: angle=incAngle-angle (2)

计算卡尔曼增益：Compute the Kalman gain:

$\begin{matrix} [\begin{matrix} K K__00 \\ K K__11 \end{matrix}] = = [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] [\begin{matrix} 00 \\ 11 \end{matrix}] (([\begin{matrix} 11 & 00 \end{matrix}] [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] [\begin{matrix} 00 \\ 11 \end{matrix}] + + {R R}_{angle the angle} {))}^{- - 11} \\ = = [\begin{matrix} P P 0000 \\ P P 1010 \end{matrix}] / / ((P P 0000 + + R R__angle the angle)) \end{matrix} - - - - - - ((33))$

方差更新：Variance update:

$\begin{matrix} {P P}_{k k | | k k} = = [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] = = ((E E. - - [\begin{matrix} K K__00 \\ K K__11 \end{matrix}] [\begin{matrix} 11 & 00 \end{matrix}])) {P P}_{k k | | k k - - 11} \\ = = [\begin{matrix} P P 0000 - - K K__00 * * P P 0000 & P P 0101 - - K K__00 * * P P 0101 \\ P P 1010 - - K K__11 * * P P 0000 & P P 1111 - - K K__11 * * P P 0101 \end{matrix}] \end{matrix} - - - - - - ((44))$

状态估计： $[\begin{matrix} {angle}_{k} \\ q_{bias}_{k} \end{matrix}] = [\begin{matrix} {angle}_{k - 1} \\ q_{bias}_{k - 1} \end{matrix}] + [\begin{matrix} K_0 \\ K_1 \end{matrix}] * angle_err - - - (5)$ State estimation: $[\begin{matrix} {the angle}_{k} \\ q_{bias}_{k} \end{matrix}] = [\begin{matrix} {the angle}_{k - 1} \\ q_{bias}_{k - 1} \end{matrix}] + [\begin{matrix} K_0 \\ K_1 \end{matrix}] * the angle_err - - - (5)$

重复计算公式(1)～(5)直至找到最优的结果。Repeat calculation formulas (1) to (5) until the optimal result is found.

本发明具有的有益效果：使用更为通用的基于步进电机的两轴(俯仰轴与方位轴)船载云台作为控制对象，更具通用性；采用低成本的微机械工艺的陀螺仪与加速度计作为传感器，降低成本；通过微控制器采集两者的数据并使用卡尔曼滤波器，将两者的数据进行迭代融合，得到更为精确并且抗干扰的云台角度信息；将云台角度数据输入经过增益以及微分优化以后的PD控制器，将控制器输出结果输入到一种新型的非线性步进电机控制器最终得到云台俯仰角的输出信号，系统更加稳定快速；通过俯仰角与翻滚角计算得出方位角的修正角度，能够进一步的抑制图像晃动。本发明不仅能保证船载稳像平台的跟踪精度和跟踪快速性，又具有强稳定性和抗干扰能力。The present invention has beneficial effects: using a more general two-axis (pitch axis and azimuth axis) ship-mounted cloud platform based on a stepping motor as the control object is more versatile; the gyroscope and the low-cost micromechanical process are adopted. The accelerometer is used as a sensor to reduce costs; the data of the two is collected by the microcontroller and the Kalman filter is used to iteratively fuse the data of the two to obtain more accurate and anti-jamming angle information of the pan/tilt; the angle of the pan/tilt The data is input to the PD controller after gain and differential optimization, and the output result of the controller is input to a new type of nonlinear stepping motor controller to finally obtain the output signal of the pitch angle of the pan/tilt. The system is more stable and fast; through the pitch angle and The roll angle is calculated to obtain the correction angle of the azimuth angle, which can further suppress image shaking. The invention not only can ensure the tracking accuracy and tracking speed of the ship-borne image stabilization platform, but also has strong stability and anti-interference ability.

附图说明Description of drawings

图1为船载稳像控制框图。Figure 1 is a block diagram of ship-borne image stabilization control.

图2为两轴云台结构图。Figure 2 is a structural diagram of the two-axis gimbal.

图3为卡尔曼滤波框图。Figure 3 is a block diagram of the Kalman filter.

图4为卡尔曼滤波效果图。Figure 4 is a Kalman filter effect diagram.

图5为俯仰轴控制框图。Figure 5 is a block diagram of pitch axis control.

图6为方位轴控制框图。Figure 6 is a block diagram of the azimuth axis control.

具体实施方式Detailed ways

为使本发明的目的、技术方案和优点更加清晰，下面结合附图对本发明的技术方案作进一步描述。In order to make the object, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be further described below in conjunction with the accompanying drawings.

参照图1～图6：一种基于陀螺仪的船载稳像控制的方法，所述方法包括如下步骤：Referring to Figures 1 to 6: a method for gyroscope-based shipborne image stabilization control, the method includes the following steps:

使用具有俯仰轴与方位轴两个自由度的船载安防云台作为稳像平台。A ship-mounted security pan/tilt with two degrees of freedom of the pitch axis and the azimuth axis is used as the image stabilization platform.

如图1所示，使用微机械工艺的三轴陀螺仪加速度计作为传感器，通过使用高性能的ARM微控制器采集其数据。通过控制算法计算出系统输出，采用串行通信方式将数据发送至步进电机驱动器。两轴云台的结构如图2所示。包含方位轴与俯仰轴，传感器与微控制器都安装在摄像机芯上。As shown in Figure 1, a three-axis gyroscope accelerometer using micro-mechanical technology is used as a sensor, and its data is collected by using a high-performance ARM microcontroller. The system output is calculated by the control algorithm, and the data is sent to the stepper motor driver by means of serial communication. The structure of the two-axis gimbal is shown in Figure 2. Including the azimuth axis and the pitch axis, the sensor and microcontroller are installed on the camera core.

如图3所示，本发明利用加速度计和陀螺仪两种传感器分别获得云台的加速度α_x，α_y，α_z及其角速度ω_x、ω_y、ω_z。As shown in FIG. 3 , the present invention uses accelerometers and gyroscopes to obtain the accelerations α _x , α _y , α _z and angular velocities ω _x , ω _y , ω _z of the pan/tilt respectively.

将获得的这两种不同的俯仰轴上的数据α_x与ω_y进行简化的卡尔曼数据融合滤波，进而获得云台的精确俯仰角θ_pitch以及俯仰角速度ω_pitch，同样的方式可以求得云台的翻滚角θ_roll。所述方法包含如下步骤：The data α _x and ω _y obtained on these two different pitch axes are subjected to simplified Kalman data fusion filtering, and then the precise pitch angle θ _pitch and pitch angular velocity ω _pitch of the gimbal can be obtained. In the same way, the cloud The roll angle θ _roll of the stage. Described method comprises the steps:

上式中angle_k是k时刻角度值，q_bias_k是陀螺仪的偏差，dt是更新周期，gyro_m是陀螺仪的过程噪声，w_angle和w_gyro分别是是加速度计与陀螺仪的测量噪声。In the above formula, angle _k is the angle value at time k, q_bias _k is the bias of the gyroscope, dt is the update period, gyro_m is the process noise of the gyroscope, and w_angle and w_gyro are the measurement noise of the accelerometer and gyroscope respectively.

角度预测：Angular projections:

方差预测：Variance prediction:

计算卡尔曼增益(Kalman Gain)：Compute the Kalman Gain:

方差更新(预测方差和方差可以共用内存)：Variance update (forecast variance and variance can share memory):

重复计算步骤(1)～(5)直至找到最优的结果。Repeat calculation steps (1) to (5) until the optimal result is found.

图4中，蓝色的曲线是加速度计得到的角度波形，紫色的是陀螺仪积分后的角度波形，黄色曲线为融合后的角度波形。In Figure 4, the blue curve is the angle waveform obtained by the accelerometer, the purple curve is the angle waveform after gyroscope integration, and the yellow curve is the angle waveform after fusion.

由上图可知陀螺仪有较大的噪声，在角速度积分后得到的角度会发生偏移，并且陀螺仪是温度敏感器件在温度发生变化时，陀螺仪的零点也会发生变化。另一方面，加速度计经过转换以后得到的角度的噪声更大，峰值可以达到0.1度的误差。而且在受到姿态扰动时会出现很大运动噪声，一般会达到5-10度，因此也无法直接使用。而通过卡尔曼滤波将两个数据融合之后，得到的角度既有陀螺仪积分后的角度的精度，也有加速度计不容易发生漂移的特性。It can be seen from the above figure that the gyroscope has a large noise, and the angle obtained after integrating the angular velocity will shift, and the gyroscope is a temperature-sensitive device. When the temperature changes, the zero point of the gyroscope will also change. On the other hand, the angle obtained by the accelerometer after conversion is more noisy, and the peak value can reach an error of 0.1 degrees. Moreover, when the attitude is disturbed, there will be a lot of motion noise, generally reaching 5-10 degrees, so it cannot be used directly. After the two data are fused through the Kalman filter, the obtained angle has the accuracy of the angle integrated by the gyroscope and the characteristic that the accelerometer is not easy to drift.

系统的俯仰轴框图如图5所示。当载体在相对惯性空间产生运动时，将会通过耦合带动平台框架一起运动。在不考虑干扰力矩和随机误差时，陀螺仪输出敏感到的平台框架的角速度信号，加速度计输出加速度信号经过反三角运算得到云台姿态，这两个传感器数据经过卡尔曼滤波的数据融合之后得到俯仰角的角度。得到的姿态角度与目标角度作差得到系统误差，负反馈到控制系统的输入端。经过改进的PD控制器校正和非线性加速控制器的修正，在驱动器放大后驱动平台的步进电机。电机产生相应的反方向扭转力矩，带动平台框架在与载体转动的相反方向上进行速率补偿，直到系统的误差信号为零，平台框架恢复到原有位置，从而实现了平台的稳定功能。The pitch axis block diagram of the system is shown in Figure 5. When the carrier moves in the relative inertial space, it will drive the platform frame to move together through coupling. When the disturbance torque and random error are not considered, the gyroscope outputs the angular velocity signal of the platform frame that is sensitive to it, and the accelerometer outputs the acceleration signal to obtain the attitude of the pan/tilt through inverse triangulation. The angle of the pitch angle. The difference between the obtained attitude angle and the target angle is obtained to obtain a system error, which is negatively fed back to the input end of the control system. After the correction of the improved PD controller and the correction of the nonlinear acceleration controller, the stepper motor of the platform is driven after the driver is amplified. The motor generates a corresponding reverse torque, which drives the platform frame to perform speed compensation in the opposite direction to the carrier rotation, until the system error signal is zero, and the platform frame returns to its original position, thereby realizing the stable function of the platform.

本发明利用得到的θ_pitch对云台的俯仰轴进行电机控制稳定俯仰角，采用改进的PD控制器。The present invention uses the obtained θ _pitch to perform motor control on the pitch axis of the platform to stabilize the pitch angle, and adopts an improved PD controller.

error(k)＝θ_pitch-θ_target (6)error(k) = θ _pitch - θ _target (6)

将式(6)得到的error(k)代入式(7)离散PD控制器中得到控制量输出值u(k)。Substitute the error(k) obtained in formula (6) into the discrete PD controller in formula (7) to obtain the control output value u(k).

式中error(k-1)是采样序号为k-1时刻的偏差数据。In the formula, error(k-1) is the deviation data at the time when the sampling number is k-1.

为增加系统的稳定性以及性能，对经典PD算法作以下的改进。In order to increase the stability and performance of the system, the following improvements are made to the classic PD algorithm.

当PD控制器输出大于Max_U(k)时，系统输出Max_U(k)，同理当PD控制器输出大于Min_U(k)时，系统输出Min_U(k)，保证系统稳定性。When the PD controller output is greater than Max_U(k), the system outputs Max_U(k). Similarly, when the PD controller output is greater than Min_U(k), the system outputs Min_U(k) to ensure system stability.

本发明根据不同的error(k)对k_p参数作出改变，是的系统响应又快又稳定。对k_p与error(k)建立基于指数函数的变化关系。实时改变系统的增益如下式9所示。The present invention changes the k _p parameter according to different error(k), so that the system response is fast and stable. Establish a change relationship based on an exponential function for k _p and error(k). Change the gain of the system in real time as shown in Equation 9 below.

使用不完全微分的PD算法，在原来的微分量u_d(k)项中引入一阶低通滤波器。如式(10)所示。Using the incompletely differentiated PD algorithm, a first-order low-pass filter is introduced into the original differential value u _d (k) term. As shown in formula (10).

同时结合了微分先行算法对θ_target改变时带来的微分量不代入计算，减少频繁改变稳像角带来的不稳定因素。如式(11)所示。At the same time, combined with the differential advance algorithm, the differential value brought about by the change of θ _target is not substituted into the calculation, reducing the unstable factors caused by frequent changes in the stabilization angle. As shown in formula (11).

在PD控制器的后级加上非线性加减速控制器，从而提高云台的俯仰角稳定性。进电机的功率、扭矩和转速是相关联的，具体关系为：A nonlinear acceleration and deceleration controller is added after the PD controller to improve the pitch angle stability of the gimbal. The power, torque and speed of the motor are related, and the specific relationship is:

P＝α*Torque*ω (12)P＝α*Torque*ω (12)

式中P为步进电机功率，α为转换系数，Torque是电机的扭矩，ω是电机转速。于是本发明提出一种限制加速度的步进电机控制方法，并且当前允许最大加速度值与当前速度呈线性相关的关系。In the formula, P is the power of the stepping motor, α is the conversion coefficient, Torque is the torque of the motor, and ω is the speed of the motor. Therefore, the present invention proposes a stepper motor control method that limits acceleration, and the current allowable maximum acceleration value is linearly related to the current speed.

PD控制器的输出角速度结果经过与后级的步进电机加减速控制相比较选择以后输出到步进电机上。检验过程中系统对比PD算法获得的速度与步进电机加减速控制算法计算得出的最大速度，并且始终选择两者的较小数值。The output angular velocity result of the PD controller is output to the stepping motor after being compared with the stepping motor acceleration and deceleration control of the subsequent stage and selected. During the inspection process, the system compares the speed obtained by the PD algorithm with the maximum speed calculated by the stepper motor acceleration and deceleration control algorithm, and always selects the smaller value of the two.

从而抑制俯仰轴摇晃对图像的干扰。Thereby suppressing the interference of the pitch axis shaking on the image.

如图6中所示，当系统受到除了俯仰角以外的翻滚轴扰动的影响，视轴在水平方向会受到干扰。本发明通过检测当前的俯仰与翻滚角解算出当前需要修正的角度进行补偿。解算稳像系统在非平衡状态下，船体出现翻滚运动对稳像平台航向角的影响。通过控制方位角补偿该影响，从而有效的抑制这种干扰，提高了图像稳定性。算法如式(14)所示。As shown in Fig. 6, when the system is affected by disturbances in the roll axis other than the pitch angle, the boresight axis will be disturbed in the horizontal direction. The present invention calculates the current angle to be corrected by detecting the current pitch and roll angle to compensate. Solve the effect of the roll motion of the hull on the course angle of the image stabilization platform when the image stabilization system is in an unbalanced state. This effect is compensated by controlling the azimuth angle, thereby effectively suppressing this interference and improving image stability. The algorithm is shown in formula (14).

式中是k时刻云台方位角，为k-1时刻云台方位角，θ_pitch(k)为k时刻的俯仰角，θ_roll(k)为k时刻的翻滚角。sin()为正弦运算，asin()为反正弦运算。In the formula is the azimuth angle of the pan-tilt at time k, is the azimuth angle of the gimbal at time k-1, θ _pitch (k) is the pitch angle at time k, and θ _roll (k) is the roll angle at time k. sin() is a sine operation, and asin() is an arcsine operation.

最后将计算结果通过RS232传输到底层步进电机驱动器。Finally, the calculation results are transmitted to the underlying stepper motor driver through RS232.

如此，船载摄像系统的运动扰动被补偿消除与抑制，从而达到抑制船载摄像时图像晃动的目的。In this way, the motion disturbance of the shipboard camera system is compensated, eliminated and suppressed, so as to achieve the purpose of suppressing image shaking during the shipboard camera system.

Claims

1. A gyroscope-based shipborne image stabilization control method, characterized in that: the method may further comprise the steps:

1) Use a stepper motor-based two-degree-of-freedom ship-mounted security pan-tilt with a pitch axis and an azimuth axis as an image stabilization platform;

2) Obtain the three-axis acceleration α _x , α _y , α _z and its three-axis angular velocity ω _x , ω _y , ω _z of the gimbal by using two sensors, the accelerometer and the gyroscope;

3) The obtained data α _x and ω _y on the two different pitch axes are fused and filtered on the computational load and memory-optimized Kalman data, and then the precise pitch angle θ _pitch and pitch angular velocity ω _pitch of the gimbal are obtained, Obtain the roll angle θ _rou of the gimbal in the same way;

4) Input the obtained pitch angle θ _pitch into the improved PD controller, and output the expected angular velocity value of the pitch axis motor controlling the gimbal;

In the improved PD controller, the PD algorithm performs the following improvement steps according to the system characteristics of the image stabilization platform:

error(k) = θ _pitch - θ _target (6)

Substituting the deviation data error(k) obtained by the formula (6) into the discrete PD controller of the formula (7) to obtain the output value u(k) of the control quantity

u u ((k k)) = = {k k}_{p p} ((error error ((k k)))) + + {k k}_{d d} \frac{error error ((k k)) - - error error ((k k - - 11))}{T T} - - - - - - ((77))

In the formula, error(k-1) is the deviation data at the time when the sampling number is k-1;

Controller limit output:

\{\begin{matrix} u (k) = Max_u (k) & u (k) > Max_u (k) \\ u (k) = Min_u (k) & u (k) < Min_u (k) \end{matrix} - - - (8)

When the output of the PD controller is greater than Max_U(k), the system outputs Max_U(k). Similarly, when the output of the PD controller is greater than Min_U(k), the system outputs Min_U(k);

Change the k _p parameter according to different error(k), establish the change relationship between k _p and error(k) based on the exponential function, and change the gain of the system in real time as shown in the following formula (9):

\{\begin{matrix} {k k}_{p p} = = {αe αe}^{β β * * error error ((k k))} & error error ((k k)) > > 00 \\ {k k}_{p p} = = α α & error error ((k k)) = = 00 \\ {k k}_{p p} = = α α {(({e e}^{- - 11}))}^{β β * * error error ((k k))} & error error ((k k)) < < 00 \end{matrix} - - - - - - ((99))

Using the incomplete differential PD algorithm, a first-order low-pass filter is introduced into the original differential component u _d (k), as shown in formula (10):

u _d (k)=k _d (1-α)(error(k)-error(k-1))+αu _d (k-1) (10)

At the same time, combined with the target angle change differential elimination algorithm, the differential value brought by the change of θ _target is not substituted into the calculation, as shown in formula (11):

u _d (k)=u _d (k)-k _d *[θ _target (k)-θ _target (k-1)] (11)

5) Solve the effect of the rolling motion of the ship on the course angle of the image stabilization platform under the unbalanced state of the image stabilization system, and compensate the effect by controlling the azimuth angle; as shown in formula (14):

In the formula, is the azimuth angle of the pan-tilt at time k, is the azimuth angle of the gimbal at time k-1, θ _pitch (k) is the pitch angle at time k, θ _roll (k) is the roll angle at time k, sin() is sine operation, asin() is arc sine operation;

6) Finally, the calculation result is transmitted to the underlying stepper motor driver.

2. a kind of gyroscope-based shipborne image stabilization control method as claimed in claim 1, is characterized in that: in described step 4), add stepper motor non-linear acceleration and deceleration control at the rear stage of PD controller The power, torque and speed of the stepper motor are related, and the specific relationship is:

P=α-Torque*ω (12)

In the formula, P is the power of the stepping motor, α is the conversion coefficient, Torque is the torque of the motor, and ω is the speed of the motor;

The current allowed maximum acceleration value is linearly related to the current speed:

\{\begin{matrix} {a a}_{max max} = = α α * * ω ω + + b b \\ {U u}_{out out} = = {u u}_{PD PD} ((k k - - 11)) + + {a a}_{max max} & (({u u}_{PD PD} ((k k)) > > {u u}_{PD PD} ((k k - - 11)) + + {a a}_{max max})) \\ {U u}_{out out} = = {u u}_{PD PD} ((k k)) & (({u u}_{PD PD} ((k k)) \leq \leq {u u}_{PD PD} ((k k - - 11)) + + {a a}_{max max})) \end{matrix} - - - - - - ((1313))

The output angular velocity result of the PD controller is compared with the acceleration and deceleration control of the stepping motor in the subsequent stage, and then output to the stepping motor after selection. During the inspection process, the system compares the speed obtained by the PD algorithm with the acceleration and deceleration control algorithm of the stepping motor. The maximum speed that can be obtained, and always choose the smaller value of the two.

3. a kind of gyroscope-based shipborne image stabilization control method as claimed in claim 1 or 2, is characterized in that: in step 3) in, described Kalman fusion algorithm process is as follows:

First establish the state equation of the system:

[\begin{matrix} {angle the angle}_{k k} \\ q q__{bias bias}_{k k} \end{matrix}] = = [\begin{matrix} 11 & - - dt dt \\ 00 & 11 \end{matrix}] [\begin{matrix} {angle the angle}_{k k - - 11} \\ q q__{bias bias}_{k k - - 11} \end{matrix}] + + [\begin{matrix} gyro gyro__m m * * dt dt \\ 00 \end{matrix}] + + [\begin{matrix} w w__angle the angle \\ w w__gyro gyro \end{matrix}]

In the above formula, angle _k is the angle value at time k, q_bias _k is the bias of the gyroscope, dt is the update period, gyro_m is the process noise of the gyroscope, and w_angle and w_gyro are the measurement noise of the accelerometer and gyroscope respectively;

Create a measurement equation:

{the angle}_{k} = [\begin{matrix} 1 & 0 \end{matrix}] [\begin{matrix} {the angle}_{k} \\ q_{bias}_{k} \end{matrix}] + v_the angle

Construct the process noise matrix:

[\begin{matrix} Q_the angle & 0 \\ 0 & Q_gyro \end{matrix}]

Construct measurement noise matrix: [R_angle]

Angular projections:

angle=angle-q_bias _k *dt+gyro _m *dt=angle+Rate*dt

Variance prediction:

\begin{matrix} {P P}_{k k | | k k - - 11} = = \begin{matrix}  \end{matrix} [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] \\ = = [\begin{matrix} 11 & - - dt dt \\ 00 & 11 \end{matrix}] [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] [\begin{matrix} 11 & - - dt dt \\ 00 & 11 \end{matrix}] + + [\begin{matrix} Q Q__angle the angle & 00 \\ 00 & Q Q__gyro gyro \end{matrix}] \end{matrix} - - - - - - ((11))

Angle error update: angle=incAngle-angle (2)

Compute the Kalman gain:

\begin{matrix} [\begin{matrix} K K__00 \\ K K__11 \end{matrix}] = = [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] [\begin{matrix} 00 \\ 11 \end{matrix}] {(\begin{matrix} [\begin{matrix} 11 & 00 \end{matrix}] [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] [\begin{matrix} 00 \\ 11 \end{matrix}] {+ +}_{{R R}_{angle the angle}} \end{matrix})}^{- - 11} \\ = = [\begin{matrix} P P 0000 \\ P P 1010 \end{matrix}] / / ((P P 0000 + + R R__angle the angle)) \end{matrix} - - - - - - ((33))

Variance update:

\begin{matrix} {P P}_{k k | | k k} = = \begin{matrix}  \end{matrix} [\begin{matrix} P P 0000 & P P 0101 \\ P P 1010 & P P 1111 \end{matrix}] = = (\begin{matrix} E E. - - [\begin{matrix} K K__00 \\ K K__11 \end{matrix}] [\begin{matrix} 11 & 00 \end{matrix}] \end{matrix}) {P P}_{k k | | k k - - 11} \\ = = [\begin{matrix} P P 0000 - - K K__00 * * P P 0000 & P P 0101 - - K K__00 * * P P 0101 \\ P P 1010 - - K K__11 * * P P 0000 & P P 1111 - - K K__11 * * P P 0101 \end{matrix}] \end{matrix} - - - - - - ((44))

State estimation:

[\begin{matrix} {the angle}_{k} \\ q_{bias}_{k} \end{matrix}] = [\begin{matrix} {the angle}_{k - 1} \\ {q_bias}_{k - 1} \end{matrix}] + [\begin{matrix} K_0 \\ K_1 \end{matrix}] * the angle_err - - - (5)

Repeat calculation formulas (1) to (5) until the optimal result is found.