CN104811588A - Shipborne image stabilization control method based on gyroscope - Google Patents

Abstract

Disclosed is a shipborne image stabilization control method based on a gyroscope. A two-degree of freedom shipborne security and protection pan-tilt with a pitch axis and an azimuth axis on basis of stepping motors is used as an image stabilization platform, two different obtained data alfa x and omega y on the pitch axis are subjected to Kalman data fusion filtering that can optimize operation amount and internal storage, and accurate pitch angle theta pitch, pitch angular velocity omega pitch and roll angle theta roll of the pan-tilt are obtained. The obtained theta pitch is input to an improved PD controller to obtain output angular velocity which can control the motor of the pitch axis of the pan-tilt, and influence of rolling movement of a ship body on course angle of the image stabilization platform can be resolved when an image stabilization system is in a non-balance state. The influence is compensated through controlling over the azimuth angle, and finally, the calculated output angular velocity omega out is transmitted to a bottom layer stepping motor driver through a serial bus RS232 for execution. By means of the shipborne image stabilization control method based on the gyroscope, movement disturbance of a shipborne image pickup system can be compensated, eliminated and inhibited, and thereby, purpose of inhibiting image shaking during shipborne image pickup can be achieved.

Description

A kind of based on gyrostatic boat-carrying steady picture control method

Technical field

The present invention is applied to the shooting control system field of self-stabilization, relates to a kind of real-time control method be applicable to based on the gyrostatic boat-carrying image stabilization system of inertial sensor.

Background technology

Boat-carrying stablizes the derivative field that camera technique is steady picture technology, relates to multiple relevant all kinds of subjects such as sensor data acquisition, data filtering fusion, motion control, motor driving.

Along with developing rapidly of safety monitoring industry in recent years, video camera, not only at road, the fixed platforms such as building uses in a large number, and is widely used in the carrier of the multi-motions such as ship, automobile and aircraft.Meanwhile, be applied to the camera supervised of these unstable carriers and all there is a problem: rock because the carrier installed exists, cannot ensure that camera angles continues to aim at the monitoring objective beyond carrier, and this problem is particularly outstanding on ships.Exactly because the problem of ships monitoring and security presence, the demand that civilian, commercial ships has the monitoring head of self-stabilization function to equipment is more and more stronger.

Ships surely has two kinds of modes as technology realizes, and wherein a kind of is rely on boat-carrying The Cloud Terrace to be arranged on the steady as platform being isolated ship motion interference of extra interpolation, to reach the effect of stable shooting.Another is exactly utilize to drive the motor of camera cradle head itself and rock to the process elimination ships of image the image brought to rock problem.Additional surely have the drawbacks such as installation is loaded down with trivial details, bulky, increase cost as the mode of platform relative to the mode directly controlling horizontal stage electric machine.And reach surely not to be suitable on ships that wide-angle rocks as the mode of object by process image and use.And processing method is complicated, very high to processor requirement, equipment is often expensive.The control treatment method that general employing motor movement compensates is too simple, and the steady of use is not the general holder with pitch axis and azimuth axis as The Cloud Terrace.Autgmentability is poor.Be such as CN203037261 U at Authorization Notice No., denomination of invention is in the utility model patent of " a kind of miniature gyroscope image stabilization system ", uses the The Cloud Terrace with pitch axis and roll axis as control object.And processing mode is too simple, is not further processed accelerometer and gyrostatic data, precision can be caused not enough.In step motor control, the motion mode of stepping motor take step angle as minimum movement unit θ _urun, that is, system has the noise of a high frequency, and this micro component u (D) for controller is that a very strong interference is to the self-excitation disturbance causing system.Due to not to the optimization of Control of Step Motor's Acceleration and Deceleration mode, be easy to cause the step-out of stepping motor to get over step and cause system unstable.

Summary of the invention

