CN102902282A - Optic axis and inertia axis superposition-based geographic tracking method - Google Patents

Abstract

The invention discloses an optic axis and inertia axis superposition-based geographic tracking method which comprises the steps of computing the space pose of a carrier aiming line in real time by using a space two-point geometric algorithm according to the positioning data and the self position information of a target, computing the space angle of the aiming line relative to a carrier coordinate system by being combined with the carrier space pose and the space coordinate transformation principle, sending the angle to a servo control unit, and adjusting corresponding space pose of a turntable by the servo control unit so as to complete the geographic tracking to the target. The method is different from the image tracking method in terms of theory, therefore, the method is not influenced by the cloud, the mist and the obstacles; and when being used on aircrafts, the method is capable of avoiding the problem of losing the target due to strenuous vibration or sight shielding, so that the scouting and searching capabilities of the battlefield weapon systems are remarkably improved. The method can also be used for guide devices of various guided missiles, so as to improve the accurate attacking capability to ground targets.

Description

The geographical tracking that overlaps with the axes of inertia based on optical axis

Technical field

The invention belongs to photoelectronic reconnaissance and target tracking domain, relate generally to a kind of geographical tracking to target, relate in particular to a kind of geographical tracking that overlaps with the axes of inertia based on optical axis.

Background technology

The optronic tracker of Modern weapon system has the function that target reconnaissance and search are followed the tracks of.Present automatic tracking function is divided into laser, infrared, three kinds of tracking modes of TV according to the form of target detector.The ultimate principle of these three kinds of tracking modes is: after target detector captures target, change the target scene into electronic image by CCD (charge-coupled image sensor), by signal processing unit image is missed the target again and measure rear automatically output miss distance, then getting poor device becomes electric signal with miss distance and sends into servo control unit as the angular displacement amount, adjusted at last the boresight angle of optronic tracker by servo control unit, thereby reduce tracking error, realize the lasting tracking to target.Must clear target acquisition scene but the prerequisite of using these trackings is detector, if target is subject to cloud and mist, barrier blocks, or background environment and object spectral characteristic poor contrast, will be so that the target scene effect that obtains with detector be undesirable, cause optronic tracker to target tracking accuracy poor even lose objects, reduce armament systems and scout and search with ability.Occurred again afterwards utilizing geography information to realize the method for following the tracks of.

" airborne computer " periodical has been published the paper that exercise question is " O-E Payload for UAV geographical follow the tracks of control research " in March, 2012.This paper is a kind of can be in geographical tracking and the system that the photoelectronic reconnaissance O-E Payload for UAV uses.Propose in the literary composition when having the concussion of manipulation or cloud layer to block, adopt image tracking technique meeting lose objects, so introduce geographical tracking as the supplementary means of image tracking method, can solve the problem to the target fast Acquisition.The method is at first according to the angle rotation relationship between carrier aircraft spatial attitude calculating carrier aircraft coordinate system and the geographic coordinate system, subsequently the target under the geographic coordinate system is converted to " pseudo-target " under the carrier aircraft coordinate system, then under the carrier aircraft coordinate system, calculate the space vector angle of " pseudo-target ", again the space angle value that obtains is inputed to servo control unit, stablize the turntable angle by the servo control unit adjustment, can the fast Acquisition target.The requirement that the optical axis that does not propose the O-E Payload system in the technical scheme that paper provides and the axes of inertia need to overlap, therefore only rely on the space vector angle of calculating target and it is inputed to servo control unit, stablize the turntable angle by the servo control unit adjustment, target is appeared in the visual field of video camera, can not guarantee that target is in the target center position of visual field.So adopt separately the geographical tracking in the paper, can not keep the tracking continual and steady to target.

Summary of the invention

The technical problem to be solved in the present invention is, for the problem that prior art exists, provides a kind of geographical tracking that can use separately under optical axis and prerequisite that the axes of inertia overlap.

Geographical tracking provided by the invention may further comprise the steps:

The first step gathers the impact point data (λ, L, h) of target locating set output and the data (λ of current carrier aircraft location point ₁, L ₁, h ₁, ψ _A, θ _A, γ _A, S), λ wherein, L, h are respectively longitude, latitude and the height of impact point; λ ₁, L ₁, h ₁Respectively current longitude, latitude and the height of carrier aircraft, ψ _A, θ _A, γ _ABe respectively in the ground current course angle, the angle of pitch and the roll angle under the coordinate system of carrier aircraft, S is that current carrier aircraft is to the distance of impact point;

Second step, calculate carrier aircraft and impact point are projected to impact point projection vector line segment by carrier aircraft in the surface level projection north orientation angle σ with following formula:

$\mathrm{\σ}=\arctan \frac{|\mathrm{\λ}-{\mathrm{\λ}}_1|\×R_M\×\cos L_1}{|L-L_1|\×R_N}$

R _M＝R _e(1-2e+3esin ²L ₁)

R _N＝R _e(1+esin ²L ₁)

Wherein, R _MThe radius of curvature of meridian of carrier aircraft current location, R _NBe the radius of curvature in prime vertical of carrier aircraft current location, e is ellipticity, R _eIt is the earth's spheroid major axis radius;

In the 3rd step, calculate the course angle ψ that boresight points to impact point with following formula _b:

As λ＞λ ₁And L＞L ₁ψ _b=σ

As λ＞λ ₁And L＜L ₁ψ _b=180 °-σ

As λ＜λ ₁And L＜L ₁ψ _b=180 °+σ

As λ＜λ ₁And L＞L ₁ψ _b=-σ

As λ=λ ₁And L＞L ₁ψ _b=0 °

As λ=λ ₁And L＜L ₁ψ _b=180 °

As λ＜λ ₁And L=L ₁ψ _b=-90 °

As λ＞λ ₁And L=L ₁ψ _b=90 °

In the 4th step, calculate boresight and horizontal plane angle δ with following formula:

$\mathrm{\δ}=\arcsin \frac{|h-h_1|}{S}$

In the 5th step, calculate the pitching angle theta that boresight points to impact point with following formula _b:

As h＞h ₁θ _b=δ

As h＜h ₁θ _b=-δ

In the 6th step, computed geographical coordinates is to the attitude matrix of carrier aircraft coordinate system

