CN107783422A - Using the gun laying systems stabilisation control method of inertial navigation - Google Patents
Using the gun laying systems stabilisation control method of inertial navigation Download PDFInfo
- Publication number
- CN107783422A CN107783422A CN201710981184.8A CN201710981184A CN107783422A CN 107783422 A CN107783422 A CN 107783422A CN 201710981184 A CN201710981184 A CN 201710981184A CN 107783422 A CN107783422 A CN 107783422A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mtd
- mtr
- beta
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
- F41—WEAPONS
- F41G—WEAPON SIGHTS; AIMING
- F41G5/00—Elevating or traversing control systems for guns
- F41G5/14—Elevating or traversing control systems for guns for vehicle-borne guns
- F41G5/24—Elevating or traversing control systems for guns for vehicle-borne guns for guns on tanks
Abstract
The invention provides a kind of gun laying systems stabilisation control method using inertial navigation, gathers the tri-axis angular rate measured by inertial navigation gyro group first and changes to the angular speed under earth coordinates；Then height and orientation under car body system are calculated and turns angular speed, gun turret pitching and rolling angle rate and car body course angular speed；Secondly, receive stability contorting instruction, collection position feedback, control driver to work in speed-regulating mode when significantly turning；Put into the laggard line position of stable mode, feedforward control, rate stabilization, the control of speed interference compensation, control driver work in electric current or torque mode；Report and aim at after lasting accuracy is reached.Control accuracy of the present invention is high, has saved cost, has improved reliability.
Description
Technical field
The invention belongs to fire zone open field, relates generally to need precise and stable, tracking fire zone open
Control method.
Background technology
With the development evolvement of military struggle, there is an urgent need to Suppressed Weapons to possess marching fire for new Military Operational Requirement
Ability, it is necessary to self-propelled gun can carry out it is dynamic to quiet, dynamic to dynamic combat duty.So cannon is needed in sighting stabilization system
Under control, first, overcoming car body to be disturbed due to the road surface of walking to caused by car body；Second, cannon will follow the tracking of fire control to take aim at
Standard turns instruction.Such function is accomplished already in the big gun control system of tank armament or amphibious asault gun.But Tank gun control
System employs reel cage combination gyro group, car body/gun turret gyro group completes the steady aim of cannon, and is equipped with inertial navigation system jointly
System, on car body, for running inertial navigation.Wherein, reel cage combination gyro group by two groups of twin shaft rate gyroscopes and
Two groups of single shaft rate gyroscopes composition, for measuring motion state of the tank gun in space, complete tank gun height to and orientation
The control of steady aim automatically and manually of servo；Car body/gun turret gyro group is that three single shaft rate gyroscope groups are used to measure car
Body course, the pitching of gun turret and rolling angle rate, for big gun control system speed disturb feedforward compensation, make big gun control system reach or
Better than height to 0.8mil and orientation 1.5mil lasting accuracy.But the gyro that tank gun control system is configured is too many, letter
Redundancy is ceased, cost is too high, and reliability is relatively low, and the inertial navigation of high value does not utilize well, the big gun control system of this quasi-tradition
Scheme, it is not suitable in certain self-propelled gun armament systems of modern Suppressed Weapons.
With inertial navigation (SINS) technological progress, its precision improves and cost reduces, long-time stability and shock resistance
Ability greatly improves, by SINS be arranged on direct measurement earth coordinates on the barrel of self-propelled gun sensing be it is modern voluntarily
The standard configuration of cannon.The servomechanism of self-propelled gun realizes using SINS directions and attitude value as the angle feed-back of servomechanism
Direct control of the cannon under earth coordinates, improves gun aiming accuracy.But the control coordinate and SINS of servomechanism
Coupling be present in measuring coordinate disunity, the height and azimuth control system for causing control system.Launched for parked
Self-propelled gun is solved using such as quaternary number, the coordinate transform of Euler's horn cupping or serials control.It is but running certainly for needs
Row cannon needs use to suppress vehicle disturbance, and and can solves the new method for controlling coupled problem very well.
The content of the invention
For overcome the deficiencies in the prior art, the present invention provides a kind of gun laying systems stabilisation control using inertial navigation
Method processed, retain the side angle device of traditional fire zone open composition, using SINS, height side angle device and orientation side angle device, use
Space coordinate transformation obtains the pitching of gun turret and traverses angular speed, and the angular speed in car body course, and systems stabilisation is using compound
Control method realizes the high-accuracy stable tracing control of cannon.The present invention uses space angles of the SINS as fire zone open
With inertial space angular speed velocity feedback device, suppress the disturbance pointed to gun barrel of vehicle body attitude and just to and orientation
Coupled to servo-actuated control, realize the high-accuracy stable tracking and controlling method problem of cannon；Utilize SINS direct measurement cannon bodies
Pipe is pointed to, and improves and points to control accuracy, and self-propelled gun armament systems can be made to complete autonomous positioning navigation feature again.
The technical solution adopted for the present invention to solve the technical problems comprises the following steps：
(1) setting speed ring controls the initial value for controlling step number k of compensation to increase one by one for 0, k；The control of setting speed ring is mended
The controlling cycle T repaids=1ms；
(2) the tri-axis angular rate ω measured by inertial navigation gyro group is gatheredp(k)=[ωpx(k) ωpy(k) ωpz
(k)]T；
(3) calculate course of the barrel under geodetic coordinates and bring up and turn angular speed
Wherein,For course angle, the angle of pitch and the roll angle of SINS outputs；J is that position ring calculates step number；
(4) the measured value β of direction side angle device and height side angle device is gatheredb(k),εb(k)；Extracted using nonlinear observer
Gun turret is on car body, cannon turns angular speed on gun turret
Whereine1(k),e2(k) it is observation error, α, δ are respectively fal functions
Parameter 0.01≤α≤1,0.01≤δ≤1, β11,β12,β21,β22Respectively single order, the second order of direction side angle device and height side angle device
Gain, z11(k)、z21(k) it is respectively βb(k),εb(k) estimate, z12(k)、z22(k) it is respectivelyEstimate；
(5) pitching and the rolling angle rate ω of gun turret are calculatedhx(k),ωhy(k)：
(6) the course angular speed of car body is calculated
(7) what setting position controlled controls step number j initial value to increase one by one for 0, j, and the controlling cycle that position controls is
10ms；Judge k values, if k is 10 multiple, performs (8) step, otherwise jump to (18) step；
(8) course, posture and the roll angle of inertial navigation are received
(9) the gun laying control instruction ψ under geodetic coordinates is receivedref(j),θref(j), comprisingWherein,
ψref(j),θref(j) it is respectively course and attitude angle；
(10) judge to adjust big gun control error eβ(j)=ψref(j)-ψ (j) and eε(j)=θref(j)-θ (j), if adjusting big gun control
Error eβ(j)≥eβmaxOr eε(j)≥eεmax, it is transferred to step (11)；Otherwise, it is transferred to step (14)；
(11) rate control instruction that the orientation under significantly turning is servo-actuated servo-driver is calculated
Wherein：keβFor radical sign e control coefrficients；umaxsβAnd uminsβFor the upper and lower bound of PI controllers output；
(12) rate control instruction that the height under significantly turning is servo-actuated servo-driver is calculated
Wherein：keεFor radical sign e control coefrficients；umaxsεAnd uminsεFor the upper and lower bound of controller output；
(13) setting driver is operated in speed-regulating mode, is sent out respectively to orientation and height servo-driver by CAN
Send rate control instruction
(14) the feedforward control amount u of computer azimuth and high and low position control systemfβ(j),ufε(j)
Wherein, kfβ,kfεThe respectively feedforward controller coefficient in orientation and height system；
(15) height and orientation disturbance velocity compensation rate u are calculateddβ(j),udε(j)：
Wherein,The interference speed that respectively height and orientation are servo-actuated；kdβ1,kdβ2,kdβ3Respectively direction compensator
Coefficient of colligation；kdε1,kdε2,kdε3, the respectively coefficient of colligation of height compensator；
(16) rate control instruction that the orientation under stable condition is servo-actuated servo-driver is calculated
upsβ(j)=Kpsβesβ(j)
upresatsβ(j)=upsβ(j)+uisβ(j)+ufβ(j)+udβ(j)
Wherein：upsβ(j) it is ratio control item；uisβ(j) it is integration control item；uimaxsβAnd uiminsβIt is defeated for integral controller
The upper and lower bound gone out, uimaxsβ=max (0, umaxsβ-upeβ(j)), uiminsβ=min (uminsβ-upeβ(j),0)；KpsβFor PI ratios
Example control coefrficient；KisβFor integral coefficient；
(17) rate control instruction that the height under stable condition is servo-actuated servo-driver is calculated
upsε(j)=Kpsεesε(j)
upresatsε(j)=upsε(j)+uisε(j)+ufε(j)+udε(j)
Wherein：upsε(j) it is ratio control item；uisε(j) it is integration control item；uimaxsεAnd uiminsεIt is defeated for integral controller
The upper and lower bound gone out, uimaxsε=max (umaxsε-upsε(j), 0), uiminsε=min (uminsε-upsε(j),0)；KpsεFor PI ratios
Example control coefrficient；KisεFor integral coefficient；
(18) the current control instruction that the orientation under stable condition is servo-actuated servo-driver is calculated
upcβ(k)=Kpcβecβ(k)
upresatcβ(k)=upcβ(k)+uicβ(k)
Wherein：upcβ(j) it is ratio control item；uicβ(j) it is integration control item；umaxcβAnd umincβExported for PI controllers
Upper and lower bound, uimaxcβAnd uimincβFor the upper and lower bound of integral controller output, uimaxcβ=max (0, umaxcβ-upeβ
(k)), uimincβ=min (umincβ-upeβ(k),0)；KpcβFor PI proportional control factors；KicβFor integral coefficient；iβFor Amimuth Transmission
Speed reducing ratio；
(19) the current control instruction that the height under stable condition is servo-actuated servo-driver is calculated
upcε(k)=Kpcεecε(k)
upresatcε(k)=upcε(k)+uicε(k)
Wherein：upcε(j) it is ratio control item；uicε(j) it is integration control item；umaxcεAnd umincεExported for PI controllers
Upper and lower bound, uimaxcεAnd uimincεFor the upper and lower bound of integral controller output, uimaxcε=max (umaxcε-upcε(k),
0), uimincε=min (umincε-upcε(k),0)；KpcεFor PI proportional control factors；KicεFor integral coefficient；iεPassed to be just servo-actuated
Dynamic speed reducing ratio；
(20) setting driver is operated in torque mode, and current-order is sent to servo-driver by CAN；
(21) if eβ(j) ＜ eβmin,eε(j) ＜ eεmin, the state of aiming is reported to layer by CAN.
The beneficial effects of the invention are as follows：It is high to play the precision that inertial navigation measurement gun barrel points under earth coordinates
Advantage, and effectively overcome attitude of carrier and control interference, and the control coupling that height and orientation are servo-actuated are pointed to gun barrel
Conjunction problem, realize stabilization, tracing control that high-precision gun barrel points to, be not easy by the self-propelled gun carrier speed of service and
The influence of environment (land, sea), angular speed and interference are not additionally turned using corresponding rate gyroscope measurement barrel
Angular speed, cost is saved, improved the reliability of system.
Brief description of the drawings
Fig. 1 is the control principle drawing of the present invention；
Fig. 2 is the control transmission function structure chart of the present invention；
Fig. 3 is the calculation flow chart of the present invention.
Embodiment
The present invention is further described with reference to the accompanying drawings and examples, and the present invention includes but are not limited to following implementations
Example.
The present invention implement the step of be：
(1) control is started, it is k to control step number, and the control of setting speed ring compensates, i.e. controlling cycle Ts=1ms,
K=k+1
The initial value for taking k is 0；
(2) the tri-axis angular rate ω measured by inertial navigation gyro group is gatheredp(k)=[ωpx(k) ωpy(k) ωpz
(k)]T；
(3) calculate barrel and turn angular speed under geodetic coordinates
Wherein：For course angle, the angle of pitch and the roll angle of SINS outputs；J is that position ring calculates step number；
(4) the measured value β of direction side angle device and height side angle device is gatheredb(k),εb(k)；Extracted using nonlinear observer
Gun turret is on car body, cannon turns angular speed on gun turret
Whereine1(k),e2(k) it is observation error, α, δ are respectively fal functions
Parameter, β11,β12,β21,β22The respectively single order of observer, second-order gain.
(5) pitching and the rolling angle rate ω of gun turret are calculatedhx(k),ωhy(k)：
(6) the course angular speed r (k) of car body is calculated：
(7) if k is 10 multiple, (8) step is performed, otherwise jumps to (18) step；
J=j+1
Wherein j initial value is 0, and this is the cycle 10ms of position control；
(8) course and the attitude angle of inertial navigation are received
(9) the gun laying control instruction ψ under geodetic coordinates is receivedref(j),θref(j), comprising
(10) big gun control error judgment is adjusted：
eβ(j)=ψref(j)-ψ(j)
eε(j)=θref(j)-θ(j)
If adjust big gun control error eβ(j)≥eβmaxOr eε(j)≥eεmax, it is transferred to (11)；Otherwise, it is transferred to (14)；
(11) rate control instruction that the orientation under significantly turning is servo-actuated servo-driver is calculated
Wherein：keβFor radical sign e control coefrficients；umaxsβAnd uminsβFor the upper and lower bound of PI controllers output；
(12) rate control instruction that the height under significantly turning is servo-actuated servo-driver is calculated
Wherein：keεFor radical sign e control coefrficients；umaxsεAnd uminsεFor the upper and lower bound of controller output；
(13) setting driver is operated in speed-regulating mode, is sent out respectively to orientation and height servo-driver by CAN
Send rate control instruction
(14) the feedforward control amount u of computer azimuth and high and low position control systemfβ(j),ufε(j)
Wherein, kfβ,kfεThe respectively feedforward controller coefficient in orientation and height system；
(15) height and orientation disturbance velocity compensation rate u are calculateddβ(j),udε(j)：
Wherein,The interference speed that respectively height and orientation are servo-actuated；kdβ1,kdβ2,kdβ3, respectively orientation compensates
The coefficient of colligation of device；kdε1,kdε2,kdε3, the respectively coefficient of colligation of height compensator；
(16) rate control instruction that the orientation under stable condition is servo-actuated servo-driver is calculated
upsβ(j)=Kpsβesβ(j)
upresatsβ(j)=upsβ(j)+uisβ(j)+ufβ(j)+udβ(j)
Wherein：upsβ(j) it is ratio control item；uisβ(j) it is integration control item；uimaxsβAnd uiminsβIt is defeated for integral controller
The upper and lower bound gone out, uimaxsβ=max (0, umaxsβ-upeβ(j)), uiminsβ=min (uminsβ-upeβ(j),0)；KpsβFor PI ratios
Example control coefrficient；KisβFor integral coefficient；
(17) rate control instruction that the height under stable condition is servo-actuated servo-driver is calculated
upsε(j)=Kpsεesε(j)
upresatsε(j)=upsε(j)+uisε(j)+ufε(j)+udε(j)
Wherein：upsε(j) it is ratio control item；uisε(j) it is integration control item；uimaxsεAnd uiminsεIt is defeated for integral controller
The upper and lower bound gone out, uimaxsε=max (umaxsε-upsε(j), 0), uiminsε=min (uminsε-upsε(j),0)；KpsεFor PI ratios
Example control coefrficient；KisεFor integral coefficient；
(18) the current control instruction that the orientation under stable condition is servo-actuated servo-driver is calculated
upcβ(k)=Kpcβecβ(k)
upresatcβ(k)=upcβ(k)+uicβ(k)
Wherein：upcβ(j) it is ratio control item；uicβ(j) it is integration control item；umaxcβAnd umincβExported for PI controllers
Upper and lower bound, uimaxcβAnd uimincβFor the upper and lower bound of integral controller output, uimaxcβ=max (0, umaxcβ-upeβ
(k)), uimincβ=min (umincβ-upeβ(k),0)；KpcβFor PI proportional control factors；KicβFor integral coefficient；iβFor Amimuth Transmission
Speed reducing ratio；
(19) the current control instruction that the height under stable condition is servo-actuated servo-driver is calculated
upcε(k)=Kpcεecε(k)
upresatcε(k)=upcε(k)+uicε(k)
Wherein：upcε(j) it is ratio control item；uicε(j) it is integration control item；umaxcεAnd umincεExported for PI controllers
Upper and lower bound, uimaxcεAnd uimincεFor the upper and lower bound of integral controller output, uimaxcε=max (umaxcε-upcε(k),
0), uimincε=min (umincε-upcε(k),0)；KpcεFor PI proportional control factors；KicεFor integral coefficient；iεPassed to be just servo-actuated
Dynamic speed reducing ratio；
(20) setting driver is operated in torque mode, and current-order is sent to servo-driver by CAN；
(21) if eβ(j) ＜ eβmin,eε(j) ＜ eεmin, the state of aiming is reported to layer by CAN.
The control principle of the present invention is shown in Fig. 1.The absolute angular speed turned in figure using SINS gyros group measurement gun barrel,
It is that earth rotation angular speed, angular speed, the gun barrel of vehicle body attitude change turn the summation of angular speed with respect to car body.It is and right
It is that gun barrel turns angular speed with respect to car body to control effective angular speed, other to can be seen as disturbing.SINS gyro group
Angular speed velocity-stabilization closed loop can be formed with servo-driver, the speed for completing to aim at is directly stable.SINS course angle and
Feedback of the attitude angle as sighting stabilization system position ring and the position closed loop under positioner composition earth coordinates.Pass through
The serials control of control loop eliminates the interference of height and two control passages in orientation, so as to reach preferable steady aim essence
Degree.Because the sighting system has cannon height low and orientation side angle device, barrel is measured respectively with respect to the gun turret pipe angle of pitch, gun turret
With respect to bodywork's direction and position angle.Therefore, according to the angular speed of SINS gyro groups, the angle of site and orientation side angle, you can extrapolate influence fire
The gun turret rolling angle rate and pitch rate of big gun sighting stabilization, and car body course angular speed.So as to convenient according to actual feelings
Condition application design speed interference inverter, to improve gun laying lasting accuracy.
The control method substantially step：First, the tri-axis angular rate ω measured by inertial navigation gyro group is gatheredp(k), will
It is changed to the angular speed under earth coordinatesThen, side angle device is gathered, calculates height and orientation under car body system
Turn angular speedAnd calculate gun turret pitching and rolling angle rate ωhx(k),ωhy(k), and car body course angle is fast
Rate r；Secondly, stability contorting instruction is received, collection position feedback, when significantly turning, control driver works in speed-regulating mode；Again
Again, into stable mode is stablized, position, feedforward control is carried out, rate stabilization, the control of speed interference compensation, controls driver
Work in electric current or torque mode；Finally, after lasting accuracy is reached, report and aim at.
The control transmission function structure of the present invention is shown in Fig. 2.In order to simplify transmission function, by SINS course angle and can bow
Measurement of elevation is by being reduced toCoupled interference is considered as the various interference passed through；By height and the electric current control of bearing servo driving
The closed loop of the compositions such as device processed, inverter, current regulating, electric current moment coefficient is reduced to first order inertial loopSide
Position and the control of height speed ring areKpsβ,KpsεRespectively proportionality coefficient, Kisβ,KisεFor integration
Coefficient；Orientation and the control of high and low position ring areKpcβ,KpcεRespectively proportionality coefficient, Kicβ,Kicε
Respectively integral coefficient；Orientation and height feedforward controller are kfβs,kfεS, kfβ,kfεIt is corresponding coefficient；Disturb speed feedforward
Controller isfdβ,fdεRespectively gain coefficient, Tdβ,TdεRespectively time constant, using bilinear transformation
By its discretization.
Implement the cannon steady aim system of the control method mainly by steady aim control system, driving governing system,
Dynamic power system.Steady aim control system uses the embedded computer based on x86.Governing system is driven with DSP28335
+ CPLD is the control panel of core, and driving power electronics IPM (intelligent drives) controlled motor rotates.Orientation permagnetic synchronous motor
(PMSM), busbar voltage 540VDC, number of pole-pairs np=3, rated current 175A, moment coefficient 0.46N.m/A, stator electricity
Feel for 0.4mH, stator resistance is 3 milliohms, rated speed 5500RPM, nominal torque 79Nm, equivalent turn of rotor and load
Dynamic inertia J summation is 0.03kgm2；Height PMSM, busbar voltage 540VDC, number of pole-pairs np=3, rated current 38A, turn
Moment coefficient is 0.53N.m/A, stator inductance 0.706mH, and stator resistance is 0.12 Europe, rated speed 4200RPM, specified turn
The equivalent moment of inertia J summations of square 38Nm, rotor and load are 0.0068kgm2.Orientation load rotating inertia is
5800kg·m2, transmission speed ratio 210.Height load rotating inertia is 900kgm2.Transmission speed ratio is 275.SINS angle speed
Rate measurement range is ± 300 °/s, and heading measure precision is not more than 0.3mil, and attitude measurement accuracy is not more than 0.1mil.
Fig. 3 is the calculation flow chart of control method of the present invention, and specific implementation process is described in detail below in conjunction with flow chart.
(1) control is started, it is k to control step number, and the control of setting speed ring compensates, i.e. controlling cycle Ts=1ms,
K=k+1
The initial value for taking k is 0；
(2) the tri-axis angular rate ω measured by inertial navigation gyro group is gatheredp(k)=[ωpx(k) ωpy(k) ωpz
(k)]T；
(3) calculate barrel and turn angular speed under geodetic coordinates
Wherein：For course angle, the angle of pitch and the roll angle of SINS outputs；J is that position ring calculates step number；
(4) the measured value β of direction side angle device and height side angle device is gatheredb(k),εb(k)；Extracted using nonlinear observer
Gun turret is on car body, cannon turns angular speed on gun turret
Whereine1(k),e2(k) it is observation error, the parameter alphas of fal functions=
0.8, δ=0.1 is respectively；Single order, the second-order gain β of observer11=90, β12=1350, β21=90, β22=1350.
(5) pitching and the rolling angle rate ω of gun turret are calculatedhx(k),ωhy(k)：
(6) the course angular speed r (k) of car body is calculated：
(7) if k is 10 multiple, (8) step is performed, otherwise jumps to (18) step；
J=j+1
Wherein j initial value is 0, and this is the cycle 10ms of position control；
(8) course and the attitude angle of inertial navigation are received
(9) the gun laying control instruction ψ under geodetic coordinates is receivedref(j),θref(j), comprising
(10) big gun control error judgment is adjusted：
eβ(j)=ψref(j)-ψ(j)
eε(j)=θref(j)-θ(j)
If adjust big gun control error eβ(j)≥eβmaxOr eε(j)≥eεmax, it is transferred to step (11)；Otherwise, it is transferred to step (14)；
(11) rate control instruction that the orientation under significantly turning is servo-actuated servo-driver is calculated
Wherein：Radical sign e control coefrficients keβ=11.3；The upper limit u of controller outputmaxsβ=6000 and lower limit uminsβ=-
6000；
(12) rate control instruction that the height under significantly turning is servo-actuated servo-driver is calculated
Wherein：Radical sign e control coefrficients keε=6.5；The upper limit u of controller outputmaxsε=4500 and lower limit uminsε=-
4500；
(13) setting driver is operated in speed-regulating mode, is sent out respectively to orientation and height servo-driver by CAN
Send rate control instruction
(14) the feedforward control amount u of computer azimuth and high and low position control systemfβ(j),ufε(j)
Wherein, feedforward controller coefficient kfβ=2.06, kfε=2.46, respectively with orientation governing system and high governing system
Transmission function association；
(15) height and orientation disturbance velocity compensation rate u are calculateddβ(j),udε(j)：
Wherein,The interference speed that respectively height and orientation are servo-actuated；kdβ1=1.11, kdβ2=-2.3, kdβ3=
2.3 be respectively the coefficient of colligation of direction compensator；The coefficient of colligation k of height compensatordε1=1.11, kdε2=-1.56, kdε3=
1.56；
(16) rate control instruction that the orientation under stable condition is servo-actuated servo-driver is calculated
upsβ(j)=Kpsβesβ(j)
upresatsβ(j)=upsβ(j)+uisβ(j)+ufβ(j)+udβ(j)
Wherein：The upper limit u of PI controllers outputmaxsβ=6000 and lower limit uminsβ=-6000, integral controller output
Upper limit uimaxsβ=1000 and lower limit uiminsβ=-1000；PI proportional control factors Kpsβ=400, integral coefficient Kisβ=5；
(17) rate control instruction that the height under stable condition is servo-actuated servo-driver is calculated
upsε(j)=Kpsεesε(j)
upresatsε(j)=upsε(j)+uisε(j)+ufε(j)+udε(j)
Wherein：The upper limit u of PI controllers outputmaxsε=4500 and lower limit uminsε=-4500；Integral controller output
Upper limit uimaxsε=1200 and lower limit uiminsε=-1200；For PI proportional control factors Kpsε=250, integral coefficient Kisε=1；
(18) the current control instruction that the orientation under stable condition is servo-actuated servo-driver is calculated
upcβ(k)=Kpcβecβ(k)
upresatcβ(k)=upcβ(k)+uicβ(k)
Wherein：The upper limit u of PI controllers outputmaxcβ=525 and lower limit umincβ=-525, integral controller export upper
Limit uimaxcβ=300 and lower limit uimincβ=-300；PI proportional control factors Kpcβ=30, integral coefficient Kicβ=0.1；Amimuth Transmission
Speed reducing ratio iβ=210；
(19) the current control instruction that the height under stable condition is servo-actuated servo-driver is calculated
upcε(k)=Kpcεecε(k)
upresatcε(k)=upcε(k)+uicε(k)
Wherein：The upper limit u of PI controllers outputmaxcε=114 and lower limit umincε=-114, integral controller export upper
Limit uimaxcε=50 and lower limit uimincε=-50；PI proportional control factors Kpcε=20；Integral coefficient Kicε=1；Height follower actuation
Speed reducing ratio iε=275；
(20) setting driver is operated in torque mode, and current-order is sent to servo-driver by CAN；
(21) if eβ(j) ＜ eβmin,eε(j) ＜ eεmin, the state of aiming is reported to layer by CAN.
The scope span of parameter used see the table below：
Claims (1)
1. a kind of gun laying systems stabilisation control method using inertial navigation, it is characterised in that comprise the steps：
(1) setting speed ring controls the initial value for controlling step number k of compensation to increase one by one for 0, k；The control compensation of setting speed ring
Controlling cycle Ts=1ms；
(2) the tri-axis angular rate ω measured by inertial navigation gyro group is gatheredp(k)=[ωpx(k) ωpy(k) ωpz(k)]T；
(3) calculate course of the barrel under geodetic coordinates and bring up and turn angular speed
Wherein, ψ (j), θ (j),For course angle, the angle of pitch and the roll angle of SINS outputs；J is that position ring calculates step number；
(4) the measured value β of direction side angle device and height side angle device is gatheredb(k),εb(k)；Gun turret is extracted using nonlinear observer
On car body, cannon turn angular speed on gun turret
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>e</mi>
<mn>1</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>z</mi>
<mn>11</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>&beta;</mi>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>z</mi>
<mn>11</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>z</mi>
<mn>11</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mn>12</mn>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mo>)</mo>
<mo>-</mo>
<msub>
<mi>&beta;</mi>
<mn>11</mn>
</msub>
<mi>f</mi>
<mi>a</mi>
<mi>l</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>e</mi>
<mn>1</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
<mi>&alpha;</mi>
<mo>,</mo>
<mi>&delta;</mi>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>z</mi>
<mn>12</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>z</mi>
<mn>12</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<msub>
<mi>&beta;</mi>
<mn>12</mn>
</msub>
<mi>f</mi>
<mi>a</mi>
<mi>l</mi>
<mrow>
<mo>(</mo>
<mi>e</mi>
<mo>,</mo>
<mi>&alpha;</mi>
<mo>,</mo>
<mi>&delta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>e</mi>
<mn>2</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>z</mi>
<mn>1</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>&epsiv;</mi>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>z</mi>
<mn>21</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>z</mi>
<mn>21</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<mrow>
<mo>(</mo>
<msub>
<mi>z</mi>
<mn>22</mn>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mo>)</mo>
<mo>-</mo>
<msub>
<mi>&beta;</mi>
<mn>21</mn>
</msub>
<mi>f</mi>
<mi>a</mi>
<mi>l</mi>
<mo>(</mo>
<mrow>
<msub>
<mi>e</mi>
<mn>2</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
<mi>&alpha;</mi>
<mo>,</mo>
<mi>&delta;</mi>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>z</mi>
<mn>22</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>z</mi>
<mn>22</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
<msub>
<mi>&beta;</mi>
<mn>22</mn>
</msub>
<mi>f</mi>
<mi>a</mi>
<mi>l</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>e</mi>
<mn>2</mn>
</msub>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
<mo>,</mo>
<mi>&alpha;</mi>
<mo>,</mo>
<mi>&delta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mrow>
<msub>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>z</mi>
<mn>12</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
<msub>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>z</mi>
<mn>22</mn>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
Whereine1(k),e2(k) it is observation error, α, δ are respectively the parameter of fal functions
0.01≤α≤1,0.01≤δ≤1, β11,β12,β21,β22Respectively the single order of direction side angle device and height side angle device, second order increase
Benefit, z11(k)、z21(k) it is respectively βb(k),εb(k) estimate, z12(k)、z22(k) it is respectivelyEstimate；
(5) pitching and the rolling angle rate ω of gun turret are calculatedhx(k),ωhy(k)：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>h</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>h</mi>
<mi>y</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>h</mi>
<mi>z</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>cos&epsiv;</mi>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>sin&epsiv;</mi>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>sin&epsiv;</mi>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>cos&epsiv;</mi>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mrow>
<mo>(</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>p</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>p</mi>
<mi>y</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>p</mi>
<mi>z</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>)</mo>
</mrow>
</mrow>
(6) the course angular speed of car body is calculated
(7) what setting position controlled controls step number j initial value to increase one by one for 0, j, and the controlling cycle that position controls is 10ms；Sentence
Determine k values, if k is 10 multiple, performs (8) step, otherwise jump to (18) step；
(8) course, posture and the roll angle ψ (j) of inertial navigation, θ (j) are received,
(9) the gun laying control instruction ψ under geodetic coordinates is receivedref(j),θref(j), comprisingWherein, ψref
(j),θref(j) it is respectively course and attitude angle；
(10) judge to adjust big gun control error eβ(j)=ψref(j)-ψ (j) and eε(j)=θref(j)-θ (j), if adjusting big gun control error
eβ(j)≥eβmaxOr eε(j)≥eεmax, it is transferred to step (11)；Otherwise, it is transferred to step (14)；
(11) rate control instruction that the orientation under significantly turning is servo-actuated servo-driver is calculated
<mrow>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>&beta;</mi>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>&beta;</mi>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>sgn</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>e</mi>
<mi>&beta;</mi>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>k</mi>
<mrow>
<mi>e</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>|</mo>
<msub>
<mi>e</mi>
<mi>&beta;</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<msup>
<mo>|</mo>
<mn>0.5</mn>
</msup>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo><</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>&beta;</mi>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>&beta;</mi>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein：keβFor radical sign e control coefrficients；umaxsβAnd uminsβFor the upper and lower bound of PI controllers output；
(12) rate control instruction that the height under significantly turning is servo-actuated servo-driver is calculated
<mrow>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>&epsiv;</mi>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>&epsiv;</mi>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>k</mi>
<mrow>
<mi>e</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mi>sgn</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>e</mi>
<mi>&beta;</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>|</mo>
<mi>e</mi>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<msup>
<mo>|</mo>
<mn>0.5</mn>
</msup>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo><</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>&epsiv;</mi>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>&epsiv;</mi>
<mn>1</mn>
</mrow>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein：keεFor radical sign e control coefrficients；umaxsεAnd uminsεFor the upper and lower bound of controller output；
(13) setting driver is operated in speed-regulating mode, and speed is sent to orientation and height servo-driver respectively by CAN
Spend control instruction
(14) the feedforward control amount u of computer azimuth and high and low position control systemfβ(j),ufε(j)
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>f</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>k</mi>
<mrow>
<mi>f</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<msub>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>r</mi>
<mi>e</mi>
<mi>f</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>f</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>k</mi>
<mrow>
<mi>f</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<msub>
<mover>
<mi>&theta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>r</mi>
<mi>e</mi>
<mi>f</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</mrow>
Wherein, kfβ,kfεThe respectively feedforward controller coefficient in orientation and height system；
(15) height and orientation disturbance velocity compensation rate u are calculateddβ(j),udε(j)：
<mrow>
<msub>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>f</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>h</mi>
<mi>y</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>f</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mo>-</mo>
<msub>
<mi>tan&epsiv;</mi>
<mi>b</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<msub>
<mi>&omega;</mi>
<mrow>
<mi>h</mi>
<mi>x</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mi>r</mi>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>d</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>k</mi>
<mrow>
<mi>d</mi>
<mi>&beta;</mi>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>u</mi>
<mrow>
<mi>d</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>k</mi>
<mrow>
<mi>d</mi>
<mi>&beta;</mi>
<mn>2</mn>
</mrow>
</msub>
<msub>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>f</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>k</mi>
<mrow>
<mi>d</mi>
<mi>&beta;</mi>
<mn>3</mn>
</mrow>
</msub>
<msub>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>f</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>d</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>k</mi>
<mrow>
<mi>d</mi>
<mi>&epsiv;</mi>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mi>u</mi>
<mrow>
<mi>d</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>k</mi>
<mrow>
<mi>d</mi>
<mi>&epsiv;</mi>
<mn>2</mn>
</mrow>
</msub>
<msub>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>f</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>k</mi>
<mrow>
<mi>d</mi>
<mi>&beta;</mi>
<mn>3</mn>
</mrow>
</msub>
<msub>
<mover>
<mi>&epsiv;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mi>f</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
Wherein,The interference speed that respectively height and orientation are servo-actuated；kdβ1,kdβ2,kdβ3Respectively direction compensator is comprehensive
Syzygy number；kdε1,kdε2,kdε3, the respectively coefficient of colligation of height compensator；
(16) rate control instruction that the orientation under stable condition is servo-actuated servo-driver is calculated
upsβ(j)=Kpsβesβ(j)
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mrow>
<mi>j</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<msub>
<mi>K</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<msub>
<mi>e</mi>
<mrow>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
upresatsβ(j)=upsβ(j)+uisβ(j)+ufβ(j)+udβ(j)
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>p</mi>
<mi>r</mi>
<mi>e</mi>
<mi>s</mi>
<mi>a</mi>
<mi>t</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo><</mo>
<msubsup>
<mi>&omega;</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein：upsβ(j) it is ratio control item；uisβ(j) it is integration control item；uimaxsβAnd uiminsβFor integral controller output
Upper and lower bound, uimaxsβ=max (0, umaxsβ-upeβ(j)), uiminsβ=min (uminsβ-upeβ(j),0)；KpsβFor PI ratio controls
Coefficient processed；KisβFor integral coefficient；
(17) rate control instruction that the height under stable condition is servo-actuated servo-driver is calculated
upsε(j)=Kpsεesε(j)
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<msub>
<mi>K</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<msub>
<mi>e</mi>
<mrow>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
upresatsε(j)=upsε(j)+uisε(j)+ufε(j)+udε(j)
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>p</mi>
<mi>r</mi>
<mi>e</mi>
<mi>s</mi>
<mi>a</mi>
<mi>t</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo><</mo>
<msubsup>
<mi>&omega;</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>&omega;</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>s</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein：upsε(j) it is ratio control item；uisε(j) it is integration control item；uimaxsεAnd uiminsεFor integral controller output
Upper and lower bound, uimaxsε=max (umaxsε-upsε(j), 0), uiminsε=min (uminsε-upsε(j),0)；KpsεFor PI ratio controls
Coefficient processed；KisεFor integral coefficient；
(18) the current control instruction that the orientation under stable condition is servo-actuated servo-driver is calculated
<mrow>
<msub>
<mi>e</mi>
<mrow>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>&omega;</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>i</mi>
<mi>&beta;</mi>
</msub>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
upcβ(k)=Kpcβecβ(k)
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>s</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>p</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<msub>
<mi>K</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<msub>
<mi>e</mi>
<mrow>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
upresatcβ(k)=upcβ(k)+uicβ(k)
<mrow>
<msubsup>
<mi>I</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>I</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>p</mi>
<mi>r</mi>
<mi>e</mi>
<mi>s</mi>
<mi>a</mi>
<mi>t</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo><</mo>
<msubsup>
<mi>I</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>I</mi>
<mi>&beta;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>c</mi>
<mi>&beta;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein：upcβ(j) it is ratio control item；uicβ(j) it is integration control item；umaxcβAnd umincβFor the upper limit of PI controllers output
And lower limit, uimaxcβAnd uimincβFor the upper and lower bound of integral controller output, uimaxcβ=max (0, umaxcβ-upeβ(k)),
uimincβ=min (umincβ-upeβ(k),0)；KpcβFor PI proportional control factors；KicβFor integral coefficient；iβFor subtracting for Amimuth Transmission
Fast ratio；
(19) the current control instruction that the height under stable condition is servo-actuated servo-driver is calculated
<mrow>
<msub>
<mi>e</mi>
<mrow>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>&omega;</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>i</mi>
<mi>&epsiv;</mi>
</msub>
<mover>
<mi>&theta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
upcε(k)=Kpcεecε(k)
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>p</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<msub>
<mi>K</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<msub>
<mi>e</mi>
<mrow>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>max</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>i</mi>
<mi>min</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
upresatcε(k)=upcε(k)+uicε(k)
<mrow>
<msubsup>
<mi>I</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>I</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>p</mi>
<mi>r</mi>
<mi>e</mi>
<mi>s</mi>
<mi>a</mi>
<mi>t</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo><</mo>
<msubsup>
<mi>I</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>max</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>I</mi>
<mi>&epsiv;</mi>
<mo>*</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>u</mi>
<mrow>
<mi>min</mi>
<mi>c</mi>
<mi>&epsiv;</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein：upcε(j) it is ratio control item；uicε(j) it is integration control item；umaxcεAnd umincεFor the upper limit of PI controllers output
And lower limit, uimaxcεAnd uimincεFor the upper and lower bound of integral controller output, uimaxcε=max (umaxcε-upcε(k), 0),
uimincε=min (umincε-upcε(k),0)；KpcεFor PI proportional control factors；KicεFor integral coefficient；iεFor height follower actuation
Speed reducing ratio；
(20) setting driver is operated in torque mode, and current-order is sent to servo-driver by CAN；
(21) if eβ(j) ＜ eβmin,eε(j) ＜ eεmin, the state of aiming is reported to layer by CAN.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710981184.8A CN107783422B (en) | 2017-10-20 | 2017-10-20 | Control method of gun aiming stabilization system adopting strapdown inertial navigation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710981184.8A CN107783422B (en) | 2017-10-20 | 2017-10-20 | Control method of gun aiming stabilization system adopting strapdown inertial navigation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107783422A true CN107783422A (en) | 2018-03-09 |
| CN107783422B CN107783422B (en) | 2020-10-23 |
Family
ID=61435007
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710981184.8A Expired - Fee Related CN107783422B (en) | 2017-10-20 | 2017-10-20 | Control method of gun aiming stabilization system adopting strapdown inertial navigation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107783422B (en) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112361877A (en) * | 2020-11-24 | 2021-02-12 | 西北机电工程研究所 | Universal control module for gun AC servo driver |
| CN112484563A (en) * | 2020-11-24 | 2021-03-12 | 西北机电工程研究所 | Semi-physical experiment method of fire line stabilizing system |
| CN112696981A (en) * | 2020-12-21 | 2021-04-23 | 西北机电工程研究所 | Full closed loop interference rate compensation self-stabilization control method under geodetic coordinate system |
| CN112729012A (en) * | 2020-12-21 | 2021-04-30 | 西北机电工程研究所 | Equivalent closed loop interference rate compensation self-stabilization control method under geodetic coordinates |
| CN113280678A (en) * | 2021-05-19 | 2021-08-20 | 中国人民解放军63966部队 | Calibration method of artillery aiming performance parameter test system |
| CN114488794A (en) * | 2021-12-30 | 2022-05-13 | 北京动力机械研究所 | Method for inhibiting nutation of stamping range-increasing cannonball by adopting rudder |
| CN115342683A (en) * | 2022-08-25 | 2022-11-15 | 西北机电工程研究所 | Gun adjusting method with automatic obstacle avoiding function |
| CN116301081A (en) * | 2023-05-17 | 2023-06-23 | 伸瑞科技（北京）有限公司 | Speed control method, device, equipment and medium of inertia test equipment |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5527003A (en) * | 1994-07-27 | 1996-06-18 | Litton Systems, Inc. | Method for in-field updating of the gyro thermal calibration of an intertial navigation system |
| US6163021A (en) * | 1998-12-15 | 2000-12-19 | Rockwell Collins, Inc. | Navigation system for spinning projectiles |
| CN103217159A (en) * | 2013-03-06 | 2013-07-24 | 郭雷 | SINS/GPS/polarized light combination navigation system modeling and dynamic pedestal initial aligning method |
| CN103616030A (en) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction |
| CN103759742A (en) * | 2014-01-22 | 2014-04-30 | 东南大学 | Serial inertial navigation nonlinear alignment method based on fuzzy self-adaptation control technology |
| CN106482749A (en) * | 2016-12-07 | 2017-03-08 | 西北工业大学 | Alignment methods are combined with tachometer based on the inertial navigation of reverse navigation algorithm |
| CN106871928A (en) * | 2017-01-18 | 2017-06-20 | 北京工业大学 | Strap-down inertial Initial Alignment Method based on Lie group filtering |
-
2017
- 2017-10-20 CN CN201710981184.8A patent/CN107783422B/en not_active Expired - Fee Related
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US5527003A (en) * | 1994-07-27 | 1996-06-18 | Litton Systems, Inc. | Method for in-field updating of the gyro thermal calibration of an intertial navigation system |
| US6163021A (en) * | 1998-12-15 | 2000-12-19 | Rockwell Collins, Inc. | Navigation system for spinning projectiles |
| CN103217159A (en) * | 2013-03-06 | 2013-07-24 | 郭雷 | SINS/GPS/polarized light combination navigation system modeling and dynamic pedestal initial aligning method |
| CN103616030A (en) * | 2013-11-15 | 2014-03-05 | 哈尔滨工程大学 | Autonomous navigation system positioning method based on strapdown inertial navigation resolving and zero-speed correction |
| CN103759742A (en) * | 2014-01-22 | 2014-04-30 | 东南大学 | Serial inertial navigation nonlinear alignment method based on fuzzy self-adaptation control technology |
| CN106482749A (en) * | 2016-12-07 | 2017-03-08 | 西北工业大学 | Alignment methods are combined with tachometer based on the inertial navigation of reverse navigation algorithm |
| CN106871928A (en) * | 2017-01-18 | 2017-06-20 | 北京工业大学 | Strap-down inertial Initial Alignment Method based on Lie group filtering |
Non-Patent Citations (2)
| Title |
|---|
| 李伟等: "基于捷联惯导的火炮随动系统应用研究", 《火炮发射与控制学报》 * |
| 李伟等: "某全闭环操瞄系统的火炮身管指向控制研究", 《兵工学报》 * |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112484563A (en) * | 2020-11-24 | 2021-03-12 | 西北机电工程研究所 | Semi-physical experiment method of fire line stabilizing system |
| CN112361877A (en) * | 2020-11-24 | 2021-02-12 | 西北机电工程研究所 | Universal control module for gun AC servo driver |
| CN112696981A (en) * | 2020-12-21 | 2021-04-23 | 西北机电工程研究所 | Full closed loop interference rate compensation self-stabilization control method under geodetic coordinate system |
| CN112729012A (en) * | 2020-12-21 | 2021-04-30 | 西北机电工程研究所 | Equivalent closed loop interference rate compensation self-stabilization control method under geodetic coordinates |
| CN112729012B (en) * | 2020-12-21 | 2022-12-23 | 西北机电工程研究所 | Equivalent closed-loop interference rate compensation self-stabilization control method under geodetic coordinates |
| CN112696981B (en) * | 2020-12-21 | 2023-02-21 | 西北机电工程研究所 | Full closed loop interference rate compensation self-stabilization control method under geodetic coordinate system |
| CN113280678A (en) * | 2021-05-19 | 2021-08-20 | 中国人民解放军63966部队 | Calibration method of artillery aiming performance parameter test system |
| CN114488794B (en) * | 2021-12-30 | 2024-04-19 | 北京动力机械研究所 | Method for restraining nutation of stamping range-extending shell by adopting rudder |
| CN114488794A (en) * | 2021-12-30 | 2022-05-13 | 北京动力机械研究所 | Method for inhibiting nutation of stamping range-increasing cannonball by adopting rudder |
| CN115342683A (en) * | 2022-08-25 | 2022-11-15 | 西北机电工程研究所 | Gun adjusting method with automatic obstacle avoiding function |
| CN115342683B (en) * | 2022-08-25 | 2023-05-12 | 西北机电工程研究所 | Gun adjusting method with automatic obstacle avoidance function |
| CN116301081A (en) * | 2023-05-17 | 2023-06-23 | 伸瑞科技（北京）有限公司 | Speed control method, device, equipment and medium of inertia test equipment |
| CN116301081B (en) * | 2023-05-17 | 2023-08-04 | 伸瑞科技（北京）有限公司 | Speed control method, device, equipment and medium of inertia test equipment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN107783422B (en) | 2020-10-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107783422A (en) | Using the gun laying systems stabilisation control method of inertial navigation | |
| CN105539884B (en) | A kind of satellite driftage control guidance method | |
| US8924054B1 (en) | Systems and methods for positioning a marine vessel | |
| CN109782785B (en) | Automatic aircraft landing control method based on direct force control | |
| CN110609553B (en) | LOS (line of sight) guide control method for circular arc path of pipe-laying ship | |
| CN109708639B (en) | Method for generating lateral guidance instruction of aircraft for tracking straight line and circular arc path in flat flight | |
| CN106871742A (en) | A kind of control system being arranged on body | |
| CN106527491A (en) | Control system for fixed-wing unmanned aerial vehicle and horizontal and lateral flight track control method | |
| CN106444822A (en) | Space vector field guidance based stratospheric airship's trajectory tracking control method | |
| CN103955224B (en) | Attitude control method for relative motion visual line tracking | |
| CN110006419A (en) | Vehicle track tracking point determination method based on preview | |
| CN104199303A (en) | Stratospheric satellite planar path tracking control method based on vector field guidance | |
| CN112198885B (en) | Unmanned aerial vehicle control method capable of meeting autonomous landing requirement of maneuvering platform | |
| CN105334854A (en) | Decoupling control device and method for hovercraft course control and heeling control | |
| CN111881514B (en) | Guidance reconstruction method under engine fault state | |
| CN112061424B (en) | Maneuvering process energy angle dynamic tracking method based on fusion target attitude | |
| CN110456816A (en) | A kind of quadrotor Trajectory Tracking Control method based on continuous terminal sliding mode | |
| US20140244076A1 (en) | Stability based taxiing and turning method for aircraft with electric taxi system | |
| CN109343551A (en) | A kind of gyroplane coordinate turn control method and system | |
| CN102880059B (en) | Yawing maneuvering control method based on sinusoidal yawing guidance principle | |
| CN108427429B (en) | Spacecraft visual axis maneuvering control method considering dynamic pointing constraint | |
| WO2021052087A1 (en) | Manipulation method suitable for non-redundant sgcmg group | |
| CN105094144A (en) | Self-adaptive windproof path tracking control method for unmanned airship | |
| CN112696981B (en) | Full closed loop interference rate compensation self-stabilization control method under geodetic coordinate system | |
| CN102607556B (en) | Integrated navigation method for medium-accuracy heading and attitude system on basis of torpedo speed |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20201023 Termination date: 20211020 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |