CN104006787B - Spacecraft Attitude motion simulation platform high-precision attitude defining method - Google Patents

Abstract

It is an object of the invention to provide a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method, the equipment adopted includes laser tracker, intelligence gauge head, gyroscope and two industrial control computers, thus building ground physical simulation experiment device, the method is a kind of improved method determined based on the attitude of EKF, by the output information of laser tracker and gyro is carried out Data Fusion Filtering process, obtain the attitude information of spacecraft Attitude motion simulation platform, more conform to real work situation, there is higher real-time.The measurement device that the present invention adopts is installed simple, and certainty of measurement is high, it is possible to the attitude angle of dynamic noncontacting measurement spacecraft Attitude motion simulation platform and angular rate information.

Description

Spacecraft Attitude motion simulation platform high-precision attitude defining method

Technical field

The present invention relates to measurement technology, be exactly specifically a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method.

Background technology

The air film that spacecraft Attitude motion simulation platform (also referred to as three-axis air-bearing table) is formed between air-bearing and bearing block according to pneumatically, simulation stage body is made to float, thus realizing approximate friction free relative motion condition, with spacecrafts such as analog satellites at the only small mechanical environment of disturbance torque suffered by outer space.As the universal simulator of spacecraft Attitude motion, three-axis air-bearing table can carry out the physical simulation experiment checking of aircraft manufacturing technology systematic function and analyze, and is the important means in spacecraft development process and method.

Spacecraft Attitude motion simulation platform requires over attitude measurement system in process of the test and dynamically provides the attitude information such as attitude angle, angular speed, to complete to control closed loop, special construction due to spacecraft Attitude motion simulation platform, the device (such as rotary transformer, inductosyn, photoelectric code disk, grating etc.) in the past measured for turntable is not suitable for the measurement of spacecraft Attitude motion simulation platform, it is necessary to consider new measuring method and device.And in current practice, the height of attitude measurement system precision is directly connected to the effect of l-G simulation test.

Find through searching document, Chinese invention patent application number: 201110249979.2, patent name is three-axis air-bearing table high-precision attitude angle measuring devices and methods therefor, this patent surrounding bottom three-axis air-bearing table table top is installed guide rail, window-blind and marker, artificial lighting system and digital CCD camera and is arranged on lifting platform.Digital CCD camera gathers the image of marker and is transferred on the computer of responsible image procossing, by marker carries out sub-pixel positioning, obtains the accurate coordinates of marker, thus the relative attitude angle calculated between marker and digital CCD camera.But due to the defect on system constructing, certainty of measurement is restricted, thus affecting its actually used scope.

Chinese invention patent application number: 201310134631.8, patent name is: three-axis air-bearing table high-precision attitude angle and angular velocity measurement device, this patent installs intelligence side head, gyroscope and kaleidoscope prism on the platform of three-axis air-bearing table, laser tracker and two photoelectric auto-collimators are installed under platform, directly obtain attitude information according to the data of laser tracker, two photoelectric auto-collimators and gyroscope.But this patent processes part in data filtering, do not consider that deviation quaternary digital-to-analogue is the constraints of 1, directly adopt Kalman filtering, it is easy to causing error covariance matrix to occur unusual, cause data scatter, attitude determines failure.This patent does not account for laser tracker and the attitude parameter conversion of photoelectric auto-collimator output information, gyroscope measure the processes such as coordinate system demarcation.Carry out additionally, data process is arranged under platform with communications portion, does not meet practical situation.

(it is published in aerospace journal at document " three-axis air-bearing table single frame servo angle measurement systematic research ", 1996,17th volume, 4th phase, the page number: 71-74) in, Zhang Xiaoyou, Liu Dun of Harbin Institute of Technology and the Li Jisu etc. of Beijing Control Engineering Inst. propose a kind of single frame servo measurement scheme, this system installs an arc arms that can rotate around air floating table center plumb line on air floating table base, and mounted thereon transportable balladeur train, by the attitude information of the rotation of sensitive arc arms and the traverse measurement air floating table of balladeur train.When this system needs to increase complicated mechanical system and sensor system, mechanism is complicated, and engineer applied is more difficult, and its precision is subject to the restriction of machinery and sensor, is extremely difficult to high accuracy.

Summary of the invention

Attitude for spacecraft Attitude motion simulation platform determines problem, it is an object of the invention to provide a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method, the method is a kind of improved method determined based on the attitude of EKF, by the output information of laser tracker and gyro is carried out Data Fusion Filtering process, obtain the attitude information of spacecraft Attitude motion simulation platform.

The present invention is achieved in that a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method, the equipment adopted includes laser tracker, intelligence gauge head, gyroscope and two industrial control computers, and laser tracker is installed under the platform of spacecraft Attitude motion simulation platform；Before attitude is determined, intelligence gauge head is arranged under platform, coordinate the measurement coordinate system of laser tracker Calibration of Laser tracker, attitude is determined in process, intelligence gauge head is fixed on the platform of spacecraft Attitude motion simulation platform, having target alignment system on intelligence gauge head, laser tracker coordinates intelligence gauge head can follow the tracks of and determine the three-dimensional perspective of intelligence gauge head rotation and the three-dimensional distance of movement；Gyroscope is fixed on the platform of spacecraft Attitude motion simulation platform, the angular rate information of output spacecraft Attitude motion simulation platform；First industrial control computer is arranged on the platform of spacecraft Attitude motion simulation platform, carries out data processing work；Second industrial control computer is arranged under platform, gathers the output data of laser tracker, and by wireless network, is sent to the first industrial control computer；First industrial control computer receives the attitude data of laser tracker and gyroscope, and carry out Data Fusion Filtering process, the attitude angle information of output spacecraft Attitude motion simulation platform, demarcates the drift of gyroscope simultaneously, revises the output angle speed of gyroscope in real time.

