CN104459750A - A Dynamic Pointing Method Based on GPS/INS - Google Patents

Abstract

A GPS/INS-based dynamic pointing method belongs to the technical field of laser communication pre-pointing capture. In the GPS/INS combined system, the GPS data is taken as an external input, the INS system error accumulated over time is adjusted, and the INS is continuously corrected when the carrier is moving. The method of the invention can solve the problem of GPS signal loss under dynamic conditions by resolving data in the INS, and enhances the capability of capturing and tracking signals of the GPS receiver. GPS/INS combined systems complement each other, with stronger anti-interference ability and higher precision. The GPS/INS combined system is used in the pre-pointing technology, the position information of the ground fixed end target obtained through GPS, and the three-axis attitude information and position information of the moving platform obtained by real-time measurement of the GPS/INS navigation system, and the corresponding coordinate transformation is calculated in real time The pointing angle value of the pointing target is obtained, and the two-dimensional turntable can point to the target in real time by rotating according to the pointing angle value.

Description

A Dynamic Pointing Method Based on GPS/INS

technical field

The invention relates to a dynamic pointing method based on GPS/INS, in particular to a dynamic pointing method for obtaining a pointing angle by solving real-time updated GPS/INS data, and belongs to the technical field of laser communication pre-pointing capture.

Background technique

The laser communication system based on the motion platform, due to the long communication distance, narrow beam and external interference (atmospheric influence, platform angular motion and vibration, etc.), the motion platform must adopt acquisition (Acquisition), tracking (Tracking) and aiming (Pointing) technology (ATP) to establish and maintain an optical communication link to the fixed-end target. Among them, the acquisition technology is used as a prerequisite for tracking and targeting, and is used to quickly establish a laser communication link or restore an interrupted communication link. Whether the field of view of the ATP (acquisition, tracking and targeting) system can accurately point to the uncertain area directly determines whether the communication link can be established and determines the maintenance time of the communication link.

As the core of the capture technology, the initial pointing method has attracted the attention and research of many scholars. For example, in the application number 201310499230.2, the invention name is "A method for initial alignment in the air with strapdown inertial navigation of floating aircraft", and the inventors are Li Baoguo and Lu Jiazhen In the inventions of , Hu Wenyuan, and Wu Meng, the inventor obtains the initial alignment by using the global positioning system (GPS) assisted method. Although the initial alignment obtained with the aid of the GPS system has the advantages of high precision, real-time, and continuity, However, when the GPS cannot receive satellite signals, it cannot perform positioning, and the controllability and reliability are also poor under high-speed dynamic conditions.

Contents of the invention

Aiming at the above defects and deficiencies, the present invention proposes a dynamic pointing method based on Global Positioning System (GPS)/Inertial Navigation System (INS). Compared with the traditional method, this method has higher reliability, better precision and stronger stability.

The present invention is achieved through the following specific technical solutions.

A dynamic pointing method based on GPS/INS, realized by the mobile terminal on the ground, the mobile terminal mainly includes GPS/INS module, PC, two-dimensional turntable, MC600 electric control box and laser, GPS/INS module includes GPS receiver The computer and inertial navigation system module, the GPS/INS module and the MC600 electric control box are respectively connected to the PC. The MC600 electric control box is controlled by a 32-bit DSP processor, and the MC600 electric control box is connected to the azimuth axis and The pitch axis is used to realize single-axis and dual-axis control of the azimuth and pitch axes of the two-dimensional turntable; the laser is installed and fixed on the two-dimensional turntable and rotates with the azimuth axis and pitch axis on the two-dimensional turntable to complete the pointing of the fixed end target , the specific steps of this dynamic pointing method are as follows:

(1) Acquisition of the position and attitude information of the fixed-end target and the mobile-end two-dimensional turntable

The position of the target at the fixed end is represented by point G, and the position of the two-dimensional turntable at the mobile end is represented by point S. The origin of the WGS-84 earth coordinate system is O, and the GPS receiver obtains the position of the fixed end target in the WGS-84 earth coordinate system. The position coordinate OGw, the current position of the two-dimensional turntable at the mobile terminal is OSw obtained by the GPS/INS module, and the current attitude angle is the yaw angle ψ, roll angle φ and pitch angle θ. The given coordinates are in the form of rectangular coordinates. The above-mentioned position coordinate parameters and various attitude angle parameters are all sent to the PC;

(2) Convert the WGS-84 earth geodetic coordinate system to the space Cartesian coordinate system

The position coordinates of the fixed-end target are obtained by the GPS receiver in the GPS/INS module, and the longitude, latitude, and height position information of the fixed-end target received by the GPS receiver are sent to the PC according to the WGS-84 ellipsoid model. Converted to the coordinate value in the space Cartesian coordinate system, the conversion formula is as formula (1)

$\left\{\begin{array}{c}\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}\\ \mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}\\ \mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; [</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; e</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; ]</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

In the formula, x, y, and z represent the coordinate values of the x-axis, y-axis, and z-axis in the space Cartesian coordinate system respectively, B is the latitude, L is the longitude, H is the height, N is the radius of the unitary circle, and

$\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; /</span>\sqrt{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; e</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; B</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

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

(3) Convert the space Cartesian coordinate system to the North East sky coordinate system

Since the installation of the two-dimensional turntable on the mobile terminal is closely related to the North East sky coordinate system, it is necessary to transform the space Cartesian coordinate system coordinates into the North East sky coordinate system. The space Cartesian coordinates have been obtained before, and the PC will convert them through the coordinate transformation matrix is the northeast sky coordinate,

The coordinate transformation matrix is as formula (3)

${\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; the\; w</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\\ \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

Wherein, B is the latitude, L is the longitude, and M _wo represents the transformation matrix of transforming the space Cartesian coordinate system to the coordinate system of the northeast sky;

(4) Convert the Beidongtian coordinate system to the aircraft body coordinate system

Due to the instability of the two-dimensional turntable at the mobile end or deviation during installation, the coordinate system of the aircraft body does not coincide with the coordinate system of the North East Sky where it is located. It is necessary to convert the coordinate system of the North East Sky into the coordinate system of the aircraft body through a PC, and calibrate or These deviations are measured and a transfer matrix is formed;

The coordinate transformation matrix for transforming the Beidongtian coordinate system to the aircraft body coordinate system is:

${\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; =</span>\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}\end{array}\right)\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\end{array}\right)\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right)<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

Step (3) has obtained the North East sky coordinate value, and converts it into the aircraft body coordinate system coordinates by formula (4), wherein M _ob represents the transformation matrix from the North East sky coordinate system to the aircraft body coordinate system, and the attitude angle is the yaw angle ψ, roll angle φ, pitch angle θ;

(5) Calculate the relative position of the fixed end target in the equipment coordinate system

According to the above coordinate transformation matrices, the coordinate transformation matrix for converting the WGS-84 earth coordinate system to the aircraft body coordinate system is

M _wi ＝M _ob ·M _wo (5)

Among them, _Mwi represents the coordinate transformation matrix converted from the WGS-84 earth coordinate system to the aircraft body coordinate system, and the relative position coordinates of the fixed-end target in the equipment coordinate system of the two-dimensional turntable obtained by the PC can be expressed as:

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

Among them, OG _w represents the coordinates of the target position of the fixed end in the WGS-84 earth coordinate system, OS _w represents the position coordinates of the two-dimensional turntable end of the mobile end in the WGS-84 earth coordinate system, (x _SG , y _SG , z _SG ) Indicates the relative position coordinates of the two-dimensional turntable in the equipment coordinate system;

(6) Calculate the pointing angle of the laser pointing to the fixed end target

According to formula (6), the relative position coordinates of the two-dimensional turntable in the equipment coordinate system calculated by the PC are used to calculate the azimuth and pitch angles of the two-dimensional turntable equipment rotation, and the following definition is adopted: the azimuth takes the positive direction of Y axis as zero , the counterclockwise direction around the Z axis is positive; the pitch angle is zero in the XY plane, the positive deflection towards the Z axis is positive, and the negative direction is negative.

According to the definition of the reference zero position of the two-dimensional turntable, the X, Y, and Z three-axis components x _SG , y _SG , z of the relative position coordinate SG _i of the fixed-end target obtained in step (5) in the equipment coordinate system of the two-dimensional turntable _SG can get the azimuth and pitch angles of the two-dimensional turntable as follows: the azimuth is positive when the right-handed spiral around the Z axis is positive, the pitch axis is positive when the positive direction of the Z axis is positive, and the range of azimuth and pitch angles is between -90 and +90 degrees. Angle and pitch angle are the pointing angles required for the laser to point to the target at the fixed end, and the calculation formula is shown in formula (7):

$\mathrm{<span\; class="notranslate">\; the\; tan</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}<span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; the\; tan</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; E.</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}{\sqrt{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

(7) point to the target

The above-mentioned steps are all calculated by a PC, and the azimuth and pitch angle values obtained in step (6) pointing to the target are input into the MC600 electric control box in real time, and the MC600 electric control box is used to control the corresponding angle of rotation of the two-dimensional turntable, and the two-dimensional turntable The laser on the laser will quickly point to the fixed-end target from the initial position, and point to the fixed-end target in real time with the movement of the mobile end, that is, the rapid dynamic pointing of the laser to the fixed-end target is completed.

The WGS-84 earth geodetic coordinate system, as shown in Figure 2, the earth geodetic coordinate system is fixed on the earth and rotates with the earth, and the earth geodetic coordinate system with the earth's center of mass as the origin is the earth geodetic coordinate system, which is often used WGS-84 coordinate system, this coordinate system is a special coordinate system for GPS positioning system, the coordinate origin is located at the center of the earth, the Z axis points to the earth's pole, the X axis points to the intersection of the zero-degree meridian plane and the equator, and the Y axis forms a right-handed coordinate system with X and Z . There are two forms of coordinate representation in the geodetic coordinate system, namely, the form of spatial rectangular coordinates and the form of geodetic coordinates.

The space Cartesian coordinate system, as shown in Figure 3, draws three mutually perpendicular straight lines Ox, Oy, Oz from a certain point O in space, and takes a fixed length unit and direction to establish a space Cartesian coordinate system, wherein the O point is called is the coordinate origin, the number axes Ox, Oy, and Oz are called coordinate axes, and the planes Oxy, Oyz, and Ozx where each two coordinate axes are located are called coordinate planes.

The North East sky coordinate system, as shown in Figure 4, the North East sky coordinate system is a common coordinate system for navigation, and the North East sky coordinate system is defined as: the z axis is along the ground station-the center of the earth and away from the center of the earth; The y-axis and the z-axis are perpendicular to the North Pole; the x-axis and y and z form a right-handed coordinate system.

Described aircraft body coordinate system, as shown in Figure 5, aircraft body coordinate system is the datum of aircraft attitude measurement and control, is fixed on the aircraft, and origin is the center of mass of aircraft, and three axes are fixed on the aircraft body. When the three axes are used as the main axis of inertia of the aircraft, it is also called the main axis coordinate system. When the main axis coordinate system is in the ideal attitude, the center of mass orbital coordinate system of the aircraft is the same as the body coordinate system. Since the aircraft has an attitude error, the attitude error value is defined by the yaw angle ψ, roll angle φ, and pitch angle θ. The yaw angle, roll angle and pitch angle are defined as the three Euler angles of the aircraft body coordinate system corresponding to the center-of-mass orbit coordinate system, and the rotation sequence is z-x-y.

As for the equipment coordinate system, the equipment coordinate system defines the coordinate axes of the payload instrument equipment. The origin is the center of mass of the instrument, the X-axis is perpendicular to the installation surface, the Z-axis coincides with the azimuth axis and the direction of the outgoing light along the rear optical path, and the Y-axis is determined by the right-hand rule. The reference zero position of the azimuth and pitch angle of the equipment coordinate system is shown in Figure 6 It shows that when the outgoing optical axis is along the -Y direction, it is the reference zero position. At this time, the azimuth axis is positive around the right-handed spiral of Zi, and the pitch angle is positive when the outgoing optical axis is upwardly inclined around the pitch axis.

The inertial navigation system (INS) of the present invention can not only realize autonomous navigation without radiating information to the outside, but also can obtain very comprehensive data such as position, attitude and speed. However, the system error assisted by INS will accumulate and increase over time, which will gradually deteriorate the navigation accuracy. GPS has high-precision positioning and speed measurement capabilities, but it cannot perform positioning when GPS cannot receive satellite signals, so its reliability is low. In the GPS/INS combination system of the present invention, the GPS data is taken as an external input, the INS system error accumulated over time can be adjusted, and the INS is continuously corrected when the carrier is moving. The INS solution data can solve the problem of GPS signal loss under dynamic conditions, and enhance the ability of GPS receivers to capture and track signals. The method of the invention makes full use of the high precision of the GPS and the respective advantages of the autonomous navigation of the INS, and complements each other, so that the ATP system capture technology has higher reliability and better stability.

Description of drawings

Fig. 1 is a block diagram of the mobile terminal of the present invention.

Figure 2 is the WGS-84 earth geodetic coordinate system. The origin of the coordinates is located at the center of the earth's mass. The Z axis points to the earth's poles.

Figure 3 is a rectangular coordinate system in space. Three mutually perpendicular straight lines Ox, Oy, and Oz are drawn from a certain point O in space. The point O is called the coordinate origin, and the number axes Ox, Oy, and Oz are called coordinate axes. The planes Oxy, Oyz, and Ozx are called coordinate planes.

Figure 4 is the earth's north-east sky coordinate system. The z-axis is along the line connecting the ground station to the center of the earth and away from the center of the earth; the y-axis and the z-axis are perpendicular to the North Pole; the x-axis and y and z form a right-handed coordinate system.

Fig. 5 is a three-axis attitude angle rotation diagram. The yaw angle ψ, roll angle φ and pitch angle θ are defined as three Euler angles corresponding to the center-of-mass orbit coordinate system of the aircraft body coordinate system, and the rotation sequence is z-x-y.

Figure 6 is a diagram of the reference zero position of the two-dimensional turntable. When the exit optical axis is along the -Y _i direction, it is the reference zero position. At this time, the azimuth axis revolves around Z _i . .

Specific implementation method

The present invention will be further described below in conjunction with the accompanying drawings and embodiments, but is not limited thereto.

Example:

The embodiment of the present invention is shown in Figure 1. A dynamic pointing method based on GPS/INS is realized by the mobile terminal on the ground. The mobile terminal mainly includes GPS/INS module, PC, two-dimensional turntable, MC600 electric control box and Laser, GPS/INS module includes GPS receiver and inertial navigation system module, GPS/INS module and MC600 electric control box are respectively connected with PC, MC600 electric control box is controlled by 32-bit DSP processor, MC600 electric control box Connected to the azimuth axis and pitch axis on the two-dimensional turntable to realize single-axis and dual-axis control of the azimuth and pitch axes of the two-dimensional turntable; the laser is fixed on the two-dimensional turntable and follows the azimuth axis and pitch axis Rotate to complete the pointing to the fixed end target, the specific steps of this dynamic pointing method are as follows:

(1) Acquisition of the position and attitude information of the fixed-end target and the mobile-end two-dimensional turntable

The position of the target at the fixed end is represented by point G, and the position of the two-dimensional turntable at the mobile end is represented by point S. The origin of the WGS-84 earth coordinate system is O, and the GPS receiver obtains the position of the fixed end target in the WGS-84 earth coordinate system. The position coordinate OGw, the current position of the two-dimensional turntable at the mobile terminal is OSw obtained by the GPS/INS module, and the current attitude angle is the yaw angle ψ, roll angle φ and pitch angle θ. The given coordinates are in the form of rectangular coordinates. The above-mentioned position coordinate parameters and various attitude angle parameters are all sent to the PC;

(2) Convert the WGS-84 earth geodetic coordinate system to the space Cartesian coordinate system

The position coordinates of the fixed-end target are obtained by the GPS receiver in the GPS/INS module, and the longitude, latitude, and height position information of the fixed-end target received by the GPS receiver are sent to the PC according to the WGS-84 ellipsoid model. Converted to the coordinate value in the space Cartesian coordinate system, the conversion formula is as formula (1)

$\left\{\begin{array}{c}\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}\\ \mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}\\ \mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; [</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; e</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; ]</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

In the formula, x, y, and z represent the coordinate values of the x-axis, y-axis, and z-axis in the space Cartesian coordinate system respectively, B is the latitude, L is the longitude, H is the height, N is the radius of the unitary circle, and

$\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; /</span>\sqrt{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; e</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; B</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

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

(3) Convert the space Cartesian coordinate system to the North East sky coordinate system

Since the installation of the two-dimensional turntable on the mobile terminal is closely related to the North East sky coordinate system, it is necessary to transform the space Cartesian coordinate system into the North East sky coordinate system. The space Cartesian coordinates have been obtained before, and the PC will convert them through the coordinate transformation matrix is the northeast sky coordinate,

The coordinate transformation matrix is as formula (3)

${\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; the\; w</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\\ \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

Wherein, B is the latitude, L is the longitude, and M _wo represents the transformation matrix of transforming the space Cartesian coordinate system to the coordinate system of the northeast sky;

(4) Convert the Beidongtian coordinate system to the aircraft body coordinate system

Due to the instability of the two-dimensional turntable at the mobile end or deviation during installation, the coordinate system of the aircraft body does not coincide with the coordinate system of the North East Sky where it is located. It is necessary to convert the coordinate system of the North East Sky into the coordinate system of the aircraft body through a PC, and calibrate or These deviations are measured and a transfer matrix is formed;

The coordinate transformation matrix for transforming the Beidongtian coordinate system to the aircraft body coordinate system is:

${\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; =</span>\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}\end{array}\right)\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\end{array}\right)\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right)<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$

