CN104913790A - Heading drift error closed-loop compensation method applied to communication-in-moving inertial navigation system - Google Patents

Abstract

A heading drift error closed-loop compensation method applied to communication-in-moving inertial navigation systemcomprises the following steps: first, after the completion of the system satellite alignment, the navigation control cycle is used for navigation solution and antenna tracking drive control; then a beacon signal is used as auxiliary information to obtain heading drift error by scanning, and drive an antenna azimuth shaft to rotate the satellite beacon signal strongest point; and finally heading angle error compensation is performed, and by use of the compensated heading angle, each strap-down matrix of the inertial navigation system is updated, and is introduced to the next time navigation period to achieve heading drift error strap-down navigation algorithm closed loop correction. The method is applicable to vehicle and ship and other communication-in-moving inertial navigation systems which are uninterruptible power in a long time.

Description

A kind of inertial navigation system heading effect error closed loop compensation method being applied to communication in moving

Technical field

The invention provides a kind of inertial navigation system heading effect error compensating method being applied to communication in moving, be applicable to require that system keeps the occasion of antenna tracking satellite precision for a long time, belong to satellite communication technology field.

Background technology

Satellite Tracking based on " communication in moving " antenna system of inertial navigation scheme controls mainly to rely on inertial navigation system energy real-time resolving and goes out the pitching of carrier, rolling and course angle information, then maintains antenna by coordinate conversion guide antenna isolation carrier movement and point to target satellite all the time.Because inertia type instrument exists drift error, inertial navigation system is when long-time not power-off runs, and the attitude of carrier that inertial navigation solution of equation can be made to calculate and course angle produce drift error, affect antenna system satellite pointing accuracy.

According to inertial navigation system error Propagation Property, carrier two horizontal attitude angle meets the shura oscillation period of 84.4min, and makes carrier two horizontal attitude angle remain on vibration in certain angular range by Schuler cycle restriction.Inertial navigation system attitude algorithm precision is higher, is subject to again Schuler cycle restriction, so carrier two horizontal attitude angle long term drift error is less, can ignore its impact on antenna tracking satellite precision.But inertial navigation system course angle does not meet shura oscillation period, and its error increases in time and constantly increases, long-time when using course angle will there is larger error accumulation, thus cause the continuous variation of antenna system satellite pointing accuracy.

Can find out according to above-mentioned analysis, simple dependence inertial navigation system high precision airmanship, realize " communication in moving " antenna system long-term high precision satellite-signal following function, there is larger difficulty, especially for boat-carrying " communication in moving " application, the long time drift error accumulation of inertial navigation system course angle can have a strong impact on the tracking accuracy of antenna system satellite-signal.

Therefore, how to overcome the drawback that inertial navigation system course angle error is constantly accumulated in time, realizing the high-accuracy stable Satellite Tracking function of antenna system in long-time uninterrupted operation situation is at present based on a difficult point of inertial navigation scheme " communication in moving " system.

Summary of the invention

Technology of the present invention is dealt with problems: overcome the deficiencies in the prior art, provide a kind of inertial navigation system heading effect error closed loop compensation method being applied to communication in moving, the problem that solution inertial navigation system course angle error is constantly accumulated in time and caused antenna for satellite communication in motion system-satellite tracking accuracy impaired.

Technical solution of the present invention:

Be applied to an inertial navigation system heading effect error closed loop compensation method for communication in moving, step is as follows:

(1), after described communication in moving completes satellite aligning, arrange and control navigation period counter M, course timer SCANFLAG and compensate mark MODIFLAG, initial value is 0;

(2) control navigation period counter M and course timer SCANFLAG and start timing, when period counter M timing of navigating is to 5ms, enter step (3);

(3) navigation calculation is carried out to the data that the inertial navigation system in communication in moving provides, obtain the attitude angle of carrier, and according to the positional information of the attitude angle of carrier, the positional information of carrier and satellite, calculate the polarizing angle of antenna, the angle of pitch and position angle, complete the gesture drive of antenna, enter step (4) afterwards; Described carrier refers to the transportation equipment of carrying communication in moving system;

(4) judge whether course timer SCANFLAG reaches default timing, if reached, then azimuth axis of antenna carries out step-scan according to default step-length, when scanning, all reads semaphore value at every turn, determines the course error Δ Y that maximum semaphore value place is corresponding; Turn in antenna bearingt angle corresponding to maximum semaphore value place according to described course error Δ Y driven antenna azimuth axis afterwards, arrange simultaneously and compensate mark MODIFLAG=1, enter step (5) afterwards; If course timer SCANFLAG does not reach default timing, then directly enter step (5);

(5) whether effectively judge to compensate mark MODIFLAG, represent effective during MODIFLAG=1, during MODIFLAG=0, represent invalid, if it is invalid to compensate mark MODIFLAG, return step (2); If it is effective to compensate mark MODIFLAG, then course error Δ Y corresponding to the maximum semaphore value place determined according to step (4) compensates current inertial navigation system course angle Y', the inertial navigation system course angle after compensation

Y _compensate=Y'+ Δ Y, according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ' and strapdown attitude matrix T, realize the Closed-cycle correction of drift error, return the drift error Closed-cycle correction that step (2) carries out the next navigation cycle afterwards.

Described according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ' and strapdown attitude matrix T to be specially:

(2.1) according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ', be specially:

Wherein, P is the present carrier angle of pitch, and R is present carrier roll angle;

(2.2) be normalized by the strapdown Quaternion Matrix Q ' after renewal, obtain the strapdown Quaternion Matrix Q after normalization, formula is as follows:

$Q=\frac{Q^{\′}}{\sqrt{Q^{\′}{\[0\]}^2+Q^{\′}{\[1\]}^2+Q^{\′}{\[2\]}^2+Q^{\′}{\[3\]}^2}};$

(2.3) upgrading strapdown attitude matrix T is:

$T=\left[\begin{array}{ccc}Q{\[0\]}^2+Q{\[1\]}^2-Q{\[2\]}^2-Q{\[3\]}^2& 2(Q\[1\]*Q\[2\]-Q\[0\]*Q\[3\])& 2(Q\[1\]*Q\[3\]+Q\[0\]*Q\[2\])\\ 2(Q\[1\]*Q\[2\]+Q\[0\]*Q\[3\])& Q{\[0\]}^2-Q{\[1\]}^2+Q{\[2\]}^2-Q{\[3\]}^2& 2(Q\[2\]*Q\[3\]-Q\[0\]*Q\[1\])\\ 2(Q\[1\]*Q\[3\]-Q\[0\]*Q\[2\])& 2(Q\[2\]*Q\[3\]+Q\[0\]*Q\[1\])& Q{\[0\]}^2-Q{\[1\]}^2-Q{\[2\]}^2+Q{\[3\]}^2\end{array}\right].$

The present invention's advantage is compared with prior art as follows:

Devise course timing scan function, utilize beacon signal assisting sifting to go out satellite-signal point of maximum intensity, and driven antenna azimuth axis aims at satellite-signal maximal value in real time, achieve the function of the strongest satellite-signal of antenna control system real-time follow-up.Devise heading effect error closed loop compensation algorithm, the course error obtained in scanning process is utilized to achieve heading effect error compensation, and Closed-cycle correction inertial navigation system strapdown Quaternion Matrix and strapdown attitude matrix, eliminate the long instrument cumulative errors of inertial navigation system, guarantee that the high precision of attitude of carrier and course angle is resolved, achieve the function of the long-term high-accuracy stable tracking satellite signal of antenna control system.

Accompanying drawing explanation

Fig. 1 is method flow diagram of the present invention;

Embodiment

Communication in moving (SOTM, Satcom On The Move) be the abbreviation of " mobile in satellite ground station communication system ", it utilizes Geo-synchronous stationary satellite as the transfer platform of signal of communication, realizes the point-to-point in its overlay area, point-to-multipoint, how point-to-multipoint real-time Communication for Power.Principal feature is: satellite coverage area is large, and do not limit by the factor such as region, distance, dedicated transmission channel, transport tape is roomy, and transfer rate is high; Long-distance video image, sound accompaniment, phone and data transmission can be realized.Described communication in moving comprises antenna, antenna control system, inertial navigation system.