The present invention also has the feature that

1, a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method as above, the method that wherein Data Fusion Filtering processes is as follows:

Step 1: according to attitude parameter conversion formula, adopts 312 turns of sequences, utilize air floating table attitude angle θ that laser tracker exports,ψ calculates the coordinate conversion matrix A obtaining air floating table body coordinate system relative inertness system_ib:

Wherein, θ,ψ is the air floating table attitude angle of laser tracker output；

Calculate further and obtain attitude quaternion:

$\begin{array}{c}{q}_0=\sqrt{1+A_{11}+A_{22}+A_{33}}/2\\ q_1=(A_{23}-A_{32})/4q_0\\ q_2=(A_{31}-a_{13})/4q_0\\ q_3=(A_{12}-A_{21})/4q_0\end{array}---\left(2\right)$

Step 2: the state equation of structure system

Select the vector section Q of deviation quaternary number_eState variable X (t) sextuple with gyroscopic drift deviation delta b composition, the state equation of structure system:

$\left[\begin{array}{c}{\stackrel{.}{Q}}_e\\ \mathrm{\Δ}\stackrel{.}{b}\end{array}\right]=\left[\begin{array}{cc}-[\mathrm{\ω}\×]& -\frac{1}{2}{C}_{\mathrm{bs}}^g\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{Q}_e\\ \mathrm{\Δb}\end{array}\right]+\left[\begin{array}{cc}-\frac{1}{2}{C}_{\mathrm{bs}}^g& 0_{3\×3}\\ 0_{3\×3}& C_{\mathrm{bs}}^g\end{array}\right]\left[\begin{array}{c}{n}_g\\ n_s\end{array}\right]---\left(3\right)$

Namely

$\stackrel{.}{X}\left(t\right)=\mathrm{FX}\left(t\right)+\mathrm{GW}\left(t\right)---\left(4\right)$

Wherein

$[\mathrm{\ω}\×]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]---\left(5\right)$

F is the state matrix of system, and G is that systematic procedure noise drives matrix, and ω is the angle of rotation speed projection in body coordinate system of the body coordinate system relative inertness system of spacecraft Attitude motion simulation platform,It is the coordinate conversion matrix of the body coordinate system of the measurement coordinate system space attitude of flight vehicle motion simulation platform of gyro, n_gIt is the measurement noise of gyro, n_sIt it is the driving noise of the associated drift of gyro；

Sliding-model control:

X_k=Φ_{K, k-1}X_k-1+Γ_k-1W_k-1(6)

Wherein

$\begin{array}{c}{\mathrm{\Φ}}_{k,k-1}=I_{6\×6}+F\·T+\frac{T^2}{2}{F}^2\\ {\mathrm{\Γ}}_{k,k-1}=(I_{6\×6}\·T+\frac{T^2}{2}F+\frac{T^3}{6}{F}^2)G\end{array}---\left(7\right)$

T is sampling time interval, W_k-1It it is the process noise of system；

Step 3: the measurement equation of structure system

Choose the vector section Q of deviation quaternary number_eFor measuring value Z_k, the measurement equation of structure system:

$Z_k=H_k{X}_k+C_{\mathrm{is}}^s{V}_k---\left(8\right)$

Wherein

H_k=[I_3×30_3×3](9)

H_kIt is measurement matrix, V_kIt is the measurement noise of laser tracker,It it is the coordinate conversion matrix of the measurement coordinate system relative inertness coordinate system of laser tracker；

Step 4: a step recursion prediction

In the k moment, it is known that the attitude quaternion estimated value in k-1 momentThe estimated value of gyroscopic driftThe measurement angular speed u of gyro_k-1, measure the output quaternary number q obtaining current k moment laser tracker_k, the output angle speed u of gyroscope_k；

Owing to quaternary number exists the constraints that mould is 1 and the multiplication cross operation relation of deviation quaternary number, process is predicted for a step recursion, it is necessary to the separately performed discussion of the gyroscopic drift in state variable and attitude quaternion:

(4.1) a step recursion predictive value of gyroscopic drift

Due to gyroscopic drift change slowly, the gyroscopic drift in k moment is exactly the estimated value in k-1 momentThat is:

${\hat{b}}_{k|k-1}={\hat{b}}_{k-1}---\left(10\right)$

(4.2) a step recursion predictive value of attitude quaternion

Kinematical equation according to attitude quaternion, obtains the rate of change of k-1 moment attitude quaternion

${\stackrel{.}{\hat{q}}}_{k-1}=\frac{1}{2}{\hat{q}}_{k-1}\⊗\left\{\begin{array}{cc}0& {\widehat{\mathrm{\ω}}}_{k-1}\end{array}\right\}---\left(11\right)$

Wherein,It is the correction value of k-1 moment gyro output angle speed, the i.e. estimated value of spacecraft Attitude motion simulation platform angular speed, by utilizing gyroscopic drift to estimateGyro is exported u_k-1In drift components reject obtain:

${\widehat{\mathrm{\ω}}}_{k-1}=u_{k-1}-{\hat{b}}_{k-1}---\left(12\right)$

UtilizeThe method adopting correlation values integration, can obtain a step recursion predictive value of k moment attitude quaternion

Further, the definition according to deviation quaternary number, utilize a step recursion predictive value of attitude quaternionOutput q with laser tracker_k, obtain a step recursion predictive value of deviation quaternary number

${\hat{q}}_{e,k|k-1}={\hat{q}}_{k|k-1}^{-1}\⊗q_k---\left(13\right)$

Step 5: state updates

Carry out state renewal according to following formula, obtain the filtering estimated value of deviation quaternary number vector sectionFiltering estimated value with gyroscope (3) drift bias

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^{|T}+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(14\right)$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(15\right)$

P_k=(1-K_kHk₎P_k/k-1(16)

$\left[\begin{array}{c}{\hat{Q}}_{e,k}\\ \mathrm{\Δ}{\hat{b}}_k\end{array}\right]=\left[\begin{array}{c}{\hat{Q}}_{e,k|k-1}\\ \mathrm{\Δ}{\hat{b}}_{k|k-1}\end{array}\right]+K_k\·{\hat{Q}}_{e,k|k-1}---\left(17\right)$

Wherein Q_kIt is process noise statistical matrix,R_kIt is measurement noise statistical matrix,P_k/k-1It it is a step recursion prediction matrix of error statistics characteristic；K_kIt it is filtering gain matrix；P_kIt it is error covariance matrix；

Step 6: attitude filtering information merges

Owing to quaternary number exists the constraints that mould is 1 and the multiplication cross operation relation of deviation quaternary number, attitude filtering information fusion process is also required to gyroscopic drift and attitude quaternion are separately discussed:

(6.1) the filtering estimated value of gyroscopic drift

Gyroscopic drift deviation filtering estimated valueWith gyroscopic drift one step recursion predictive valueIt is added, obtains the filtering estimated value of gyroscopic drift

${\hat{b}}_k={\hat{b}}_{k|k-1}+\mathrm{\Δ}{\hat{b}}_k={\hat{b}}_{k-1}+\mathrm{\Δ}{\hat{b}}_k---\left(18\right)$

(6.2) the filtering estimated value of attitude quaternion

Definition according to deviation quaternary number, obtains the filtering estimated value of attitude quaternion

${\hat{q}}_k={\hat{q}}_{k|k-1}\⊗{\hat{q}}_{e,k}---\left(19\right)$

${\hat{q}}_{e,k}={\left[\begin{array}{cc}1& {\hat{Q}}_{e,k}\end{array}\right]}^T---\left(20\right)$

Step 7: output attitude parameter

(7.1) spacecraft Attitude motion simulation platform angular speed

Utilize the filtering estimated value of gyroscopic driftBy gyro output angle speed u_kIn drift value exclude, namely obtain spacecraft Attitude motion simulation platform angular speed

${\widehat{\mathrm{\ω}}}_k=u_k-{\hat{b}}_k---\left(21\right)$

(7.2) spacecraft Attitude motion simulation platform attitude angleθ, ψ:

Conversion formula according to attitude parameter, utilizes the filtering estimated value of attitude quaternionThe attitude angle of spacecraft Attitude motion simulation platform current time can be obtained:

2, a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method as above, the wherein coordinate conversion matrix of the measurement coordinate system relative inertness coordinate system of laser trackerScaling method be:

Use laser tracker measurement space attitude of flight vehicle motion simulation platform attitude angle time, it is necessary to the measurement coordinate system of known laser tracker relative to the position relation between inertial coodinate system, i.e. the measurement coordinate system O of Calibration of Laser tracker_s-X_sY_sZ_s, concrete grammar is as follows:

Initial point in inertial coodinate system and survey on east northeast both direction axle and place three target balls on measured fixing point respectively, measure three target balls and measure the coordinate in coordinate system at laser tracker, thus calibrating laser tracker to measure the position relation between coordinate system relative inertness coordinate system

First adjust laser tracker and make its level, measure O again through laser tracker_sTarget ball [0y on point and north orientation axle₁0]^TCoordinate in laser tracker coordinate system is [abc] respectively^T[xyz]^T, had by homogeneous coordinate transformation relation:

$\left[\begin{array}{c}0\\ y1\\ 0\\ 1\end{array}\right]=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_{\mathrm{is}}& -\sin {\mathrm{\θ}}_{\mathrm{is}}& 0& a\\ \sin {\mathrm{\θ}}_{\mathrm{is}}& \cos {\mathrm{\θ}}_{\mathrm{is}}& 0& b\\ 0& 0& 1& c\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]---\left(23\right)$

Thus solve θ_is, due to laser tracker level, therefore laser tracker measures coordinate system O_s-X_sY_sZ_sRelative inertness coordinate system O_i-X_iY_iZ_iBetween coordinate conversion matrixFor:

$C_{\mathrm{is}}^s=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_{\mathrm{is}}& \sin {\mathrm{\θ}}_{\mathrm{is}}& 0\\ -\sin {\mathrm{\θ}}_{\mathrm{is}}& \cos {\mathrm{\θ}}_{\mathrm{is}}& 0\\ 0& 0& 1\end{array}\right]---\left(24\right)$

3, a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method as above, gyro measures the coordinate conversion matrix of coordinate system space attitude of flight vehicle motion simulation platform body coordinate systemScaling method, as follows:

Installation according to gyroscope and measurement, show that gyroscope measures coordinate system O_gX_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle α between axle forward₁、α₁、α₃, O_gY_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle β between axle forward₁、β₂、β₃, O_gZ_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle γ between axle forward₁、γ₂、γ₃, then gyroscope measures coordinate system O_g-X_gY_gZ_gSpace attitude of flight vehicle motion simulation platform body coordinate system O_b-X_bY_bZ_bRotational transformation matrixFor:

$C_{\mathrm{bs}}^g=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_1& \cos {\mathrm{\β}}_1& \cos {\mathrm{\γ}}_1\\ \cos {\mathrm{\α}}_2& \cos {\mathrm{\β}}_2& \cos {\mathrm{\γ}}_2\\ \cos {\mathrm{\α}}_3& \cos {\mathrm{\β}}_3& \cos {\mathrm{\γ}}_3\end{array}\right]---\left(25\right).$

The present invention provides a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method, measurement device is installed simple, certainty of measurement is high, can the dynamically attitude angle of noncontacting measurement spacecraft Attitude motion simulation platform and angular rate information, and demarcate the drift of gyroscope simultaneously, revise the output angle speed of gyroscope in real time.The present invention builds ground physical simulation experiment device, on platform, the output information of laser tracker and gyro is carried out Data Fusion Filtering process, more conforms to real work situation, has higher real-time.The present invention is also used for the accurate measurement of single axle table attitude angle, angular speed.

Accompanying drawing explanation

Fig. 1 is the composition schematic diagram that spacecraft Attitude motion simulation platform high-precision attitude determines device；

Fig. 2 is spacecraft Attitude motion simulation platform attitude angle precision curve chart；

Fig. 3 is spacecraft Attitude motion simulation platform angle speed precision curve chart；

Fig. 4 is the schematic diagram that Calibration of Laser tracker measures coordinate system；

Fig. 5 is each ordinate transform relation schematic diagram；

Fig. 6 is the communication pattern schematic diagram that spacecraft Attitude motion simulation platform high-precision attitude determines device.

Detailed description of the invention

Below in conjunction with accompanying drawing citing, the invention will be further described.

Embodiment 1:

In conjunction with Fig. 1, one spacecraft Attitude motion simulation platform high-precision attitude defining method of the present invention, the equipment adopted includes laser tracker 1, intelligence gauge head 2, gyroscope 3 and two industrial control computers 4.5, and laser tracker 1 is installed under the platform of spacecraft Attitude motion simulation platform；Before attitude is determined, intelligence gauge head 2 is arranged under platform, coordinate the measurement coordinate system of laser tracker 1 Calibration of Laser tracker 1, attitude is determined in process, intelligence gauge head 2 is fixed on the platform of spacecraft Attitude motion simulation platform, having target alignment system on intelligence gauge head 2, laser tracker 1 coordinates intelligence gauge head 2 can follow the tracks of and determine the three-dimensional perspective of intelligence gauge head 2 rotation and the three-dimensional distance of movement；Gyroscope 3 is fixed on the platform of spacecraft Attitude motion simulation platform, the angular rate information of output spacecraft Attitude motion simulation platform；First industrial control computer 4 is arranged on the platform of spacecraft Attitude motion simulation platform, carries out data processing work；Second industrial control computer 5 is arranged under platform, gathers the output data of laser tracker 1, and by wireless network, is sent to the first industrial control computer 4；First industrial control computer 4 receives laser tracker 1 and the attitude data of gyroscope 3, and carry out Data Fusion Filtering process, the attitude angle information of output spacecraft Attitude motion simulation platform, demarcates the drift of gyroscope 3 simultaneously, revises the output angle speed of gyroscope 3 in real time.

Realize step as follows:

Step one: open device power supply (DPS), carries out next step after each equipment has preheated；

Step 2: the measurement coordinate system of Calibration of Laser tracker 1；

Step 3: demarcate the measurement coordinate system of gyroscope 3；

Step 4: build wireless network, it is achieved the network service of two industrial control computers 4.5 above and below the stage；

Step 5: utilize the output data of laser tracker 1 and gyroscope 3, obtains attitude angle and the angular rate information of air floating table by Data Fusion Filtering software 6.

The present invention utilizes laser tracker 1 and gyroscope 3 to combine the high-precision attitude realizing spacecraft Attitude motion simulation platform to determine, wherein laser tracker 1 can select Leica company of Switzerland AT901 product or the product of API company of the U.S., this sentences Leica company AT901 product is example, the certainty of measurement of its 3 d pose angle is 3, and " (3 σ) measures and ranges up to ± 60 °.During use, laser tracker 1 is arranged on the support under air floating table, intelligence gauge head 2 is arranged on air floating table platform and moves together with stage body, the visual field demand between laser tracker 1 optical axis and intelligence gauge head 2 target ball is ensured during installation, to ensure that test system does not lose measurement degree of freedom in the course of the work, laser tracker 1 calculates the relative attitude information of intelligence gauge head 2 in real time according to measurement data.

The measurement of spacecraft Attitude motion simulation platform angular speed is realized by high accuracy hemispherical resonant gyro.Hemispherical resonant gyro is vertically mounted on the spacecraft Attitude motion simulation platform center after leveling, and its sensitive axes is vertical with spacecraft Attitude motion simulation platform rotating shaft.The output of hemispherical resonant gyro contains the constant value drift of spacecraft Attitude motion simulation platform, associated drift, the drift noises such as measurement noise, when actually used, adopt the attitude information output angle speed through Data Fusion Filtering software correction gyro of laser tracker.

Embodiment 2:

The present invention proposes a kind of spacecraft Attitude motion simulation platform attitude based on EKF and determines improvement project, build ground physical simulation experiment device, by the output information of laser tracker and gyroscope is carried out Data Fusion Filtering process, obtain the attitude information of spacecraft Attitude motion simulation platform, specifically comprise the following steps that

Step 1: according to attitude parameter conversion formula, adopts 312 turns of sequences, utilize air floating table attitude angle θ that laser tracker exports,ψ calculates the coordinate conversion matrix A obtaining air floating table body coordinate system relative inertness system_ib:

Wherein, θ,ψ is the air floating table attitude angle of laser tracker output；

Calculate further and obtain attitude quaternion:

$\begin{array}{c}{q}_0=\sqrt{1+A_{11}+A_{22}+A_{33}}/2\\ q_1=(A_{23}-A_{32})/4q_0\\ q_2=(A_{31}-a_{13})/4q_0\\ q_3=(A_{12}-A_{21})/4q_0\end{array}---\left(27\right)$

Step 2: the state equation of structure system

Select the vector section Q of deviation quaternary number_eState variable X (t) sextuple with gyroscopic drift deviation delta b composition, the state equation of structure system:

$\left[\begin{array}{c}{\stackrel{.}{Q}}_e\\ \mathrm{\Δ}\stackrel{.}{b}\end{array}\right]=\left[\begin{array}{cc}-[\mathrm{\ω}\×]& -\frac{1}{2}{C}_{\mathrm{bs}}^g\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{Q}_e\\ \mathrm{\Δb}\end{array}\right]+\left[\begin{array}{cc}-\frac{1}{2}{C}_{\mathrm{bs}}^g& 0_{3\×3}\\ 0_{3\×3}& C_{\mathrm{bs}}^g\end{array}\right]\left[\begin{array}{c}{n}_g\\ n_s\end{array}\right]---\left(28\right)$

Namely

$\stackrel{.}{X}\left(t\right)=\mathrm{FX}\left(t\right)+\mathrm{GW}\left(t\right)---\left(29\right)$

Wherein

$[\mathrm{\ω}\×]=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]---\left(30\right)$

F is the state matrix of system, and G is that systematic procedure noise drives matrix, and ω is the angle of rotation speed projection in body coordinate system of the body coordinate system relative inertness system of spacecraft Attitude motion simulation platform,It is the coordinate conversion matrix of the body coordinate system of the measurement coordinate system space attitude of flight vehicle motion simulation platform of gyro, n_gIt is the measurement noise of gyro, n_sIt it is the driving noise of the associated drift of gyro；

Sliding-model control:

X_k=Φ_{K, k-1}X_k-1+Γ_k-1W_k-1(31)

Wherein

$\begin{array}{c}{\mathrm{\Φ}}_{k,k-1}=I_{6\×6}+F\·T+\frac{T^2}{2}{F}^2\\ {\mathrm{\Γ}}_{k,k-1}=(I_{6\×6}\·T+\frac{T^2}{2}F+\frac{T^3}{6}{F}^2)G\end{array}---\left(32\right)$

T is sampling time interval, W_k-1It it is the process noise of system；

Step 3: the measurement equation of structure system

Choose the vector section Q of deviation quaternary number_eFor measuring value Z_k, the measurement equation of structure system:

$Z_k=H_k{X}_k+C_{\mathrm{is}}^s{V}_k---\left(33\right)$

Wherein

H_k=[I_3×30_3×3](34)

H_kIt is measurement matrix, V_kIt is the measurement noise of laser tracker,It it is the coordinate conversion matrix of the measurement coordinate system relative inertness coordinate system of laser tracker；

Step 4: a step recursion prediction

In the k moment, it is known that the attitude quaternion estimated value in k-1 momentThe estimated value of gyroscopic driftThe measurement angular speed u of gyro_k-1, measure the output quaternary number q obtaining current k moment laser tracker_k, the output angle speed u of gyroscope_k；

Owing to quaternary number exists the constraints that mould is 1 and the multiplication cross operation relation of deviation quaternary number, process is predicted for a step recursion, it is necessary to the separately performed discussion of the gyroscopic drift in state variable and attitude quaternion:

(4.1) a step recursion predictive value of gyroscopic drift

Due to gyroscopic drift change slowly, the gyroscopic drift in k moment is exactly the estimated value in k-1 momentThat is:

${\hat{b}}_{k|k-1}={\hat{b}}_{k-1}---\left(35\right)$

(4.2) a step recursion predictive value of attitude quaternion

Kinematical equation according to attitude quaternion, obtains the rate of change of k-1 moment attitude quaternion

${\stackrel{.}{\hat{q}}}_{k-1}=\frac{1}{2}{\hat{q}}_{k-1}\⊗\left\{\begin{array}{cc}0& {\widehat{\mathrm{\ω}}}_{k-1}\end{array}\right\}---\left(36\right)$

Wherein,It is the correction value of k-1 moment gyro output angle speed, the i.e. estimated value of spacecraft Attitude motion simulation platform angular speed, by utilizing gyroscopic drift to estimateGyro is exported u_k-1In drift components reject obtain:

${\widehat{\mathrm{\ω}}}_{k-1}=u_{k-1}-{\hat{b}}_{k-1}---\left(37\right)$

UtilizeThe method adopting correlation values integration, can obtain a step recursion predictive value of k moment attitude quaternion

Further, the definition according to deviation quaternary number, utilize a step recursion predictive value of attitude quaternionOutput q with laser tracker_k, obtain a step recursion predictive value of deviation quaternary number

${\hat{q}}_{e,k|k-1}={\hat{q}}_{k|k-1}^{-1}\⊗q_k---\left(38\right)$

Step 5: state updates

Carry out state renewal according to following formula, obtain the filtering estimated value of deviation quaternary number vector sectionFiltering estimated value with gyroscope (3) drift bias

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^{|T}+{\mathrm{\Γ}}_{k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k-1}^T---\left(39\right)$

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}---\left(40\right)$

P_k=(1-K_kH_k)P_k/k-1(41)

$\left[\begin{array}{c}{\hat{Q}}_{e,k}\\ \mathrm{\Δ}{\hat{b}}_k\end{array}\right]=\left[\begin{array}{c}{\hat{Q}}_{e,k|k-1}\\ \mathrm{\Δ}{\hat{b}}_{k|k-1}\end{array}\right]+K_k\·{\hat{Q}}_{e,k|k-1}---\left(42\right)$

Wherein Q_kIt is process noise statistical matrix,R_kIt is measurement noise statistical matrix,P_k/k-1It it is a step recursion prediction matrix of error statistics characteristic；K_kIt it is filtering gain matrix；P_kIt it is error covariance matrix；

Step 6: attitude filtering information merges

Owing to quaternary number exists the constraints that mould is 1 and the multiplication cross operation relation of deviation quaternary number, attitude filtering information fusion process is also required to gyroscopic drift and attitude quaternion are separately discussed:

(6.1) the filtering estimated value of gyroscopic drift

Gyroscopic drift deviation filtering estimated valueWith gyroscopic drift one step recursion predictive valueIt is added, obtains the filtering estimated value of gyroscopic drift

${\hat{b}}_k={\hat{b}}_{k|k-1}+\mathrm{\Δ}{\hat{b}}_k={\hat{b}}_{k-1}+\mathrm{\Δ}{\hat{b}}_k---\left(43\right)$

(6.2) the filtering estimated value of attitude quaternion

Definition according to deviation quaternary number, obtains the filtering estimated value of attitude quaternion

${\hat{q}}_k={\hat{q}}_{k|k-1}\⊗{\hat{q}}_{e,k}---\left(44\right)$

${\hat{q}}_{e,k}={\left[\begin{array}{cc}1& {\hat{Q}}_{e,k}\end{array}\right]}^T---\left(45\right)$

Step 7: output attitude parameter

(7.1) spacecraft Attitude motion simulation platform angular speed

Utilize the filtering estimated value of gyroscopic driftBy gyro output angle speed u_kIn drift value exclude, namely obtain spacecraft Attitude motion simulation platform angular speed

${\widehat{\mathrm{\ω}}}_k=u_k-{\hat{b}}_k---\left(46\right)$

(7.2) spacecraft Attitude motion simulation platform attitude angleθ, ψ:

Conversion formula according to attitude parameter, utilizes the filtering estimated value of attitude quaternionThe attitude angle of spacecraft Attitude motion simulation platform current time can be obtained:

In conjunction with Fig. 2-3, the certainty of measurement of laser tracker is 3 " (3 σ); the constant value drift of gyroscope is 1.5 °/h (3 σ); random drift is 0.003 °/h (3 σ); measurement noise is 10 " (3 σ), through mathematical simulation, spacecraft Attitude motion simulation platform attitude angle precision is 0.05 ", angle speed precision is 0.7 ".

Embodiment 3:

In conjunction with Fig. 4, introduce the measurement coordinate system O of laser tracker_s-X_sY_sZ_sRelative inertness coordinate system O_i-X_iY_iZ_iCoordinate conversion matrixScaling method.

First adjust laser tracker and make its level, measure O again through laser tracker_sTarget ball [0y on point and north orientation axle₁0]^TCoordinate in laser tracker coordinate system is [abc] respectively^T[xyz]^T, had by homogeneous coordinate transformation relation:

$\left[\begin{array}{c}0\\ y1\\ 0\\ 1\end{array}\right]=\left[\begin{array}{cccc}\cos {\mathrm{\θ}}_{\mathrm{is}}& -\sin {\mathrm{\θ}}_{\mathrm{is}}& 0& a\\ \sin {\mathrm{\θ}}_{\mathrm{is}}& \cos {\mathrm{\θ}}_{\mathrm{is}}& 0& b\\ 0& 0& 1& c\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]---\left(23\right)$

