CN101299148A - Three-axis accurate angular motion control method - Google Patents

Abstract

The present invention belongs to a device for simulating the three-axis gesture movement, and relates to a method for controlling the three-axis accurate angular movement. The device timely generates an orthogonal harmonic compensation control signal related to the variation of rotation angle with a control calculating device according to the harmonic characteristic of the disturbance of in-axis coupling torque and real-time calculates and regulates the amplitude of each stage of subharmonic signal according to the servo error of system, and applies the compensation control signal to the independent basic closed-loop servo-controlled loop of the corresponding axis. The invention adopts a strategy and a system structure which execute harmonic self-adapting feedback control to the in-axis movement coupling torque disturbance caused by the structural mass arrangement. The tracking error of position servo between each axis in the linkage angular rate state is evidently reduced, and the linkage capability is greatly improved.

Description

Three-axis accurate angular motion control method

Technical field

The invention belongs to a kind of three-axis attitude motion simulation equipment, relate to a kind of three-axis accurate angular motion control method.

Background technology

Three-axle table (both 3 d pose motion simulation equipment) is the important testing apparatus of a kind of navigation/guidance system, sensor for the test of inertial sensor and navigational system provides accurate angle location positioning, accurate attitude-simulatings such as angular velocity motion.Along with the development of technology, multiaxis angular speed interlock (comprising: the angular speed motion simultaneously of two diaxons, another axle location or three angular speed motions simultaneously) has become sensor and the indispensable technological means of system testing.Three-axle table is made up of load installing space 1, interior ring 2, inner axle 3, middle ring 4, middle annulate shaft 5, outer shroud 6, outer annulate shaft 7 and base 8 usually, each control loop includes: ring stand dynamical structure, motor and power driven system, angular motion sensor and measuring system, servo controller and Comprehensive Control unit. (as Fig. 1) wherein: the motion servo controller accept to be measured feedback signal, and constitutes the corresponding closed-loop servo-control system; External command or the corresponding movement locus of inner generation are accepted in the Comprehensive Control unit.

The control of existing three-axle table diagonal motion is by kinematic axis home position separately, the composite control method that the speed closed loop adds feedforward, that is: independently position, speed closed loop are formed basic motion servo control loop for each, a certain proportion of speed, acceleration instruction are comprehensively arrived servo loop as feed-forward signal, to reduce the tracking error of controlled device to the instruction motion.

Above-mentioned control method mainly exists following 2 deficiencies:

1. owing to the architecture quality distribution reason of three-axle table, the multi-shaft interlocked between centers dynamics coupling torque that exists causes the periodic disturbance that control system is changed about corner, make kinematic axis not carry out accurate tracking to instruction, interlock angular speed performance descends, the location is unstable, even closed loop can not reliably working.

2. owing to comprise uncertain and variation (as load, rotating speed etc.) such as the systematic parameter of architecture quality distribution etc., exist difference on the Model Matching between feedforward control link and the controlling object, make conventional feedforward control not play effective inhibiting effect disturbance.

Summary of the invention

The three-axis accurate angular motion control method that the purpose of this invention is to provide a kind of effectively periodic disturbance that the dynamics coupling torque changes the control system corner between Control Shaft.Technical solution of the present invention is, utilize the control calculation element in each control servo period, harmonic characteristic according to the disturbance of between centers coupling torque regularly produces the quadrature harmonic compensation control signal that changes about corner, calculate and adjust the amplitude of each order harmonic signal simultaneously according to system's servo error in real time, and compensating control signal is put on the independently basic closed loop servo control loop of corresponding axis.

The control calculation element in control servo period k according to formula

$u_{\mathrm{CTHAFC}}\left(k\right)=\underset{l=1}{\overset{N}{\mathrm{\Σ}}}(p_{\mathrm{sl}}\left(k\right)\sin ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right))+p_{\mathrm{cl}}\left(k\right)\cos ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)))---\left(1\right)$

And formula

$\left.\begin{array}{c}{p}_{\mathrm{sl}}\left(k\right)={\mathrm{\γ}}_{\mathrm{sl}}{T}_{s}e\left(k\right)\sin ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)\±{\mathrm{\β}}_{\mathrm{sl}})+p_{\mathrm{sl}}(k-1)\\ p_{\mathrm{cl}}\left(k\right)={\mathrm{\γ}}_{\mathrm{cl}}{T}_{s}e\left(k\right)\cos ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)\±{\mathrm{\β}}_{\mathrm{cl}})+p_{\mathrm{cl}}(k-1)\end{array}\right\}---\left(2\right)$