In order to overcome the installation complexity of existing boat-carrying camera technique, poor universality, precision is not enough, stability not and the deficiency of high cost, the invention provides a kind of based on gyrostatic boat-carrying steady picture control method, while ensureing the precision that steady picture controls and the stability of a system, there is again the features such as volume is little, versatility good, low cost.

The present invention solves the problems of the technologies described above and is achieved through the following technical solutions:

A kind of based on gyrostatic boat-carrying steady picture control method, described method comprises the steps:

1) use the two degrees of freedom boat-carrying security tripod head based on stepping motor with pitch axis and azimuth axis as steady picture platform;

2) accelerometer and gyroscope two kinds of transducers are utilized to obtain the 3-axis acceleration α of The Cloud Terrace respectively _x, α _y, α _zand three axis angular rate ω _x, ω _y, ω _z;

3) by the data α on these two kinds different pitch axis obtaining _xwith ω _ycarry out the Kalman's Data Fusion Filtering to operand and internal memory optimization, and then obtain the accurate pitching angle theta of The Cloud Terrace _pitchand rate of pitch ω _pitch, same mode tries to achieve the roll angle θ of The Cloud Terrace _roll;

4) pitching angle theta obtained is utilized _pitchin the PD controller that input improves, export the expectation magnitude of angular velocity of the pitch axis motor controlling The Cloud Terrace;

In the PD controller of described improvement, PD algorithm carries out following improvement step according to the system features surely as The Cloud Terrace:

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

Be that deviation data error (k) in k moment substitutes in the discrete PD controller of formula (7) and obtains controlled quentity controlled variable output valve u (k) by the sampling sequence number that formula (6) obtains

$u\left(k\right)=k_p\left(\mathrm{error}\left(k\right)\right)+k_d\frac{\mathrm{error}\left(k\right)-\mathrm{error}(k-1)}{T}---\left(7\right)$

In formula, the deviation data in error (k-1) to be sampling sequence number be k-1 moment;

Controller amplitude limit exports: $\left\{\begin{array}{cc}u\left(k\right)=\mathrm{Max}\_U\left(k\right)& u\left(k\right)>\mathrm{Max}\_U\left(k\right)\\ u\left(k\right)=\mathrm{Min}\_U\left(k\right)& u\left(k\right)<\mathrm{Min}\_U\left(k\right)\end{array}\right.---\left(8\right)$

When PD controller export be greater than Max_U (k) time, system export Max_U (k), with should PD controller export be greater than Min_U (k) time, system export Min_U (k);

According to different error (k) to k _pparameter makes change, to k _pset up the variation relation based on exponential function with error (k), the gain of real-time change system is as shown in the formula shown in (9):

$\left\{\begin{array}{cc}{k}_p={\mathrm{\&alpha;e}}^{\mathrm{\&beta;}*\mathrm{error}\left(k\right)}& \mathrm{error}\left(k\right)>0\\ k_p=\mathrm{\&alpha;}& \mathrm{error}\left(k\right)=0\\ k_p=\mathrm{\&alpha;}{\left(e^{-1}\right)}^{\mathrm{\&beta;}*\mathrm{error}\left(k\right)}& \mathrm{error}\left(k\right)<0\end{array}\right.---\left(9\right)$

Cannot be used up total differential PD algorithm, at original micro component u _dlow-pass first order filter is introduced, shown in (10) in (k) item:

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

Combine target angle simultaneously and change differential elimination algorithm to θ _targetthe micro component brought during change does not substitute into calculating, shown in (11):

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

5) resolve image stabilization system under nonequilibrium condition, there is the impact of tumbling motion on steady picture platform course angle in hull, by this impact of control azimuth angle compensation; Shown in (14):

In formula, k moment The Cloud Terrace azimuth, for k-1 moment The Cloud Terrace azimuth, θ _pitchk () is the angle of pitch in k moment, θ _rollk () is the roll angle in k moment, sin () is sine operation, and asin () is arcsine computing;

6) finally result of calculation is transferred to bottom stepper motor driver.

Further, described step 4) in, add the non-linear feed speed control device of stepping motor in the rear class of PD controller; The power of stepping motor, moment of torsion and rotating speed are associated, and physical relationship is:

P＝α*Torque*ω (12)

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

The relation that current permission maximum acceleration value is linearly relevant to present speed:

$\left\{\begin{array}{cc}{a}_{\max }=\mathrm{\&alpha;}*\mathrm{\&omega;}+b& \\ U_{\mathrm{out}}=u_{\mathrm{PD}}(k-1)+a_{\max }& (u_{\mathrm{PD}}\left(k\right)>u_{\mathrm{PD}}(k-1)+a_{\max })\\ U_{\mathrm{out}}=u_{\mathrm{PD}}\left(k\right)& (u_{\mathrm{PD}}\left(k\right)\&le;u_{\mathrm{PD}}(k-1)+a_{\max })\end{array}\right.---\left(13\right)$

The Output speed result of PD controller is through compared with the Control of Step Motor's Acceleration and Deceleration of rear class, output on stepping motor after selecting, the speed that in checkout procedure, system contrast PD algorithm obtains and the maximal rate that Control of Step Motor's Acceleration and Deceleration algorithm calculates, and the comparatively fractional value both selecting all the time.

Further again, in step 3) in, described Kalman's blending algorithm process is as follows:

First the state equation of system is set up:

$\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{angle}}_{k-1}\\ q\_{\mathrm{bias}}_{k-1}\end{array}\right]+\left[\begin{array}{c}\mathrm{gyro}\_m*\mathrm{dt}\\ 0\end{array}\right]+\left[\begin{array}{c}w\_\mathrm{angle}\\ w\_\mathrm{gyro}\end{array}\right]$

Angle in above formula _kk moment angle value, q_bias _kbe gyrostatic deviation, dt is the update cycle, and gyro_m is gyrostatic process noise, w_angle and w_gyro is accelerometer and gyrostatic measurement noises respectively;

Set up and measure equation: ${\mathrm{angle}}_k=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]+v\_\mathrm{angle}$

Construction process noise matrix: $\left[\begin{array}{cc}Q\_\mathrm{angle}& 0\\ 0& Q\_\mathrm{gyro}\end{array}\right]$

Structure measurement noises matrix: [R_angle]

Angle is predicted:

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

Variance is predicted:

$\begin{array}{c}{P}_{k|k-1}=\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\\ =\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]+\left[\begin{array}{cc}Q\_\mathrm{angle}& 0\\ 0& Q\_\mathrm{gyro}\end{array}\right]\end{array}---\left(1\right)$

Angular error upgrades: angle=incAngle-angle (2)

Calculate kalman gain:

$\begin{array}{c}\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]=\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right](\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]+R_{\mathrm{angle}}{)}^{-1}\\ =\left[\begin{array}{c}P00\\ P10\end{array}\right]/(P00+R\_\mathrm{angle})\end{array}---\left(3\right)$

Variance upgrades:

$\begin{array}{c}{P}_{k|k}=\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]=(E-\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]\left[\begin{array}{cc}1& 0\end{array}\right])P_{k|k-1}\\ =\left[\begin{array}{cc}P00-K\_0*P00& P01-K\_0*P01\\ P10-K\_1*P00& P11-K\_1*P01\end{array}\right]\end{array}---\left(4\right)$

State estimation: $\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]=\left[\begin{array}{c}{\mathrm{angle}}_{k-1}\\ q\_{\mathrm{bias}}_{k-1}\end{array}\right]+\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]*\mathrm{angle}\_\mathrm{err}---\left(5\right)$

Double counting formula (1) ~ (5) are until find optimum result.

The beneficial effect that the present invention has: use the more general diaxon based on stepping motor (pitch axis and azimuth axis) boat-carrying The Cloud Terrace as control object, to have more versatility; The gyroscope of the micromechanical process of employing low cost and accelerometer, as transducer, reduce costs; Data both being gathered by microcontroller also use Kalman filter, and both data are carried out iteration fusion, obtain more accurate and jamproof The Cloud Terrace angle information; By the PD controller of The Cloud Terrace angle-data input after gain and differential are optimized, controller Output rusults is input to the output signal that a kind of novel non-linear controllor for step-by-step motor finally obtains the The Cloud Terrace angle of pitch, system is more fast stable; Calculate azimuthal angle correction by the angle of pitch and roll angle, can further suppress image to rock.The present invention can not only ensure the tracking accuracy of boat-carrying steady picture platform and follow the tracks of rapidity, has again stiff stability and antijamming capability.

Accompanying drawing explanation

Fig. 1 is boat-carrying steady picture control block diagram.

Fig. 2 is diaxon cradle head structure figure.

Fig. 3 is Kalman filtering block diagram.

Fig. 4 is Kalman filtering design sketch.

Fig. 5 is pitch axis control block diagram.

Fig. 6 is azimuth axis control block diagram.

Embodiment

For making the object, technical solutions and advantages of the present invention more clear, below in conjunction with accompanying drawing, technical scheme of the present invention is further described.

With reference to Fig. 1 ~ Fig. 6: a kind of method controlled based on the steady picture of gyrostatic boat-carrying, described method comprises the steps:

Use the boat-carrying security tripod head with pitch axis and azimuth axis two degrees of freedom as steady picture platform.

As shown in Figure 1, using the three-axis gyroscope accelerometer of micromechanical process as transducer, gathering its data by using high performance ARM microcontroller.Calculate system by control algolithm to export, adopt serial communication mode to send data to stepper motor driver.The structure of diaxon The Cloud Terrace as shown in Figure 2.Comprise azimuth axis and pitch axis, transducer and microcontroller are all arranged on makes a video recording on movement.

As shown in Figure 3, the present invention utilizes accelerometer and gyroscope two kinds of transducers to obtain the acceleration alpha of The Cloud Terrace respectively _x, α _y, α _zand angular velocity omega _x, ω _y, ω _z.

By the data α on these two kinds different pitch axis obtaining _xwith ω _ycarry out the Kalman's Data Fusion Filtering simplified, and then obtain the accurate pitching angle theta of The Cloud Terrace _pitchand rate of pitch ω _pitch, same mode can in the hope of the roll angle θ of The Cloud Terrace _roll.Described method comprises following steps:

First the state equation of system is set up:

$\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{angle}}_{k-1}\\ q\_{\mathrm{bias}}_{k-1}\end{array}\right]+\left[\begin{array}{c}\mathrm{gyro}\_m*\mathrm{dt}\\ 0\end{array}\right]+\left[\begin{array}{c}w\_\mathrm{angle}\\ w\_\mathrm{gyro}\end{array}\right]$

Angle in above formula _kk moment angle value, q_bias _kbe gyrostatic deviation, dt is the update cycle, and gyro_m is gyrostatic process noise, w_angle and w_gyro is accelerometer and gyrostatic measurement noises respectively.

Set up and measure equation: ${\mathrm{angle}}_k=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]+v\_\mathrm{angle}$

Construction process noise matrix: $\left[\begin{array}{cc}Q\_\mathrm{angle}& 0\\ 0& Q\_\mathrm{gyro}\end{array}\right]$

Structure measurement noises matrix: [R_angle]

Angle is predicted:

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

Variance is predicted:

$\begin{array}{c}{P}_{k|k-1}=\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\\ =\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]+\left[\begin{array}{cc}Q\_\mathrm{angle}& 0\\ 0& Q\_\mathrm{gyro}\end{array}\right]\end{array}---\left(1\right)$

Angular error upgrades: angle=incAngle-angle (2)

Calculate kalman gain (Kalman Gain):