$C_A^n=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \sin {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \cos {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& -\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\sin {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\θ}}_A& \cos {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A\end{array}\right]$

In the 7th step, calculate boresight to the attitude matrix of geographic coordinate system

$C_n^b=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_b& -\sin {\mathrm{\ψ}}_b& 0\\ \sin {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \cos {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \sin {\mathrm{\θ}}_b\\ -\sin {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& -\cos {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& \cos {\mathrm{\θ}}_b\end{array}\right]$

In the 8th step, calculate boresight to the attitude matrix of carrier aircraft coordinate system

$C_A^b=C_n^b*C_A^n=\left[\begin{array}{ccc}{C}_1\left(\mathrm{1,1}\right)& C_1\left(\mathrm{2,1}\right)& C_1\left(\mathrm{3,1}\right)\\ C_1\left(\mathrm{1,2}\right)& C_1\left(\mathrm{2,2}\right)& C_1\left(\mathrm{3,2}\right)\\ C_1\left(\mathrm{1,3}\right)& C_1\left(\mathrm{2,3}\right)& C_1\left(\mathrm{3,3}\right)\end{array}\right]$

In the formula, C ₁(i, j) and (i=1,2,3 j=1,2,3) is matrix

Every element;

The 9th step, angle of pitch β, azimuth angle alpha and the roll angle χ of calculating carrier aircraft optical axis stable turntable:

9.1 with the angle of pitch β of following formula calculating carrier aircraft optical axis stable turntable, with reference to roll angle γ _CWith reference azimuth ψ _C:

β＝arcsinC ₁(3，2)

${\mathrm{\γ}}_C=\arctan (-\frac{C_1\left(\mathrm{3,1}\right)}{C_1\left(\mathrm{3,3}\right)})$

${\mathrm{\ψ}}_C=\arctan \left(\frac{C_1\left(\mathrm{1,2}\right)}{C_1\left(\mathrm{2,2}\right)}\right)$

9.2 calculate carrier aircraft optical axis stable turntable azimuth angle alpha with following discrimination formula:

Work as C ₁(2,2) → 0 and C ₁(1,2)＞0 α=90 °

Work as C ₁(2,2) → 0 and C ₁(1,2)＜0 α=-90 °

Work as C ₁(2,2)＞0 α=ψ _C

Work as C ₁(2,2)＜0 and C ₁(1,2)＞0 α=ψ _C+ 180 °

Work as C ₁(2,2)＜0 and C ₁(1,2)＜0 α=ψ _C-180 °

9.3 calculate carrier aircraft optical axis stable turntable roll angle χ with following discrimination formula:

Work as C ₁(3,3)＞0 χ=γ _C

Work as C ₁(3,3)＜0 and γ _C＞0 χ=γ _C-180 °

Work as C ₁(3,3)＜0 and γ _C＜0 χ=γ _C+ 180 °

In the tenth step, azimuth angle alpha, angle of pitch β, the roll angle χ that calculates the carrier aircraft optical axis stable turntable that obtains is transferred to servo control unit;

The 11 step repeated ten steps of the first step to the, until superior system finishes after sending geographical tracking END instruction.

Overall technology effect of the present invention is presented as following four aspects.

(1) the present invention utilizes the geography information of carrier and target and in conjunction with attitude of carrier, according to the space geometry algorithm, calculate in real time the carrier boresight with respect to the space angle of carrier coordinate system, then give servo control unit with this angular metric, finish the geography of target is followed the tracks of.Therefore desirable in the target scene, or background environment and object spectral characteristic poor contrast, even under target such as is blocked at the situation, the tracking that the inventive method can both be continual and steady to target.Thereby the photoelectricity investigation that significantly improves system is searched with ability with target, further brings into play inertial navigation and surely takes aim at the information fusion of control, the effect of cooperation.

(2) we are bright has the advantages that optical axis overlaps with the axes of inertia, so the spatial attitude angle of boresight and the spatial attitude angle of stable turntable are in full accord, be exactly that the spatial attitude of boresight is controlled to the spatial attitude control essence of turntable so, thereby by guaranteeing that the accurate control of stablizing turntable has been realized the accurate aiming of boresight to target.

(3) the present invention only uses the space geometry algorithm, again through a matrix computations, just can finish calculating to the space instruction angle of tracking target boresight.So simple operating steps of the present invention, the data calculated amount is little, requires lower to system hardware resources.Can be in the system of multiple weapon platform graft application, have versatility.

(4) the present invention takes full advantage of geography information and inertia information is calculated in real time, not only be used for photoelectronic reconnaissance and Target Tracking System, and need to be applicable to geography information to obtain the attack weapon system of trace command, for example provide a kind of target guiding method for seeker.

Description of drawings

Fig. 1 is the operational flowchart of the geographical tracking of the present invention.

Fig. 2 is the schematic diagram that concerns between carrier aircraft, optical axis stable turntable, the geographic coordinate system.

Fig. 3 is that geographical trace model space geometry concerns schematic diagram.

Embodiment

The present invention is described in further detail below in conjunction with accompanying drawing and preferred embodiment.

The preferred embodiment of the geographical tracking of the present invention is used for the UAV system photodetection/tracker to certain fixed target tracking.This system comprises optical axis stable turntable, inertial navigation unit, servo control unit, information process unit.Wherein, the optical axis on the optical axis stable turntable overlaps with the axes of inertia of inertial navigation unit, and information process unit comprises the computing machine that geographical tracking module is housed.This method is computer implemented by in the information process unit.Parameter ellipticity e and earth's spheroid major axis radius R have been deposited in advance in the hard disk of computing machine _eIn the unmanned plane during flying process, if airborne photoelectric detection/tracker locks a certain target, and photodetection/tracker is when receiving superior system and sending geographical trace command, and geographical tracking module will be finished following operating process according to flow process shown in Figure 1.

The first step gathers the impact point data (λ, L, h) of target locating set output and the data (λ of current carrier aircraft location point ₁, L ₁, h ₁, ψ _A, θ _A, γ _A, S).λ wherein, L, h are respectively longitude, latitude and the height of impact point; λ ₁, L ₁, h ₁Respectively current longitude, latitude and the height of carrier aircraft, ψ _A, θ _A, γ _ABe respectively current course angle, the angle of pitch and the roll angle of carrier aircraft under geographic coordinate system, S is that current carrier aircraft is to the distance of impact point.

According to shown in Figure 2, geographic coordinate system n is defined as: initial point O is the earth point place at carrier aircraft place, and X-axis is pointed to the due east direction from initial point O, and Y-axis is pointed to direct north from initial point O, and Z axis points to zenith perpendicular to ground, and X-axis, Y-axis and Z axis consist of right-handed coordinate system.Carrier aircraft coordinate system A is defined as: initial point O is the aircraft barycenter, and X-axis is pointed to right along the carrier aircraft transverse axis, and Y-axis is pointed to head along the carrier aircraft longitudinal axis, and Z axis is vertically being carried on the back by ventral sensing machine in the symmetrical plane, and X-axis, Y-axis and Z axis consist of right-handed coordinate system.

Carrier aircraft course angle ψ _ABe defined as: the angle that carrier aircraft is rotated around Z axis under geographic coordinate system, regulation is just rotating to be to the right; The carrier aircraft pitching angle theta _ABe defined as: the angle that carrier aircraft is rotated around X-axis under geographic coordinate system, regulation rotates up as just; Carrier aircraft roll angle γ _ABe defined as: the angle that carrier aircraft is rotated around Y-axis under geographic coordinate system, regulation is rotated counterclockwise as just.

Current goal point data are in this preferred embodiment: (λ, L, h)=(108.8929,34.1504,5.2)

Current carrier aircraft location point data are:

(λ ₁，L ₁，h ₁，ψ _A，θ _A，γ _A，S)＝(108.8552，34.13，1235.3，3.49221°，1.03811°，-0.0002°，4320.5)

Second step is according to longitude λ, the λ of impact point and carrier aircraft location point ₁With latitude L, L ₁, calculate carrier aircraft and impact point are projected to impact point projection vector line segment by carrier aircraft in the surface level projection north orientation angle σ with following formula.

$\mathrm{\σ}=\arctan \frac{|\mathrm{\λ}-{\mathrm{\λ}}_1|\×R_M\×\cos L_1}{|L-L_1|\×R_N}$

R _M＝R _e(1-2e+3esin ²L ₁)

R _N＝R _e(1+esin ²L ₁)

$e=\frac{1}{298.257}$

R _e＝6378137

Wherein, R _MThe radius of curvature of meridian of carrier aircraft current location, R _NBe the radius of curvature in prime vertical of carrier aircraft current location, e is the ellipticity of the earth, R _eIt is the major axis radius of the earth.

Fig. 3 is carrier aircraft and Target space position relation and space geometry calculating schematic diagram in this preferred embodiment.This figure is based upon under the geographic coordinate system, F represents current carrier aircraft location point, T represents impact point, M and N are respectively F point and the projection of T point on surface level, NK is illustrated in line segment parallel with Y-axis under the geographic coordinate system, so some M, N and K consist of the right-angle triangle Δ MNK on the surface level, JT is the line segment parallel with MN.North orientation angle σ is the angle of line segment MN and line segment NK, i.e. ∠ MNK in the drawings.

The 3rd step, according to the result of calculation of second step, and the longitude λ of the longitude λ of combining target point and carrier aircraft location point ₁, calculate the course angle ψ that boresight points to impact point with following discrimination formula _b

As λ＞λ ₁And L＞L ₁ψ _b=σ

As λ＞λ ₁And L＜L ₁ψ _b=180 °-σ

As λ＜λ ₁And L＜L ₁ψ _b=180 °+σ

As λ＜λ ₁And L＞L ₁ψ _b=-σ

As λ=λ ₁And L＞L ₁ψ _b=0 °

As λ=λ ₁And L＜L ₁ψ _b=180 °

As λ＜λ ₁And L=L ₁ψ _b=-90 °

As λ＞λ ₁And L=L ₁ψ _b=90 °

Boresight points to the position angle ψ of impact point _bBe defined as: the angle that a little the boresight of aiming at the mark rotates around Z axis under geographic coordinate system, regulation just rotates to be to the right.

In this preferred embodiment, calculated the course angle ψ of boresight sensing impact point by the 3rd step _bBe:

ψ _b＝56.70638°

The 4th step is according to height h, the h of impact point and carrier aircraft location point ₁And carrier aircraft between the impact point apart from S, with following formula calculating boresight and horizontal plane angle δ:

$\mathrm{\δ}=\arcsin \frac{|h-h_1|}{S}$

In Fig. 3, line segment FM represents carrier aircraft location point height h, and line segment TN represents impact point height h ₁, line segment FT be carrier aircraft between the impact point apart from S.Boresight and horizontal plane angle δ are the angle of line segment FT and line segment JT, i.e. ∠ FTJ in the drawings.

The 5th step, according to the four-step calculation result, and height h, the h of combining target point and carrier aircraft location point ₁, calculate the pitching angle theta that boresight points to impact point with following discrimination formula _b:

As h＞h ₁θ _b=δ

As h＜h ₁θ _b=-δ

Boresight points to the pitching angle theta of impact point _bBe defined as: the angle that a little the boresight of aiming at the mark under geographic coordinate system rotates around X-axis, regulation rotate up as just.

In this preferred embodiment, calculated the pitching angle theta of boresight sensing impact point by the 5th step _bBe:

θ _b＝-16.54163°

The 6th step is according to the course angle ψ of carrier aircraft location point _A, pitching angle theta _AWith roll angle γ _A, with the attitude matrix of following formula computed geographical coordinates to the carrier aircraft coordinate system

$C_A^n=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \sin {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \cos {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& -\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\sin {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\θ}}_A& \cos {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A\end{array}\right]$

In the 7th step, according to the result of calculation in the 3rd step and the 5th step, calculate boresight to the attitude matrix of geographic coordinate system with following formula

$C_n^b=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_b& -\sin {\mathrm{\ψ}}_b& 0\\ \sin {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \cos {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \sin {\mathrm{\θ}}_b\\ -\sin {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& -\cos {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& \cos {\mathrm{\θ}}_b\end{array}\right]$

In the 8th step, according to the result of calculation in the 6th step and the 7th step, calculate boresight to the attitude matrix of carrier aircraft coordinate system with following formula

$C_A^b=C_n^b*C_A^n=\left[\begin{array}{ccc}{C}_1\left(\mathrm{1,1}\right)& C_1\left(\mathrm{2,1}\right)& C_1\left(\mathrm{3,1}\right)\\ C_1\left(\mathrm{1,2}\right)& C_1\left(\mathrm{2,2}\right)& C_1\left(\mathrm{3,2}\right)\\ C_1\left(\mathrm{1,3}\right)& C_1\left(\mathrm{2,3}\right)& C_1\left(\mathrm{3,3}\right)\end{array}\right]$

In the formula, C ₁(i, j) and (i=1,2,3 j=1,2,3) is matrix

Every element.

The 9th step, angle of pitch β, azimuth angle alpha and the roll angle χ of calculating carrier aircraft optical axis stable turntable.

9.1 according to the 8th step calculating attitude matrix

The C that obtains ₁(3,2), C ₁(3,1), C ₁(3,3), C ₁(1,2) and C ₁(2,2) are with the angle of pitch β of following formula calculating carrier aircraft optical axis stable turntable, with reference to roll angle γ _CWith reference azimuth ψ _C

β＝arcsinC ₁(3，2)

${\mathrm{\γ}}_C=\arctan (-\frac{C_1\left(\mathrm{3,1}\right)}{C_1\left(\mathrm{3,3}\right)})$

${\mathrm{\ψ}}_C=\arctan \left(\frac{C_1\left(\mathrm{1,2}\right)}{C_1\left(\mathrm{2,2}\right)}\right)$

The optical axis stable turntable coordinate system b of photodetection/tracker is defined as: initial point O is optical axis stable rotation of rotary table center, X-axis is its main horizontal line, namely rotate pitch axis, Y-axis is pointed to run-home along the boresight of optical axis stable turntable, namely rotate the roll axle, Z axis is perpendicular to the XY plane and point to zenith, i.e. rotational orientation axle, and X-axis, Y-axis and Z axis consist of right-handed coordinate system.

Carrier aircraft optical axis stable turntable angle of pitch β is defined as: the angle that the optical axis stable turntable rotates around X-axis under the carrier aircraft coordinate system, regulation rotates up as just.

In this preferred embodiment, the data that calculated by the 8th step are:

C ₁(3，2)＝-0.2950682

C ₁(3，1)＝0.01450774

C ₁(3，3)＝0.955366

C ₁(1，2)＝0.7677324

C ₁(2，2)＝0.5687895

Calculate accordingly:

β＝-17.16163°

γ _C＝-0.8699°

ψ _C＝53.46638°

9.2 go on foot the reference azimuth ψ that calculates according to the 9.1st _C, and in conjunction with attitude matrix

In C ₁(1,2) and C ₁(2,2) two elements, calculate carrier aircraft optical axis stable turntable azimuth angle alpha with following discrimination formula:

Work as C ₁(2,2) → 0 and C ₁(1,2)＞0 α=90 °

Work as C ₁(2,2) → 0 and C ₁(1,2)＜0 α=-90 °

Work as C ₁(2,2)＞0 α=ψ _C

Work as C ₁(2,2)＜0 and C ₁(1,2)＞0 α=ψ _C+ 180 °

Work as C ₁(2,2)＜0 and C ₁(1,2)＜0 α=ψ _C-180 °

Carrier aircraft optical axis stable turntable azimuth angle alpha is defined as: the angle that the optical axis stable turntable rotates around Z axis under the carrier aircraft coordinate system, and regulation is just rotating to be to the right;

In this preferred embodiment, α=53.46638 °

9.3 go on foot the reference roll angle γ that calculates according to the 9.1st _C, and in conjunction with attitude matrix

In C ₁(3,3), calculate carrier aircraft optical axis stable turntable roll angle χ with following discrimination formula:

Work as C ₁(3,3)＞0 χ=γ _C

Work as C ₁(3,3)＜0 and γ _C＞0 χ=γ _C-180 °

Work as C ₁(3,3)＜0 and γ _C＜0 χ=γ _C+ 180 °

Carrier aircraft optical axis stable turntable roll angle χ is defined as: the angle that the optical axis stable turntable rotates around Y-axis under the carrier aircraft coordinate system, regulation is rotated counterclockwise as just.

In this preferred embodiment, χ=-0.8699 °.

In the tenth step, carrier aircraft optical axis stable turntable azimuth angle alpha, angle of pitch β, the roll angle χ that obtains is transferred to servo control unit and carries out the angle position servo control.

The 11 step repeated ten steps of the first step to the, until superior system finishes after sending geographical tracking END instruction.

Claims (1)

1. geographical tracking that overlaps with the axes of inertia based on optical axis, it is characterized in that: the method comprises following operation steps:

The first step gathers the impact point data (λ, L, h) of target locating set output and the data (λ of current carrier aircraft location point ₁, L ₁, h ₁, ψ _A, θ _A, γ _A, S), λ wherein, L, h are respectively longitude, latitude and the height of impact point; λ ₁, L ₁, h ₁Respectively current longitude, latitude and the height of carrier aircraft, ψ _A, θ _A, γ _ABe respectively in the ground current course angle, the angle of pitch and the roll angle under the coordinate system of carrier aircraft, S is that current carrier aircraft is to the distance of impact point;

Second step, calculate carrier aircraft and impact point are projected to impact point projection vector line segment by carrier aircraft in the surface level projection north orientation angle σ with following formula:

$\mathrm{\σ}=\arctan \frac{|\mathrm{\λ}-{\mathrm{\λ}}_1|\×R_M\×\cos L_1}{|L-L_1|\×R_N}$

R _M＝R _e(1-2e+3esin ²L ₁)

R _N＝R _e(1+esin ²L ₁)

Wherein, R _MThe radius of curvature of meridian of carrier aircraft current location, R _NBe the radius of curvature in prime vertical of carrier aircraft current location, e is ellipticity, R _eIt is the earth's spheroid major axis radius;

In the 3rd step, calculate the course angle ψ that boresight points to impact point with following formula _b:

As λ＞λ ₁And L＞L ₁ψ _b=σ

As λ＞λ ₁And L＜L ₁ψ _b=180 °-σ

As λ＜λ ₁And L＜L ₁ψ _b=180 °+σ

As λ＜λ ₁And L＞L ₁ψ _b=-σ

As λ=λ ₁And L＞L ₁ψ _b=0 °

As λ=λ ₁And L＜L ₁ψ _b=180 °

As λ＜λ ₁And L=L ₁ψ _b=-90 °

As λ＞λ ₁And L=L ₁ψ _b=90 °

In the 4th step, calculate boresight and horizontal plane angle δ with following formula:

$\mathrm{\δ}=\arcsin \frac{|h-h_1|}{S}$

In the 5th step, calculate the pitching angle theta that boresight points to impact point with following formula _b:

As h＞h ₁θ _b=δ

As h＜h ₁θ _b=-δ

In the 6th step, computed geographical coordinates is to the attitude matrix of carrier aircraft coordinate system

$C_A^n=\left[\begin{array}{ccc}\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \sin {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\cos {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A+\sin {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A& \cos {\mathrm{\ψ}}_A\cos {\mathrm{\θ}}_A& -\sin {\mathrm{\γ}}_A\sin {\mathrm{\ψ}}_A-\cos {\mathrm{\γ}}_A\cos {\mathrm{\ψ}}_A\sin {\mathrm{\θ}}_A\\ -\sin {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A& \sin {\mathrm{\θ}}_A& \cos {\mathrm{\γ}}_A\cos {\mathrm{\θ}}_A\end{array}\right]$

In the 7th step, calculate boresight to the attitude matrix of geographic coordinate system

$C_n^b=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_b& -\sin {\mathrm{\ψ}}_b& 0\\ \sin {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \cos {\mathrm{\ψ}}_b\cos {\mathrm{\θ}}_b& \sin {\mathrm{\θ}}_b\\ -\sin {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& -\cos {\mathrm{\ψ}}_b\sin {\mathrm{\θ}}_b& \cos {\mathrm{\θ}}_b\end{array}\right]$

In the 8th step, calculate boresight to the attitude matrix of carrier aircraft coordinate system

$C_A^b=C_n^b*C_A^n=\left[\begin{array}{ccc}{C}_1\left(\mathrm{1,1}\right)& C_1\left(\mathrm{2,1}\right)& C_1\left(\mathrm{3,1}\right)\\ C_1\left(\mathrm{1,2}\right)& C_1\left(\mathrm{2,2}\right)& C_1\left(\mathrm{3,2}\right)\\ C_1\left(\mathrm{1,3}\right)& C_1\left(\mathrm{2,3}\right)& C_1\left(\mathrm{3,3}\right)\end{array}\right]$

In the formula, C ₁(i, j) and (i=1,2,3 j=1,2,3) is matrix Every element;

The 9th step, angle of pitch β, azimuth angle alpha and the roll angle χ of calculating carrier aircraft optical axis stable turntable:

9.1 with the angle of pitch β of following formula calculating carrier aircraft optical axis stable turntable, with reference to roll angle γ _CWith reference azimuth ψ _C:

β＝arcsin?C ₁(3，2)

${\mathrm{\γ}}_C=\arctan (-\frac{C_1\left(\mathrm{3,1}\right)}{C_1\left(\mathrm{3,3}\right)})$

${\mathrm{\ψ}}_C=\arctan \left(\frac{C_1\left(\mathrm{1,2}\right)}{C_1\left(\mathrm{2,2}\right)}\right)$

9.2 calculate carrier aircraft optical axis stable turntable azimuth angle alpha with following discrimination formula:

Work as C ₁(2,2) → 0 and C ₁(1,2)＞0 α=90 °

Work as C ₁(2,2) → 0 and C ₁(1,2)＜0 α=-90 °

Work as C ₁(2,2)＞0 α=ψ _C

Work as C ₁(2,2)＜0 and C ₁(1,2)＞0 α=ψ _C+ 180 °

Work as C ₁(2,2)＜0 and C ₁(1,2)＜0 α=ψ _C-180 °

9.3 calculate carrier aircraft optical axis stable turntable roll angle χ with following discrimination formula:

Work as C ₁(3,3)＞0 χ=γ _C

Work as C ₁(3,3)＜0 and γ _C＞0 χ=γ _C-180 °

Work as C ₁(3,3)＜0 and γ _C＜0 χ=γ _C+ 180 °

In the tenth step, azimuth angle alpha, angle of pitch β, the roll angle χ that calculates the carrier aircraft optical axis stable turntable that obtains is transferred to servo control unit;

The 11 step repeated ten steps of the first step to the, until superior system finishes after sending geographical tracking END instruction.

Images