CN107270882B - A kind of angular speed calculation method that base motion causes stage body to drift about - Google Patents
A kind of angular speed calculation method that base motion causes stage body to drift about Download PDFInfo
- Publication number
- CN107270882B CN107270882B CN201710324363.4A CN201710324363A CN107270882B CN 107270882 B CN107270882 B CN 107270882B CN 201710324363 A CN201710324363 A CN 201710324363A CN 107270882 B CN107270882 B CN 107270882B
- Authority
- CN
- China
- Prior art keywords
- axis
- stage body
- coordinate system
- frame
- angle
- 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.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C19/00—Gyroscopes; Turn-sensitive devices using vibrating masses; Turn-sensitive devices without moving masses; Measuring angular rate using gyroscopic effects
- G01C19/02—Rotary gyroscopes
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Gyroscopes (AREA)
Abstract
The invention discloses the angular speed calculation method that a kind of base motion causes stage body to drift about, this method is applied to twin shaft Inertial Platform System, comprising: obtains stage body in XP、YPAnd ZPThree gyroscope output valves on axis;The inside that measurement obtains the twin shaft Inertial Platform System relatively rotates angle;The inside that the output valve of gyroscope and measurement are obtained relatively rotates angle as the input value of servo loop, decoupling output valve is obtained, so that controller controls servo loop steady operation according to the decoupling output valve;When the servo loop steady operation, according to the external angular rate sensor with the twin shaft Inertial Platform System cobasis seat, measurement obtains pedestal angular speed;The angular speed of pedestal is obtained according to measurement and the internal angle that relatively rotates is resolved, and obtains drift angle velocity component.Method of the present invention, all without singular value, can effectively improve carrier without the full posture adaptability under trajectory constraint in any frame corners.
Description
Technical field
The invention belongs to twin shaft Inertial Platform System technical fields more particularly to a kind of base motion, and stage body to be caused to drift about
Angular speed calculation method.
Background technique
Twin shaft Inertial Platform System has been widely used in vehicle positioning orientation or Seeker Coordinator.It is fixed in vehicle positioning
In in application, twin shaft Inertial Platform System is mainly used for isolating the angular movement of two axial directions of carrier.For example, realizing to pitching
The isolation at angle and roll angle, it can also be used to pitch angle and azimuthal isolation.Under the conditions of vehicular applications, due to the motion of automobile
Less, therefore between stage body axis and gimbal axis it is not carried out decoupling, therefore, the corner of stage body axis is necessary for low-angle.
Requirement with tactical missile to Inertial Platform System small size, using four axis Inertial Platform Systems or three axis inertia
Plateform system has been unable to meet requirement, for this purpose, the twin shaft Inertial Platform System of small volume is used, twin shaft Inertial Platform System
The advantages of be it is small in size and meanwhile be isolated two freedom degrees angular movement.
However, due to there is the requirement of the full posture of high maneuver, existing two servo loops of twin shaft Inertial Platform System are not understood
The measure of coupling is gradually difficult to meet actual demand.
Summary of the invention
Technology of the invention solves the problems, such as: overcoming the deficiencies of the prior art and provide a kind of base motion and stage body is caused to drift about
Angular speed calculation method, all can effectively improve carrier without trajectory constraint without singular value in any frame corners
Under full posture adaptability.
In order to solve the above-mentioned technical problem, the invention discloses the angular speed calculating that a kind of base motion causes stage body to drift about
Method, the method are applied to twin shaft Inertial Platform System, and the servo loop of the twin shaft Inertial Platform System is sat using plane
Mark decomposer is decoupled, and the twin shaft Inertial Platform System includes pedestal, frame and stage body, wherein base body coordinate system
For X1Y1Z1, frame body coordinate system be XP1YP1ZP1, stage body body coordinate system is XPYPZP;The origin weight of three body coordinate systems
It closes, and: the Z of stage body body coordinate systemPThe Z of axis and frame body coordinate systemP1Overlapping of axles, the Y of base body coordinate system1Axis with
The Y of frame body coordinate systemP1Overlapping of axles;Wherein, pedestal and carrier are connected, when twin shaft Inertial Platform System is issued in carrier drive
When raw internal relative rotation, Y of the pedestal around frame body coordinate systemP1Axis rotation, Z of the frame around stage body body coordinate systemPAxis turns
It is dynamic;
Wherein, the base motion cause stage body drift about angular speed calculation method include:
Stage body is obtained in XP、YPAnd ZPThree gyroscope output valve u on axisix、uiyAnd uiz;
The inside that measurement obtains the twin shaft Inertial Platform System relatively rotates angle;Wherein, the internal relative rotation
Angle, comprising: Y of the pedestal around frame body coordinate systemP1The angle beta of axis rotationykWith frame around the Z of stage body body coordinate systemPAxis
The angle beta of rotationzk;
By uix、uiy、uizAnd βzkAs the input value of servo loop, decoupling output valve is obtained, so that controller is according to
It decouples output valve and controls servo loop steady operation;
When the servo loop steady operation, according to the external angular speed with the twin shaft Inertial Platform System cobasis seat
Sensor, measurement obtain the X of pedestal1Shaft angle rate ωx1And Z1Shaft angle rate ωz1;
Stage body is calculated separately according to the following formula in XPAxis, YPAxis and ZPDrift angle velocity component ω on axisxp、ωypWith
ωzp:
In the angular speed calculation method that said base movement causes stage body to drift about, the measurement obtains the twin shaft inertia
The inside of plateform system relatively rotates angle, comprising:
By the Y for being mounted on frameP1Angular transducer on axis, measurement obtain pedestal around the Y of frame body coordinate systemP1
The angle beta of axis rotationyk;
By being mounted on stage body ZPAngular transducer on axis, measurement obtain frame around the Z of stage body body coordinate systemPAxis turns
Dynamic angle betazk。
In the angular speed calculation method that said base movement causes stage body to drift about, rotational angle βykAnd βzkValue model
Enclose is 0~360 °.
The invention has the following advantages that
The angular speed calculation method that base motion of the present invention causes stage body to drift about, it is in office to give two attitude angles
The drift angle quantitative calculation method that image is prescribed a time limit, overcomes the restrictive condition that must operate at low-angle in the prior art.Secondly,
The present invention gives the quantitative calculation methods of drift angular speed, and servo loop is made to be no longer limited by the rate gyroscopes such as optical gyroscope
Requirement, this method be equally applicable to dynamic tuning gyroscope instrument, liquid floated gyroscope, three float-type gyroscopes etc. do not have directly output angular speed
Inertia type instrument as servo loop sensing element.
Detailed description of the invention
Fig. 1 is that a kind of coordinate system of twin shaft Inertial Platform System defines schematic diagram in the embodiment of the present invention;
Fig. 2 is a kind of rough schematic view of servo loop in the embodiment of the present invention;
Fig. 3 is a kind of step process for the angular speed calculation method that base motion causes stage body to drift about in the embodiment of the present invention
Figure;
Fig. 4 is a kind of base motion causes stage body to drift about in the embodiment of the present invention angular speed calculation method in engineer application
In decoupling and Computing Principle block diagram.
Specific embodiment
To make the object, technical solutions and advantages of the present invention clearer, public to the present invention below in conjunction with attached drawing
Embodiment is described in further detail.
The angular speed calculation method that base motion of the present invention causes stage body to drift about, is installed on twin shaft inertia with orthogonal
The output valve u of three gyroscopes of plateform system stage bodyix、uiyAnd uizAs the input information of decoupling link, merged by information
Two shaft end torque motors for being applied to stage body axis and gimbal axis respectively are exported afterwards, meanwhile, believe using pedestal angular speed as input
Breath, provides the drift angular speed in stage body relative inertness space.
In embodiments of the present invention, the angular speed calculation method that the base motion causes stage body to drift about is mainly used in double
Axis Inertial Platform System;Wherein, the twin shaft Inertial Platform System includes pedestal, frame and stage body, wherein base body coordinate
System is X1Y1Z1, frame body coordinate system be XP1YP1ZP1, stage body body coordinate system is XPYPZP;The origin of three body coordinate systems
It is overlapped, and: the Z of stage body body coordinate systemPThe Z of axis and frame body coordinate systemP1Overlapping of axles, the Y of base body coordinate system1Axis
With the Y of frame body coordinate systemP1Overlapping of axles;Wherein, pedestal and carrier are connected, when twin shaft Inertial Platform System is under carrier drive
When internal relatively rotate occurs, Y of the pedestal around frame body coordinate systemP1Axis rotation, Z of the frame around stage body body coordinate systemPAxis
Rotation.
Referring to Fig.1, it shows a kind of coordinate system of twin shaft Inertial Platform System in the embodiment of the present invention and defines schematic diagram.Figure
1 describes the relationship between each body coordinate system of twin shaft Inertial Platform System, whereinRepresentational framework is opposite with respect to stage body
Angular speed,Indicate the opposite angular speed of pedestal (rocket body) opposing frame.
When carrier and frame are around stage body axis ZPTurn over βzkWhen angle, have:
Wherein, ωxp、ωypAnd ωzpStage body is respectively indicated around XPAxis, YPAxis and ZPThe drift angle velocity component of axis;ωxp1、
ωyp1And ωzp1Carrier is respectively indicated together with frame around XP1、YP1And ZP1The absolute angular speed of axis.
When carrier is around the Y of frame body coordinate systemP1Axis turns over βykWhen angle, have:
Wherein, ωx1、ωy1And ωz1Carrier is respectively indicated around X1、Y1And Z1The absolute angular speed of axis.
It can be seen that the stage body angular speed of twin shaft Inertial Platform System, frame angular speed and pedestal (carrier) angular speed it
Between relational expression it is as follows:
Referring to Fig. 2, showing a kind of rough schematic view of servo loop, such as Fig. 2 in the embodiment of the present invention has:
Wherein, Gxp(s)≈Gyp(s)≈Gzp(s)≈Gp(s), Gyc(s)≈Gzc(s)≈Gc(s), have:
Based on foregoing description, referring to Fig. 3, a kind of angle that base motion causes stage body to drift about in the embodiment of the present invention is shown
The step flow chart of rate calculations method.In the present embodiment, the angular speed calculating side that the base motion causes stage body to drift about
Method, comprising:
Step 101, stage body is obtained in XP、YPAnd ZPThree gyroscope output valve u on axisix、uiyAnd uiz。
Step 102, the inside that measurement obtains the twin shaft Inertial Platform System relatively rotates angle.
In the present embodiment, the internal angle that relatively rotates includes: Y of the pedestal around frame body coordinate systemP1Axis rotation
Angle betaykWith frame around the Z of stage body body coordinate systemPThe angle beta of axis rotationzk。
It preferably, can be by being mounted on the Y of frameP1Angular transducer on axis, measurement obtain pedestal around frame body
The Y of coordinate systemP1The angle beta of axis rotationyk;By being mounted on stage body ZPAngular transducer on axis, measurement obtain frame around stage body
The Z of body coordinate systemPThe angle beta of axis rotationzk.Wherein, rotational angle βykAnd βzkValue range be 0~360 ° (including 0 ° and
360 °), i.e., this method is suitable for full Attitude Calculation.
Step 103, by uix、uiy、uizAnd βzkAs the input value of servo loop, decoupling output valve is obtained, so that controller
Servo loop steady operation is controlled according to the decoupling output valve.
Such as Fig. 2, decoupling output valve is uozAnd uoy;Wherein, uoz=uiz, uoy=uiycosβzk-uixsinβzk。
Step 104, when the servo loop steady operation, according to outer with the twin shaft Inertial Platform System cobasis seat
Portion's angular rate sensor, measurement obtain the X of pedestal1Shaft angle rate ωx1And Z1Shaft angle rate ωz1。
Step 105, stage body is calculated separately according to the following formula in XPAxis, YPAxis and ZPDrift angle velocity component on axis
ωxp、ωypAnd ωzp:
In conjunction with above-described embodiment, the angular speed calculation method for causing stage body to drift about below with reference to the base motion is in reality
Situation in is illustrated.Referring to Fig. 4, a kind of angle that base motion causes stage body to drift about in the embodiment of the present invention is shown
Decoupling and Computing Principle block diagram of the rate calculations method in engineer application.
First, carrying out stage body drift angle rate calculations using method of the present invention, impose a condition as follows: pedestal is around frame
The Y of frame body coordinate systemP1The angle beta of axis rotationyk=0 °;Z of the frame around stage body body coordinate systemPThe angle beta of axis rotationzk=
0°;Then, method according to the present invention can obtain: uoz=uiz、uoy=uiy、ωxp=ωx1、ωyp=0, ωzp=0.
It can be seen that stage body ZPAxis and frame YP1Axis controller input quantity respectively with stage body ZPAxis and YPGyroscope on axis
Measured value it is consistent.At this point, stage body XPAxis will be with pedestal X1Axis rotation.
Second, carrying out stage body drift angle rate calculations using method of the present invention, impose a condition as follows: pedestal is around frame
The Y of frame body coordinate systemP1The angle beta of axis rotationyk=0 °;Z of the frame around stage body body coordinate systemPThe angle beta of axis rotationzk=
90 °, then, method according to the present invention can obtain: uoz=uiz、uoy=-uix、ωxp=ωz1、ωyp=ωy1、ωzp=0.
It can be seen that stage body ZPThe controller input quantity and Z of axisPThe measured value of gyroscope on axis is consistent, but gimbal axis
YP1Controller input quantity and YPGyroscope on axis is unrelated, and is solely dependent upon XPGyroscope on axis, and it is contrary.This
When, stage body YPAxis will be with pedestal X1Axis rotation.
It is correct that above-mentioned two specific embodiment can verify calculation method of the invention.
In conclusion the angular speed calculation method that base motion of the present invention causes stage body to drift about, gives two
The drift angle quantitative calculation method that attitude angle image in office is prescribed a time limit, overcomes the limitation that must operate at low-angle in the prior art
Condition.Secondly, making servo loop be no longer limited by optical gyroscope The present invention gives the quantitative calculation method of drift angular speed
The requirement of equal rate gyroscopes, this method, which is equally applicable to dynamic tuning gyroscope instrument, liquid floated gyroscope, three float-type gyroscopes etc., not to be had directly
The inertia type instrument of angular speed is exported as servo loop sensing element.
Various embodiments are described in a progressive manner in this explanation, the highlights of each of the examples are with its
The difference of his embodiment, the same or similar parts between the embodiments can be referred to each other.
The above, optimal specific embodiment only of the invention, but scope of protection of the present invention is not limited thereto,
In the technical scope disclosed by the present invention, any changes or substitutions that can be easily thought of by anyone skilled in the art,
It should be covered by the protection scope of the present invention.
The content that description in the present invention is not described in detail belongs to the well-known technique of professional and technical personnel in the field.
Claims (3)
1. the angular speed calculation method that a kind of base motion causes stage body to drift about, which is characterized in that the method is applied to twin shaft
The servo loop of Inertial Platform System, the twin shaft Inertial Platform System is decoupled using plane coordinates decomposer, described double
Axis Inertial Platform System includes pedestal, frame and stage body, wherein base body coordinate system is X1Y1Z1, frame body coordinate system be
XP1YP1ZP1, stage body body coordinate system is XPYPZP;The origin of three body coordinate systems is overlapped, and: stage body body coordinate system
ZPThe Z of axis and frame body coordinate systemP1Overlapping of axles, the Y of base body coordinate system1The Y of axis and frame body coordinate systemP1Axis weight
It closes;Wherein, pedestal and carrier are connected, when twin shaft Inertial Platform System relatively rotates inside generation under carrier drive, pedestal
Around the Y of frame body coordinate systemP1Axis rotation, Z of the frame around stage body body coordinate systemPAxis rotation;
Wherein, the base motion cause stage body drift about angular speed calculation method include:
Stage body is obtained in XP、YPAnd ZPThree gyroscope output valve u on axisix、uiyAnd uiz;
The inside that measurement obtains the twin shaft Inertial Platform System relatively rotates angle;Wherein, the internal relative rotation angle,
It include: Y of the pedestal around frame body coordinate systemP1The angle beta of axis rotationykWith frame around the Z of stage body body coordinate systemPAxis rotation
Angle betazk;
By uix、uiy、uizAnd βzkAs the input value of servo loop, decoupling output valve is obtained, so that controller is according to the decoupling
Output valve controls servo loop steady operation;
When the servo loop steady operation, sensed according to the external angular speed of the twin shaft Inertial Platform System cobasis seat
Device, measurement obtain the X of pedestal1Shaft angle rate ωx1And Z1Shaft angle rate ωz1;
Stage body is calculated separately according to the following formula in XPAxis, YPAxis and ZPDrift angle velocity component ω on axisxp、ωypAnd ωzp:
Wherein, decoupling output valve is uozAnd uoy;Wherein, uoz=uiz, uoy=uiycosβzk-uixsinβzk;
It imposes a condition as follows: Y of the pedestal around frame body coordinate systemP1The angle beta of axis rotationyk=0 °;Frame is sat around stage body ontology
Mark the Z of systemPThe angle beta of axis rotationzk=0 °;Then, it can obtain: uoz=uiz、uoy=uiy、ωxp=ωx1、ωyp=0, ωzp=0;
It imposes a condition as follows: Y of the pedestal around frame body coordinate systemP1The angle beta of axis rotationyk=0 °;Frame is sat around stage body ontology
Mark the Z of systemPThe angle beta of axis rotationzkIt=90 °, then, can obtain: uoz=uiz、uoy=-uix、ωxp=ωz1、ωyp=ωy1、ωzp=
0。
2. the angular speed calculation method that base motion according to claim 1 causes stage body to drift about, which is characterized in that described
The inside that measurement obtains the twin shaft Inertial Platform System relatively rotates angle, comprising:
By the Y for being mounted on frameP1Angular transducer on axis, measurement obtain pedestal around the Y of frame body coordinate systemP1Axis rotation
Angle betayk;
By the Z for being mounted on stage bodyPAngular transducer on axis, measurement obtain frame around the Z of stage body body coordinate systemPAxis rotation
Angle betazk。
3. the angular speed calculation method that base motion according to claim 1 or 2 causes stage body to drift about, which is characterized in that
Rotational angle βykAnd βzkValue range be 0~360 °.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710324363.4A CN107270882B (en) | 2017-05-10 | 2017-05-10 | A kind of angular speed calculation method that base motion causes stage body to drift about |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710324363.4A CN107270882B (en) | 2017-05-10 | 2017-05-10 | A kind of angular speed calculation method that base motion causes stage body to drift about |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107270882A CN107270882A (en) | 2017-10-20 |
CN107270882B true CN107270882B (en) | 2019-11-29 |
Family
ID=60073897
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710324363.4A Active CN107270882B (en) | 2017-05-10 | 2017-05-10 | A kind of angular speed calculation method that base motion causes stage body to drift about |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107270882B (en) |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB1318872A (en) * | 1969-03-26 | 1973-05-31 | Sperry Rand Ltd | Stabilized platform |
CN103727939A (en) * | 2013-12-31 | 2014-04-16 | 清华大学 | Biaxial rotating attitude measurement system and measuring method thereof |
CN103872965A (en) * | 2014-02-28 | 2014-06-18 | 北京航天控制仪器研究所 | Servo loop control system and control method for floating oil support inertia instrument |
CN105021195A (en) * | 2015-07-06 | 2015-11-04 | 北京航天控制仪器研究所 | Servo loop decoupling method for four-axis inertial stabilized platform |
-
2017
- 2017-05-10 CN CN201710324363.4A patent/CN107270882B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
GB1318872A (en) * | 1969-03-26 | 1973-05-31 | Sperry Rand Ltd | Stabilized platform |
CN103727939A (en) * | 2013-12-31 | 2014-04-16 | 清华大学 | Biaxial rotating attitude measurement system and measuring method thereof |
CN103872965A (en) * | 2014-02-28 | 2014-06-18 | 北京航天控制仪器研究所 | Servo loop control system and control method for floating oil support inertia instrument |
CN105021195A (en) * | 2015-07-06 | 2015-11-04 | 北京航天控制仪器研究所 | Servo loop decoupling method for four-axis inertial stabilized platform |
Non-Patent Citations (2)
Title |
---|
三轴转台陀螺伺服系统的模型分析与解耦设计;张东纯等;《中国惯性技术学报》;19990930;第7卷(第3期);第59-61页 * |
两轴稳定雷达导引头解耦方案研究;朱士青等;《弹箭与制导学报》;20091031;第29卷(第5期);第6-10页 * |
Also Published As
Publication number | Publication date |
---|---|
CN107270882A (en) | 2017-10-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103245360B (en) | Carrier-borne aircraft rotation type strapdown inertial navigation system Alignment Method under swaying base | |
CN109596018B (en) | High-precision estimation method for flight attitude of spinning projectile based on magnetic roll angle rate information | |
CN110926468B (en) | Communication-in-motion antenna multi-platform navigation attitude determination method based on transfer alignment | |
CN109211269B (en) | Attitude angle error calibration method for double-shaft rotary inertial navigation system | |
CN105180936B (en) | A kind of servo loop decoupling method of four axles inertially stabilized platform system | |
CN103363992B (en) | Based on four rotor wing unmanned aerial vehicle attitude heading reference system calculation methods of Gradient Descent | |
CN103983276B (en) | A kind of three framework four axle inertial platform error calibrating methods based on navigation benchmark system | |
CN105910606B (en) | A kind of adjustment in direction method based on angular speed difference | |
CN108871326B (en) | A kind of single-shaft-rotation modulation inertia-astronomy deep integrated navigation method | |
CN103697894B (en) | Multi-source information unequal interval federated filter method based on the correction of wave filter variance battle array | |
CN107289971B (en) | A kind of angular speed calculating and compensation method that base motion causes stage body to drift about | |
CN105021195A (en) | Servo loop decoupling method for four-axis inertial stabilized platform | |
CN110243377B (en) | Cluster aircraft collaborative navigation method based on hierarchical structure | |
CN105115505B (en) | A kind of second order dynamic disturbance torque compensation method of four axles inertially stabilized platform system | |
CN110058288A (en) | Unmanned plane INS/GNSS integrated navigation system course error modification method and system | |
CN111024091A (en) | Three-dimensional attitude algorithm for indoor flight of vision-assisted micro unmanned aerial vehicle | |
CN105115503B (en) | A kind of servo loop decoupling method of three-axle steady platform system | |
CN109443352B (en) | Servo loop decoupling method of four-axis inertially stabilized platform system | |
CN107238386B (en) | A kind of angular speed calculating and compensation method that base motion causes stage body to drift about | |
WO2022057350A1 (en) | Inertial pre-integration method for combined motion measurement system based on nonlinear integral compensation | |
CN107270882B (en) | A kind of angular speed calculation method that base motion causes stage body to drift about | |
CN110488853B (en) | Hybrid inertial navigation system stability control instruction calculation method for reducing rotating shaft vortex influence | |
CN115574817B (en) | Navigation method and navigation system based on three-axis rotation type inertial navigation system | |
CN105277212B (en) | A kind of second order dynamic disturbance torque compensation method of three axis inertially stabilized platform system | |
CN109506649B (en) | Inner frame zero locking method and system for four-axis inertial stabilization platform system |
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 |