CN104317217A - Non-overshooting servo control method for aerial camera stabilization platform - Google Patents

Abstract

The invention discloses a non-overshooting servo control method for an aerial camera stabilization platform, and belongs to the technical field of automatic control. A position command of a rolling channel is planned, a curve in which transition to the position command angle from the initial gesture angle according to a 1/4 positive rotation curve is carried out is obtained, and thus the step position command given by a camera control system is converted into a smooth transition curve; and the gesture angle of an aiming line is calculated through gesture calculation algorithm, and thus the angle position of the stabilization platform at the geographic coordinates can be obtained. According to the method of the invention, the problem that overshooting happens during the known stabilization platform step process can be solved, and the efficiency of overhead shooting can be greatly improved; the sight line of the camera stabilization platform can be controlled to precisely point to the target channel at the geographic coordinates; and the curve planning algorithm and the gesture calculation algorithm used by the invention is simple, convenient to realize, and good to transplant, and thus, the method has wider application prospect.

Description

Aerial camera stable platform non-overshoot method of servo-controlling

Technical field

The invention belongs to control field, relate generally to a kind of method of servo-controlling of stable platform, particularly relate to a kind of non-overshoot method of servo-controlling being applicable to step sweep type aerial camera stable platform.

Background technology

Aerial camera system is mainly used in obtaining High Resolution Visible Light target image information in the air from height.Stable platform and camera are the important component parts of aerial camera system, camera generally adopts area array CCD digital camera and is fixed on stable platform, stable platform is in order to isolate the disturbance of carrier aircraft, for camera provides good working environment, camera is enable to stablize and obtain high-resolution target image fast.

Publication number is that the Chinese patent application of CN1825203A discloses a kind of airborne inclined camera photographing device, this device comprises stable platform and 2 ~ 5 high resolution cameras and the field angle of camera is 44 °, these cameras are arranged on stable platform according to certain angle, with obtain forward sight, backsight, a left side depending on, the right side depending on or under look the aviation image of i.e. 2 ~ 5 different angles, wherein down depending on the image of camera shooting for making space model and orthography, the image of other angle camera shooting can be used as the texture source of building wall.The working depth of camera is 2000m.Because camera quantity is more and field angle is larger, stable platform only need provide the horizontal reference of certain precision, just can obtain Large visual angle image without the need to carrying out swinging fast, thus, namely the requirement of camera to stable platform be lower does not turn and the requirement of non-overshoot fast; In addition, the flying height of carrier aircraft is lower, and speed is also comparatively slow, less to the disturbance of stable platform, and therefore, this kind of stable platform is easier to realize.

But, for the high-altitude aircraft camera system that field angle is 3.1 °, focal length is greater than 1m, working depth is 6000m, stable platform can only being installed a camera, thus cannot realize large area shooting by installing multiple stage camera on stable platform.In order to the image of shooting of flying can cover larger shooting area at every turn, in shooting process, need stable platform to drive camera to point to different navigation channels, and the image taken in different navigation channel carry out splicing could obtain large area high-definition picture.For this reason, require that stable platform can drive camera rapid translating between adjacent navigation channel also accurately to put in place, at once notify after putting in place that camera is taken pictures, after having clapped piece image, just turn to adjacent navigation channel to take pictures, therefore the whole process of taking pictures of camera be one the circulation great-jump-forward course of work of " start-stop-taking pictures ".To take pictures efficiency to improve high-altitude, requiring that stable platform has navigation channel conversion non-overshoot, Fast Convergent, the characteristic that switching time is short.Because the load of stable platform and camera quality are comparatively large, therefore the rapid translating of stable platform between each navigation channel and non-overshoot are the contradiction being difficult to balance a pair.In addition, in order to ensure image quality, the rate stabilization of inertial space ensures that accurate geographic coordinate points to again also to require stable platform to ensure, also require simultaneously camera take a picture there is certain Duplication, like this, stable platform just needs the geographic coordinate benchmark setting up oneself, and traditional heading control platform does not possess this function.

Summary of the invention

The technical problem to be solved in the present invention is, for the stable platform of the aerial camera system needing many navigation channels rapid translating to take, a kind of non-overshoot method of servo-controlling is provided, adopt the stable platform of this method of servo-controlling can not only keep stable at inertial space, also can carry out the rotation of phase step type position under geographic coordinate system simultaneously, thus drive camera to take pictures to different geographic position.

For solving above technical matters, non-overshoot method of servo-controlling provided by the invention is realized by the DSP being built-in with servo control module, and servo control module contains roll passage and pitch channel, and after DSP powers on, servo control module performs following operation steps:

The first step, judges whether to receive " starting working " instruction that aerial camera system controller sends, and if NO, waits for, if yes, proceeds to second step;

Second step, receives the roll channel position instruction θ that aerial camera system controller provides _{ro_cmd}; Pitch channel position command is set to zero, even θ _{el_cmd}=0; Make variable i=1;

3rd step, gathers the carrier aircraft initial horizontal roll angle θ that vertical gyro exports _{ro_v0}with initial pitch angle θ _{el_v0}, gather the roll outside framework initial angle position signal θ that roll rotary transformer exports _{ro_r0}with the pitching inner frame initial angle position signal θ that pitching rotary transformer exports _{el_r0}, and calculate according to following formula:

${\mathrm{\θ}}_{\mathrm{ro}\_\mathrm{los}0}=\arctan \left(\frac{a_{11}\cos {\mathrm{\θ}}_{\mathrm{el}\_r0}\sin {\mathrm{\θ}}_{\mathrm{ro}\_r0}+a_{12}{\sin \mathrm{\θ}}_{\mathrm{el}\_r0}+a_{13}\cos {\mathrm{\θ}}_{\mathrm{el}\_r0}\cos {\mathrm{\θ}}_{\mathrm{ro}\_r0}}{a_{31}\cos {\mathrm{\θ}}_{\mathrm{el}\_r0}\sin {\mathrm{\θ}}_{\mathrm{ro}\_r0}+a_{32}\sin {\mathrm{\θ}}_{\mathrm{rl}\_r0}{0a}_{33}\cos {\mathrm{\θ}}_{\mathrm{el}\_r0}\cos {\mathrm{\θ}}_{\mathrm{ro}\_r0}}\right)$

a ₁₁＝cosθ _{ro_v0}

a ₁₂＝0

a ₁₃＝sinθ _{ro_v0}

a ₃₁＝-sinθ _{ro_v0}cosθ _{el_v0}

a ₃₂＝sinθ _{el_v0}

a ₃₃＝cosθ _{ro_v0}cosθ _{el_v0}

In formula, θ _ro_ _los0for the initial roll attitude angle of camera boresight;

4th step, generates the planned position instruction curve of roll passage:

${\mathrm{\θ}}_{\mathrm{ro}\_\mathrm{cmd}\_i}=\left[\mathrm{Sin}(2\mathrm{\π}\×\left(\frac{T\×i}{4\×T_s}\right))\right]\×({\mathrm{\θ}}_{\mathrm{ro}\_\mathrm{cmd}}-{\mathrm{\θ}}_{\mathrm{ro}\_\mathrm{los}0})+{\mathrm{\θ}}_{\mathrm{ro}\_\mathrm{los}0}$

In formula, θ _{ro_cmd_i}represent the planned position instruction corresponding with variable i; T is the sampling period; T _sfor roll channel position regulating time;

5th step, gathers the carrier aircraft roll angle θ of the current output of vertical gyro _{ro_v}with carrier aircraft pitching angle theta _{el_v}, gather the angle position signal θ of the roll outside framework of the current output of roll rotary transformer _{ro_r}with the angle position signal θ of the pitching inner frame of the current output of pitching rotary transformer _{el_r}, and organize formulae discovery according to next:

θ _{el_los}＝arcsin(a ₃₁cosθ _{el_r}sinθ _{ro_r}+a ₃₂sinθ _{el_r}+a ₃₃cosθ _{el_r}cosθ _{ro_r})

${\mathrm{\θ}}_{\mathrm{ro}\_\mathrm{los}}=\arctan \left(\frac{a_{11}\cos {\mathrm{\θ}}_{\mathrm{el}\_r}\sin {\mathrm{\θ}}_{\mathrm{ro}\_r}+a_{12}{\sin \mathrm{\θ}}_{\mathrm{el}\_r}+a_{13}\cos {\mathrm{\θ}}_{\mathrm{el}\_r}\cos {\mathrm{\θ}}_{\mathrm{ro}\_r}}{a_{31}\cos {\mathrm{\θ}}_{\mathrm{el}\_r}\sin {\mathrm{\θ}}_{\mathrm{ro}\_r}+a_{32}\sin {\mathrm{\θ}}_{\mathrm{el}\_r}{+a}_{33}\cos {\mathrm{\θ}}_{\mathrm{el}\_r}\cos {\mathrm{\θ}}_{\mathrm{ro}\_r}}\right)$

a ₁₁＝cosθ _{ro_v}

a ₁₂＝0

a ₁₃＝sinθ _{ro_v}

a ₃₁＝-sinθ _{ro_v}cosθ _{el_v}

a ₃₂＝sinθ _{el_v}

a ₃₃＝cosθ _{ro_v}cosθ _{el_v}

In formula, θ _{el_los}for the current pitch attitude angle of camera boresight, θ _{ro_los}for the current roll attitude angle of camera boresight;

6th step, judges i < T _s/ T, if yes, proceeds to the 7th step, if NO, proceeds to the 8th step;

7th step, substitutes into the planned position controlling curve of roll passage by i, obtain planned position instruction θ _{ro_cmd_i}, then proceed to the 9th step;

8th step, the roll channel position instruction θ that aerial camera system controller is provided _{ro_cmd}as the planned position instruction θ of current roll passage _{ro_cmd_i}even, θ _{ro_cmd_i}=θ _{ro_cmd}and proceed to the 9th step;

9th step, carry out roll passage and pitch channel position loop computing:

9.1 according to following formulae discovery site error:

p _{ro_err}＝θ _{ro_cmd_i}－θ _{ro_los}

p _{el_err}＝θ _{el_cmd}－θ _{el_los}

In formula, p _{ro_err}for roll site error amount, p _{el_err}for the pitch position margin of error;

9.2 adopt pi regulator 1 to carry out position loop resolves:

${\mathrm{\&omega;}}_{\mathrm{ro}\_\mathrm{cmd}}=k_{\mathrm{pr}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{pr}}+1}{s}{p}_{\mathrm{ro}\_\mathrm{err}}$

${\mathrm{\&omega;}}_{\mathrm{el}\_\mathrm{cmd}}=k_{\mathrm{pe}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{pe}}+1}{s}{p}_{\mathrm{el}\_\mathrm{err}}$

In formula, ω _{ro_cmd}for roll channel rate circuit controls amount, ω _{el_cmd}for pitch channel rate loop controlled quentity controlled variable, k _prfor the gain coefficient of pi regulator 1 roll passage, k _pefor the gain coefficient of pi regulator 1 pitch channel, ω _prfor the integrator controling parameters of pi regulator 1 roll passage, ω _pefor the integrator controling parameters of pi regulator 1 pitch channel;

Tenth step, carry out roll passage and the computing of pitch channel rate loop:

The angle rate signal ω of the roll outside framework of the current output of 10.1 collection twin shaft rate gyro _rowith the angle rate signal ω of pitching inner frame _el, and adopt low-pass first order filter to carry out filtering to the output signal of twin shaft rate gyro:

${\mathrm{\&omega;}}_{\mathrm{ro}1}=\frac{1}{1+S/{\mathrm{\&omega;}}_{1\mathrm{p\; ro}}}{\mathrm{\&omega;}}_{\mathrm{ro}}$

${\mathrm{\&omega;}}_{\mathrm{el}1}=\frac{1}{1+S/{\mathrm{\&omega;}}_{1\mathrm{p\; el}}}{\mathrm{\&omega;}}_{\mathrm{el}}$

In formula, ω _ro1for roll channel filtering signal, ω _el1for pitch channel filtering signal, ω _{lp_ro}for the corner frequency of low-pass first order filter roll passage, ω _{lp_e1}for the corner frequency of low-pass first order filter pitch channel;

10.2, according to following formulae discovery velocity error amount:

ω _{ro_err}＝ω _{ro_cmd}－ω _ro1

ω _{el_err}＝ω _{el_cmd}－ω _el1

In formula, ω _{err_ro}for the velocity error amount of roll passage, ω _{err_el}for the velocity error amount of pitch channel;

10.3, adopt pi regulator 2 to carry out rate loop and resolve:

$I_{\mathrm{ro}\_\mathrm{cmd}}=k_{\mathrm{gr}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{gr}}+1}{s}{\mathrm{\&omega;}}_{\mathrm{ro}\_\mathrm{err}}$

$I_{\mathrm{el}\_\mathrm{cmd}}=k_{\mathrm{ge}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{ge}}+1}{s}{\mathrm{\&omega;}}_{\mathrm{el}\_\mathrm{err}}$

In formula, I _{cmd_r}for the controlled quentity controlled variable of roll passage pwm power amplifier, I _{cmd_e}for the controlled quentity controlled variable of pitch channel pwm power amplifier, k _grfor the gain coefficient of pi regulator 2 roll passage, k _gefor the gain coefficient of pi regulator 2 pitch channel, ω _grfor pi regulator 2 roll tunnel integrator controling parameters, ω _gefor the integrator controling parameters of pi regulator 2 pitch channel;

11 step, by the controlled quentity controlled variable I of roll passage pwm power amplifier _{cmd_r}be applied to roll channel power amplifier, by the controlled quentity controlled variable I of pitch channel pwm power amplifier _{cmd_e}be applied to pitch channel power amplifier;

12 step, makes variable i add 1, i.e. i=i+1;

13 step, judges i > T _c/ T, if yes, proceeds to the 14 step, if NO, proceeds to the 15 step, T _cfor stable platform step response index;

14 step, notice camera starts takes pictures;

15 step, judges i > T _p/ T, if yes, proceeds to the 16 step, if NO, returns the 5th step, T _pfor the time interval of roll channel position instruction;

Whether the 16 step, receive end-of-job instruction, if NO, then returns second step, and if YES, then servo control module is out of service.

Beneficial effect of the present invention is embodied in the following aspects.

(1) the curve planning algorithm in the present invention, using 1/4 cycle before dextrorotation curve as typical curve, the position command cooked up like this is the process of approaching gradually, therefore, the invention solves the overshoot problem occurred in the step process of stable platform position, make stable platform non-overshoot in the quick rotation process of position, substantially increase the efficiency taken pictures in high-altitude.

(2) the present invention sets up the geographic coordinate benchmark of stable platform by the vertical gyro be arranged on stable platform pedestal, the Angle Position of sight line under geographic coordinate system is obtained by attitude algorithm algorithm, feed back to servo-control system, the sight line controlling stable platform accurately points to target navigation channel under geographic coordinate system, the present invention can ensure that the picture captured under carrier aircraft attitude disturbance of camera system can splice mutually thus, and then obtains Large visual angle picture rich in detail.

(3) in the present invention, it is simple that the attitude algorithm algorithm adopted and curve planning algorithm have algorithm, and it is convenient to realize, and transplantability is good, thus makes the present invention have more wide application prospect.

Accompanying drawing explanation

Fig. 1 is the general flow chart of non-overshoot method of servo-controlling of the present invention.

Fig. 2 is the process flow diagram of attitude algorithm subroutine.

Fig. 3 is planned position instruction curve map in the present invention.

Fig. 4 a is the step instruction response diagram of position loop in prior art.

Fig. 4 b is the step instruction response diagram of position loop in the present invention.

Embodiment

Below in conjunction with accompanying drawing and preferred embodiment, the present invention is described in further detail.

The non-overshoot method of servo-controlling that the preferred embodiment of the present invention provides above realizes at the servocontrol computing machine (DSP) of aerial camera stable platform (hereinafter referred to as stable platform).Stable platform comprises roll outside framework and pitching bearing wall and frame structure, and roll outside framework is arranged in carrier aircraft, roll outside framework is equipped with roll axle and the roll motor being used for driving roll axle to rotate; Pitching inner frame is arranged on roll axle, and pitching inner frame is with the pitching main drive shaft be parallel to each other and pitching driven axle and the 2:1 angle transmission mechanism that is arranged between the two, and pitching motor drives pitching main drive shaft to rotate, and catoptron is arranged on pitching driven axle.Roll rotary transformer is connected on roll axle, and pitching rotary transformer is connected on pitching main drive shaft; Twin shaft rate gyro is connected in the back side of surely taking aim at catoptron and two sensitive axes is parallel with pitch axis with the roll axle of stable platform respectively; Vertical gyro is arranged on pedestal and two sensitive axes is parallel with pitch axis with the roll axle of stable platform respectively; The camera lens of camera is arranged on roll axle.Formed by camera lens, objective optics image focuses on the target surface of CCD camera after catoptron reflection.

Be equipped with the servo control module be made up of pitch channel and roll passage in DSP, after DSP powers on, servo control module performs following operation steps by according to the workflow shown in Fig. 1.

The first step, judges whether to receive " starting working " instruction that aerial camera system controller sends, and if NO, waits for, if yes, proceeds to second step.

Second step, receives the roll channel position instruction θ that aerial camera system controller provides _{ro_cmd}; Even pitch channel position command is set to zero θ _{el_cmd}=0; Make variable i=1.

Do not need to carry out position because pitch channel only needs to be stabilized in geographical zero-bit to turn, therefore the position command of pitch channel is always zero i.e. θ _{el_cmd}=0.

3rd step, call attitude algorithm subroutine and perform following operation steps according to Fig. 2:

3.1, gather the carrier aircraft initial horizontal roll angle θ that vertical gyro exports _{ro_v0}with initial pitch angle θ _{el_v0}, gather the roll outside framework initial angle position signal θ that roll rotary transformer exports _{ro_r0}with the pitching inner frame initial angle position signal θ that pitching rotary transformer exports _{el_r0}.

3.2, organize formulae discovery according to next:

${\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{los}0}=\arctan \left(\frac{a_{11}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r0}\sin {\mathrm{\&theta;}}_{\mathrm{ro}\_r0}+a_{12}{\sin \mathrm{\&theta;}}_{\mathrm{el}\_r0}+a_{13}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r0}\cos {\mathrm{\&theta;}}_{\mathrm{ro}\_r0}}{a_{31}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r0}\sin {\mathrm{\&theta;}}_{\mathrm{ro}\_r0}+a_{32}\sin {\mathrm{\&theta;}}_{\mathrm{rl}\_r0}{0a}_{33}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r0}\cos {\mathrm{\&theta;}}_{\mathrm{ro}\_r0}}\right)$

a ₁₁＝cosθ _{ro_v0}

a ₁₂＝0

a ₁₃＝sinθ _{ro_v0}

a ₃₁＝-sinθ _{ro_v0}cosθ _{el_v0}

a ₃₂＝sinθ _{el_v0}

a ₃₃＝cosθ _{ro_v0}cosθ _{el_v0}

In formula, θ _{ro_los0}for the initial roll attitude angle of camera boresight.

4th step, generates the planned position instruction curve of roll passage:

${\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{cmd}\_i}=\left[\mathrm{Sin}(2\mathrm{\&pi;}\&times;\left(\frac{T\&times;i}{4\&times;T_s}\right))\right]\&times;({\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{cmd}}-{\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{lod}0})+{\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{los}0}$

In formula, θ _{ro_cmd_i}represent the planned position instruction corresponding with variable i; T is the sampling period; T _sbe roll channel position regulating time, this parameter is relevant with stable platform step response index and value should be less than stable platform step response index.In the preferred embodiment, T=5ms is got; T _s=300ms.

Because the position command of taking pictures between navigation channel is a step instruction, direct coal addition position loop can cause the overshoot that aerial camera rotation process is larger.For this reason, the factors such as the position of the present invention residing for stable platform, the position that will forward to, regulating time, control cycle are planned position command according to desirable movement locus, generate the planned position instruction curve (see Fig. 3) of position loop.This curve is using 1/4 cycle before dextrorotation curve as typical curve, and like this, the position command curve cooked up is exactly a process of approaching gradually.The leading portion variable quantity of this position command curve is very large, and stable platform can be made to have high acceleration; But arrived the back segment of regulating time, the variable quantity of this position command curve diminishes and gradually becomes zero; When the optical axis of camera lens is close to target location, the acceleration of stable platform is just in time zero, and like this, camera lens just can avoid significantly galloping motion at whole rotation process.

5th step, call attitude algorithm subroutine and perform following operation steps according to Fig. 2:

5.1, gather the carrier aircraft roll angle θ of the current output of vertical gyro _{ro_v}with carrier aircraft pitching angle theta _{el_v}; Gather the angle position signal θ of the roll outside framework of the current output of roll rotary transformer _{ro_r}with the angle position signal θ of the pitching inner frame of the current output of pitching rotary transformer _{el_r}.

5.2, organize formulae discovery according to next:

θ _{el_los}＝arcsin(a ₃₁cosθ _{el_r}sinθ _{ro_r}+a ₃₂sinθ _{el_r}+a ₃₃cosθ _{el_r}cosθ _{ro_r})

${\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{los}}=\arctan \left(\frac{a_{11}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r}\sin {\mathrm{\&theta;}}_{\mathrm{ro}\_r}+a_{12}{\sin \mathrm{\&theta;}}_{\mathrm{el}\_r}+a_{13}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r}\cos {\mathrm{\&theta;}}_{\mathrm{ro}\_r}}{a_{31}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r}\sin {\mathrm{\&theta;}}_{\mathrm{ro}\_r}+a_{32}\sin {\mathrm{\&theta;}}_{\mathrm{el}\_r}{+a}_{33}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r}\cos {\mathrm{\&theta;}}_{\mathrm{ro}\_r}}\right)$

a ₁₁＝cosθ _{ro_v}

a ₁₂＝0

a ₁₃＝sinθ _{ro_v}

a ₃₁＝-sinθ _{ro_v}cosθ _{el_v}

a ₃₂＝sinθ _{el_v}

a ₃₃＝cosθ _{ro_v}cosθ _{el_v}

In formula, θ _{el_los}for the current pitch attitude angle of camera boresight, θ _{ro_los}for the current roll attitude angle of camera boresight.

6th step, judges i < T _s/ T, if yes, proceeds to the 7th step, if NO, proceeds to the 8th step.

7th step, substitutes into the planned position controlling curve of roll passage by i, obtain planned position instruction θ _{ro_cmd_i}, then proceed to the 9th step.

8th step, the roll channel position instruction θ that aerial camera system controller is provided _{ro_cmd}as the planned position instruction θ of current roll passage _{ro_cmd_i}even, θ _{ro_cmd_i}=θ _{ro_cmd}and proceed to the 9th step.

9th step, carries out roll passage and pitch channel position loop computing.

First, according to the planned position instruction θ of roll passage _{ro_cmd_i}with the current roll attitude angle θ of camera boresight _{ro_los}carry out roll channel position loop summation operation, obtain roll site error amount p _{ro_err}; According to pitch channel position command θ _{el_cmd}=0 and the current pitch attitude angle θ of camera boresight _{el_los}carry out pitch channel position loop summation operation, obtain pitch position margin of error p _{el_err};

p _{ro_err}＝θ _{ro_cmd_i}－θ _{ro_los}

p _{el_err}＝θ _{el_cmd}－θ _{el_los}

Then, adopt pi regulator 1 to carry out position loop to resolve.By roll site error amount p _{ro_err}with pitch position margin of error p _{el_err}the position loop that feeding pi regulator 1 carries out roll passage and pitch channel is respectively resolved, and obtains roll channel rate circuit controls amount ω respectively _{ro_cmd}with pitch channel rate loop controlled quentity controlled variable ω _{el_cmd}; The algorithm model of pi regulator 1 is:

${\mathrm{\&omega;}}_{\mathrm{ro}\_\mathrm{cmd}}=k_{\mathrm{pr}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{pr}}+1}{s}{p}_{\mathrm{ro}\_\mathrm{err}}$

${\mathrm{\&omega;}}_{\mathrm{el}\_\mathrm{cmd}}=k_{\mathrm{pe}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{pe}}+1}{s}{p}_{\mathrm{el}\_\mathrm{err}}$

In formula, k _pr, k _pebe respectively the gain coefficient of pi regulator 1 roll passage and pitch channel, ω _pr, ω _pebe respectively the integrator controling parameters of pi regulator 1 roll passage and pitch channel, above-mentioned four parameters all obtain according to test adjustment.In the preferred embodiment, k is got _pr=0.8, k _pe=0.6, ω _pr=1.88, ω _pe=1.88.

Tenth step, carries out roll passage and the computing of pitch channel rate loop.

First the angle rate signal ω of the roll outside framework of the current output of twin shaft rate gyro is gathered _rowith the angle rate signal ω of pitching inner frame _el, and adopt two angle rate signal ω that low-pass first order filter exports twin shaft rate gyro _ro, ω _elcarry out filtering, obtain roll channel filtering signal ω respectively _ro1with pitch channel filtering signal ω _el1, the model of low-pass first order filter used is:

${\mathrm{\&omega;}}_{\mathrm{ro}1}=\frac{1}{1+S/{\mathrm{\&omega;}}_{1\mathrm{p\; ro}}}{\mathrm{\&omega;}}_{\mathrm{ro}}$

${\mathrm{\&omega;}}_{\mathrm{el}1}=\frac{1}{1+S/{\mathrm{\&omega;}}_{1\mathrm{p\; el}}}{\mathrm{\&omega;}}_{\mathrm{el}}$

In formula, ω _{lp_ro}for the corner frequency of low-pass first order filter roll passage, ω _{lp_e1}for the corner frequency of low-pass first order filter pitch channel, above-mentioned two parameters all obtain according to test adjustment.In the preferred embodiment, ω is got _{lp_ro}=503, ω _{lp_e1}=503.

Next speed loop summation operation is carried out.According to roll channel rate circuit controls amount ω _{ro_cmd}with pitch channel rate loop controlled quentity controlled variable ω _{el_cmd}and roll channel filtering signal ω _ro1with pitch channel filtering signal ω _el1carry out speed loop summation operation, obtain the velocity error amount ω of roll passage _{err_ro}with the velocity error amount ω of pitch channel _{err_el}:

ω _{ro_err}＝ω _{ro_cmd}－ω _ro1

ω _{el_err}＝ω _{el_cmd}－ω _el1

Then adopt pi regulator 2 to carry out rate loop again to resolve, by the velocity error amount ω of roll passage _{err_ro}with the velocity error amount ω of pitch channel _{err_el}feeding pi regulator 2 carries out rate loop and resolves, and obtains the controlled quentity controlled variable I of roll passage pwm power amplifier respectively _{cmd_r}with the controlled quentity controlled variable I of pitch channel pwm power amplifier _{cmd_e}.The algorithm model of pi regulator 2 used is:

$I_{\mathrm{ro}\_\mathrm{cmd}}=k_{\mathrm{gr}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{gr}}+1}{s}{\mathrm{\&omega;}}_{\mathrm{ro}\_\mathrm{err}}$

$I_{\mathrm{el}\_\mathrm{cmd}}=k_{\mathrm{ge}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{ge}}+1}{s}{\mathrm{\&omega;}}_{\mathrm{el}\_\mathrm{err}}$

In formula, k _gr, k _gebe respectively the gain coefficient of pi regulator 2 roll passage and pitch channel, ω _gr, ω _gebe respectively the integrator controling parameters of pi regulator 2 roll passage and pitch channel, above-mentioned four parameters all obtain according to test adjustment.In the preferred embodiment, k is got _gr=8.6, k _ge=6.3, ω _gr=62.8, ω _ge=50.2.

11 step, by the controlled quentity controlled variable I of roll passage pwm power amplifier _{cmd_r}with the controlled quentity controlled variable I of pitch channel pwm power amplifier _{cmd_e}be applied to roll channel power amplifier and pitch channel power amplifier respectively, two channel amplifiers produce driving moment, control roll motor and pitching motor rotation respectively.

12 step, makes variable i add 1, i.e. i=i+1.

13 step, judges i > T _c/ T, if yes, proceeds to the 14 step, if NO, proceeds to the 15 step.T _cfor stable platform step response index, that is camera taking pictures the moment within each work period.In the preferred embodiment, stable platform step response index T _c=400ms, namely require that stable platform moves to the location point required by the instruction of roll channel position in 400ms, when stable platform arrives this location point, camera could start to take pictures.

14 step, notice camera starts takes pictures.

15 step, judges i > T _p/ T, if yes, proceeds to the 16 step, if NO, returns the 5th step.T _pfor the time interval of roll channel position instruction, this parameter is aerial camera system assignment to the work period of stable platform and camera.That is work period is by corresponding stable platform step and camera carries out once photo taking.In the preferred embodiment, the work period of aerial camera system requirements stable platform and camera is 600ms, therefore, gets T _p=600ms.

Whether the 16 step, receive end-of-job instruction, if NO, then returns second step, and if YES, then servo control module is out of service.

Fig. 4 a be do not carry out the instruction of roll channel position planning stable platform servo-control system to the response curve of step position instruction, in figure, curve 1 is position command, curve 2 is control system response, can find out that the overshoot of nearly 30% has appearred in this stable platform servo-control system, regulating time is greater than 400ms, therefore can not meet the requirement that camera system rotates between navigation channel.

Fig. 4 b adopts stable platform servo-control system of the present invention to the response curve of step position instruction, in figure, curve 1 is the position command after planning, curve 2 is control system response, can find out that stable platform servo-control system reaches stable state after 300ms, completely eliminate overshoot, regulating time is less than 400ms.

Claims (2)

1. an aerial camera stable platform non-overshoot method of servo-controlling, it is characterized in that: the method is realized by the DSP being equipped with servo control module, servo control module contains roll passage and pitch channel, and after DSP powers on, servo control module performs following operation steps:

The first step, judges whether to receive " starting working " instruction that aerial camera system controller sends, and if NO, waits for, if yes, proceeds to second step;

Second step, receives the roll channel position instruction θ that aerial camera system controller provides _{ro_cmd}; Even pitch channel position command is set to zero θ _{el_cmd}=0; Make variable i=1;

3rd step, gathers the carrier aircraft initial horizontal roll angle θ that vertical gyro exports _{ro_v0}with initial pitch angle θ _{el_v0}, gather the roll outside framework initial angle position signal θ that roll rotary transformer exports _{ro_r0}with the pitching inner frame initial angle position signal θ that pitching rotary transformer exports _{el_r0}, and calculate according to following formula:

${\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{los}0}=\arctan \left(\frac{a_{11}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r0}\sin {\mathrm{\&theta;}}_{\mathrm{ro}\_r0}+a_{12}\sin {\mathrm{\&theta;}}_{\mathrm{el}\_r0}+a_{13}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r0}\cos {\mathrm{\&theta;}}_{\mathrm{ro}\_r0}}{a_{31}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r0}\sin {\mathrm{\&theta;}}_{\mathrm{ro}\_r0}+a_{32}\sin {\mathrm{\&theta;}}_{\mathrm{el}\_r0}0a_{33}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r0}\cos {\mathrm{\&theta;}}_{\mathrm{ro}\_r0}}\right)$

a ₁₁＝cosθ _{ro_v0}

a ₁₂＝0

a ₁₃＝sinθ _{ro_v0}

a ₃₁＝-sinθ _{ro_v0}cosθ _{el_v0}

a ₃₂＝sinθ _{el_v0}

a ₃₃＝cosθ _{ro_v0}cosθ _{el_v0}

In formula, θ _{ro_los0}for the initial roll attitude angle of camera boresight;

4th step, generates the planned position instruction curve of roll passage:

${\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{cmd}\_i}=\left[\mathrm{Sin}(2\mathrm{\&pi;}\&times;\left(\frac{T\&times;i}{4\&times;T_s}\right))\right]({\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{cmd}}-{\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{los}0})+{\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{los}0}$

In formula, θ _{ro_cmd_i}represent the planned position instruction corresponding with variable i; T is the sampling period; T _sfor roll channel position regulating time;

5th step, gathers the carrier aircraft roll angle θ of the current output of vertical gyro _{ro_v}with carrier aircraft pitching angle theta _{el_v}, gather the angle position signal θ of the roll outside framework of the current output of roll rotary transformer _{ro_r}with the angle position signal θ of the pitching inner frame of the current output of pitching rotary transformer _{el_r}, and organize formulae discovery according to next:

θ _{el_los}＝arcsin(a ₃₁cosθ _{el_r}sinθ _{ro_r}+a ₃₂sinθ _{el_r}+a ₃₃cosθ _{el_r}cosθ _{ro_r})

${\mathrm{\&theta;}}_{\mathrm{ro}\_\mathrm{los}}=\arctan \left(\frac{a_{11}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r}\sin {\mathrm{\&theta;}}_{\mathrm{ro}\_r}+a_{12}\sin {\mathrm{\&theta;}}_{\mathrm{el}\_r}+a_{13}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r}\cos {\mathrm{\&theta;}}_{\mathrm{ro}\_r}}{a_{31}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r}\sin {\mathrm{\&theta;}}_{\mathrm{ro}\_r}+a_{32}\sin {\mathrm{\&theta;}}_{\mathrm{el}\_r}+a_{33}\cos {\mathrm{\&theta;}}_{\mathrm{el}\_r}\cos {\mathrm{\&theta;}}_{\mathrm{ro}\_r}}\right)$

a ₁₁＝cosθ _{ro_v}

a ₁₂＝0

a ₁₃＝sinθ _{ro_v}

a ₃₁＝-sinθ _{ro_v}cosθ _{el_v}

a ₃₂＝sinθ _{el_v}

a ₃₃＝cosθ _{ro_v}cosθ _{el_v}

In formula, θ _{el_los}for the current pitch attitude angle of camera boresight, θ _{ro_los}for the current roll attitude angle of camera boresight;

6th step, judges i < T _s/ T, if yes, proceeds to the 7th step, if NO, proceeds to the 8th step;

7th step, substitutes into the planned position controlling curve of roll passage by i, obtain planned position instruction θ _{ro_cmd_i}, then proceed to the 9th step;

8th step, the roll channel position instruction θ that aerial camera system controller is provided _{ro_cmd}as the planned position instruction θ of current roll passage _{ro_cmd_i}even, θ _{ro_cmd_i}=θ _{ro_cmd}and proceed to the 9th step;

9th step, carry out roll passage and pitch channel position loop computing:

9.1 according to following formulae discovery site error:

p _{ro_err}＝θ _{ro_cmd_i}－θ _{ro_los}

p _{el_err}＝θ _{el_cmd}－θ _{el_los}

In formula, p _{ro_err}for roll site error amount, p _{el_err}for the pitch position margin of error;

9.2 adopt pi regulator 1 to carry out position loop resolves:

${\mathrm{\&omega;}}_{\mathrm{ro}\_\mathrm{cmd}}=k_{\mathrm{pr}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{pr}}+1}{s}{p}_{\mathrm{ro}\_\mathrm{err}}$

${\mathrm{\&omega;}}_{\mathrm{el}\_\mathrm{cmd}}=k_{\mathrm{pe}}\frac{s/{\mathrm{\&omega;}}_{\mathrm{pe}}+1}{s}{p}_{\mathrm{el}\_\mathrm{err}}$

In formula, ω _{ro_cmd}for roll channel rate circuit controls amount, ω _{el_cmd}for pitch channel rate loop controlled quentity controlled variable, k _prfor the gain coefficient of pi regulator 1 roll passage, k _pefor the gain coefficient of pi regulator 1 pitch channel, ω _prfor the integrator controling parameters of pi regulator 1 roll passage, ω _pefor the integrator controling parameters of pi regulator 1 pitch channel;

Tenth step, carry out roll passage and the computing of pitch channel rate loop:

The angle rate signal ω of the roll outside framework of the current output of 10.1 collection twin shaft rate gyro _rowith the angle rate signal ω of pitching inner frame _el, and adopt low-pass first order filter to carry out filtering to the output signal of twin shaft rate gyro:

${\mathrm{\&omega;}}_{\mathrm{ro}1}=\frac{1}{1+S/{\mathrm{\&omega;}}_{\mathrm{lp}\_\mathrm{ro}}}{\mathrm{\&omega;}}_{\mathrm{ro}}$

${\mathrm{\&omega;}}_{\mathrm{el}1}=\frac{1}{1+S/{\mathrm{\&omega;}}_{\mathrm{lp}\_\mathrm{el}}}{\mathrm{\&omega;}}_{\mathrm{el}}$

In formula, ω _ro1for roll channel filtering signal, ω _el1for pitch channel filtering signal, ω _{lp_ro}for the corner frequency of low-pass first order filter roll passage, ω _{lp_e1}for the corner frequency of low-pass first order filter pitch channel;

10.2, according to following formulae discovery velocity error amount:

ω _{ro_err}＝ω _{ro_cmd}－ω _ro1

ω _{el_err}＝ω _{el_cmd}－ω _el1

In formula, ω _{err_ro}for the velocity error amount of roll passage, ω _{err_el}for the velocity error amount of pitch channel;

10.3, adopt pi regulator 2 to carry out rate loop and resolve:

$I_{\mathrm{ro}\_\mathrm{cmd}}=k_{\mathrm{gr}}\frac{S/{\mathrm{\&omega;}}_{\mathrm{gr}}+1}{s}{\mathrm{\&omega;}}_{\mathrm{ro}\_\mathrm{err}}$

$I_{\mathrm{el}\_\mathrm{cmd}}=k_{\mathrm{ge}}\frac{S/{\mathrm{\&omega;}}_{\mathrm{ge}}+1}{s}{\mathrm{\&omega;}}_{\mathrm{el}\_\mathrm{err}}$

In formula, I _{cmd_r}for the controlled quentity controlled variable of roll passage pwm power amplifier, I _{cmd_e}for the controlled quentity controlled variable of pitch channel pwm power amplifier, k _grfor the gain coefficient of pi regulator 2 roll passage, k _gefor the gain coefficient of pi regulator 2 pitch channel, ω _grfor pi regulator 2 roll tunnel integrator controling parameters, ω _gefor the integrator controling parameters of pi regulator 2 pitch channel;

11 step, by the controlled quentity controlled variable I of roll passage pwm power amplifier _{cmd_r}be applied to roll channel power amplifier, by the controlled quentity controlled variable I of pitch channel pwm power amplifier _{cmd_e}be applied to pitch channel power amplifier;

12 step, makes variable i add 1, i.e. i=i+1;

13 step, judges i > T _c/ T, if yes, proceeds to the 14 step, if NO, proceeds to the 15 step, T _cfor stable platform step response index;

14 step, notice camera starts takes pictures;

15 step, judges i > T _p/ T, if yes, proceeds to the 16 step, if NO, returns the 5th step, T _pfor the time interval of roll channel position instruction;

Whether the 16 step, receive end-of-job instruction, if NO, then returns second step, and if YES, then servo control module is out of service.

2. aerial camera stable platform non-overshoot method of servo-controlling according to claim 1, is characterized in that: sampling period T=5ms; Roll channel position regulating time T _s=300ms; Stable platform step response index T _c=400ms; The time interval T of roll channel position instruction _p=600ms; Pi regulator 1 roll channel gain coefficient k _pr=0.8, pi regulator 1 pitch channel gain coefficient, k _pe=0.6; The integrator controling parameters ω of pi regulator 1 roll passage _pr=1.88, the integrator controling parameters ω of pi regulator 1 pitch channel _pe=1.88; The corner frequency ω of low-pass first order filter roll passage _{lp_ro}=503, the corner frequency ω of low-pass first order filter pitch channel _{lp_e1}=503; Pi regulator 2 roll channel gain coefficient k _gr=8.6, pi regulator 2 pitch channel gain coefficient k _ge=6.3; The integrator controling parameters ω of pi regulator 2 roll passage _gr=62.8, the integrator controling parameters ω of pi regulator 2 pitch channel _ge=50.2.