Thus can solve θ_is, due to laser tracker level, therefore measured coordinate system O by laser tracker_s-X_sY_sZ_sRelative inertness coordinate system O_i-X_iY_iZ_iBetween coordinate conversion matrixFor:

$C_{\mathrm{is}}^s=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_{\mathrm{is}}& \sin {\mathrm{\θ}}_{\mathrm{is}}& 0\\ -\sin {\mathrm{\θ}}_{\mathrm{is}}& \cos {\mathrm{\θ}}_{\mathrm{is}}& 0\\ 0& 0& 1\end{array}\right]---\left(24\right)$

Embodiment 4:

In conjunction with Fig. 5, introduce gyro and measure coordinate system O_g-X_gY_gZ_gSpace attitude of flight vehicle motion simulation platform body coordinate system O_b-X_bY_bZ_bCoordinate conversion matrixScaling method, as follows

Installation according to gyroscope and measurement, show that gyroscope measures coordinate system O_gX_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle α between axle forward₁、α₂、α₃, O_gY_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle β between axle forward₁、β₂、β₃, O_gZ_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle γ between axle forward₁、γ₂、γ₃, then gyroscope measures coordinate system O_g-X_gY_gZ_gSpace attitude of flight vehicle motion simulation platform body coordinate system O_b-X_bY_bZ_bRotational transformation matrixFor:

$C_{\mathrm{bs}}^g=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_1& \cos {\mathrm{\β}}_1& \cos {\mathrm{\γ}}_1\\ \cos {\mathrm{\α}}_2& \cos {\mathrm{\β}}_2& \cos {\mathrm{\γ}}_2\\ \cos {\mathrm{\α}}_3& \cos {\mathrm{\β}}_3& \cos {\mathrm{\γ}}_3\end{array}\right]---\left(25\right).$

Embodiment 5:

In conjunction with Fig. 6, introduce spacecraft Attitude motion simulation platform high-precision attitude and determine the communication pattern of device.

Industrial control computer on platform gathers the angular rate data from gyroscope output by data acquisition scale.Industrial control computer under platform gathers the attitude angle data from laser tracker output by data acquisition scale, and is sent to the industrial control computer on platform by wireless network.Industrial control computer on platform utilizes wireless network to receive the data from the industrial control computer under platform, and carry out Data Fusion Filtering process, the attitude angle information of output spacecraft Attitude motion simulation platform, demarcate the drift of gyroscope simultaneously, revise the output angle speed of gyroscope in real time.

Claims (4)

1. a spacecraft Attitude motion simulation platform high-precision attitude defining method, the equipment adopted includes laser tracker, intelligence gauge head, gyroscope and two industrial control computers, it is characterised in that: laser tracker is installed under the platform of spacecraft Attitude motion simulation platform；Before attitude is determined, intelligence gauge head is arranged under platform, coordinate the measurement coordinate system of laser tracker Calibration of Laser tracker, attitude is determined in process, intelligence gauge head is fixed on the platform of spacecraft Attitude motion simulation platform, having target alignment system on intelligence gauge head, laser tracker coordinates intelligence gauge head can follow the tracks of and determine the three-dimensional perspective of intelligence gauge head rotation and the three-dimensional distance of movement；Gyroscope is fixed on the platform of spacecraft Attitude motion simulation platform, the angular rate information of output spacecraft Attitude motion simulation platform；First industrial control computer is arranged on the platform of spacecraft Attitude motion simulation platform, carries out data processing work；Second industrial control computer is arranged under platform, gathers the output data of laser tracker, and by wireless network, is sent to the first industrial control computer；First industrial control computer receives the attitude data of laser tracker and gyroscope, and carry out Data Fusion Filtering process, the attitude angle information of output spacecraft Attitude motion simulation platform, demarcates the drift of gyroscope simultaneously, revises the output angle speed of gyroscope in real time.

2. a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method according to claim 1, it is characterised in that the method that described Data Fusion Filtering processes is as follows:

Step 1: according to attitude parameter conversion formula, adopts 312 turns of sequences, utilize air floating table attitude angle θ that laser tracker exports,ψ calculates the coordinate conversion matrix A obtaining air floating table body coordinate system relative inertness system_ib:

Wherein, θ,ψ is the air floating table attitude angle of laser tracker output；

Calculate further and obtain attitude quaternion:

q₁=(A₂₃-A₃₂)/4q₀(2)

q₂=(A₃₁-A₁₃)/4q₀

q₃=(A₁₂-A₂₁)/4q₀

Step 2: the state equation of structure system

Select the vector section Q of deviation quaternary number_eState variable X (t) sextuple with gyroscopic drift deviation delta b composition, the state equation of structure system:

Namely

Wherein

F is the state matrix of system, and G is that systematic procedure noise drives matrix, and ω is the angle of rotation speed projection in body coordinate system of the body coordinate system relative inertness system of spacecraft Attitude motion simulation platform,It is the coordinate conversion matrix of the body coordinate system of the measurement coordinate system space attitude of flight vehicle motion simulation platform of gyro, n_gIt is the measurement noise of gyro, n_sIt it is the driving noise of the associated drift of gyro；