Calculate and generate about m θ _r± n θ _pThe control of each order quadrature harmonic compensation, and to its amplitude p _Sl, p _ClCarry out self-adaptation and calculate and adjust in real time, utilize the initial phase shifting angle β that is provided with _Sl, β _ClThe delay of bucking-out system, and with compensating control signal u _CTHAFCPut on the independently basic closed loop servo control loop of corresponding axis.

The control calculation element utilizes interior, middle ring instruction angle position θ _rAnd θ _pProduce each order quadrature harmonic component and multiply each other θ with corresponding range coefficient _rAnd θ _pThe harmonic compensation feed-forward control signals u at each Coupled Disturbances effect is calculated and produces in timing respectively according to formula (1) _CTHAFC

The control calculation element is with the servo error e of system _pMultiply each other according to formula (2) and each order quadrature harmonic component with initial phase shifting angle, amplitude through each order compensating control signal of integration self-adaptation correction, the independently basic closed loop servo control loop of each compensating control signal and existing corresponding axis is mutually comprehensive, simultaneously the amplitude and the phase place of corresponding order compensating control signal are adjusted.

Described control calculation element is meant that microcontroller or control computer etc. can be used for the device of calculation control.

Advantageous effect of the present invention, the present invention has adopted (CTHAFC) strategy and the system architecture of between centers motion coupling torque disturbance the carrying out harmonic wave adaptive feedforward compensation control that distribution causes to architecture quality, position servo tracking error when making the interlock angular speed state of each between centers significantly reduces, and the interlock performance is improved greatly.Because the coupling torque disturbance has obtained effective inhibition, the scope of three-axle table interlock angular speed has also been widened in harmonic wave adaptive feedforward compensation control (CTHAFC), improve the reliability of system, thereby provide new measuring technology means for inertial sensor and system testing.Because harmonic wave adaptive feedforward compensation control (CTHAFC) microcontroller has taked to have the quadrature harmonic wave adaptive feedforward control method of initial phase compensation, make the amplitude of compensating control signal and phase adjusted be converted into and utilize the amplitude adaptive control of servo error and command signal, and it is opposite to have guaranteed that compensating signal stably levels off to corresponding coupling perturbation amplitude and direction on amplitude compensating signal.Owing to do not need when the feedback metrical information of anterior angle position and initial angle position information accurately, do not adopt the calculating such as complexity such as Fourier transforms commonly used in the harmonic data processing, simplify the control system algorithm greatly, improved the real-time of arithmetic speed and total system.For the rotating mechanism that is different from accompanying drawing 1 version, this method also can be effectively applied to eliminate the system disturbance with harmonic characteristic.

The present invention can the uncertain parameter of On-line Estimation also revise its variation in real time, eliminates the disturbance of between centers coupling torque effectively by applying harmonic compensation control, and the tracking performance of three-axle table under multiaxis angular speed linkage status increased substantially.

The present invention has still kept existing each the independently basic closed loop servo control loop of three-axle table, and is fundamental construction CTHAFC system about coupling torque harmonic component order that corner changes with each.

Description of drawings

Fig. 1 is existing independently motion servo control system synoptic diagram;

Fig. 2 is existing typical three-axle table structural representation;

Fig. 3 has the three-axis accurate angular motion control system structural representation of the control calculation element of harmonic wave adaptive feedforward compensation for the present invention;

The structured flowchart that Fig. 4 calculates for harmonic wave adaptive feedforward compensation control of the present invention.

Embodiment

The control calculation element regularly produces the θ about corner m according to the harmonic characteristic of between centers coupling torque disturbance in each control servo period _r± n θ _pThe quadrature harmonic compensation control signal that changes, the while calculates and adjusts the amplitude p of each order harmonic signal in real time according to system's servo error _Sl, p _Cl, and compensating control signal put on the independently basic closed loop servo control loop of corresponding axis, reach the purpose of accurate elimination between centers coupling torque disturbance.By the initial phase shifting angle β of control signal is set _Sl, β _Cl, bucking-out system postpones, and guarantees the stability of adaptive mechanism.

The computing formula of between centers coupling torque harmonic wave adaptive feedforward compensation (CTHAFC) is:

$u_{\mathrm{CTHAFC}}\left(k\right)=\underset{l=1}{\overset{N}{\mathrm{\Σ}}}(p_{\mathrm{sl}}\left(k\right)\sin ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right))+p_{\mathrm{cl}}\left(k\right)\cos ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)))---\left(1\right)$

$\left.\begin{array}{c}{p}_{\mathrm{sl}}\left(k\right)={\mathrm{\γ}}_{\mathrm{sl}}{T}_{s}e\left(k\right)\sin ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)\±{\mathrm{\β}}_{\mathrm{sl}})+p_{\mathrm{sl}}(k-1)\\ p_{\mathrm{cl}}\left(k\right)={\mathrm{\γ}}_{\mathrm{cl}}{T}_{s}e\left(k\right)\cos ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)\±{\mathrm{\β}}_{\mathrm{cl}})+p_{\mathrm{cl}}(k-1)\end{array}\right\}---\left(2\right)$

In the formula: m, n=0,1,2 ... it is the compensating control signal harmonic parameters;

N is and m the higher harmonic wave component order of compensating control signal that n is relevant;

p _Sl, p _ClBe the amplitude coefficient of compensating control signal, p _Sl(0)=p _Sl0, p _Cl(0)=p _Cl0

γ _Sl, γ _ClIt is adaptive gain; T _SIt is the control system servo sample cycle;

β _Sl, β _ClIt is the initial phase shifting angle of compensating control signal harmonic wave.

In formula (1), produce each order quadrature harmonic component and multiply each other with corresponding range coefficient, form the compensating control signal of coupling torque perturbation action; In formula (2), system's servo error multiplies each other with each the order quadrature harmonic component with initial phase shifting angle, amplifies back formation through integration the amplitude of compensating control signal is calculated, simultaneously the effect that the amplitude and the phase place of corresponding order compensating control signal are adjusted.

As Fig. 4, in the cycle, CTHAFC 14 utilizes interior, middle ring instruction angle position θ at k servo sample _r, θ _pWith initial phase shifting angle β _Sl, β _ClRespectively by mutually orthogonal harmonic signal sin (the m θ of 15 and 16 generations among Fig. 4 _r± n θ _p), sin (m θ _r± n θ _p± β _Sl) and cos (m θ _r± n θ _p), cos (m θ _r± n θ _p± β _Cl); To have initial phase shifting angle quadrature harmonic signal respectively at servo error e _pMultiply each other, utilize formula (2) to carry out digital integration, take advantage of γ _Sl, γ _ClAmplify and form each order quadrature harmonic constant p that self-adaptation is regulated _SlAnd p _ClSimultaneously quadrature harmonic signal and corresponding harmonic constant are multiplied each other, utilize formula (1) to carry out the compensating control signal u that addition produces the k cycle _CTHAFC

As Fig. 3, with the compensating control signal u in k cycle _CTHAFCPut on the basic closed loop servo control loop that speed closed loop 12, position closed loop 13 etc. are formed, comprehensively form Coupled Disturbances M with speed closed loop 12 _dThe compensation control action.Lasting iterative computation through the k cycle, on amplitude, stably level off to the amplitude of Coupled Disturbances effect and direction when opposite through revising each order compensating control signal of back, compensation control will be offset the Coupled Disturbances effect exactly, thereby reach inhibition purpose, make the output θ of system the disturbance of between centers motion coupling torque _{R_o}, θ _{P_o}, θ _{A_o}In no longer contain the influence of Coupled Disturbances effect.Among Fig. 3, middle annulate shaft control system 10, outer annulate shaft control system 9 is basic identical with CTHAFC 14 implementation processes of inner axle control system 11, only since the quadrature harmonic wave 15,16 that the harmonic characteristic of each annulate shaft coupling torque disturbance causes be formed in m θ _r± n θ _pOn to some extent the difference.

3 d pose motion simulation equipment accurate angular motion control system (accompanying drawing 3) with between centers coupling torque harmonic wave adaptive feedforward compensation (CTHAFC) is calculated and is generated about m θ according to formula (1) formula (2) in the cycle in each servocontrol by the control calculation element _r± n θ _pThe control of each order quadrature harmonic compensation, and to its amplitude p _Sl, p _ClCarry out self-adaptation and estimate and adjustment in real time, utilize initial phase shifting angle β is set _Sl, β _ClThe delay of bucking-out system, and with compensating control signal u _CTHAFCPut on the independently basic closed loop servo control loop of corresponding axis, thereby constitute any method that control system is offset between centers coupling disturbance, all in claim of the present invention.Three-axle table is done under the modes such as even rotary speed movement or oscillating motion, this method link at two diaxons (another axle is located) or three all can be effective when linking simultaneously.