As shown in Figure 1, the invention provides a kind of inertial navigation system heading effect error closed loop compensation method being applied to communication in moving, it is characterized in that step is as follows:

(1), after described communication in moving completes satellite aligning, arrange and control navigation period counter M, course timer SCANFLAG and compensate mark MODIFLAG, initial value is 0;

(2) control navigation period counter M and course timer SCANFLAG and start timing, when period counter M timing of navigating is to 5ms, enter step (3);

(3) navigation calculation is carried out to the data that the inertial navigation system in communication in moving provides, obtain the attitude angle of carrier, and according to the positional information of the attitude angle of carrier, the positional information of carrier and satellite, calculate the polarizing angle of antenna, the angle of pitch and position angle, complete the gesture drive of antenna, enter step (4) afterwards; Described carrier refers to the transportation equipment of carrying communication in moving system; Circular is as follows:

First resolve according to the air navigation aid in Chen Zhe " strap-down inertial principle " the 6th chapter and obtain carrier angle of pitch P, roll angle R and course angle Y';

Then the projection fX that satellite position information is fastened at geographic coordinate is calculated _t, fY _t, fZ _t:

Wherein, R _efor earth radius, h ₀for carrier present level, λ _sat, λ ₀, be respectively satellite longitude, carrier longitude and carrier latitude;

The projection fX of satellite in carrier coordinate system is calculated again according to attitude of carrier and course angle _c, fY _c, fZ _c:

$\left[\begin{array}{c}{\mathrm{fX}}_c\\ {\mathrm{fY}}_c\\ {\mathrm{fZ}}_c\end{array}\right]=\left[\begin{array}{ccc}\cos \left(R\right)\cos \left(Y^{\′}\right)-\sin \left(P\right)\sin \left(R\right)\sin \left(Y^{\′}\right)& -\cos \left(P\right)\sin \left(Y^{\′}\right)& \sin \left(R\right)\cos \left(Y^{\′}\right)+\cos \left(R\right)\sin \left(P\right)\sin \left(Y^{\′}\right)\\ \cos \left(R\right)\sin \left(Y^{\′}\right)+\sin \left(P\right)\sin \left(R\right)\cos \left(Y^{\′}\right)& \cos \left(P\right)\cos \left(Y^{\′}\right)& \sin \left(R\right)\sin \left(Y^{\′}\right)-\cos \left(R\right)\sin \left(R\right)\cos \left(Y^{\′}\right)\\ -\cos \left(P\right)\sin \left(R\right)& \sin \left(P\right)& \cos \left(R\right)\cos \left(P\right)\end{array}\right]\left[\begin{array}{c}{\mathrm{fX}}_t\\ {\mathrm{fY}}_t\\ {\mathrm{fZ}}_t\end{array}\right]$ ; Finally calculate antenna elevation angle antenna azimuth $fYaw=arctan\left(\frac{{\mathrm{fX}}_c}{{\mathrm{fY}}_c}\right),$ Antenna polarization angle

(4) judge whether course timer SCANFLAG reaches default timing, if reached, then azimuth axis of antenna carries out step-scan according to default step-length, when scanning, all reads semaphore value at every turn, determines the course error Δ Y that maximum semaphore value place is corresponding; Concrete realization in accordance with the following steps:

A () arranges scan counter SCnt and step-length total angle Sum, initial value is zero;

B () increases azimuth axis of antenna angle fYaw=fYaw+SAngle according to presetting step-length SAngle stepping, and material calculation total angle Sum=Sum+SAngle, scan counter SCnt=SCnt+1, read beacon real-time voltage value fXB simultaneously; Judge whether beacon real-time voltage value fXB is greater than predetermined maximum voltage value XBMAX, i.e. fXB >=XBMAX, if satisfy condition, then maximum voltage value XBMAX=fXB, and record course error Δ Y=Sum corresponding to beacon maximum of points, enter step (b) afterwards; Otherwise directly enter step (c);

C () judges whether scan counter SCnt is less than default single side scan total amount ASINGLE, i.e. Sum<ASINGLE, returns step (b) if be less than; Otherwise judge whether Sum=ASINGLE, if satisfy condition, then default step-length SAngle=-SAngle is set, make azimuth axis of antenna carry out opposite direction scanning, return step (b); Otherwise judge whether that Sum<2 × ASINGLE judges Sum>ASINGLE simultaneously, if satisfy condition, return step (b); Otherwise judge Sum=2 × ASINGLE, if satisfy condition, then preset step-length SAngle=SAngle, make azimuth axis of antenna scanning in the other direction again, and return step (b); Otherwise judge that Sum>2 × ASINGLE judges Sum<3 × ASINGLE simultaneously, if satisfy condition, return step (b); Otherwise directly enter step (d);

D () completes azimuth axis of antenna step-scan, and determine course error Δ Y corresponding to maximum semaphore value place; (the AGC magnitude of voltage that described semaphore value obtains after referring to and utilizing satellite beacon receiver that the band satellite signals such as C, Ka and Ku are carried out signal transacting and demodulation.)

Turn in antenna bearingt angle corresponding to maximum semaphore value place according to described course error Δ Y driven antenna azimuth axis afterwards, arrange simultaneously and compensate mark MODIFLAG=1, enter step (5) afterwards; If course timer SCANFLAG does not reach default timing, then directly enter step (5);

(5) whether effectively judge to compensate mark MODIFLAG, represent effective during MODIFLAG=1, during MODIFLAG=0, represent invalid, if it is invalid to compensate mark MODIFLAG, return step (2); If it is effective to compensate mark MODIFLAG, then course error Δ Y corresponding to the maximum semaphore value place determined according to step (4) compensates current inertial navigation system course angle Y', the inertial navigation system course angle after compensation

Y _compensate=Y'+ Δ Y, according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ' and strapdown attitude matrix T, realize the Closed-cycle correction of drift error, return the drift error Closed-cycle correction that step (2) carries out the next navigation cycle afterwards.

Described according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ' and strapdown attitude matrix T to be specially:

(2.1) according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ', be specially:

Wherein, P is the present carrier angle of pitch, and R is present carrier roll angle;

(2.2) be normalized by the strapdown Quaternion Matrix Q ' after renewal, obtain the strapdown Quaternion Matrix Q after normalization, formula is as follows:

$Q=\frac{Q^{\&prime;}}{\sqrt{Q^{\&prime;}{\&lsqb;0\&rsqb;}^2+Q^{\&prime;}{\&lsqb;1\&rsqb;}^2+Q^{\&prime;}{\&lsqb;2\&rsqb;}^2+Q^{\&prime;}{\&lsqb;3\&rsqb;}^2}};$

(2.3) upgrading strapdown attitude matrix T is:

$T=\left[\begin{array}{ccc}Q{\&lsqb;0\&rsqb;}^2+Q{\&lsqb;1\&rsqb;}^2-Q{\&lsqb;2\&rsqb;}^2-Q{\&lsqb;3\&rsqb;}^2& 2(Q\&lsqb;1\&rsqb;*Q\&lsqb;2\&rsqb;-Q\&lsqb;0\&rsqb;*Q\&lsqb;3\&rsqb;)& 2(Q\&lsqb;1\&rsqb;*Q\&lsqb;3\&rsqb;+Q\&lsqb;0\&rsqb;*Q\&lsqb;2\&rsqb;)\\ 2(Q\&lsqb;1\&rsqb;*Q\&lsqb;2\&rsqb;+Q\&lsqb;0\&rsqb;*Q\&lsqb;3\&rsqb;)& Q{\&lsqb;0\&rsqb;}^2-Q{\&lsqb;1\&rsqb;}^2+Q{\&lsqb;2\&rsqb;}^2-Q{\&lsqb;3\&rsqb;}^2& 2(Q\&lsqb;2\&rsqb;*Q\&lsqb;3\&rsqb;-Q\&lsqb;0\&rsqb;*Q\&lsqb;1\&rsqb;)\\ 2(Q\&lsqb;1\&rsqb;*Q\&lsqb;3\&rsqb;-Q\&lsqb;0\&rsqb;*Q\&lsqb;2\&rsqb;)& 2(Q\&lsqb;2\&rsqb;*Q\&lsqb;3\&rsqb;+Q\&lsqb;0\&rsqb;*Q\&lsqb;1\&rsqb;)& Q{\&lsqb;0\&rsqb;}^2-Q{\&lsqb;1\&rsqb;}^2-Q{\&lsqb;2\&rsqb;}^2+Q{\&lsqb;3\&rsqb;}^2\end{array}\right].$

The present invention devises course timing scan function, utilizes beacon as auxiliary signal, and timing scans azimuth axis of antenna misalignment satellite error, guarantees that azimuth axis of antenna can aim at satellite-signal maximal value in real time; Determine inertial navigation system heading effect error simultaneously, and devise heading effect closed loop compensation algorithm, upgraded by navigation attitude matrix and achieve the long-time instrument cumulative errors compensation of inertial navigation system, the high precision that improve attitude of carrier and course angle is resolved, and achieves the function of the long-term high-accuracy stable tracking satellite signal of antenna control system.

Embodiment

1. arrange and control navigation period counter M, course timer SCANFLAG and compensate mark MODIFLAG, initial value is 0, predetermined maximum voltage value XBMAX=4.3V, step-length SAngle=0.01 °, single side scan total amount ASINGLE=200;

2. control navigation period counter M and course timer SCANFLAG and start timing, when period counter M timing of navigating is to 5ms, enter step (3);

3. inertial navigation system calculates the current angle of pitch P=0.5 of carrier °, roll angle R=0.5 °, course angle Y=120.0 °, then resolve aft antenna angle of pitch fPitch=49.057437 °, polarizing angle fJihua=16.596453 ° and antenna azimuth fYaw=9.373444 °;

4. judge course timer SCANFLAG >=2min, if meet, then azimuth axis of antenna angle fYaw=fYaw+SAngle=9.373444 °+0.01 °=9.38344 °, and drive azimuth axis to rotate, now Sum=0.01 °, SCnt=1, the beacon real-time voltage value fXB=4.2V simultaneously read; Judge fXB >=XBMAX, do not satisfy condition, continue scanning; To complete about azimuth axis of antenna after 2 ° of scannings according to scan method, beacon voltage max when obtaining SCnt=300, now corresponding step-length total angle Sum=1 °, course error Δ Y=1 ° is set, and driven antenna azimuth axis turns to corresponding maximum semaphore value place, most post-equalization mark MODIFLAG=1, enters step 5;

5. indicate MODIFLAG=1, then compensate inertial navigation system course angle, the course angle Y after compensation _compensate=121 °, and according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ' and strapdown attitude matrix T, and strapdown Quaternion Matrix Q ' and strapdown attitude matrix T is brought in the attitude algorithm in next navigation cycle.

Claims (3)

1. be applied to an inertial navigation system heading effect error closed loop compensation method for communication in moving, it is characterized in that step is as follows:

(1), after described communication in moving completes satellite aligning, arrange and control navigation period counter M, course timer SCANFLAG and compensate mark MODIFLAG, initial value is 0;

(2) control navigation period counter M and course timer SCANFLAG and start timing, when period counter M timing of navigating is to 5ms, enter step (3);

(3) navigation calculation is carried out to the data that the inertial navigation system in communication in moving provides, obtain the attitude angle of carrier, and according to the positional information of the attitude angle of carrier, the positional information of carrier and satellite, calculate the polarizing angle of antenna, the angle of pitch and position angle, complete the gesture drive of antenna, enter step (4) afterwards; Described carrier refers to the transportation equipment of carrying communication in moving system;

(4) judge whether course timer SCANFLAG reaches default timing, if reached, then azimuth axis of antenna carries out step-scan according to default step-length, when scanning, all reads semaphore value at every turn, determines the course error Δ Y that maximum semaphore value place is corresponding; Turn in antenna bearingt angle corresponding to maximum semaphore value place according to described course error Δ Y driven antenna azimuth axis afterwards, arrange simultaneously and compensate mark MODIFLAG=1, enter step (5) afterwards; If course timer SCANFLAG does not reach default timing, then directly enter step (5);

(5) whether effectively judge to compensate mark MODIFLAG, represent effective during MODIFLAG=1, during MODIFLAG=0, represent invalid, if it is invalid to compensate mark MODIFLAG, return step (2); If it is effective to compensate mark MODIFLAG, then course error Δ Y corresponding to the maximum semaphore value place determined according to step (4) compensates current inertial navigation system course angle Y', the inertial navigation system course angle after compensation

Y _compensate=Y'+ Δ Y, according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ' and strapdown attitude matrix T, realize the Closed-cycle correction of drift error, return the drift error Closed-cycle correction that step (2) carries out the next navigation cycle afterwards.

2. a kind of inertial navigation system heading effect error closed loop compensation method being applied to communication in moving according to claim 1, is characterized in that: described according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ' and strapdown attitude matrix T to be specially:

(2.1) according to Y _compensateupgrade inertial navigation system strapdown Quaternion Matrix Q ', be specially:

wherein, P is the present carrier angle of pitch, and R is present carrier roll angle;

(2.2) be normalized by the strapdown Quaternion Matrix Q ' after renewal, obtain the strapdown Quaternion Matrix Q after normalization, formula is as follows:

$Q=\frac{Q^{\&prime;}}{\sqrt{Q^{\&prime;}{\&lsqb;0\&rsqb;}^2+Q^{\&prime;}{\&lsqb;1\&rsqb;}^2+Q^{\&prime;}{\&lsqb;2\&rsqb;}^2+Q^{\&prime;}{\&lsqb;3\&rsqb;}^2}};$

(2.3) upgrading strapdown attitude matrix T is:

$T=\left[\begin{array}{ccc}Q{\&lsqb;0\&rsqb;}^2+Q{\&lsqb;1\&rsqb;}^2-Q{\&lsqb;2\&rsqb;}^2-Q{\&lsqb;3\&rsqb;}^2& 2\left(Q\&lsqb;1\&rsqb;*Q\&lsqb;2\&rsqb;-Q\&lsqb;0\&rsqb;*Q\&lsqb;3\&rsqb;\right)& 2\left(Q\&lsqb;1\&rsqb;*Q\&lsqb;3\&rsqb;+Q\&lsqb;0\&rsqb;*Q\&lsqb;2\&rsqb;\right)\\ 2\left(Q\&lsqb;1\&rsqb;*Q\&lsqb;2\&rsqb;+Q\&lsqb;0\&rsqb;*Q\&lsqb;3\&rsqb;\right)& Q{\&lsqb;0\&rsqb;}^2-Q{\&lsqb;1\&rsqb;}^2+Q{\&lsqb;2\&rsqb;}^2-Q{\&lsqb;3\&rsqb;}^2& 2\left(Q\&lsqb;2\&rsqb;*Q\&lsqb;3\&rsqb;-Q\&lsqb;0\&rsqb;*Q\&lsqb;1\&rsqb;\right)\\ 2(Q\&lsqb;1\&rsqb;*Q\&lsqb;3\&rsqb;-Q\&lsqb;0\&rsqb;*Q\&lsqb;2\&rsqb;& 2\left(Q\&lsqb;2\&rsqb;*Q\&lsqb;3\&rsqb;+Q\&lsqb;0\&rsqb;*Q\&lsqb;1\&rsqb;\right)& Q{\&lsqb;0\&rsqb;}^2-Q{\&lsqb;1\&rsqb;}^2-Q{\&lsqb;2\&rsqb;}^2+Q{\&lsqb;3\&rsqb;}^2\end{array}\right].$

3. a kind of inertial navigation system heading effect error closed loop compensation method being applied to communication in moving according to claim 1, is characterized in that: the AGC magnitude of voltage that described semaphore value obtains after referring to and utilizing satellite beacon receiver that C, Ka and Ku band satellite signal is carried out signal transacting and demodulation.