$\begin{array}{c}\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]=\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right](\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]+R_{\mathrm{angle}}{)}^{-1}\\ =\left[\begin{array}{c}P00\\ P10\end{array}\right]/(P00+R\_\mathrm{angle})\end{array}---\left(3\right)$

Variance upgrades (prediction variance and variance can common memory):

$\begin{array}{c}{P}_{k|k}=\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]=(E-\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]\left[\begin{array}{cc}1& 0\end{array}\right])P_{k|k-1}\\ =\left[\begin{array}{cc}P00-K\_0*P00& P01-K\_0*P01\\ P10-K\_1*P00& P11-K\_1*P01\end{array}\right]\end{array}---\left(4\right)$

State estimation: $\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]=\left[\begin{array}{c}{\mathrm{angle}}_{k-1}\\ q\_{\mathrm{bias}}_{k-1}\end{array}\right]+\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]*\mathrm{angle}\_\mathrm{err}---\left(5\right)$

Double counting step (1) ~ (5) are until find optimum result.

In Fig. 4, blue curve is the angle waveform that accelerometer obtains, purple be angle waveform after gyroscope integration, yellow curve is the angle waveform after merging.

Have larger noise by the known gyroscope of upper figure, the angle obtained after angular speed integration can offset, and gyroscope be temperature sensor when temperature changes, gyrostatic zero point also can change.On the other hand, the noise of the angle that accelerometer obtains after conversion is larger, and peak value can reach the error of 0.1 degree.And very large motion artifacts is there will be when being subject to attitude disturbance, generally can reach 5-10 degree, therefore also cannot directly use.And by Kalman filtering by after two data fusion, the precision of the angle after the existing gyroscope integration of the angle obtained, also has accelerometer that the characteristic of drifting about is less likely to occur.

The pitch axis block diagram of system as shown in Figure 5.When carrier produces motion in relative inertness space, will be moved together by strap moving platform framework.When not considering disturbance torque and random error, gyroscope exports the angular velocity signal of the responsive platform framework arrived, accelerometer exports acceleration signal and obtains The Cloud Terrace attitude through anti-triangulo operation, and these two sensing datas obtain the angle of the angle of pitch after the data fusion of Kalman filtering.The attitude angle obtained and angle on target are made difference and are obtained systematic error, and negative feedback is to the input of control system.The correction of improved PD controller correction and non-linear acceleration controller, amplifies the stepping motor of rear drive platform at driver.Motor produces corresponding torsional moment in the other direction, drive platform framework with the rightabout of carrier rotation on carry out rate compensation, until the error signal of system is zero, platform framework returns to original position, thus achieves the stabilization function of platform.

The present invention utilizes the θ obtained _pitchelectric Machine Control is carried out to the pitch axis of The Cloud Terrace and stablizes the angle of pitch, adopt the PD controller improved.

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

The error (k) that formula (6) obtains is substituted in the discrete PD controller of formula (7) and obtain controlled quentity controlled variable output valve u (k).

$u\left(k\right)=k_p\left(\mathrm{error}\left(k\right)\right)+k_d\frac{\mathrm{error}\left(k\right)-\mathrm{error}(k-1)}{T}---\left(7\right)$

The deviation data in error (k-1) to be sampling sequence number be k-1 moment in formula.

For increasing stability and the performance of system, following improvement is done to classical PD algorithm.

Controller amplitude limit exports: $\left\{\begin{array}{cc}u\left(k\right)=\mathrm{Max}\_U\left(k\right)& u\left(k\right)>\mathrm{Max}\_U\left(k\right)\\ u\left(k\right)=\mathrm{Min}\_U\left(k\right)& u\left(k\right)<\mathrm{Min}\_U\left(k\right)\end{array}\right.---\left(8\right)$

When PD controller export be greater than Max_U (k) time, system export Max_U (k), with should PD controller export be greater than Min_U (k) time, system export Min_U (k), ensure the stability of a system.

The present invention according to different error (k) to k _pparameter makes change, and the system responses being is not only fast but also stable.To k _pthe variation relation based on exponential function is set up with error (k).The gain of real-time change system is as shown in the formula shown in 9.

$\left\{\begin{array}{cc}{k}_p={\mathrm{\&alpha;e}}^{\mathrm{\&beta;}*\mathrm{error}\left(k\right)}& \mathrm{error}\left(k\right)>0\\ k_p=\mathrm{\&alpha;}& \mathrm{error}\left(k\right)=0\\ k_p=\mathrm{\&alpha;}{\left(e^{-1}\right)}^{\mathrm{\&beta;}*\mathrm{error}\left(k\right)}& \mathrm{error}\left(k\right)<0\end{array}\right.---\left(9\right)$

Cannot be used up total differential PD algorithm, at original micro component u _dlow-pass first order filter is introduced in (k) item.Shown in (10).

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

Combine differential forward algorithm to θ simultaneously _targetthe micro component brought during change does not substitute into calculating, reduces the frequent destabilizing factor changing steady image angle and bring.Shown in (11).

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

Add non-linear feed speed control device in the rear class of PD controller, thus improve the angle of pitch stability of The Cloud Terrace.Enter the power of motor, moment of torsion and rotating speed to be associated, physical relationship is:

P＝α*Torque*ω (12)

In formula, P is stepping motor power, and α is conversion coefficient, and Torque is the moment of torsion of motor, and ω is motor speed.So the present invention proposes a kind of method for controlling stepping motor limiting acceleration, and the relation that current permission maximum acceleration value is linearly relevant to present speed.

$\left\{\begin{array}{cc}{a}_{\max }=\mathrm{\&alpha;}*\mathrm{\&omega;}+b& \\ U_{\mathrm{out}}=u_{\mathrm{PD}}(k-1)+a_{\max }& (u_{\mathrm{PD}}\left(k\right)>u_{\mathrm{PD}}(k-1)+a_{\max })\\ U_{\mathrm{out}}=u_{\mathrm{PD}}\left(k\right)& (u_{\mathrm{PD}}\left(k\right)\&le;u_{\mathrm{PD}}(k-1)+a_{\max })\end{array}\right.---\left(13\right)$

The Output speed result of PD controller outputs on stepping motor after selecting compared with the Control of Step Motor's Acceleration and Deceleration of rear class.The speed that in checkout procedure, system contrast PD algorithm obtains and the maximal rate that Control of Step Motor's Acceleration and Deceleration algorithm calculates, and the comparatively fractional value both selecting all the time.

Thus suppression pitch axis rocks the interference to image.

As shown in Figure 6, when system is subject to the impact of the roll axis disturbance except the angle of pitch, the optical axis can be interfered in the horizontal direction.The present invention calculates the current angle revised that needs by the current pitching of detection and roll angle and compensates.Resolving image stabilization system under nonequilibrium condition, there is the impact of tumbling motion on steady picture platform course angle in hull.By this impact of control azimuth angle compensation, thus this interference of effective suppression, improve picture steadiness.Algorithm is such as formula shown in (14).

In formula k moment The Cloud Terrace azimuth, for k-1 moment The Cloud Terrace azimuth, θ _pitchk () is the angle of pitch in k moment, θ _rollk () is the roll angle in k moment.Sin () is sine operation, and asin () is arcsine computing.

Finally result of calculation is transferred to bottom stepper motor driver by RS232.

So, the Moving Disturbance of ship-borne camera system is compensated to be eliminated and suppresses, thus reaches the object that when suppressing boat-carrying shooting, image rocks.

Claims (3)

1., based on a gyrostatic boat-carrying steady picture control method, it is characterized in that: described method comprises the steps:

1) use the two degrees of freedom boat-carrying security tripod head based on stepping motor with pitch axis and azimuth axis as steady picture platform;

2) accelerometer and gyroscope two kinds of transducers are utilized to obtain the 3-axis acceleration α of The Cloud Terrace respectively _x, α _y, α _zand three axis angular rate ω _x, ω _y, ω _z;

3) by the data α on these two kinds different pitch axis obtaining _xwith ω _ycarry out the Kalman's Data Fusion Filtering to operand and internal memory optimization, and then obtain the accurate pitching angle theta of The Cloud Terrace _pitchand rate of pitch ω _pitch, same mode tries to achieve the roll angle θ of The Cloud Terrace _rou;

4) pitching angle theta obtained is utilized _pitchin the PD controller that input improves, export the expectation magnitude of angular velocity of the pitch axis motor controlling The Cloud Terrace;

In the PD controller of described improvement, PD algorithm carries out following improvement step according to the system features surely as The Cloud Terrace:

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

Be that deviation data error (k) in k moment substitutes in the discrete PD controller of formula (7) and obtains controlled quentity controlled variable output valve u (k) by the sampling sequence number that formula (6) obtains

$u\left(k\right)=k_p\left(\mathrm{error}\left(k\right)\right)+k_d\frac{\mathrm{error}\left(k\right)-\mathrm{error}(k-1)}{T}---\left(7\right)$

In formula, the deviation data in error (k-1) to be sampling sequence number be k-1 moment;

Controller amplitude limit exports: $\left\{\begin{array}{cc}u\left(k\right)=\mathrm{Max}\_U\left(k\right)& u\left(k\right)>\mathrm{Max}\_U\left(k\right)\\ u\left(k\right)=\mathrm{Min}\_U\left(k\right)& u\left(k\right)<\mathrm{Min}\_U\left(k\right)\end{array}\right.---\left(8\right)$

When PD controller export be greater than Max_U (k) time, system export Max_U (k), with should PD controller export be greater than Min_U (k) time, system export Min_U (k);

According to different error (k) to k _pparameter makes change, to k _pset up the variation relation based on exponential function with error (k), the gain of real-time change system is as shown in the formula shown in (9):

$\left\{\begin{array}{cc}{k}_p={\mathrm{\&alpha;e}}^{\mathrm{\&beta;}*\mathrm{error}\left(k\right)}& \mathrm{error}\left(k\right)>0\\ k_p=\mathrm{\&alpha;}& \mathrm{error}\left(k\right)=0\\ k_p=\mathrm{\&alpha;}{\left(e^{-1}\right)}^{\mathrm{\&beta;}*\mathrm{error}\left(k\right)}& \mathrm{error}\left(k\right)<0\end{array}\right.---\left(9\right)$

Cannot be used up total differential PD algorithm, at original micro component u _dlow-pass first order filter is introduced, shown in (10) in (k) item:

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

Combine target angle simultaneously and change differential elimination algorithm to θ _targetthe micro component brought during change does not substitute into calculating, shown in (11):

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

5) resolve image stabilization system under nonequilibrium condition, there is the impact of tumbling motion on steady picture platform course angle in hull, by this impact of control azimuth angle compensation; Shown in (14):

In formula, k moment The Cloud Terrace azimuth, for k-1 moment The Cloud Terrace azimuth, θ _pitchk () is the angle of pitch in k moment, θ _rollk () is the roll angle in k moment, sin () is sine operation, and asin () is arcsine computing;

6) finally result of calculation is transferred to bottom stepper motor driver.

2. as claimed in claim 1 a kind of based on gyrostatic boat-carrying steady picture control method, it is characterized in that: described step 4) in, add the non-linear feed speed control device of stepping motor in the rear class of PD controller; The power of stepping motor, moment of torsion and rotating speed are associated, and physical relationship is:

P＝α-Torque*ω (12)

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

The relation that current permission maximum acceleration value is linearly relevant to present speed:

$\left\{\begin{array}{cc}{a}_{\max }=\mathrm{\&alpha;}*\mathrm{\&omega;}+b& \\ U_{\mathrm{out}}=u_{\mathrm{PD}}(k-1)+a_{\max }& (u_{\mathrm{PD}}\left(k\right)>u_{\mathrm{PD}}(k-1)+a_{\max })\\ U_{\mathrm{out}}=u_{\mathrm{PD}}\left(k\right)& (u_{\mathrm{PD}}\left(k\right)\&le;u_{\mathrm{PD}}(k-1)+a_{\max })\end{array}\right.---\left(13\right)$

The Output speed result of PD controller is through compared with the Control of Step Motor's Acceleration and Deceleration of rear class, output on stepping motor after selecting, the speed that in checkout procedure, system contrast PD algorithm obtains and the maximal rate that Control of Step Motor's Acceleration and Deceleration algorithm calculates, and the comparatively fractional value both selecting all the time.

3. as claimed in claim 1 or 2 a kind of based on gyrostatic boat-carrying steady picture control method, it is characterized in that: in step 3) in, described Kalman's blending algorithm process is as follows:

First the state equation of system is set up:

$\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]=\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{\mathrm{angle}}_{k-1}\\ q\_{\mathrm{bias}}_{k-1}\end{array}\right]+\left[\begin{array}{c}\mathrm{gyro}\_m*\mathrm{dt}\\ 0\end{array}\right]+\left[\begin{array}{c}w\_\mathrm{angle}\\ w\_\mathrm{gyro}\end{array}\right]$

Angle in above formula _kk moment angle value, q_bias _kbe gyrostatic deviation, dt is the update cycle, and gyro_m is gyrostatic process noise, w_angle and w_gyro is accelerometer and gyrostatic measurement noises respectively;

Set up and measure equation: ${\mathrm{angle}}_k=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]+v\_\mathrm{angle}$

Construction process noise matrix: $\left[\begin{array}{cc}Q\_\mathrm{angle}& 0\\ 0& Q\_\mathrm{gyro}\end{array}\right]$

Structure measurement noises matrix: [R_angle]

Angle is predicted:

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

Variance is predicted:

$\begin{array}{c}{P}_{k|k-1}=\begin{array}{}\end{array}\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\\ =\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{cc}1& -\mathrm{dt}\\ 0& 1\end{array}\right]+\left[\begin{array}{cc}Q\_\mathrm{angle}& 0\\ 0& Q\_\mathrm{gyro}\end{array}\right]\end{array}---\left(1\right)$

Angular error upgrades: angle=incAngle-angle (2)

Calculate kalman gain:

$\begin{array}{c}\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]=\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]{\left(\begin{array}{c}\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]\left[\begin{array}{c}0\\ 1\end{array}\right]{+}_{R_{\mathrm{angle}}}\end{array}\right)}^{-1}\\ =\left[\begin{array}{c}P00\\ P10\end{array}\right]/(P00+R\_\mathrm{angle})\end{array}---\left(3\right)$

Variance upgrades:

$\begin{array}{c}{P}_{k|k}=\begin{array}{}\end{array}\left[\begin{array}{cc}P00& P01\\ P10& P11\end{array}\right]=\left(\begin{array}{c}E-\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]\left[\begin{array}{cc}1& 0\end{array}\right]\end{array}\right)P_{k|k-1}\\ =\left[\begin{array}{cc}P00-K\_0*P00& P01-K\_0*P01\\ P10-K\_1*P00& P11-K\_1*P01\end{array}\right]\end{array}---\left(4\right)$

State estimation: $\left[\begin{array}{c}{\mathrm{angle}}_k\\ q\_{\mathrm{bias}}_k\end{array}\right]=\left[\begin{array}{c}{\mathrm{angle}}_{k-1}\\ {q\_\mathrm{bias}}_{k-1}\end{array}\right]+\left[\begin{array}{c}K\_0\\ K\_1\end{array}\right]*\mathrm{angle}\_\mathrm{err}---\left(5\right)$

Double counting formula (1) ~ (5) are until find optimum result.