In the practical application, compensating control signal harmonic parameters m, n can select γ to the influence degree of system according to each Coupled Disturbances _Sl, γ _ClAnd β _Sl, β _ClAccording to each independently system performance selection of basic closed loop servo control loop, T _sIt is the intrinsic parameter of system.

Among Fig. 3, θ _r, θ _p, θ _aBe respectively three-axle table instruction angle position and angular velocity; θ _{R_o}, θ _{P_o}, θ _{A_o}Be respectively three actual attitude angle positions; e _pBe corresponding axis system servo error; P is controlling object (being controlled turntable corresponding sports axle); G _p, G _vBe respectively independent closed loop position, rate controller; M _dBe Coupled Disturbances moment; CTHAFC is a coupling torque feedforward compensation adaptive control mechanism.

Among Fig. 4, θ _r, θ _pBe respectively in the three-axle table, axis instruction angle position; On behalf of the quadrature harmonic component, sin-15, cos-16 produce mechanism; * be multiplier; ∫ is an integrator; γ _Sl, γ _ClBe adaptive gain, β _Sl, β _ClBe the initial phase shifting angle of compensating control signal harmonic wave.

Claims (5)

1. three-axis accurate angular motion control method, it is characterized in that, the control calculation element is in each control servo period, harmonic characteristic according to the disturbance of between centers coupling torque regularly produces the quadrature harmonic compensation control signal that changes about corner, calculate and adjust the amplitude of each order harmonic signal simultaneously according to system's servo error in real time, and compensating control signal is put on the independently basic closed loop servo control loop of corresponding axis.

2. three-axis accurate angular motion control method according to claim 1 is characterized in that, described control calculation element is meant microcontroller or control computer.

3. three-axis accurate angular motion control method according to claim 1 is characterized in that, the control calculation element in control servo period k according to formula

$u_{\mathrm{CTHAFC}}\left(k\right)=\underset{l=1}{\overset{N}{\mathrm{\Σ}}}(p_{\mathrm{sl}}\left(k\right)\sin ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right))+p_{\mathrm{cl}}\left(k\right)\cos ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)))---\left(1\right)$

And formula

$\left.\begin{array}{c}{p}_{\mathrm{sl}}\left(k\right)={\mathrm{\γ}}_{\mathrm{sl}}{T}_{s}e\left(k\right)\sin ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)\±{\mathrm{\β}}_{\mathrm{sl}})+p_{\mathrm{sl}}(k-1)\\ p_{\mathrm{cl}}\left(k\right)={\mathrm{\γ}}_{\mathrm{cl}}{T}_{s}e\left(k\right)\cos ({\mathrm{m\θ}}_r\left(k\right)\±{\mathrm{n\θ}}_p\left(k\right)\±{\mathrm{\β}}_{\mathrm{cl}})+p_{\mathrm{cl}}(k-1)\end{array}\right\}---\left(2\right)$

Calculate and generate about m θ _r± n θ _pThe control of each order quadrature harmonic compensation, and to its amplitude p _Sl, p _ClCarry out self-adaptation and calculate and adjust in real time, utilize the initial phase shifting angle β that is provided with _Sl, β _ClThe delay of bucking-out system, and with compensating control signal u _CTHAFCPut on the independently basic closed loop servo control loop of corresponding axis.

4. three-axis accurate angular motion control method according to claim 3 is characterized in that, each order quadrature harmonic component that the control calculation element will produce multiplies each other with corresponding range coefficient, utilizes interior, middle ring instruction angle position θ _rAnd θ _pThe harmonic compensation feed-forward control signals u at each Coupled Disturbances effect is calculated and produces in timing respectively according to formula (1) _CTHAFC

5. three-axis accurate angular motion control method according to claim 3 is characterized in that, the control calculation element is with the servo error e of system _pMultiply each other according to formula (2) and each order quadrature harmonic component with initial phase shifting angle, amplitude through each order compensating control signal of integration self-adaptation correction, independently the closed loop servo control loop is mutually comprehensive with existing corresponding axis with each compensating control signal, simultaneously the amplitude and the phase place of corresponding order compensating control signal is adjusted.

Images