CN104459750A - GPS/INS-based dynamic pointing method - Google Patents

Abstract

The invention belongs to the technical field of laser communication pre-pointing capturing and discloses a GPS/INS-based dynamic pointing method. In a GPS/INS combined system, GPS data serve as outer input, INS system errors accumulated along with time are adjusted, and an INS is constantly amended when a carrier moves. The GPS/INS-based dynamic pointing method can solve the problem that GPS signals are lost under the dynamic condition through the INS computed data, and the signal capturing and tracking capacity of a GPS receiver is enhanced. The GPS/INS combined system plays a role in complement, the anti-interference capacity is higher and the precision is higher. The GPS/INS combined system is applied in the pre-pointing technology, the pointing angle value pointing to a target is worked out through the position information, obtained by a GPS, of a ground fixed end target, the movement platform three-axis attitude information and position information which are obtained by real-time measurement of the GPS/INS navigation system, corresponding coordinate conversion and real-time computing, and a two-dimension rotary table can rotate according to the pointing angle value to point to the target in real time.

Description

A kind of dynamic pointing method based on GPS/INS

Technical field

The present invention relates to a kind of dynamic pointing method based on GPS/INS, specifically relate to a kind of GPS/INS data to real-time update and resolve the dynamic pointing method drawing orientation angle, belong to laser communication and point to capture technique field in advance.

Background technology

Based on the laser communication system of motion platform, because communication distance is far away, light beam is narrow and there is external interference (atmospheric effect, mesa corners motion and vibration etc.), motion platform must adopt catches (Acquisition), and tracking (Tracking) and (Pointing) technology of aiming (ATP) set up the optical communication link maintained stiff end target.Wherein, capture technique, as the prerequisite of tracking and aiming, is used for setting up fast laser communication link or recovers the communication link that interrupts.Uncertain region accurately be pointed in the visual field of ATP (namely catch, tracking and aiming) system can, and can directly determine communication link set up, and determines communication link and hold time.

Initial directional method obtains concern and the research of a lot of scholar as the core of capture technique, be such as 201310499230.2 at application number, denomination of invention is " the aerial Initial Alignment Method of a kind of lighter-than-air flight device inertial navigation ", invention people is Li Baoguo, Lu Jiazhen, Hu Wenyuan, in the invention of Wu Meng, inventor is by adopting the method for the acquisition initial alignment of GPS (GPS) supplementary mode, though utilize the auxiliary initial alignment that obtains of gps system to possess high precision, real-time, the advantages such as continuity, but just cannot position when GPS does not receive satellite-signal, under high speed dynamical state, controllability and reliability are also poor.

Summary of the invention

The present invention is directed to above defect and deficiency, propose a kind of dynamic pointing method based on GPS (GPS)/inertial navigation system (INS).The method reliability compared with tradition is higher, and precision is better, and stability is stronger.

The present invention is realized by following concrete technical scheme.

A kind of dynamic pointing method based on GPS/INS, realized by the mobile terminal on ground, this mobile terminal mainly comprises GPS/INS module, PC, dimensional turntable, MC600 electric cabinet and laser instrument, GPS/INS module comprises GPS and inertial navigation system module, GPS/INS module and MC600 electric cabinet are connected with PC respectively, MC600 electric cabinet adopts 32 bit DSP processors to control, MC600 electric cabinet be connected to azimuth axis in dimensional turntable and pitch axis to realize orientation to dimensional turntable, pitch axis carries out single, double axle control; Laser instrument to be fixed in dimensional turntable and to have rotated the sensing to stiff end target with the azimuth axis in dimensional turntable and pitch axis, and the concrete steps of this dynamic pointing method are as follows:

(1) stiff end target and mobile terminal dimensional turntable position and attitude acquisition of information

Stiff end target position G point represents, the dimensional turntable end position of mobile terminal represents with S point, WGS-84 earth ground coordinate origin is O, the position coordinates OGw of stiff end target under WGS-84 earth ground coordinate system is obtained by GPS, the current location being obtained mobile terminal dimensional turntable by GPS/INS module is OSw, and current attitude angle and crab angle ψ, roll angle φ and pitching angle theta, given coordinate is Cartesian form, and above-mentioned position coordinate parameters and each attitude angle parameter are all sent in PC;

(2) WGS-84 earth ground ordinate transform is rectangular coordinate system in space

The position coordinates of stiff end target is obtained by the GPS in GPS/INS module, the longitude of stiff end target GPS received according to WGS-84 ellipsoidal model, latitude and height position information send in PC the coordinate figure be converted under rectangular coordinate system in space, and conversion formula is such as formula (1)

$\left\{\begin{array}{c}x=(N+H)\cos B\cos L\\ y=(N+H)\cos B\sin L\\ z=[N(1-e^2)+H]\sin B\end{array}\right.---\left(1\right)$

In formula, x-axis, y-axis and z-axis coordinate figure in x, y, z difference representation space rectangular coordinate system, B is latitude, L is longitude, H is height, N is prime vertical radius, and

$N=a/\sqrt{1-e^2{\sin }^{2}B}---\left(2\right)$

In formula (2), a is major radius of ellipsoid 6378137m; E2 is the first ellipsoid bias 0.0066943799013;

(3) rectangular coordinate system in space is transformed into east northeast sky coordinate system

Due to the installation of mobile terminal dimensional turntable and east northeast sky coordinate system closely related, therefore must be east northeast sky coordinate system by rectangular coordinate system in space coordinate conversion, obtain rectangular space coordinate above, be converted into east northeast sky coordinate by PC by transformation matrix of coordinates

Transformation matrix of coordinates is such as formula (3)

$M_{\mathrm{wo}}=\left[\begin{array}{ccc}-\sin L& \cos L& 0\\ -\sin B\cos L& -\sin B\sin L& \cos B\\ \cos B\cos L& \cos B\cos L& \sin B\end{array}\right]---\left(3\right)$

Wherein, B is latitude, and L is longitude, M _worepresentation space rectangular coordinate system is transformed into east northeast sky coordinate system transformation of coordinates matrix;

(4) east northeast sky ordinate transform is to aircraft body coordinate system

Because mobile terminal dimensional turntable is unstable or have deviation when installing, aircraft body coordinate system does not overlap with the east northeast sky coordinate system at place, needing east northeast sky ordinate transform by PC is aircraft body coordinate system, and demarcates or measure these deviations and form transition matrix;

East northeast sky ordinate transform to the coordinate conversion matrix of aircraft body coordinate system is:

$M_{\mathrm{ob}}=\left(\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& \sin \mathrm{\φ}\\ 0& -\sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right)\left(\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right)---\left(4\right)$

Step (3) has obtained east northeast sky coordinate figure, and through type (4) is converted to aircraft body coordinate system coordinate, wherein M _obrepresent the transformation matrix of east northeast sky ordinate transform to aircraft body coordinate system, attitude angle and crab angle ψ, roll angle φ, pitching angle theta;

(5) relative position of stiff end target under computing equipment coordinate system

According to above each coordinate conversion matrix, WGS-84 earth ground ordinate transform to the transformation matrix of coordinates of aircraft body coordinate system is

M _wi＝M _ob·M _wo(5)

Wherein M _wirepresent by PC, WGS-84 earth ground ordinate transform is to the transformation matrix of coordinates of aircraft body coordinate system, show that the relative position coordinates of stiff end target under the device coordinate system of dimensional turntable just can be expressed as:

SG _i＝M _wi(OG _w-OS _w)＝(x _SG,y _SG,z _SG) (6)

Wherein OG _wrepresent the coordinate of stiff end target position at WGS-84 earth ground coordinate system, OS _wrepresent the position coordinates of dimensional turntable end at WGS-84 earth ground coordinate system of mobile terminal, (x _sG, y _sG, z _sG) represent the relative position coordinates of dimensional turntable under device coordinate system;

(6) the sensing angle that laser instrument points to stiff end target is calculated

According to the relative position coordinates of dimensional turntable under device coordinate system that formula (6) is calculated by PC, calculate position angle and the angle of pitch of the rotation of dimensional turntable equipment, adopt as given a definition: position angle for zero-bit, is counterclockwise just around Z axis with Y-axis forward; Luffing angle is zero point in XY plane, and be just toward Z axis positive deflection, negative sense is negative,

According to the definition of dimensional turntable benchmark zero-bit, by the relative position coordinates SG of stiff end target under the device coordinate system of dimensional turntable of step (5) gained _ix, Y, Z tri-axle component x _sG, y _sG, z _sGthe azimuth pitch angle that can obtain dimensional turntable is: position angle for around Z axis right-handed helix for just, pitch axis is past Z axis positive dirction is just, azimuth pitch angular region is all between-90 ~+90 degree, position angle and the angle of pitch are exactly the sensing angle that laser instrument points to needed for stiff end target, and computing formula is such as formula (7):

$\tan \left(A_s\right)=-\left(\frac{x_{\mathrm{SG}}}{y_{\mathrm{SG}}}\right)$

$\tan \left(E_s\right)=\left(\frac{z_{\mathrm{SG}}}{\sqrt{x_{\mathrm{SG}}^2+y_{\mathrm{SG}}^2}}\right)---\left(7\right)$

(7) target is pointed to

Above steps calculates by PC, the position angle and the pitching angle value that step (6) are solved sensing target input MC600 electric cabinet in real time, utilize MC600 electric cabinet to control dimensional turntable and rotate corresponding angle, laser instrument in dimensional turntable just points to stiff end target rapidly from initial position, and point to stiff end target in real time along with the motion of mobile terminal, namely complete laser instrument and stiff end target ground is dynamically pointed to fast.

Described WGS-84 earth ground coordinate system, as shown in Figure 2, earth ground coordinate system consolidation on earth, rotates with the earth, using earth centroid as the body-fixed coordinate system of initial point be earth ground coordinate system, normal employing WGS-84 coordinate system, this coordinate is the special coordinate system of GPS positioning system, and true origin is positioned at earth centroid, and Z axis points to earth earth polar, X-axis points to zero degree meridian ellipse and equatorial node, and Y-axis and X, Z form right-handed coordinate system.Two kinds of coordinate representations are had, i.e. rectangular space coordinate form and the earth ginseng heart coordinate form under earth coordinates.

Described rectangular coordinate system in space, as shown in Figure 3, from space, 1 O draws three mutually perpendicular straight line Ox, Oy, Oz, and get measured length unit and direction, just establish rectangular coordinate system in space, wherein O point is called true origin, number axis Ox, Oy, Oz are called coordinate axis, and plane Oxy, Oyz, the Ozx at every two coordinate axis places are called coordinate plane.

Described east northeast sky coordinate system, as shown in Figure 4, east northeast sky coordinate is navigation Common Coordinate, and east northeast sky coordinate system is defined as: z-axis is along land station-the earth's core line and away from direction, the earth's core; Y-axis is vertical with z-axis points to direction to the north pole; X-axis and y, z form right-handed coordinate system.

Described aircraft body coordinate system, as shown in Figure 5, aircraft body coordinate system is the benchmark of aircraft attitude measurement and control, and fix on board the aircraft, initial point is aircraft barycenter, and three axles are fixed on aircraft body.When three axles are as the principal axis of inertia for aircraft, also known as main shaft coordinate system.When main shaft coordinate system is in ideal pose, aircraft Centroid orbit coordinate system is identical with body coordinate system.Because aircraft has attitude error, by crab angle ψ, roll angle φ, pitching angle theta definition attitude error value.Crab angle, roll angle and the angle of pitch are defined as three Eulerian angle of aircraft body coordinate system corresponding to Centroid orbit coordinate system, and its rotation order is z-x-y.

Described device coordinate system, device coordinate system defines the coordinate axis of useful load instrument and equipment.Initial point is instrument barycenter, X-axis perpendicular to installed surface upwards, Z axis coincides with azimuth axis and along rear light path emergent ray direction, Y-axis is determined by the right-hand rule, the benchmark zero-bit at the azimuth pitch angle of device coordinate system as shown in Figure 6, emergent light axis is along being benchmark zero-bit during-Yi direction, and now azimuth axis is just around Zi right-handed helix, and the angle of pitch is emergent light axis is just partially on pitch axis.

Inertial navigation system of the present invention (INS) not only can realize independent navigation and not outside radiation information, can also obtain the data such as very comprehensive position, attitude and speed.But the systematic error that INS assists can change in time and accumulate increase, thus makes navigation accuracy be deteriorated gradually.GPS has high-precision location and the ability that tests the speed, but just cannot position when GPS does not receive satellite-signal, and thus reliability is lower.In GPS/INS combined system of the present invention, gps data is used as outside input, the INS systematic error along with time integral can be adjusted, constantly revise INS when carrier movement.And INS resolved data can solve the problem that gps signal is lost in the dynamic case, enhance the ability of the catching of GPS, tracking signal.The inventive method takes full advantage of the high precision of GPS and the respective advantage of INS independent navigation, complements each other, and make ATP system capture technique reliability higher, stability is better.

Accompanying drawing explanation

Fig. 1 be mobile terminal of the present invention connect structure block diagram.

Fig. 2 is WGS-84 earth ground coordinate system, and true origin is positioned at earth centroid, and Z axis points to earth earth polar, and X-axis points to zero degree meridian ellipse and equatorial node, and Y-axis and X, Z form right-handed coordinate system.

Fig. 3 is rectangular coordinate system in space, and from space, 1 O draws three mutually perpendicular straight line Ox, Oy, Oz, and O point is called true origin, number axis Ox, and Oy, Oz are called coordinate axis, and plane Oxy, Oyz, the Ozx at every two coordinate axis places are called coordinate plane.

Fig. 4 is earth east northeast sky coordinate system, and z-axis is along land station-the earth's core line and away from direction, the earth's core; Y-axis is vertical with z-axis points to direction to the north pole; X-axis and y, z form right-handed coordinate system.

Fig. 5 is three-axis attitude angle rotation relationship figure, and crab angle ψ, roll angle φ and pitching angle theta are defined as three Eulerian angle of aircraft body coordinate system corresponding to Centroid orbit coordinate system, and its rotation order is z-x-y.

Fig. 6 is dimensional turntable benchmark zero-bit figure, and emergent light axis is along-Y _ibe benchmark zero-bit during direction, now azimuth axis is around Z _iright-handed helix is just, the angle of pitch is emergent light axis is just partially on pitch axis.

Specific implementation method

Below in conjunction with drawings and Examples, the invention will be further described, but be not limited thereto.

Embodiment:

The embodiment of the present invention as shown in Figure 1, a kind of dynamic pointing method based on GPS/INS, realized by the mobile terminal on ground, this mobile terminal mainly comprises GPS/INS module, PC, dimensional turntable, MC600 electric cabinet and laser instrument, GPS/INS module comprises GPS and inertial navigation system module, , GPS/INS module and MC600 electric cabinet are connected with PC respectively, MC600 electric cabinet adopts 32 bit DSP processors to control, MC600 electric cabinet is connected to azimuth axis in dimensional turntable and pitch axis to realize the orientation to dimensional turntable, pitch axis carries out list, twin shaft controls, laser instrument to be fixed in dimensional turntable and to have rotated the sensing to stiff end target with the azimuth axis in dimensional turntable and pitch axis, and the concrete steps of this dynamic pointing method are as follows:

(1) stiff end target and mobile terminal dimensional turntable position and attitude acquisition of information

Stiff end target position G point represents, the dimensional turntable end position of mobile terminal represents with S point, WGS-84 earth ground coordinate origin is O, the position coordinates OGw of stiff end target under WGS-84 earth ground coordinate system is obtained by GPS, the current location being obtained mobile terminal dimensional turntable by GPS/INS module is OSw, and current attitude angle and crab angle ψ, roll angle φ and pitching angle theta, given coordinate is Cartesian form, and above-mentioned position coordinate parameters and each attitude angle parameter are all sent in PC;

(2) WGS-84 earth ground ordinate transform is rectangular coordinate system in space

The position coordinates of stiff end target is obtained by the GPS in GPS/INS module, the longitude of stiff end target GPS received according to WGS-84 ellipsoidal model, latitude and height position information send in PC the coordinate figure be converted under rectangular coordinate system in space, and conversion formula is such as formula (1)

$\left\{\begin{array}{c}x=(N+H)\cos B\cos L\\ y=(N+H)\cos B\sin L\\ z=[N(1-e^2)+H]\sin B\end{array}\right.---\left(1\right)$

In formula, x-axis, y-axis and z-axis coordinate figure in x, y, z difference representation space rectangular coordinate system, B is latitude, L is longitude, H is height, N is prime vertical radius, and

$N=a/\sqrt{1-e^2{\sin }^{2}B}---\left(2\right)$

In formula (2), a is major radius of ellipsoid 6378137m; E2 is the first ellipsoid bias 0.0066943799013;

(3) rectangular coordinate system in space is transformed into east northeast sky coordinate system

Due to the installation of mobile terminal dimensional turntable and east northeast sky coordinate system closely related, therefore must be east northeast sky coordinate system by rectangular coordinate system in space coordinate conversion, obtain rectangular space coordinate above, be converted into east northeast sky coordinate by PC by transformation matrix of coordinates

Transformation matrix of coordinates is such as formula (3)

$M_{\mathrm{wo}}=\left[\begin{array}{ccc}-\sin L& \cos L& 0\\ -\sin B\cos L& -\sin B\sin L& \cos B\\ \cos B\cos L& \cos B\cos L& \sin B\end{array}\right]---\left(3\right)$

Wherein, B is latitude, and L is longitude, M _worepresentation space rectangular coordinate system is transformed into east northeast sky coordinate system transformation of coordinates matrix;

(4) east northeast sky ordinate transform is to aircraft body coordinate system

Because mobile terminal dimensional turntable is unstable or have deviation when installing, aircraft body coordinate system does not overlap with the east northeast sky coordinate system at place, needing east northeast sky ordinate transform by PC is aircraft body coordinate system, and demarcates or measure these deviations and form transition matrix;

East northeast sky ordinate transform to the coordinate conversion matrix of aircraft body coordinate system is:

$M_{\mathrm{ob}}=\left(\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \mathrm{\θ}\\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& \sin \mathrm{\φ}\\ 0& -\sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right)\left(\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right)---\left(4\right)$

Step (3) has obtained east northeast sky coordinate figure, and through type (4) is converted to aircraft body coordinate system coordinate, wherein M _obrepresent the transformation matrix of east northeast sky ordinate transform to aircraft body coordinate system, attitude angle and crab angle ψ, roll angle φ, pitching angle theta;

(5) relative position of stiff end target under computing equipment coordinate system

According to above each coordinate conversion matrix, WGS-84 earth ground ordinate transform to the transformation matrix of coordinates of aircraft body coordinate system is

M _wi＝M _ob·M _wo(5)

Wherein M _wirepresent by PC, WGS-84 earth ground ordinate transform is to the transformation matrix of coordinates of aircraft body coordinate system, show that the relative position coordinates of stiff end target under the device coordinate system of dimensional turntable just can be expressed as:

SG _i＝M _wi(OG _w-OS _w)＝(x _SG,y _SG,z _SG) (6)

Wherein OG _wrepresent the coordinate of stiff end target position at WGS-84 earth ground coordinate system, OS _wrepresent the position coordinates of dimensional turntable end at WGS-84 earth ground coordinate system of mobile terminal, (x _sG, y _sG, z _sG) represent the relative position coordinates of dimensional turntable under device coordinate system;

(6) the sensing angle that laser instrument points to stiff end target is calculated

According to the relative position coordinates of dimensional turntable under device coordinate system that formula (6) is calculated by PC, calculate position angle and the angle of pitch of the rotation of dimensional turntable equipment, adopt as given a definition: position angle for zero-bit, is counterclockwise just around Z axis with Y-axis forward; Luffing angle is zero point in XY plane, and be just toward Z axis positive deflection, negative sense is negative,

According to the definition of dimensional turntable benchmark zero-bit, by the relative position coordinates SG of stiff end target under the device coordinate system of dimensional turntable of step (5) gained _ix, Y, Z tri-axle component x _sG, y _sG, z _sGthe azimuth pitch angle that can obtain dimensional turntable is: position angle for around Z axis right-handed helix for just, pitch axis is past Z axis positive dirction is just, azimuth pitch angular region is all between-90 ~+90 degree, position angle and the angle of pitch are exactly the sensing angle that laser instrument points to needed for stiff end target, and computing formula is such as formula (7):

$\tan \left(A_s\right)=-\left(\frac{x_{\mathrm{SG}}}{y_{\mathrm{SG}}}\right)$

$\tan \left(E_s\right)=\left(\frac{z_{\mathrm{SG}}}{\sqrt{x_{\mathrm{SG}}^2+y_{\mathrm{SG}}^2}}\right)---\left(7\right)$

(7) target is pointed to

Above steps calculates by PC, the position angle and the pitching angle value that step (6) are solved sensing target input MC600 electric cabinet in real time, utilize MC600 electric cabinet to control dimensional turntable and rotate corresponding angle, laser instrument in dimensional turntable just points to stiff end target rapidly from initial position, and point to stiff end target in real time along with the motion of mobile terminal, namely complete laser instrument and stiff end target ground is dynamically pointed to fast.

Claims (1)

1. the dynamic pointing method based on GPS/INS, realized by the mobile terminal on ground, this mobile terminal mainly comprises GPS/INS module, PC, dimensional turntable, MC600 electric cabinet and laser instrument, GPS/INS module comprises GPS and inertial navigation system, GPS/INS module and MC600 electric cabinet are connected with PC respectively, MC600 electric cabinet adopts 32 bit DSP processors to control, MC600 electric cabinet be connected to azimuth axis in dimensional turntable and pitch axis to realize orientation to dimensional turntable, pitch axis carries out single, double axle control; Laser instrument to be fixed in dimensional turntable and to have rotated the sensing to stiff end target with the azimuth axis in dimensional turntable and pitch axis, and the concrete steps of this dynamic pointing method are as follows:

(1) stiff end target and mobile terminal dimensional turntable position and attitude acquisition of information

Stiff end target position G point represents, the dimensional turntable end position of mobile terminal represents with S point, WGS-84 earth ground coordinate origin is O, the position coordinates OGw of stiff end target under WGS-84 earth ground coordinate system is obtained by GPS, the current location being obtained mobile terminal dimensional turntable by GPS/INS module is OSw, and current attitude angle and crab angle ψ, roll angle φ and pitching angle theta, given coordinate is Cartesian form, and above-mentioned position coordinate parameters and each attitude angle parameter are all sent in PC;

(2) WGS-84 earth ground ordinate transform is rectangular coordinate system in space

The position coordinates of stiff end target is obtained by the GPS in GPS/INS module, the longitude of stiff end target GPS received according to WGS-84 ellipsoidal model, latitude and height position information send in PC the coordinate figure be converted under rectangular coordinate system in space, and conversion formula is such as formula (1)

$\left\{\begin{array}{c}x=(N+H)\cos B\cos L\\ y=(N+H)\cos B\sin L\\ z=[N(1-e^2)+H]\sin B\end{array}\right.---\left(1\right)$

In formula, x-axis, y-axis and z-axis coordinate figure in x, y, z difference representation space rectangular coordinate system, B is latitude, L is longitude, H is height, N is prime vertical radius, and

$N=a/\sqrt{1-e^2{\sin }^{2}B}---\left(2\right)$

In formula (2), a is major radius of ellipsoid 6378137m; E2 is the first ellipsoid bias 0.0066943799013;

(3) rectangular coordinate system in space is transformed into east northeast sky coordinate system

Due to the installation of mobile terminal dimensional turntable and east northeast sky coordinate system closely related, therefore must be east northeast sky coordinate system by rectangular coordinate system in space coordinate conversion, obtain rectangular space coordinate above, be converted into east northeast sky coordinate by PC by transformation matrix of coordinates

Transformation matrix of coordinates is such as formula (3)

$M_{\mathrm{wo}}=\left[\begin{array}{ccc}-\sin L& \cos L& 0\\ -\sin B\cos L& -\sin B\sin L& \cos B\\ \cos B\cos L& \cos B\cos L& \sin B\end{array}\right]---\left(3\right)$

Wherein, B is latitude, and L is longitude, M _worepresentation space rectangular coordinate system is transformed into east northeast sky coordinate system transformation of coordinates matrix;

(4) east northeast sky ordinate transform is to aircraft body coordinate system

Because mobile terminal dimensional turntable is unstable or have deviation when installing, aircraft body coordinate system does not overlap with the east northeast sky coordinate system at place, needing east northeast sky ordinate transform by PC is aircraft body coordinate system, and demarcates or measure these deviations and form transition matrix;

East northeast sky ordinate transform to the coordinate conversion matrix of aircraft body coordinate system is:

$M_{\mathrm{ob}}=\left(\begin{array}{ccc}\cos \mathrm{\θ}& 0& -\sin \\ 0& 1& 0\\ \sin \mathrm{\θ}& 0& \cos \mathrm{\θ}\end{array}\right)\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\φ}& \sin \mathrm{\φ}\\ 0& -\sin \mathrm{\φ}& \cos \mathrm{\φ}\end{array}\right)\left(\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right)---\left(4\right)$

Step (3) has obtained east northeast sky coordinate figure, and through type (4) is converted to aircraft body coordinate system coordinate, wherein M _obrepresent the transformation matrix of east northeast sky ordinate transform to aircraft body coordinate system, attitude angle and crab angle ψ, roll angle φ, pitching angle theta;

(5) relative position of stiff end target under computing equipment coordinate system

According to above each coordinate conversion matrix, WGS-84 earth ground ordinate transform to the transformation matrix of coordinates of aircraft body coordinate system is

M _wi＝M _ob·M _wo(5)

Wherein M _wirepresent by PC, WGS-84 earth ground ordinate transform is to the transformation matrix of coordinates of aircraft body coordinate system, show that the relative position coordinates of stiff end target under the device coordinate system of dimensional turntable just can be expressed as:

SG _i＝M _wi(OG _w-OS _w)＝(x _SG,y _SG,z _SG) (6)

Wherein OG _wrepresent the coordinate of stiff end target position at WGS-84 earth ground coordinate system, OS _wrepresent the position coordinates of dimensional turntable end at WGS-84 earth ground coordinate system of mobile terminal, (x _sG, y _sG, z _sG) represent the relative position coordinates of dimensional turntable under device coordinate system;

(6) the sensing angle that laser instrument points to stiff end target is calculated

According to the relative position coordinates of dimensional turntable under device coordinate system that formula (6) is calculated by PC, calculate position angle and the angle of pitch of the rotation of dimensional turntable equipment, adopt as given a definition: position angle for zero-bit, is counterclockwise just around Z axis with Y-axis forward; Luffing angle is zero point in XY plane, and be just toward Z axis positive deflection, negative sense is negative,

According to the definition of dimensional turntable benchmark zero-bit, by the relative position coordinates SG of stiff end target under the device coordinate system of dimensional turntable of step (5) gained _ix, Y, Z tri-axle component x _sG, y _sG, z _sGthe azimuth pitch angle that can obtain dimensional turntable is: position angle for around Z axis right-handed helix for just, pitch axis is past Z axis positive dirction is just, azimuth pitch angular region is all between-90 ~+90 degree, position angle and the angle of pitch are exactly the sensing angle that laser instrument points to needed for stiff end target, and computing formula is such as formula (7):

$\tan \left(A_S\right)=-\left(\frac{x_{\mathrm{SG}}}{y_{\mathrm{SG}}}\right)$

$\tan \left(E_s\right)=\left(\frac{z_{\mathrm{SG}}}{\sqrt{x_{\mathrm{SG}}^2+y_{\mathrm{SG}}^2}}\right)---\left(7\right)$

(7) target is pointed to

Above steps calculates by PC, the position angle and the pitching angle value that step (6) are solved sensing target input MC600 electric cabinet in real time, utilize MC600 electric cabinet to control dimensional turntable and rotate corresponding angle, laser instrument in dimensional turntable just points to stiff end target rapidly from initial position, and point to stiff end target in real time along with the motion of mobile terminal, namely complete laser instrument and stiff end target ground is dynamically pointed to fast.