CN1047241C - Filter servo method and system - Google Patents

Abstract

The present invention discloses a software filter servo method and a system thereof. In the method, according to sampled data of main and follow-up signals, servo signals are calculated by limiting amplitude correction or weighting redundancy, and filtering processing and the automatic regulation of parameters can be realized. The system adopts a form of the combination of a sensor and a computer, a servo mechanism is omitted, and electromechanical parameter matching is avoided; the system obviously enhances reliability, lowers manufacturing cost, simplifies design, production, installation, maintenance and debugging, improves performance, and realizes one machine with multiple systems, convenient error correction and precision increase. A pendulum sensor-gyro bearing attitude system using the method can use a common program and calculation to eliminate frame errors.

Description

Filter servo method and system thereof

The present invention relates to a kind of filter servo method and make in this way filter servo system.Specifically, this method is applicable to that the active signal that often has the alternation error that the aggressive device in this system is provided carries out filtering, then in order to revise the follow-up signal that has drift error that hunting gear provides, exports its servosignal through revising.

Existing this filter servo method and make in this way the different filter servo system of various functions all use servo control mechanism to drive hunting gear or its follow-up signal sensor, make its output servo in the engineering detecting field in the servosignal of active signal; The difference of its servo speed and servosignal and active signal is proportional, to the filter action of active signal by reducing this ratio or realizing for this difference amplitude limit.This filter servo system is complex structure not only, and has increased many restrictive requirements and supporting adjustment test for making each parts have supporting interchangeability; However, the system support debugging is still very difficult, and all will debug again after installing at every turn and keeping in repair, and makes system cost be much higher than the parts sum, and reliability is significantly less than each parts, and dynamic and static state performance is not good, and loading and unloading are also inconvenient; In addition, this filter servo method lacks selectivity to its working method and parameter, more can not regulate parameter automatically, is difficult to take appropriate ancillary method for improving performance, for example mistake is whole will to adopt cam band correction mechanism pointwise adjustment in order to revise, and both inconvenience was also inaccurate.

The purpose of this invention is to provide a kind of filter servo method and make in this way filter servo system, it is simple in structure, supporting convenience; Reliable operation, cost is lower.

For achieving the above object, filter servo method of the present invention adopts a kind of data operating method, makes filter servo system in this way realize the filter servo function by software.

This filter servo method is to gather real-time continuous synchrodata main, follow-up signal, and follows these steps to carry out data operation:

A) according to master, follow-up signal synchronously sampled data or through considering the data x that ripple is handled ₀, y ₀, the initial correction amount of calculating follow-up signal sampled data

Δ ₀=x ₀-y ₀；

B), follow-up signal synchronously sampled data x main according to above-mentioned revised (the i time) each time _i, y _jBefore this (the j time) calculate the follow-up signal sampled data correction amount of gained _j, by to (Δ _j+ y _i) and x _jBe weighted remaining and calculate, or use (Δ _j+ y _j-x _j) the value correction (Δ of amplitude limit _j+ y _j) method, obtain servosignal real time data Z _i

C), calculate the correction of this sampled data of follow-up signal according to the sampling computational data

Δ _i=z _i-y _i

For the usefulness of after this calculating the servosignal real time data.

Said method can be to use following formula or its equivalent transformation formula, calculates the servosignal real time data $z_j=\frac{{\mathrm{\Δ}}_j+y_i+k{x}_i}{1+k}$ Or $z_j=\frac{{\mathrm{\Δ}}_j+y_j+k{x}_i^{\′}}{1+k}$ Wherein The given function of k---given constant or sampling computational data; Δ X ₀---given constant;

The aforementioned calculation Δ ₀Used data x through the processing of worry ripple ₀, y ₀Can be with a collection of real-time continuous synchronously sampled data of master, follow-up signal, be arranged in ordered series of numbers x in chronological order ₀₁, x ₀₂X _0jAnd y ₀₁, y ₀₂Y _0j, or the ordered series of numbers Δ of its difference ₀₁, Δ ₀₂Δ _0i, and carry out medium filtering, get then at a distance of farthest two peak values or the assembly average of all data between the valley.

Aforementioned calculation Z _jThe given function of used sampling computational data can be $k=\frac{k^{\′}}{|z_j-x_i|}$ X wherein _j, Z _j---the active signal sampled data of (the j time) gained and the servosignal real time data of calculating gained before this sampling are Δ _jThe synchronized sampling computational data,

K '---given constant.

The filter servo system that uses said method also comprises sampling apparatus and computing machine except master that main, follow-up signal is provided, hunting gear; Hunting gear and follow-up signal sensor thereof are not driven; Sampling apparatus carries out the real-time continuous synchronized sampling to master, follow-up signal, and is defeated by computing machine; The program of computer installation comprises carries out the described data operation program of above-mentioned filter servo method, and storage is relevant to given data, and exports the real time continuous data of servosignal through data operation.

This filter servo system can also comprise mode selector or the special instruction line that can directly send instructions to computing machine; Corresponding state address of computer installation and state option program according to manually-operated mode selector or by the different instruction that special instruction line of other device control sends, select also to carry out the described different data operation program of said method.

The advantage of filter servo method of the present invention and system thereof is: (1) does not need servo control mechanism, simplifies the structure; (2) system does not have machinery and electric adjusting factor, has simplified the auxiliary work after production, the installation, maintenance; (3) avoid machinery, electric parameter coupling, do not needed to limit the Performance Characteristics of driving and driven device, improved interchangeability; (4) reduce cost; (5) significantly improve reliability; (6) system stability and the quality of regulation problem of having avoided factors such as insensitive sum of errors gap that servo control mechanism brings, inertia to cause improved performance; (7) can change filter servo working method and parameter easily as required, various states is set, and can regulate parameter (as k) automatically; Realize optimal selection; (8) can utilize computing machine to carry out various corrections and compensation, improve parts and system accuracy; (9) can realize that multisystem is comprehensive, except driving and driven device, other parts even software all can be shared.

Below in conjunction with accompanying drawing with software filtering servo-type orientation attitude system as embodiment, the present invention is described in further detail.

Fig. 1 is a software filtering servo-type orientation attitude system structured flowchart;

Fig. 2 is the operation program block diagram of software filtering servo-type orientation attitude system.

As shown in Figure 1, the described software filtering servo-type of present embodiment orientation attitude system comprises pendulum-type magnetic azimuth sensor 3, frame-type traverse gyro 4, two attitude sensors 5 of pendulum-type, frame-type vertical gyro 6, sampling apparatus 2, computing machine 1, mode selector 7 and the special instruction line 8 of directly sending instructions for computing machine 1; Actual is shared sampling apparatus 2, computing machine 1, mode selector 7, the orientation of special instruction line 8 and data operation program shown in Figure 2, roll, pitching three cover filter servo systems.Wherein, sampling apparatus 2 is gathered the bearing signal x that pendulum-type magnetic azimuth sensor 8 provides _ψ, the bearing signal y that frame-type traverse gyro 4 provides _ψ, the roll signal x that two attitude sensors 5 of pendulum-type provide _γWith pitching signal x _β, the roll signal y that provides of frame-type vertical gyro 6 _γWith pitching signal y _θThe real-time continuous synchrodata, and the input computing machine 1; Mode selector 7 is by hand control, can send " once sampling is revised ", " continuous sampling correction ", " servo interrupt ", " servo at a slow speed ", " middling speed is servo " or " fast response servo " each status command to computing machine 1, also can place " not having instruction " state; Special instruction line 8 is by linear accelerometer on the carrier outside the native system and the control of centrifugal acceleration meter, and its instruction is equivalent with " servo interrupt " instruction of mode selector; Computing machine 1 is provided with corresponding state address and state option program, and the regulation special instruction has precedence over mode selector instruction, and the corresponding operation program of various instructions shown in Figure 2 also is set.The applicable situation of its different conditions, program condition precedent and data operation formula are as follows:

A) the correction state of once sampling: the correction state is used to calculate the initial correction value Δ of gyro (comprise traverse gyro and vertical gyro, down with) output signal sampled data ₀Because when carrier carries out rectilinear motion or turning, single error will appear in sensor (comprising two attitude sensors of pendulum-type magnetic azimuth sensor and pendulum-type, down together), so the regulation revision directive is only effective when not having special instruction.When carrier be in static or linear uniform motion and undervibration when causing sensor output signal stagger to occur, can be by manually sending the revision directive of once sampling, computing machine is according to a sampled data x of sensor and gyro output signal ₀And y ₀, the initial correction value of calculating gyro output signal

Δ ₀=x ₀-y ₀； (1)

B) continuous sampling correction state: when carrier vibrates, jolts, wave etc. when causing sensor output signal stagger to occur, sampled data all is incredible each time, this moment can be by manually sending the continuous sampling revision directive, and computing machine is according to a collection of sampled data x of sensor and gyro output signal ₀₁, x ₀₂X _0iAnd y ₀₁, y ₀₂Y _0i, after certain routine processes, obtain Δ ₀, its program is: elder generation's basis is (the i time) sampled data each time, calculated difference

Δ ₀₁=x _0i-y _0i(2) the ordered series of numbers Δ to arranging in chronological order again ₀₁, Δ ₀₂Δ _0iCarry out medium filtering, reject wherein exceptional value and its envelope made in order and be to carry out the peak-to-valley value addressing at last by the sinuous pulsation curve, and get at a distance of farthest two peak values or the assembly average Δ of all data between the paddy letter _Ccp, and order

Δ ₀=Δ _ccp (3)

C) servo interrupt state: when carrier accelerated motion or turning, the single error of sensor output signal is quite big, thereby insincere, at this moment, if interruption is servo, though gyro has drift error, still the influence of the single error of ratio sensor is much smaller in the long duration.So, can or can instruct according to special instruction by manually sending servo interrupt, computing machine is carried out following operation program: the servosignal real time data

Z _j=Δ _i+y _i (4)

Following formula is actually and makes Z _iThe fundamental operation formula $z_j=\frac{{\mathrm{\Δ}}_j+y_i+k{x}_i}{1+k}$

In given constant k=0,

Y wherein _i---real-time (the i time) sampled data of gyro output signal

x _i---the real-time sampling data of sensor output signal

Δ _j---the correction of (the j time) before this sampled data of gyro output signal

D) servo condition at a slow speed: servo condition can only be used for the condition that sensor output signal does not have single error, thus all under the prerequisite of first special instruction by manually sending.Violent or when occurring jolting, waving etc., the alternation error of sensor output signal is bigger when carrier vibration.For preventing that its output signal filtering from thoroughly not making the servosignal real time data copying error occur, can carry out following operation program this moment by manually sending at a slow speed servo instruction, computing machine: the servosignal real time data $z_i=\frac{{\mathrm{\Δ}}_j+y_i+k_L{x}_i^{\′}}{1+k_L}---\left(5\right)$

Wherein

Δ x _m---given constant,

k _L---the less value of given constant k,

Δ X _mBe amplitude limit value, k is the algorithm coefficient.Amplitude limit and less k value make the servo correction of each time computing only enough eliminate the drift error of gyro output signal, thereby limit it because the copying error that the sensor output signal swing causes;

E) middling speed servo condition: when carrier vibration, top ripple, wave etc. gentlier or not too through often, the alternation error of sensor output signal is less, for offsetting the drift error of gyro output signal effectively, this moment can be by manually sending the middling speed servo instruction, and computing machine is carried out following operation program: the servosignal real time data $z_j=\frac{{\mathrm{\Δ}}_j+y_i+k_H{x}_i^{\′}}{1+k_H}----\left(7\right)$

K wherein _H---the bigger value of given constant k,

This state still gives amplitude limit, but a little less than the filtering, thereby suitably improve servo velocity;

F) fast response servo state: when carrier vibrates, jolts, waves etc. when very faint, the alternation error of sensor output signal is not enough to influence the servosignal real time data, this moment can be by manually sending the fast response servo instruction, and computing machine is carried out following operation program: the servosignal real time data $z_i=\frac{{\mathrm{\Δ}}_j+y_i+k_H{x}_i}{1+k_H}----\left(8\right)$

This state is refused amplitude limit, and a little less than the filtering, thereby realize fast response servo.

G) constant speed servo condition: when the operator vibrates carrier, jolt, it is unclear or to the accuracy requirement of servosignal real time data when not high the alternation error of the sensor output signal that causes such as to wave, can be by manually mode selector being placed " not having instruction " state, computing machine is carried out following operation program: the servosignal real time data $z_j=\frac{{\mathrm{\Δ}}_j+y_i+k_c{x}_j}{1+k_c}----\left(9\right)$

Wherein $k_c=\frac{k^{\′}}{|z_j-x_j|},----\left(10\right)$

x _j, z _j---the active signal data of (the j time) sampling gained and the servosignal real time data of calculating gained before this (the i time) sampling, be the synchronized sampling computational data of Δ 1,

K '---given constant.

This is the program that computing machine is regulated parameter k automatically.Wherein k ' represents the gyro output signal in the drift error of double sampling in interval time.| z _j-x _j| the approximate representative sensor output signal instantaneous error of sampling before this, so k _cChange with it, play the effect of automatic adjusting amplitude limit letter and algorithm coefficient, be enough to offset the drift error of gyro output signal again.

Above-mentioned c, d, e, f, g state computation Z _iAlso calculate the gyro output signal real time data y of this collection afterwards _iCorrection

Δ _i=Z _i-y _i (11)

For the usefulness of after this calculating the servosignal real time data.

Above-mentioned software filtering servo method calculates the Δ of gained _iAlso can be used as servo control mechanism and eliminate the signal source of gyro output signal error.As long as computing machine output Δ _i, and drive gyroscopic procession, or servomotor and reducing gear drive gyro output signal angular displacement sensor and rotate with respect to the gyro signal axle through D/A, power amplifier and torquer, just constitute the hardware filtering servo-drive system.Azimuth system there is no need to constitute this kind system; But the armature spindle of vertical gyro should not too depart from perpendicular attitude, can use above-mentioned hardware filtering servo method in case of necessity and regularly set upright it.

Above-mentioned bearing servo signal real time data z _{ψ i}The prerequisite that computing formula is set up is that frame-type traverse gyro 4 output signals have only drift error, but the frame-type traverse gyro 4 in the present embodiment is not provided with the servo-actuated carriage, very big bracket error can occur when carrier inclined, need be deducted.This also can be realized by software.Its operation program is:

A) according to carrier from the roll angle servosignal real time data Z of horizontal attitude around the rotation of self longitudinal axis _{γ i}With after the carrier roll again around the angle of pitch servosignal real time data z of self transverse axis rotation _{θ i}, calculate carrier inclined degree real time data α _iWith vergence direction real time data β with respect to self longitudinal axis _i:

_αi=cos ^-1(cosz _θicosz _γi) ${\mathrm{\β}}_i={\mathrm{tg}}^{-1}\frac{-{\cos z}_{\mathrm{\θi}}{\sin z}_{\mathrm{\γi}}}{{\sin z}_{\mathrm{\θi}}}$

B) according to α _i, β _iThe real-time synchronously sampled data y of output signal that changes with the sign carrier longitudinal axis orientation of frame-type traverse gyro 4 _{ψ i}, use relational expression

tg(β _i+y _ψi-2Δy _ψj)=tg(β _j+y _ψj-Δy _ψj)oos _αj

Calculate or adopt iterative approach method approximate treatment bracket error real time data Δ y _{ψ i}

C) according to y _{ψ i}With Δ y _{ψ j}Real time data behind the calculating deduction bracket error

y′ _ψi=y _ψi-Δy _ψj

And with y ' _{ψ i}Substitute y _{ψ 1}For computer azimuth servosignal real time data z _{ψ j}Usefulness.

Claims (7)

1. filter servo method, this method is used for often there being the active signal filtering of alternation error, the follow-up signal that has drift error then in order to correction, export its servosignal through revising, it is characterized in that: gather real-time continuous synchrodata main, follow-up signal, and follow these steps to carry out data operation:

A) according to master, follow-up signal synchronously sampled data or through considering the data x that ripple is handled ₀, y ₀, the initial correction amount of calculating follow-up signal sampled data

Δ ₀=x ₀-y ₀；

B), follow-up signal sampled data x main according to above-mentioned revised (the i time) each time _i, y _iBefore this (the j time) calculate the follow-up signal sampled data correction amount of gained _i, by to (Δ i+Y _i) and X _iBe weighted remaining and calculate, or use (Δ _i+ Y _i-X _i) the value correction (Δ of amplitude limit _i+ Y _i) method, obtain servosignal real time data Z _i

C), calculate the correction of this sampled data of follow-up signal according to the sampling computational data

Δ _i=Z _i-y _i

For the usefulness of after this calculating the servosignal real time data.

2. filter servo method according to claim 1 is characterized in that: use following formula or its equivalent transformation formula, calculate the servosignal real time data $z_j=\frac{{\mathrm{\Δ}}_j+y_i+k{x}_j}{1+k}$ Or $z_i=\frac{{\mathrm{\Δ}}_j+y_i+k{x}_i^{\′}}{1+k}$ Wherein The given function of k---given constant or sampling computational data; Δ X _m---given constant;

Calculate Δ ₀Used data X through the processing of worry ripple ₀, y ₀Be that a collection of real-time continuous synchronously sampled data with master, follow-up signal is arranged in ordered series of numbers X in chronological order ₀₁, X ₀₂... X _0iAnd y ₀₁, y ₀₂... y _0i, or the ordered series of numbers Δ of its difference ₀₁, Δ ₀₂... Δ _0i, and carry out medium filtering; Get then at a distance of farthest two peak values or the assembly average of all data between the valley;

Calculate Z _iThe given function of used sampling computational data $k=\frac{k^{\′}}{|z_i-x_i|}$

X wherein _j, Z _j---the active signal sampled data of (the j time) gained and the servosignal real time data of calculating gained before this sampling are Δ _jThe synchronized sampling computational data,

K '---given constant.

3. filter servo system, it comprises provides the described master of claim 1, the master of follow-up signal, hunting gear, it is characterized in that: it also comprises sampling apparatus and computing machine; Hunting gear and follow-up signal sensor thereof are not driven; Sampling apparatus carries out the real-time continuous synchronized sampling to master, follow-up signal, and is defeated by computing machine; The program of computer installation comprises carries out claim 1 or 2 described data operation programs, stores relevant given constant, and exports the real time continuous data of servosignal through data operation.

4. filter servo according to claim 3 system, it is characterized in that: it also comprises mode selector or the special instruction line that can directly send instructions to computing machine; Corresponding state address of computer installation and state option program according to manually-operated mode selector or by the different instruction that special instruction line of other device control sends, are selected and enforcement of rights requires 1 or 2 described various data operation programs.

5. filter servo according to claim 4 system, aggressive device in this system is a pendulum-type magnetic azimuth sensor, hunting gear is the frame-type traverse gyro, constitute software filtering servo-type azimuth system, it is characterized in that: the state of this mode selector setting also comprises the state of servo interrupt state and different servo speed, is respectively given constant k=0 and the operation program that equals various different values with the corresponding operation program of these states; This special instruction line is controlled by linear accelerometer or the centrifugal acceleration meter on the carrier, and the instruction and the equivalence of servo interrupt status command of acceleration appears in its expression carrier, and other instruction has right of priority to mode selector.

6, filter servo according to claim 5 system, it is characterized in that: this sampling apparatus is also gathered the attitude signal synchrodata that the attitude sensor on the carrier provides; This computing machine also is provided with down the program of column operations: the output signal real-time sampling data y that changes according to the sign carrier longitudinal axis orientation of frame-type traverse gyro _{ψ i}, the attitude sensor on the carrier provide or as calculated and carrier with respect to the real-time synchrodata β of vergence direction of self longitudinal axis _iPut α with step number in real time with degree of tilt _i, use relational expression

Tg (β _i+ y _{ψ i}-2 Δ y _{ψ j})=tg (β _i+ y _{ψ i}-Δ y _{ψ j}) cos α _iCalculate or adopt the real-time synchrodata Δ of iterative approach method approximate treatment bracket error y _{ψ 3}, calculate the real-time effect certificate behind the deduction bracket error then

Y ' _{ψ i}=y _{ψ i}-Δ y _{ψ 1}And with y ' _{ψ i}Substitute y _{ψ i}Usefulness for computer azimuth servosignal real time data.

7. filter servo according to claim 4 system, aggressive device in this system is the pendulum-type attitude sensor, hunting gear is the frame-type vertical gyro, constitute software filtering servo-type attitude system, it is characterized in that: the state of this mode selector setting also comprises the state of servo interrupt state and various different servo speed, is respectively given constant k=0 and the operation program that equals various different values with the corresponding operation program of these states; This special instruction line is controlled by linear accelerometer and the centrifugal acceleration meter on the carrier, and the instruction and the equivalence of servo interrupt status command of acceleration appears in its expression carrier, and other instruction has right of priority to mode selector.

Images