CN104317217B - Aerial camera stabilized platform non-overshoot method of servo-controlling - Google Patents

Abstract

The invention discloses a kind of aerial camera stabilized platform non-overshoot method of servo-controlling, belong to automatic control technology field.It is mainly characterized by, and plans the position command of roll passage, obtains a curve curving into position command angle from initial attitude angle according to 1/4 dextrorotation, and the instruction morphing one-tenth of step position thus camera control system be given seamlessly transits curve；Calculated the attitude angle of aiming line by attitude algorithm algorithm, be derived from stabilized platform Angle Position under geographic coordinate system.The present invention solves the overshoot problem appeared in the step process of existing stabilized platform position, substantially increases the efficiency taken pictures in high-altitude；Meanwhile, it is capable to the sight line controlling camera stabilized platform is accurately directed to target navigation channel under geographic coordinate system.Curve planning algorithm used by the present invention and attitude algorithm algorithm are simple, it is achieved convenient, transplantability is good, thus the present invention has broader practice prospect.

Description

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

Technical field

The invention belongs to control field, relate generally to the method for servo-controlling of a kind of stabilized platform, particularly relate to a kind of suitable Non-overshoot method of servo-controlling for step sweep type aerial camera stabilized platform.

Background technology

Aerial camera system is mainly used in obtaining in the air High Resolution Visible Light target image information from height.Stabilized platform and Camera is the important component part of aerial camera system, and camera typically uses area array CCD digital camera and is fixed on stabilized platform On, stabilized platform, in order to isolate the disturbance of carrier aircraft, provides good working environment for camera, enables camera stably quick Obtain high-resolution target image.

The Chinese patent application of Publication No. CN1825203A discloses a kind of airborne inclined camera photographing device, this device The angle of visual field including stabilized platform and 2～5 high resolution cameras and camera is 44 °, and these cameras are according to certain angle peace It is contained on stabilized platform, to obtain forward sight, backsight, left view, the right aviation image regarded or regard down i.e. 2～5 different angles, wherein Image depending on camera shooting is used for making space model and orthography down, and the image of other angle camera shooting can conduct The texture source of building wall.The working depth of photographic attachment is 2000m.Owing to camera quantity is more and the angle of visual field is relatively big, surely Fixed platform only need to provide the horizontal reference of certain precision, it is not necessary to quickly swings and is achieved with big view field image, thus, photography Device is the most quickly turned and the requirement of non-overshoot the requirement of stabilized platform is relatively low；Additionally, the flying height of carrier aircraft is relatively low, Speed is the slowest, and the disturbance to stabilized platform is less, and therefore, this kind of stabilized platform is easier to realize.

But, for the angle of visual field be 3.1 °, focal length comes more than the high-altitude aircraft camera system that 1m, working depth are 6000m Say, stabilized platform can only be installed a camera, thus big face cannot be realized by installing multiple stage camera on stabilized platform Long-pending shooting.In order to the image of shooting of flying can cover bigger shooting area every time, shooting process needs stabilized platform Drive camera to point to different navigation channel, and the image of different navigation channels shooting is carried out splicing could obtain large area high resolution graphics Picture.To this end, require that stabilized platform can drive camera rapid translating accurately putting in place between adjacent navigation channel, at once lead to after putting in place Know that camera is taken pictures, after having clapped piece image, go to adjacent navigation channel take pictures, therefore the whole process of taking pictures of camera is The circulation saltatory work process of one " start-stop-take pictures ".Take pictures efficiency to improve high-altitude, it is desirable to stabilized platform There is navigation channel conversion non-overshoot, Fast Convergent, the characteristic that the conversion time is short.Due to stabilized platform load i.e. camera quality relatively Greatly, the contradiction of therefore stabilized platform rapid translating between each navigation channel and non-overshoot to be a pair be difficult to balance.Additionally, in order to protect Card image quality, the rate stabilization of inertial space ensures that accurate geographical coordinate refers to again also to require stabilized platform to ensure To, the most also require that camera is taken a picture and there is certain Duplication, so, stabilized platform is accomplished by setting up the geographical seat of oneself Mark benchmark, and traditional heading control platform is not possess this function.

Summary of the invention

The technical problem to be solved in the present invention is, for aerial camera system steady needing the shooting of many navigation channels rapid translating Fixed platform, it is provided that a kind of non-overshoot method of servo-controlling, the stabilized platform using this method of servo-controlling can not only be at inertia Space keeps stable, also is able to carry out the rotation of phase step type position simultaneously under geographic coordinate system, thus drives camera to different Take pictures in geographical position.

For solving above technical problem, the non-overshoot method of servo-controlling that the present invention provides is by being built-in with servo control module DSP realize, servo control module contains roll passage and pitch channel, after DSP powers on, servo control module perform below Operating procedure:

The first step, it may be judged whether receive " starting working " instruction that aerial camera system controller sends, if it has not, etc. Treat, if it is, proceed to second step；

Second step, receives the roll channel position instruction θ that aerial camera system controller provides_{ro_cmd}；By pitch channel position Put instruction and be set to zero, even θ_{el_cmd}=0；Make variable i=1；

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

${\mathrm{\θ}}_{ro\_los0}=\arctan \left(\frac{a_{11}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{sin\θ}}_{ro\_r0}+a_{12}{\mathrm{sin\θ}}_{el\_r0}+a_{13}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{cos\θ}}_{ro\_r0}}{a_{31}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{sin\θ}}_{ro\_r0}+a_{32}{\mathrm{sin\θ}}_{el\_r0}+a_{33}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{cos\θ}}_{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}Initial roll attitude angle for camera aiming line；

4th step, the planned position controlling curve of generation roll passage:

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

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 θ that vertical gyro currently exports_{ro_v}With carrier aircraft pitching angle theta_{el_v}, gather roll rotation Change the angle position signal θ of the roll outside framework that depressor currently exports_{ro_r}In the pitching currently exported with pitching rotary transformer The angle position signal θ of framework_{el_r}, and calculate according to following set of formula:

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

${\mathrm{\θ}}_{ro\_los}=\arctan \left(\frac{a_{11}{\mathrm{cos\θ}}_{el\_r}{\mathrm{sin\θ}}_{ro\_r}+a_{12}{\mathrm{sin\θ}}_{el\_r}+a_{13}{\mathrm{cos\θ}}_{el\_r}{\mathrm{cos\θ}}_{ro\_r}}{a_{31}{\mathrm{cos\θ}}_{el\_r}{\mathrm{sin\θ}}_{ro\_r}+a_{32}{\mathrm{sin\θ}}_{el\_r}+a_{33}{\mathrm{cos\θ}}_{el\_r}{\mathrm{cos\θ}}_{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 aiming line, θ_{ro_los}Current roll appearance for camera aiming line State angle；

6th step, it may be judged whether i ＜ T_s/ T, if it is, proceed to the 7th step, if it has not, proceed to the 8th step；

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

8th step, the roll channel position that aerial camera system controller is given instruction θ_{ro_cmd}Lead to as current roll The planned position instruction θ in road_{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:

9.1 according to below equation calculating 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 use pi regulators 1 to carry out position loop resolving:

${\mathrm{\ω}}_{ro\_cmd}=k_{pr}\frac{s/{\mathrm{\ω}}_{pr}+1}{s}{p}_{ro\_err}$

${\mathrm{\ω}}_{el\_cmd}=k_{pe}\frac{s/{\mathrm{\ω}}_{pe}+1}{s}{p}_{el\_err}$

In formula, ω_{ro_cmd}For roll channel rate circuit controls amount, ω_{el_cmd}For pitch channel rate loop controlled quentity controlled variable, k_pr For the gain coefficient of pi regulator 1 roll passage, k_peFor the gain coefficient of pi regulator 1 pitch channel, ω_prFor pi regulator 1 The integrator of roll passage controls parameter, ω_peIntegrator for pi regulator 1 pitch channel controls parameter；

Tenth step, carries out roll passage and pitch channel rate loop computing:

The 10.1 angle rate signal ω gathering the roll outside framework that twin shaft rate gyroscope currently exports_roWith pitching inner frame Angle rate signal ω_el, and use low-pass first order filter that the output signal of twin shaft rate gyroscope is filtered:

${\mathrm{\ω}}_{ro1}=\frac{1}{1+S/{\mathrm{\ω}}_{lp\_ro}}{\mathrm{\ω}}_{ro}$

${\mathrm{\ω}}_{el1}=\frac{1}{1+S/{\mathrm{\ω}}_{lp\_el}}{\mathrm{\ω}}_{el}$

In formula, ω_ro1For roll channel filtering signal, ω_el1For pitch channel filtering signal, ω_{lp_ro}For first-order low-pass The corner frequency of ripple device roll passage, ω_{lp_e1}Corner frequency for low-pass first order filter pitch channel；

10.2 according to the below equation computation rate margin of error:

ω_{ro_err}=ω_{ro_cmd}-ω_ro1

ω_{el_err}=ω_{el_cmd}-ω_el1

In formula, ω_{ro_err}For the rate error amount of roll passage, ω_{el_err}Rate error amount for pitch channel；

10.3 use pi regulators 2 to carry out rate loop resolving:

$I_{ro\_cmd}=k_{gr}\frac{s/{\mathrm{\ω}}_{gr}+1}{s}{\mathrm{\ω}}_{ro\_err}$

$I_{el\_cmd}=k_{ge}\frac{s/{\mathrm{\ω}}_{ge}+1}{s}{\mathrm{\ω}}_{el\_err}$

In formula, I_{ro_cmd}For the controlled quentity controlled variable of roll passage pwm power amplifier, I_{el_cmd}Amplify for pitch channel pwm power The controlled quentity controlled variable of device, k_grFor the gain coefficient of pi regulator 2 roll passage, k_geFor the gain coefficient of pi regulator 2 pitch channel, ω_grParameter, ω is controlled for pi regulator 2 roll tunnel integrator_geIntegrator for pi regulator 2 pitch channel controls parameter；

11st step, by controlled quentity controlled variable I of roll passage pwm power amplifier_{ro_cmd}It is applied to roll channel power amplify Device, by controlled quentity controlled variable I of pitch channel pwm power amplifier_{el_cmd}It is applied to pitch channel power amplifier；

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

13rd step, it may be judged whether i ＞ T_c/ T, if it is, proceed to the 14th step, if it has not, proceed to the 15th step, T_cFor Stabilized platform step response index；

14th step, notice camera starts takes pictures；

15th step, it may be judged whether i ＞ T_p/ T, if it is, proceed to the 16th step, if it has not, return the 5th step, T_pFor horizontal stroke The time interval of rolling channel position instruction；

16th step, if receive end-of-job instruction, if NO, then return second step, if YES, then servo control Molding block is out of service.

Beneficial effects 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 standard curve, so plans The position command gone out is the process gradually approached, and therefore, the present invention solves appeared in the step process of stabilized platform position Overshoot problem so that stabilized platform is non-overshoot during the quick rotation of position, substantially increases the efficiency taken pictures in high-altitude.

(2) present invention sets up the geographical coordinate base of stabilized platform by the vertical gyro being arranged on stabilized platform pedestal Standard, obtains sight line Angle Position under geographic coordinate system by attitude algorithm algorithm, feeds back to servo-control system, controls stable The sight line of platform is accurately directed to target navigation channel under geographic coordinate system, thus present invention can ensure that camera system is at carrier aircraft attitude Picture captured under disturbance can mutually splice, and then obtains big visual field picture rich in detail.

(3) it is simple that the attitude algorithm algorithm in the present invention, used and curve planning algorithm have algorithm, it is achieved side Just, transplantability is good, so that the present invention has broader practice 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 flow chart of attitude algorithm subprogram.

Fig. 3 is planned position instruction curve chart 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.

Detailed description of the invention

Below in conjunction with the accompanying drawings 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 is given is at aerial camera stabilized platform (hereinafter referred to as Stabilized platform) SERVO CONTROL computer (DSP) upper realize.Stabilized platform includes roll outside framework and pitching inner frame knot Structure, roll outside framework is arranged in carrier aircraft, electric equipped with roll axle and the roll being used for driving roll axle to rotate on roll outside framework Machine；Pitching inner frame is arranged on roll axle, pitching inner frame with the pitching driving shaft being parallel to each other and pitching driven axle with And it being arranged on 2:1 angle transmission mechanism between the two, pitching motor drives pitching driving shaft to rotate, and reflecting mirror is arranged on pitching quilt On moving axis.Roll rotary transformer is connected on roll axle, and pitching rotary transformer is connected on pitching driving shaft；Twin shaft speed Gyro is connected in surely takes aim at the back side of reflecting mirror and two sensitive axes is parallel with the roll axle of stabilized platform and pitch axis respectively；Hang down On pedestal and two sensitive axes is parallel with the roll axle of stabilized platform and pitch axis respectively for straight gyro installation；The camera lens of camera It is arranged on roll axle.Focus on after being reflected mirror reflection by objective optics image formed by camera lens on the target surface of CCD camera.

DSP is equipped with the servo control module being made up of pitch channel and roll passage, after DSP powers on, SERVO CONTROL Module will perform following operating procedure according to the workflow shown in Fig. 1.

The first step, it may be judged whether receive " starting working " instruction that aerial camera system controller sends, if it has not, etc. Treat, if it is, proceed to second step.

Second step, receives the roll channel position instruction θ that aerial camera system controller provides_{ro_cmd}；By pitch channel position Put instruction and be set to zero, even θ_{el_cmd}=0；Make variable i=1.

Only need due to pitch channel to stablize to turn without carrying out position in geographical zero-bit, the therefore position of pitch channel Instruction is always zero i.e. θ_{el_cmd}=0.

3rd step, calls attitude algorithm subprogram and performs following operating procedure according to Fig. 2:

The 3.1 carrier aircraft initial horizontal roll angle θ gathering vertical gyro output_{ro_v0}With initial pitch angle θ_{el_v0}, gather roll and rotate The roll outside framework initial angle position signal θ of transformator output_{ro_r0}Initial with the pitching inner frame of pitching rotary transformer output Angle position signal θ_{el_r0}。

3.2 calculate according to following set of formula:

${\mathrm{\θ}}_{ro\_los0}=\arctan \left(\frac{a_{11}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{sin\θ}}_{ro\_r0}+a_{12}{\mathrm{sin\θ}}_{el\_r0}+a_{13}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{cos\θ}}_{ro\_r0}}{a_{31}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{sin\θ}}_{ro\_r0}+a_{32}{\mathrm{sin\θ}}_{el\_r0}+a_{33}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{cos\θ}}_{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}Initial roll attitude angle for camera aiming line.

4th step, the planned position controlling curve of generation roll passage:

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

In formula, θ_{ro_cmd_i}Represent the planned position instruction corresponding with variable i；T is the sampling period；T_sIt it is roll channel position Regulating time, this parameter is relevant with stabilized platform step response index and value should be less than stabilized platform step response index.? In this preferred embodiment, take T=5ms；T_s=300ms.

Owing to the position command between navigation channel of taking pictures is a step instruction, it is directly added into position loop and can cause aviation phase The overshoot that machine rotation process is bigger.To this end, the present invention according to stabilized platform location, will forward to position, regulation time Between, the factor such as control cycle according to preferable movement locus, position command is planned, generate the planned position of position loop Instruction curve (seeing Fig. 3).This curve is using 1/4 cycle before dextrorotation curve as standard curve, and so, the position cooked up refers to Making curve is exactly a process gradually approached.The leading portion variable quantity of this position command curve is very big, and stabilized platform can be made to have High acceleration；But having arrived the back segment of regulating time, the variable quantity of this position command curve diminishes and gradually becomes zero；When camera mirror When the optical axis of head is close to target location, the acceleration of stabilized platform is just zero, and so, camera lens just can be in whole rotation Process avoids significantly galloping motion.

5th step, calls attitude algorithm subprogram and performs following operating procedure according to Fig. 2:

5.1 gather the carrier aircraft roll angle θ that vertical gyro currently exports_{ro_v}With carrier aircraft pitching angle theta_{el_v}；Gather roll and rotate change The angle position signal θ of the roll outside framework that depressor currently exports_{ro_r}The pitching inner frame currently exported with pitching rotary transformer Angle position signal θ_{el_r}。

5.2 calculate according to following set of formula:

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

${\mathrm{\θ}}_{ro\_los}=\arctan \left(\frac{a_{11}{\mathrm{cos\θ}}_{el\_r}{\mathrm{sin\θ}}_{ro\_r}+a_{12}{\mathrm{sin\θ}}_{el\_r}+a_{13}{\mathrm{cos\θ}}_{el\_r}{\mathrm{cos\θ}}_{ro\_r}}{a_{31}{\mathrm{cos\θ}}_{el\_r}{\mathrm{sin\θ}}_{ro\_r}+a_{32}{\mathrm{sin\θ}}_{el\_r}+a_{33}{\mathrm{cos\θ}}_{el\_r}{\mathrm{cos\θ}}_{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 aiming line, θ_{ro_los}Current roll appearance for camera aiming line State angle.

6th step, it may be judged whether i ＜ T_s/ T, if it is, proceed to the 7th step, if it has not, proceed to the 8th step.

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

8th step, the roll channel position that aerial camera system controller is given instruction θ_{ro_cmd}Lead to as current roll The planned position instruction θ in road_{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, θ is instructed according to the planned position of roll passage_{ro_cmd_i}Current roll attitude angle with camera aiming line θ_{ro_los}Carry out roll channel position loop summation operation, it is thus achieved that roll site error amount p_{ro_err}；Refer to according to pitch channel position Make θ_{el_cmd}=0 and the current pitch attitude angle, θ of camera aiming line_{el_los}Carry out pitch channel position loop summation operation, obtain 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, pi regulator 1 is used to carry out position loop resolving.By roll site error amount p_{ro_err}With pitch position by mistake Residual quantity p_{el_err}It is respectively fed to pi regulator 1 and carries out the position loop resolving of roll passage and pitch channel, obtain roll respectively and lead to Road rate loop controlled quentity controlled variable ω_{ro_cmd}With pitch channel rate loop controlled quentity controlled variable ω_{el_cmd}；The algorithm model of pi regulator 1 is:

${\mathrm{\ω}}_{ro\_cmd}=k_{pr}\frac{s/{\mathrm{\ω}}_{pr}+1}{s}{p}_{ro\_err}$

${\mathrm{\ω}}_{el\_cmd}=k_{pe}\frac{s/{\mathrm{\ω}}_{pe}+1}{s}{p}_{el\_err}$

In formula, k_pr,k_peIt is respectively pi regulator 1 roll passage and the gain coefficient of pitch channel, ω_pr,ω_peIt is respectively The integrator of pi regulator 1 roll passage and pitch channel controls parameter, and aforementioned four parameter obtains all in accordance with test adjustment.? In this preferred embodiment, take k_pr=0.8, k_pe=0.6, ω_pr=1.88, ω_pe=1.88.

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

First the angle rate signal ω of the roll outside framework that twin shaft rate gyroscope currently exports is gathered_roWith pitching inner frame Angle rate signal ω_el, and use two angle rate signal ω that twin shaft rate gyroscope exports by low-pass first order filter_ro, ω_el It is filtered, obtains roll channel filtering signal ω respectively_ro1With pitch channel filtering signal ω_el1, first-order low-pass ripple used The model of device is:

${\mathrm{\ω}}_{ro1}=\frac{1}{1+S/{\mathrm{\ω}}_{lp\_ro}}{\mathrm{\ω}}_{ro}$

${\mathrm{\ω}}_{el1}=\frac{1}{1+S/{\mathrm{\ω}}_{lp\_el}}{\mathrm{\ω}}_{el}$

In formula, ω_{lp_ro}For the corner frequency of low-pass first order filter roll passage, ω_{lp_e1}Bow for low-pass first order filter Facing upward the corner frequency of passage, above-mentioned two parameter obtains all in accordance with test adjustment.In the preferred embodiment, ω is taken_{lp_ro}= 503, ω_{lp_e1}=503.

Followed by rate loop summation operation.According to roll channel rate circuit controls amount ω_{ro_cmd}And pitch channel Rate loop controlled quentity controlled variable ω_{el_cmd}And roll channel filtering signal ω_ro1With pitch channel filtering signal ω_el1Carry out speed to return Road summation operation, it is thus achieved that rate error amount ω of roll passage_{ro_err}Rate error amount ω with pitch channel_{el_err}:

ω_{ro_err}=ω_{ro_cmd}-ω_ro1

ω_{el_err}=ω_{el_cmd}-ω_el1

Rate loop resolving is carried out, by rate error amount ω of roll passage followed by using pi regulator 2_{ro_err}With bow Face upward rate error amount ω of passage_{el_err}Send into pi regulator 2 and carry out rate loop resolving, obtain roll passage pwm power respectively Controlled quentity controlled variable I of amplifier_{ro_cmd}Controlled quentity controlled variable I with pitch channel pwm power amplifier_{el_cmd}.The algorithm mould of pi regulator 2 used Type is:

$I_{ro\_cmd}=k_{gr}\frac{s/{\mathrm{\ω}}_{gr}+1}{s}{\mathrm{\ω}}_{ro\_err}$

$I_{el\_cmd}=k_{ge}\frac{s/{\mathrm{\ω}}_{ge}+1}{s}{\mathrm{\ω}}_{el\_err}$

In formula, k_gr,k_geIt is respectively pi regulator 2 roll passage and the gain coefficient of pitch channel, ω_gr,ω_geIt is respectively The integrator of pi regulator 2 roll passage and pitch channel controls parameter, and aforementioned four parameter obtains all in accordance with test adjustment.? In this preferred embodiment, take k_gr=8.6, k_ge=6.3, ω_gr=62.8, ω_ge=50.2.

11st step, by controlled quentity controlled variable I of roll passage pwm power amplifier_{ro_cmd}With pitch channel pwm power amplifier Controlled quentity controlled variable I_{el_cmd}Being applied respectively to roll channel power amplifier and pitch channel power amplifier, two channel amplifiers produce Raw driving moment, controls roll motor respectively and pitching motor rotates.

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

13rd step, it may be judged whether i ＞ T_c/ T, if it is, proceed to the 14th step, if it has not, proceed to the 15th step.T_cFor Stabilized platform step response index, that is camera taking pictures the moment within each working cycle.In the preferred embodiment, stable Platform step response index T_c=400ms, i.e. requires that stabilized platform moves to the instruction of roll channel position in 400ms required Location point, when stabilized platform arrives this location point, camera could start to take pictures.

14th step, notice camera starts takes pictures.

15th step, it may be judged whether i ＞ T_p/ T, if it is, proceed to the 16th step, if it has not, return the 5th step.

T_pFor the time interval of roll channel position instruction, this parameter is that aerial camera system distributes to stabilized platform and phase The working cycle of machine.That is a step and the camera of corresponding stabilized platform are carried out once photo taking by a working cycle.At this In preferred embodiment, the working cycle of aerial camera system requirements stabilized platform and camera is 600ms, therefore, takes T_p= 600ms。

16th step, if receive end-of-job instruction, if NO, then return second step, if YES, then servo control Molding block is out of service.

Fig. 4 a is that step position is instructed by the stabilized platform servo-control system not carrying out the instruction planning of roll channel position Response curve, in figure, curve 1 is position command, curve 2 be control system response, it can be seen that this stabilized platform SERVO CONTROL System occurs in that the overshoot of nearly 30%, and regulating time is more than 400ms, therefore can not meet what camera system rotated between navigation channel Requirement.

Fig. 4 b is the response curve using the stabilized platform servo-control system of the present invention to instruct step position, bent in figure Line 1 is the position command after planning, and curve 2 responds for control system, it can be seen that stabilized platform servo-control system is at 300ms After reach stable state, completely eliminate overshoot, regulating time be less than 400ms.

Claims (2)

1. an aerial camera stabilized platform non-overshoot method of servo-controlling, it is characterised in that: the method is by being equipped with SERVO CONTROL The DSP of module realizes, and servo control module contains roll passage and pitch channel, and after DSP powers on, servo control module performs Following operating procedure:

The first step, it may be judged whether receive " starting working " instruction that aerial camera system controller sends, if it has not, wait, If it is, proceed to second step；

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

3rd step, gathers the carrier aircraft initial horizontal roll angle θ of vertical gyro output_{ro_v0}With initial pitch angle θ_{el_v0}, gather roll and rotate The roll outside framework initial angle position signal θ of transformator output_{ro_r0}Initial with the pitching inner frame of pitching rotary transformer output Angle position signal θ_{el_r0}, and calculate according to following formula:

${\mathrm{\θ}}_{ro\_los0}=\arctan \left(\frac{a_{11}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{sin\θ}}_{ro\_r0}+a_{12}{\mathrm{sin\θ}}_{el\_r0}+a_{13}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{cos\θ}}_{ro\_r0}}{a_{31}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{sin\θ}}_{ro\_r0}+a_{32}{\mathrm{sin\θ}}_{el\_r0}+a_{33}{\mathrm{cos\θ}}_{el\_r0}{\mathrm{cos\θ}}_{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}Initial roll attitude angle for camera aiming line；

4th step, the planned position controlling curve of generation roll passage:

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

In formula, θ_{ro_cmd_i}Represent the planned position instruction corresponding with variable i；T is the sampling period；T_sRegulate for roll channel position Time；

5th step, gathers the carrier aircraft roll angle θ that vertical gyro currently exports_{ro_v}With carrier aircraft pitching angle theta_{el_v}, gather roll and rotate change The angle position signal θ of the roll outside framework that depressor currently exports_{ro_r}The pitching inner frame currently exported with pitching rotary transformer Angle position signal θ_{el_r}, and calculate according to following set of formula:

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

${\mathrm{\θ}}_{ro\_los}=\arctan \left(\frac{a_{11}{\mathrm{cos\θ}}_{el\_r}{\mathrm{sin\θ}}_{ro\_r}+a_{12}{\mathrm{sin\θ}}_{el\_r}+a_{13}{\mathrm{cos\θ}}_{el\_r}{\mathrm{cos\θ}}_{ro\_r}}{a_{31}{\mathrm{cos\θ}}_{el\_r}{\mathrm{sin\θ}}_{ro\_r}+a_{32}{\mathrm{sin\θ}}_{el\_r}+a_{33}{\mathrm{cos\θ}}_{el\_r}{\mathrm{cos\θ}}_{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 aiming line, θ_{ro_los}Current roll attitude angle for camera aiming line；

6th step, it may be judged whether i ＜ T_s/ T, if it is, proceed to the 7th step, if it has not, proceed to the 8th step；

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

8th step, the roll channel position that aerial camera system controller is given instruction θ_{ro_cmd}As current roll passage Planned position instruction θ_{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:

9.1 according to below equation calculating 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 use pi regulators 1 to carry out position loop resolving:

${\mathrm{\ω}}_{ro\_cmd}=k_{pr}\frac{s/{\mathrm{\ω}}_{pr}+1}{s}{p}_{ro\_err}$

${\mathrm{\ω}}_{el\_cmd}=k_{pe}\frac{s/{\mathrm{\ω}}_{pe}+1}{s}{p}_{el\_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 PI The gain coefficient of actuator 1 roll passage, k_peFor the gain coefficient of pi regulator 1 pitch channel, ω_prFor pi regulator 1 roll The integrator of passage controls parameter, ω_peIntegrator for pi regulator 1 pitch channel controls parameter；

Tenth step, carries out roll passage and pitch channel rate loop computing:

The 10.1 angle rate signal ω gathering the roll outside framework that twin shaft rate gyroscope currently exports_roAngle speed with pitching inner frame Rate signal ω_el, and use low-pass first order filter that the output signal of twin shaft rate gyroscope is filtered:

${\mathrm{\ω}}_{ro1}=\frac{1}{1+S/{\mathrm{\ω}}_{lp\_ro}}{\mathrm{\ω}}_{ro}$

${\mathrm{\ω}}_{el1}=\frac{1}{1+S/{\mathrm{\ω}}_{lp\_el}}{\mathrm{\ω}}_{el}$

In formula, ω_ro1For roll channel filtering signal, ω_el1For pitch channel filtering signal, ω_{lp_ro}For low-pass first order filter The corner frequency of roll passage, ω_{lp_e1}Corner frequency for low-pass first order filter pitch channel；

10.2 according to the below equation computation rate margin of error:

ω_{ro_err}=ω_{ro_cmd}-ω_ro1

ω_{el_err}=ω_{el_cmd}-ω_el1

In formula, ω_{ro_err}For the rate error amount of roll passage, ω_{el_err}Rate error amount for pitch channel；

10.3 use pi regulators 2 to carry out rate loop resolving:

$I_{ro\_cmd}=k_{gr}\frac{s/{\mathrm{\ω}}_{gr}+1}{s}{\mathrm{\ω}}_{ro\_err}$

$I_{e1\_cmd}=k_{ge}\frac{s/{\mathrm{\ω}}_{ge}+1}{s}{\mathrm{\ω}}_{el\_err}$

In formula, I_{ro_cmd}For the controlled quentity controlled variable of roll passage pwm power amplifier, I_{el_cmd}For pitch channel pwm power amplifier Controlled quentity controlled variable, 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 controls parameter, ω_geIntegrator for pi regulator 2 pitch channel controls parameter；

11st step, by controlled quentity controlled variable I of roll passage pwm power amplifier_{ro_cmd}It is applied to roll channel power amplifier, will Controlled quentity controlled variable I of pitch channel pwm power amplifier_{el_cmd}It is applied to pitch channel power amplifier；

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

13rd step, it may be judged whether i ＞ T_c/ T, if it is, proceed to the 14th step, if it has not, proceed to the 15th step, T_cIt is stable Platform step response index；

14th step, notice camera starts takes pictures；

15th step, it may be judged whether i ＞ T_p/ T, if it is, proceed to the 16th step, if it has not, return the 5th step, T_pLead to for roll The time interval of road position command；

16th step, if receive end-of-job instruction, if NO, then return second step, if YES, then servo control molding Block is out of service.

Aerial camera stabilized platform non-overshoot method of servo-controlling the most according to claim 1, it is characterised in that: sampling week Phase T=5ms；Roll channel position regulating time T_s=300ms；Stabilized platform step response index T_c=400ms；Roll passage Time interval T of position command_p=600ms；Pi regulator 1 roll channel gain coefficient k_pr=0.8, pi regulator 1 pitching is led to Road gain coefficient, k_pe=0.6；The integrator of pi regulator 1 roll passage controls parameter ω_pr=1.88, pi regulator 1 pitching The integrator of passage controls parameter ω_pe=1.88；The corner frequency ω of low-pass first order filter roll passage_{lp_ro}=503, one The corner frequency ω of rank low pass filter pitch channel_{lp_e1}=503；Pi regulator 2 roll channel gain coefficient k_gr=8.6, PI Actuator 2 pitch channel gain coefficient k_ge=6.3；The integrator of pi regulator 2 roll passage controls parameter ω_gr=62.8, PI The integrator of actuator 2 pitch channel controls parameter ω_ge=50.2.