Step (3) has obtained the North East sky coordinate value, and converts it into the aircraft body coordinate system coordinates by formula (4), wherein M _ob represents the transformation matrix from the North East sky coordinate system to the aircraft body coordinate system, and the attitude angle is the yaw angle ψ, roll angle φ, pitch angle θ;

(5) Calculate the relative position of the fixed end target in the equipment coordinate system

According to the above coordinate transformation matrices, the coordinate transformation matrix for converting the WGS-84 earth coordinate system to the aircraft body coordinate system is

M _wi ＝M _ob ·M _wo (5)

Among them, _Mwi represents the coordinate transformation matrix converted from the WGS-84 earth coordinate system to the aircraft body coordinate system, and the relative position coordinates of the fixed-end target in the equipment coordinate system of the two-dimensional turntable obtained by the PC can be expressed as:

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

Among them, OG _w represents the coordinates of the target position of the fixed end in the WGS-84 earth coordinate system, OS _w represents the position coordinates of the two-dimensional turntable end of the mobile end in the WGS-84 earth coordinate system, (x _SG , y _SG , z _SG ) Indicates the relative position coordinates of the two-dimensional turntable in the equipment coordinate system;

(6) Calculate the pointing angle of the laser pointing to the fixed end target

According to formula (6), the relative position coordinates of the two-dimensional turntable in the equipment coordinate system calculated by the PC are used to calculate the azimuth and pitch angles of the two-dimensional turntable equipment rotation, and the following definition is adopted: the azimuth takes the positive direction of Y axis as zero , the counterclockwise direction around the Z axis is positive; the pitch angle is zero in the XY plane, the positive deflection towards the Z axis is positive, and the negative direction is negative.

According to the definition of the reference zero position of the two-dimensional turntable, the X, Y, and Z three-axis components x _SG , y _SG , z of the relative position coordinate SG _i of the fixed-end target obtained in step (5) in the equipment coordinate system of the two-dimensional turntable _SG can get the azimuth and pitch angles of the two-dimensional turntable as follows: the azimuth is positive when the right-handed spiral around the Z axis is positive, the pitch axis is positive when the positive direction of the Z axis is positive, and the range of azimuth and pitch angles is between -90 and +90 degrees. Angle and pitch angle are the pointing angles required for the laser to point to the target at the fixed end, and the calculation formula is shown in formula (7):

$\mathrm{<span\; class="notranslate">\; the\; tan</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}<span\; class="notranslate">\; )</span>$

$\mathrm{<span\; class="notranslate">\; the\; tan</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; E.</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}{\sqrt{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

(7) point to the target

The above steps are all calculated by the PC, and the azimuth and pitch angle values of the target obtained in step (6) are input into the MC600 electric control box in real time, and the MC600 electric control box is used to control the corresponding angle of rotation of the two-dimensional turntable, and the two-dimensional turntable The laser on the mobile device quickly points to the fixed-end target from the initial position, and points to the fixed-end target in real time with the movement of the mobile end, that is, the rapid dynamic pointing of the laser to the fixed-end target is completed.

Claims (1)

1. A dynamic pointing method based on GPS/INS, realized by the mobile terminal on the ground, the mobile terminal mainly includes GPS/INS module, PC, two-dimensional turntable, MC600 electric control box and laser, GPS/INS module includes GPS receiver and inertial navigation system, GPS/INS module and MC600 electric control box are respectively connected with PC, MC600 electric control box is controlled by 32-bit DSP processor, MC600 electric control box is connected to the azimuth axis on the two-dimensional turntable and pitch axis to realize single-axis and dual-axis control of the azimuth and pitch axes of the two-dimensional turntable; the laser is installed and fixed on the two-dimensional turntable and rotates with the azimuth and pitch axes on the two-dimensional turntable to complete the target on the fixed end Pointing, the specific steps of this dynamic pointing method are as follows: (1) Acquisition of the position and attitude information of the fixed-end target and the mobile-end two-dimensional turntable The position of the target at the fixed end is represented by point G, and the position of the two-dimensional turntable at the mobile end is represented by point S. The origin of the WGS-84 earth coordinate system is O, and the GPS receiver obtains the position of the fixed end target in the WGS-84 earth coordinate system. The position coordinate OGw, the current position of the two-dimensional turntable at the mobile terminal is OSw obtained by the GPS/INS module, and the current attitude angle is the yaw angle ψ, roll angle φ and pitch angle θ. The given coordinates are in the form of rectangular coordinates. The above-mentioned position coordinate parameters and various attitude angle parameters are all sent to the PC; (2) Convert the WGS-84 earth geodetic coordinate system to the space Cartesian coordinate system The position coordinates of the fixed-end target are obtained by the GPS receiver in the GPS/INS module, and the longitude, latitude, and height position information of the fixed-end target received by the GPS receiver are sent to the PC according to the WGS-84 ellipsoid model. Converted to the coordinate value in the space Cartesian coordinate system, the conversion formula is as formula (1) $\left\{\begin{array}{c}\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}\\ \mathrm{<span\; class="notranslate">\; the\; y</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}\\ \mathrm{<span\; class="notranslate">\; z</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; [</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; e</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; ]</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$ In the formula, x, y, and z represent the coordinate values of the x-axis, y-axis, and z-axis in the space Cartesian coordinate system respectively, B is the latitude, L is the longitude, H is the height, N is the radius of the unitary circle, and $\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; a</span>}<span\; class="notranslate">\; /</span>\sqrt{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; e</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; sin</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}\mathrm{<span\; class="notranslate">\; B</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$ In formula (2), a is the major radius of the ellipsoid 6378137m; e2 is the eccentricity of the first ellipsoid 0.0066943799013; (3) Convert the space Cartesian coordinate system to the North East sky coordinate system Since the installation of the two-dimensional turntable on the mobile terminal is closely related to the North East sky coordinate system, it is necessary to transform the space Cartesian coordinate system coordinates into the North East sky coordinate system. The space Cartesian coordinates have been obtained before, and the PC will convert them through the coordinate transformation matrix is the northeast sky coordinate, The coordinate transformation matrix is as formula (3) ${\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; the\; w</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\\ \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; B</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; B</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$ Wherein, B is the latitude, L is the longitude, and M _wo represents the transformation matrix of transforming the space Cartesian coordinate system to the coordinate system of the northeast sky; (4) Convert the Beidongtian coordinate system to the aircraft body coordinate system Due to the instability of the two-dimensional turntable at the mobile end or deviation during installation, the coordinate system of the aircraft body does not coincide with the coordinate system of the North East Sky where it is located. It is necessary to convert the coordinate system of the North East Sky into the coordinate system of the aircraft body through a PC, and calibrate or These deviations are measured and a transfer matrix is formed; The coordinate transformation matrix for transforming the Beidongtian coordinate system to the aircraft body coordinate system is: ${\mathrm{<span\; class="notranslate">\; m</span>}}_{\mathrm{<span\; class="notranslate">\; ob</span>}}<span\; class="notranslate">\; =</span>\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&theta;</span>}\end{array}\right)\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&phi;</span>}\end{array}\right)\left(\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; \&psi;</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}\end{array}\right)<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 4</span>}<span\; class="notranslate">\; )</span>$ Step (3) has obtained the North East sky coordinate value, and converts it into the aircraft body coordinate system coordinates by formula (4), wherein M _ob represents the transformation matrix from the North East sky coordinate system to the aircraft body coordinate system, and the attitude angle is the yaw angle ψ, roll angle φ, pitch angle θ; (5) Calculate the relative position of the fixed end target in the equipment coordinate system According to the above coordinate transformation matrices, the coordinate transformation matrix for converting the WGS-84 earth coordinate system to the aircraft body coordinate system is M _wi ＝M _ob ·M _wo (5) Among them, _Mwi represents the coordinate transformation matrix converted from the WGS-84 earth coordinate system to the aircraft body coordinate system, and the relative position coordinates of the fixed-end target in the equipment coordinate system of the two-dimensional turntable obtained by the PC can be expressed as: SG _i ＝M _wi (OG _w −OS _w )＝(x _SG , y _SG , z _SG ) (6) Among them, OG _w represents the coordinates of the target position of the fixed end in the WGS-84 earth coordinate system, OS _w represents the position coordinates of the two-dimensional turntable end of the mobile end in the WGS-84 earth coordinate system, (x _SG , y _SG , z _SG ) Indicates the relative position coordinates of the two-dimensional turntable in the equipment coordinate system; (6) Calculate the pointing angle of the laser pointing to the fixed end target According to formula (6), the relative position coordinates of the two-dimensional turntable in the equipment coordinate system calculated by the PC are used to calculate the azimuth and pitch angles of the two-dimensional turntable equipment rotation, and the following definition is adopted: the azimuth takes the positive direction of Y axis as zero , the counterclockwise direction around the Z axis is positive; the pitch angle is zero in the XY plane, the positive deflection towards the Z axis is positive, and the negative direction is negative. According to the definition of the reference zero position of the two-dimensional turntable, the X, Y, and Z three-axis components x _SG , y _SG , z of the relative position coordinate SG _i of the fixed-end target obtained in step (5) in the equipment coordinate system of the two-dimensional turntable _SG can get the azimuth and pitch angles of the two-dimensional turntable as follows: the azimuth is positive when the right-handed spiral around the Z axis is positive, the pitch axis is positive when the positive direction of the Z axis is positive, and the range of azimuth and pitch angles is between -90 and +90 degrees. Angle and pitch angle are the pointing angles required for the laser to point to the target at the fixed end, and the calculation formula is shown in formula (7): $\mathrm{<span\; class="notranslate">\; the\; tan</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; A</span>}}_{\mathrm{<span\; class="notranslate">\; S</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}<span\; class="notranslate">\; )</span>$ $\mathrm{<span\; class="notranslate">\; the\; tan</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; E.</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\frac{{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}}{\sqrt{{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; SG</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$ (7) point to the target The above-mentioned steps are all calculated by a PC, and the azimuth and pitch angle values obtained in step (6) pointing to the target are input into the MC600 electric control box in real time, and the MC600 electric control box is used to control the corresponding angle of rotation of the two-dimensional turntable, and the two-dimensional turntable The laser on the laser will quickly point to the fixed-end target from the initial position, and point to the fixed-end target in real time with the movement of the mobile end, that is, the rapid dynamic pointing of the laser to the fixed-end target is completed.