Sliding-model control:

X_k=Φ_{K, k-1}X_k-1+Γ_k-1W_k-1(6)

Wherein

T is sampling time interval, W_k-1It it is the process noise of system；

Step 3: the measurement equation of structure system

Choose the vector section Q of deviation quaternary number_eFor measuring value Z_k, the measurement equation of structure system:

Wherein

H_k=[I_3×30_3×3](9)

H_kIt is measurement matrix, V_kIt is the measurement noise of laser tracker,It it is the coordinate conversion matrix of the measurement coordinate system relative inertness coordinate system of laser tracker；

Step 4: a step recursion prediction

In the k moment, it is known that the attitude quaternion estimated value in k-1 momentThe estimated value of gyroscopic driftThe measurement angular speed u of gyro_k-1, measure the output quaternary number q obtaining current k moment laser tracker_k, the output angle speed u of gyroscope_k；

Process is predicted for a step recursion, it is necessary to the separately performed discussion of the gyroscopic drift in state variable and attitude quaternion:

(4.1) a step recursion predictive value of gyroscopic drift

Due to gyroscopic drift change slowly, the gyroscopic drift in k moment is exactly the estimated value in k-1 momentThat is:

(4.2) a step recursion predictive value of attitude quaternion

Kinematical equation according to attitude quaternion, obtains the rate of change of k-1 moment attitude quaternion

Wherein,It is the correction value of k-1 moment gyro output angle speed, the i.e. estimated value of spacecraft Attitude motion simulation platform angular speed, by utilizing gyroscopic drift to estimateGyro is exported u_k-1In drift components reject obtain:

UtilizeThe method adopting correlation values integration, can obtain a step recursion predictive value of k moment attitude quaternion

Further, the definition according to deviation quaternary number, utilize a step recursion predictive value of attitude quaternionOutput q with laser tracker_k, obtain a step recursion predictive value of deviation quaternary number

Step 5: state updates

Carry out state renewal according to following formula, obtain the filtering estimated value of deviation quaternary number vector sectionFiltering estimated value with gyroscopic drift deviation

P_k=(1-K_kH_k)P_k/k-1(16)

Wherein Q_kIt is process noise statistical matrix,R_kIt is measurement noise statistical matrix,P_k/k-1It it is a step recursion prediction matrix of error statistics characteristic；K_kIt it is filtering gain matrix；P_kIt it is error covariance matrix；

Step 6: attitude filtering information merges

Attitude filtering information fusion process is also required to gyroscopic drift and attitude quaternion are separately discussed:

(6.1) the filtering estimated value of gyroscopic drift

Gyroscopic drift deviation filtering estimated valueWith gyroscopic drift one step recursion predictive valueIt is added, obtains the filtering estimated value of gyroscopic drift

(6.2) the filtering estimated value of attitude quaternion

Definition according to deviation quaternary number, obtains the filtering estimated value of attitude quaternion

Step 7: output attitude parameter

(7.1) spacecraft Attitude motion simulation platform angular speed

Utilize the filtering estimated value of gyroscopic driftBy gyro output angle speed u_kIn drift value exclude, namely obtain spacecraft Attitude motion simulation platform angular speed

(7.2) spacecraft Attitude motion simulation platform attitude angleθ, ψ:

Conversion formula according to attitude parameter, utilizes the filtering estimated value of attitude quaternionObtain the attitude angle of spacecraft Attitude motion simulation platform current time:

3. a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method according to claim 1, it is characterised in that the coordinate conversion matrix of the measurement coordinate system relative inertness coordinate system of laser trackerScaling method, i.e. the measurement coordinate system O of Calibration of Laser tracker_s-X_sY_sZ_s, concrete grammar is as follows:

Initial point in inertial coodinate system and survey on east northeast both direction axle and place three target balls on measured fixing point respectively, measure three target balls and measure the coordinate in coordinate system at laser tracker, thus calibrating laser tracker to measure the position relation between coordinate system relative inertness coordinate system

First adjust laser tracker and make its level, measure O again through laser tracker_sTarget ball [0y on point and north orientation axle₁0]^TCoordinate in laser tracker coordinate system is [abc] respectively^T[xyz]^T, had by homogeneous coordinate transformation relation:

Thus solve θ_is, due to laser tracker level, therefore laser tracker measures coordinate system O_s-X_sY_sZ_sRelative inertness coordinate system O_i-X_iY_iZ_iBetween coordinate conversion matrixFor:

4. a kind of spacecraft Attitude motion simulation platform high-precision attitude defining method according to claim 1, it is characterised in that gyro measures the coordinate conversion matrix of coordinate system space attitude of flight vehicle motion simulation platform body coordinate systemScaling method, as follows:

Installation according to gyroscope and measurement, show that gyroscope measures coordinate system O_gX_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle α between axle forward₁、α₂、α₃, O_gY_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle β between axle forward₁、β₂、β₃, O_gZ_gAxle forward and platform body coordinate system O_bX_bAxle, O_bY_bAxle, O_bZ_bAngle γ between axle forward₁、γ₂、γ₃, then gyroscope measures coordinate system O_g-X_gY_gZ_gSpace attitude of flight vehicle motion simulation platform body coordinate system O_b-X_bY_bZ_bRotational transformation matrixFor: