Servo mechanism for detection

Abstract

The invention discloses a servo mechanism for detection, and the mechanism comprises a collection module which is used for collecting the speed data of an optical cabin in the rolling and azimuth directions, and collecting the speed data of the optical cabin in the pitching axial direction; the integration module is used for resolving azimuth coordinates and pitching coordinates of the two-axis turntable to a geodetic coordinate system based on the current pitching angle, rolling angle and azimuth angle of the unmanned ship output by the unmanned ship inertial navigation equipment and pitching, rolling and azimuth frame angle information of the optical cabin obtained from the rolling coded disc and the pitching coded disc; the resolving module is used for resolving the actual three-axis frame angle information of orientation, pitching and rolling of the optical cabin; and the compensation module is used for respectively differentiating the three-axis frame angle information and carrying out drift compensation on the gyroscope data. According to the invention, a two-axis stabilization platform is used to realize three-axis stabilization, and performance indexes of the optical cabin are kept unchanged. The hardware cost can be saved, and the redesign time is shortened.

Description

Technical Field

[0001] The present invention relates to the technical field of servo control, and in particular to a servo mechanism for detection. Background Art

[0002] Unmanned super-high-speed vessels are unmanned vessels, mainly including unmanned surface vessels and unmanned submarines. They are mainly used to perform dangerous tasks that are not suitable for manned ships.

[0003] Since the 1990s, the advancement of artificial intelligence and automation technology has ushered in a truly active period for unmanned ships on the surface and underwater, and more complex underwater and surface robots have also moved from concept to reality. Most unmanned ships in Western countries are used for reconnaissance missions, and modified versions with attack capabilities have also begun to be deployed.

[0004] Unmanned ultra-high-speed ships have fast travel speed and good maneuverability, which requires that the optical cabin installed on the unmanned ultra-high-speed ships has the advantages of light weight, large field of view, and good stability.

[0005] At present, most of the optoelectronic detection optical cabins installed on ships are two-axis stabilized platforms. In most cases, due to the needs of actual scenarios, it is necessary to realize the function of a three-axis stabilized platform. However, for the optical cabins that have been installed on ships, redesigning the three-axis stabilized platform is extremely expensive and it is difficult to guarantee the performance indicators of the original optical cabin. Summary of the invention

[0006] In view of this, the present invention provides a servo mechanism for detection, which can solve the technical problem of achieving three-axis stability of a two-axis turntable in an optical cabin.

[0007] In order to solve the above technical problems, the present invention is implemented as follows.

[0008] A two-axis turntable of an unmanned ship optical cabin realizes a three-axis stable detection servo mechanism, comprising:

[0009] The acquisition module is configured to configure a dual-axis gyroscope at the rolling position of the two-axis turntable to collect the rate data of the optical cabin in the rolling and azimuth directions, and a single-axis gyroscope at the pitching position to collect the speed data of the optical cabin in the pitching axis direction;

[0010] Integration module: It is configured to solve the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk; among which, the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction;

[0011] A calculation module: configured to calculate the actual three-axis frame angle information of the azimuth, pitch and roll of the optical cabin;

[0012] Compensation module: configured to differentiate the three-axis frame angle information respectively, use the differentiated value of the azimuth frame angle information as the azimuth loop velocity value for closed loop, use the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation velocity, as drift compensation for the gyro data of the pitch and roll stabilization loops.

[0013] A method for achieving three-axis stabilization of a two-axis turntable in an optical cabin of an unmanned ship, the method comprising:

[0014] Step S1: a dual-axis gyroscope is configured at the rolling position of the two-axis turntable to collect the rate data of the optical cabin in the rolling and azimuth directions, and a single-axis gyroscope is configured at the pitching position to collect the speed data of the optical cabin in the pitching direction;

[0015] Step S2: Based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk, the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system are solved; wherein the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction;

[0016] Step S3: Calculating the actual three-axis frame angle information of the azimuth, pitch and roll of the optical cabin;

[0017] Step S4: Differentiate the three-axis frame angle information respectively, use the differentiated value of the azimuth frame angle information as the azimuth loop velocity value for closed-loop, and use the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation velocity, as drift compensation for the pitch and roll stabilization loop gyro data.

[0018] Preferably, the dual-axis gyroscope is a rolling gyroscope, and the single-axis gyroscope is a pitch gyroscope.

[0019] Preferably, in step S1, the collected data is output in the form of angles via a rolling code disc and a pitch code disc respectively.

[0020] Preferably, step S2: based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk, the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system are solved; wherein the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction, including:

[0021] Step S21: Initialize variables based on the current pitch angle, roll angle, and azimuth angle of the unmanned ship output by the unmanned ship inertial navigation device

[0022] x sin=sin(θ x );

[0023] x cos = cos(θ x );

[0024] y sin=sin(θ y );

[0025] y cos = cos(θ y );

[0026] z sin=sin(θ z )

[0027] z cos=cos(θ z )

[0028] θ x The current pitch angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ y The current rolling angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ z The current azimuth of the unmanned ship output by the unmanned ship inertial navigation equipment;

[0029] x0sin=sin(φ x );

[0030] x0cos=cos(φ x );

[0031] y0sin=sin(φ y );

[0032] y0cos=cos(φ y );

[0033] z0sin=sin(φ z );

[0034] z0cos=cos(φ z );

[0035] φ x is the pitch frame angle of the optical cabin, φ y is the optical cabin rolling frame angle, φ z is the heading frame angle of the optical cabin; Step S22: Calculate the intermediate variable

[0036] t11=z cos*y cos+x sin*y sin*z sin;

[0037] t12 = z sin*x cos;

[0038] t13=z cos*y sin-x sin*y cos*z sin;

[0039] t21=-z sin*y cos+x sin*y sin*z cos;

[0040] t22 = z cos*x cos;

[0041] t23=-z sin*y sin-x sin*y cos*z cos;

[0042] t31 = -y sin*x cos;

[0043] t32 = x sin;

[0044] t33 = x cos*y cos;

[0045] t111=t11*(y0 cos*z0 sin+y0 sin*x0 sin*z0 cos)+t12*(x0 cos*z0 cos)

[0046] +t13*(-y0sin*z0 sin+y0 cos*x0 sin*z0 cos);

[0047] t211=t21*(y0 cos*z0 sin+y0 sin*x0 sin*z0 cos)+t22*(x0 cos*z0 cos)

[0048] +t23*(-y0sin*z0 sin+y0 cos*x0 sin*z0 cos);

[0049] t311=t31*(y0 cos*z0 sin+y0 sin*x0 sin*z0 cos)+t32*(x0 cos*z0 cos)

[0050] +t33*(-y0sin*z0 sin+y0 cos*x0 sin*z0 cos);

[0051] Step S23: Setting boundary points

[0052] When t211 is zero and t111 is greater than zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d =3.1415926 / 2.0;

[0053] When t211 is zero and t111 is less than zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A. d = -3.1415926 / 2.0;

[0054] Step S24: Check the signs of t111 and t211 to obtain the azimuth and elevation coordinates of the two-axis turntable relative to the earth coordinate system.

[0055] When t211 is greater than zero and t111 is greater than or equal to zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d =arctan(t111*1 / t211);

[0056] When t211 is less than zero and t111 is greater than or equal to zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d =arctan(t111*1 / t211)+π;

[0057] When t211 is less than zero and t111 is less than zero, the azimuth coordinates of the two-axis turntable to the geodetic coordinate system are A d =arctan(t111*1 / t211)-π;

[0058] When t211 is greater than zero and t111 is less than or equal to zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d =arctan(t111*1 / t211); Step S25: Calculate the pitch coordinate E of the two-axis turntable to the earth coordinate system d

[0059] E d =asin(t311).

[0060] Step S3: Calculate the actual three-axis frame angle information of the azimuth, pitch, and roll of the optical cabin.

[0061] Step S31: Initialize variables

[0062] x sin=sin(θ x );

[0063] x cos = cos(θ x );

[0064] y sin=sin(θ y );

[0065] y cos = cos(θ y );

[0066] zs in=sin(θ z )

[0067] z cos=cos(θ z )

[0068] θ x The current pitch angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ y The current rolling angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ z The current azimuth of the unmanned ship output by the unmanned ship inertial navigation equipment;

[0069] A d sin=sin(α d )

[0070] A d cos = cos(α d )

[0071] E d sin=sin(ε d )

[0072] E d cos = cos(ε d )

[0073] Rsin=sin(γ)

[0074] Rcos = cos(γ)

[0075] α d is the geodetic coordinate information of the unmanned ship’s heading sight axis to the earth; εd is the geodetic coordinate information of the unmanned ship’s pitch sight axis to the earth; γ is the rolling angle output by the optical cabin’s rolling code disk;

[0076] Calculate intermediate variables:

[0077] t'11=z cos*y cos+x sin*y sin*z sin;

[0078] t'12=-z sin*y cos+x sin*y sin*z cos;

[0079] t'13 = -y sin*x cos;

[0080] t'21 = x cos*z sin;

[0081] t'22 = z cos*x cos;

[0082] t'23 = x sin;

[0083] t'31=y sin*z cos-x sin*y cos*z sin;

[0084] t'32=-y sin*z sin-x sin*y cos*z cos;

[0085] t'33 = x cos*y cos;

[0086] t'111=t11'*E d cos*A d sin+t12'E d cos*A d cos+t13'*E d sin;

[0087] t'211=t'21*E d cos*A d sin+t'22E d cos*A d cos+t'23*E d sin;

[0088] t'311=t'31*E d cos*A d sin+t'32E d cos*A d cos+t'33*E d sin

[0089] Step S32: The actual rolling frame angle information of the optical cabin is zero, that is,

[0090] γ=0;

[0091] Step S33: Calculate the actual azimuth frame angle information and pitch frame angle information of the optical cabin

[0092] A_end=arcsin(t′111), which is the inverse sine;

[0093] E_end=arctan(t′311 / t′211), which is the inverse tangent;

[0094] R_end = γ;

[0095] A_end is the actual azimuth frame angle information of the optical cabin, E_end is the actual pitch frame angle information of the optical cabin, and R_end is the actual roll frame angle information of the optical cabin.

[0096] Preferably, step S4 comprises: using a PID dual-loop control algorithm to set two loops, namely an azimuth loop and a pitch and roll stabilization loop loop; in the inner loops of the pitch and roll stabilization loop loops, respectively configuring a roll gyro output and a pitch gyro output with the obtained value after differentiating the roll frame angle information and the obtained value after differentiating the pitch frame angle information for correcting the optical axis drift.

[0097] A computer-readable storage medium stores a plurality of instructions; the plurality of instructions are used for a processor to load and execute the method as described above.

[0098] An electronic device, characterized in that the electronic device comprises:

[0099] A processor, which is used to execute multiple instructions;

[0100] A memory for storing a plurality of instructions;

[0101] The plurality of instructions are used to be stored by the memory and loaded and executed by the processor to implement the method as described above.

[0102] Beneficial technical effects brought by the present invention:

[0103] (1) The present invention makes technical improvements on the existing hardware platform, i.e., the two-axis turntable of the ship's optical cabin, and uses a two-axis stable platform to achieve the three-axis stable function while keeping the performance indicators of the optical cabin unchanged. This can save hardware costs and reduce redesign time.

[0104] (2) The present invention solves the problem of optical axis drift of the optical cabin configured on the unmanned ship through the speed loop and positioning loop of the optical cabin.

[0105] (3) The present invention solves the problem of optical axis drift of the optical cabin of the unmanned ship, thereby solving the problem of optical cabin drift and meeting the performance index requirements of the product.

[0106] (4) The control method of the present invention is simple and easy to implement. BRIEF DESCRIPTION OF THE DRAWINGS

[0107] Figure 1 This is a schematic diagram of the structure of a servo mechanism for detection that realizes three-axis stability of a two-axis turntable in the optical cabin of an unmanned ship of the present invention.

[0108] Figure 2 The present invention is a flow chart of a method for achieving three-axis stabilization of a two-axis turntable in an optical cabin of an unmanned ship. DETAILED DESCRIPTION

[0109] The present invention is described in detail below in conjunction with the accompanying drawings and embodiments.

[0110] like Figure 1 As shown, the present invention proposes a two-axis turntable for an unmanned ship optical cabin to achieve a three-axis stable detection servo mechanism, comprising:

[0111] The acquisition module is configured to configure a dual-axis gyroscope at the rolling position of the two-axis turntable to collect the rate data of the optical cabin in the rolling and azimuth directions, and a single-axis gyroscope at the pitching position to collect the speed data of the optical cabin in the pitching axis direction;

[0112] Integration module: It is configured to solve the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk; among which, the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction;

[0113] A calculation module: configured to calculate the actual three-axis frame angle information of the azimuth, pitch and roll of the optical cabin;

[0114] Compensation module: configured to differentiate the three-axis frame angle information respectively, use the differentiated value of the azimuth frame angle information as the azimuth loop velocity value for closed loop, use the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation velocity, as drift compensation for the gyro data of the pitch and roll stabilization loops.

[0115] like Figure 2 As shown, the present invention proposes a method for achieving three-axis stabilization of a two-axis turntable in an optical cabin of an unmanned ship, the method comprising:

[0116] Step S1: a dual-axis gyroscope is configured at the rolling position of the two-axis turntable to collect the rate data of the optical cabin in the rolling and azimuth directions, and a single-axis gyroscope is configured at the pitching position to collect the speed data of the optical cabin in the pitching direction;

[0117] Step S2: Based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk, the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system are solved; wherein the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction;

[0118] Step S3: Calculating the actual three-axis frame angle information of the azimuth, pitch and roll of the optical cabin;

[0119] Step S4: Differentiate the three-axis frame angle information respectively, use the differentiated value of the azimuth frame angle information as the azimuth loop velocity value for closed-loop, and use the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation velocity, as drift compensation for the pitch and roll stabilization loop gyro data.

[0120] Furthermore, the dual-axis gyroscope is a rolling gyroscope, and the single-axis gyroscope is a pitch gyroscope.

[0121] In the step S1, the collected data is output in the form of angles through a rolling code disc and a pitch code disc respectively.

[0122] The step S2: based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk, the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system are solved; wherein the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction, including:

[0123] Step S21: Initialize variables based on the current pitch angle, roll angle, and azimuth angle of the unmanned ship output by the unmanned ship inertial navigation device

[0124] x sin=sin(θ x );

[0125] x cos = cos(θ x );

[0126] y sin=sin(θ y );

[0127] y cos = cos(θ y );

[0128] z sin=sin(θ z )

[0129] z cos=cos(θ z )

[0130] θ x The current pitch angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ y The current rolling angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ z The current azimuth of the unmanned ship output by the unmanned ship inertial navigation equipment;

[0131] x0sin=sin(φ x );

[0132] x0cos=cos(φ x );

[0133] y0sin=sin(φ y );

[0134] y0cos=cos(φ y );

[0135] z0sin=sin(φ z );

[0136] z0cos=cos(φ z );

[0137] φ x is the pitch frame angle of the optical cabin, φ y is the optical cabin rolling frame angle, φ z is the heading frame angle of the optical cabin; Step S22: Calculate the intermediate variable

[0138] t11=z cos*y cos+x sin*y sin*z sin;

[0139] t12 = z sin*x cos;

[0140] t13=z cos*y sin-x sin*y cos*z sin;

[0141] t21=-z sin*y cos+x sin*y sin*z cos;

[0142] t22 = z cos*x cos;

[0143] t23=-z sin*y sin-x sin*y cos*z cos;

[0144] t31 = -y sin*x cos;

[0145] t32 = x sin;

[0146] t33 = x cos*y cos;

[0147] t111=t11*(y0 cos*z0 sin+y0 sin*x0 sin*z0 cos)+t12*(x0 cos*z0 cos)

[0148] +t13*(-y0sin*z0 sin+y0 cos*x0 sin*z0 cos);

[0149] t211=t21*(y0 cos*z0 sin+y0 sin*x0 sin*z0 cos)+t22*(x0 cos*z0 cos)

[0150] +t23*(-y0sin*z0 sin+y0 cos*x0 sin*z0 cos);

[0151] t311=t31*(y0 cos*z0 sin+y0 sin*x0 sin*z0 cos)+t32*(x0 cos*z0 cos)

[0152] +t33*(-y0sin*z0 sin+y0 cos*x0 sin*z0 cos);

[0153] Step S23: Setting boundary points

[0154] When t211 is zero and t111 is greater than zero, the azimuth coordinates of the two-axis turntable to the geodetic coordinate system are

[0155] A d =3.1415926 / 2.0;

[0156] When t211 is zero and t111 is less than zero, the azimuth coordinates of the two-axis turntable to the geodetic coordinate system are

[0157] A d = -3.1415926 / 2.0;

[0158] Step S24: Check the signs of t111 and t211 to obtain the azimuth and elevation coordinates of the two-axis turntable relative to the earth coordinate system.

[0159] When t211 is greater than zero and t111 is greater than or equal to zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d =arctan(t111*1 / t211);

[0160] When t211 is less than zero and t111 is greater than or equal to zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d =arctan(t111*1 / t211)+π;

[0161] When t211 is less than zero and t111 is less than zero, the azimuth coordinates of the two-axis turntable to the geodetic coordinate system are A d =arctan(t111*1 / t211)-π;

[0162] When t211 is greater than zero and t111 is less than or equal to zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d=arctan(t111*1 / t211); Step S25: Calculate the pitch coordinate E of the two-axis turntable to the earth coordinate system d

[0163] E d =asin(t311).

[0164] Step S3: Calculate the actual three-axis frame angle information of the azimuth, pitch, and roll of the optical cabin.

[0165] Step S31: Initialize variables

[0166] x sin=sin(θ x );

[0167] x cos = cos(θ x );

[0168] y sin=sin(θ y );

[0169] y cos = cos(θ y );

[0170] z sin=sin(θ z )

[0171] z cos=cos(θ z )

[0172] θ x The current pitch angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ y The current rolling angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ z The current azimuth of the unmanned ship output by the unmanned ship inertial navigation equipment;

[0173] A d sin=sin(α d )

[0174] A d cos = cos(α d )

[0175] E d sin=sin(ε d )

[0176] E d cos = cos(ε d )

[0177] Rsin=sin(γ)

[0178] Rcos = cos(γ)

[0179] α d is the geographic coordinate information of the unmanned ship's heading line of sight to the earth; ε d is the geographic coordinate information of the unmanned ship's pitch and elevation axis relative to the earth; γ is the rolling angle output by the optical cabin's rolling code disk;

[0180] Calculate intermediate variables:

[0181] t'11=z cos*y cos+x sin*y sin*z sin;

[0182] t'12=-z sin*y cos+x sin*y sin*z cos;

[0183] t'13 = -y sin*x cos;

[0184] t'21 = x cos*z sin;

[0185] t'22 = z cos*x cos;

[0186] t'23 = x sin;

[0187] t'31=y sin*z cos-x sin*y cos*z sin;

[0188] t'32=-y sin*z sin-x sin*y cos*z cos;

[0189] t'33 = x cos*y cos;

[0190] t'111=t11'*E d cos*A d sin+t12'E d cos*A d cos+t13'*E d sin;

[0191] t'211=t'21*E d cos*A d sin+t'22E d cos*A d cos+t'23*E d sin;

[0192] t'311=t'31*E d cos*A d sin+t'32E d cos*A d cos+t'33*E d sin

[0193] Step S32: The actual rolling frame angle information of the optical cabin is zero, that is,

[0194] γ=0;

[0195] Step S33: Calculate the actual azimuth frame angle information and pitch frame angle information of the optical cabin

[0196] A_end=arcsin(t′111), which is the inverse sine;

[0197] E_end=arctan(t′311 / t′211), which is the inverse tangent;

[0198] R_end = γ;

[0199] A_end is the actual azimuth frame angle information of the optical cabin, E_end is the actual pitch frame angle information of the optical cabin, and R_end is the actual roll frame angle information of the optical cabin.

[0200] The step S4: respectively differentiate the three-axis frame angle information, use the differentiated value of the azimuth frame angle information as the azimuth loop velocity value for closed-loop, and use the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation velocity, as drift compensation for the pitch and roll stabilization loop gyro data.

[0201] Furthermore, a PID double-loop control algorithm is used to set two loops, namely, an azimuth loop, and a pitch and roll stabilization loop loop. In the inner loops of the pitch and roll stabilization loop loops, the values ​​obtained after the differentiation of the roll frame angle information and the values ​​obtained after the differentiation of the pitch frame angle information are configured as roll gyro outputs and pitch gyro outputs, respectively, for correcting the optical axis drift of the stabilization loop. A DC torque motor is respectively configured on the roll axis and the pitch axis of the control system of the two-axis turntable, which is used to drive the motor according to the control quantity output by the controller to achieve the effect of correcting the optical axis.

[0202] In the present invention, the optical cabin has a two-axis frame of pitch and roll. The azimuth, pitch, and roll frame angles provided by the inertial navigation on the unmanned ship and the roll frame angle and pitch frame angle of the optical cabin itself are used to solve the frame angle of the optical cabin azimuth. The azimuth frame angle is differentiated to obtain the azimuth speed, so that the speed loop control and position loop control on the azimuth axis can be performed, and the two-axis frame is converted to three-axis control. At the same time, the frame angles of roll and pitch are obtained by solving, and the frame angles are differentiated to obtain the speed compensation values ​​of roll and pitch, so that the optical axis drift compensation of the roll axis and the pitch axis is realized.

[0203] The present invention provides an embodiment of a method for achieving three-axis stabilization of a two-axis turntable in a ship's optical cabin.

[0204] Step S1: a dual-axis gyroscope is configured at the rolling position of the two-axis turntable to collect rate data on the rolling and azimuth axes, and a single-axis gyroscope is configured at the pitching position to collect speed data on the pitching axis; the dual-axis gyroscope is a rolling gyroscope, and the single-axis gyroscope is a pitching gyroscope; the angle output values ​​of the rolling code disk and the pitching code disk;

[0205] Step S2: Based on the current pitch angle, roll angle, and heading angle of the unmanned ship output by the unmanned ship inertial navigation device, and the pitch, roll, and heading frame angle information sent by the encoder of the optical cabin's own position sensor, the heading frame angle at this time is obtained by the integral value of the rolling gyro in the heading output, and the heading coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system are solved;

[0206] Step S3: Based on the current pitch angle, roll angle, heading angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the heading coordinates and pitch coordinates of the two-axis turntable to the earth coordinate system; solve the three-axis frame angle information of the azimuth, pitch and roll of the two-axis turntable;

[0207] Step S4: Differentiate the three-axis frame angle information respectively, use the differentiated value of the azimuth frame angle as the azimuth loop velocity value for closed loop, and use the differentiated results of the pitch and roll frame angles as compensation velocities for drift compensation of the gyro data of the pitch and roll stabilization loops.

[0208] The above specific embodiments only describe the design principle of the present invention. The shapes and names of the components in the description may be different and are not limited. Therefore, those skilled in the art in the field of the present invention may modify or replace the technical solutions recorded in the above embodiments; and these modifications and replacements do not deviate from the creative purpose and technical solutions of the present invention and should all fall within the protection scope of the present invention.

Claims

1. A two-axis turntable in an unmanned ship optical cabin realizes a three-axis stable detection servo mechanism, characterized in that: include: The acquisition module is configured to configure a dual-axis gyroscope at the rolling position of the two-axis turntable to collect the rate data of the optical cabin in the rolling and azimuth directions, and a single-axis gyroscope at the pitching position to collect the speed data of the optical cabin in the pitching axis direction; Integration module: It is configured to solve the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk; among which, the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction; A calculation module: configured to calculate the actual three-axis frame angle information of the azimuth, pitch and roll of the optical cabin; Compensation module: configured to differentiate the three-axis frame angle information respectively, use the differentiated value of the azimuth frame angle information as the azimuth loop velocity value for closed loop, use the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation velocity, as drift compensation for the gyro data of the pitch and roll stabilization loops.

2. A method for achieving three-axis stabilization of a two-axis turntable in an unmanned ship optical cabin, characterized in that: include: Step S1: a dual-axis gyroscope is configured at the rolling position of the two-axis turntable to collect the rate data of the optical cabin in the rolling and azimuth directions, and a single-axis gyroscope is configured at the pitching position to collect the speed data of the optical cabin in the pitching direction; Step S2: Based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk, the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system are solved; wherein the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction; Step S3: Calculating the actual three-axis frame angle information of the azimuth, pitch and roll of the optical cabin; Step S4: Differentiate the three-axis frame angle information respectively, use the differentiated value of the azimuth frame angle information as the azimuth loop velocity value for closed-loop, and use the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation velocity, as drift compensation for the pitch and roll stabilization loop gyro data.

3. The method according to claim 2, characterized in that The dual-axis gyroscope is a rolling gyroscope, and the single-axis gyroscope is a pitch gyroscope.

4. The method according to any one of claims 2 to 3, characterized in that: In the step S1, the collected data is output in the form of angles through a rolling code disc and a pitch code disc respectively.

5. The method according to any one of claims 2 to 3, characterized in that: The step S2: based on the current pitch angle, roll angle, azimuth angle of the unmanned ship output by the inertial navigation device of the unmanned ship, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the rolling code disk and the pitch code disk, the azimuth coordinates and pitch coordinates of the two-axis turntable to the geodetic coordinate system are solved; wherein the pitch frame angle information is obtained by integrating the single-axis gyroscope, and the pitch and roll frame angle information is obtained by integrating the dual-axis gyroscope in the corresponding direction, including: Step S21: Initialize variables based on the current pitch angle, roll angle, and azimuth angle of the unmanned ship output by the unmanned ship inertial navigation device <h2 style=";text-align:left;direction:ltr">xsin = sin(θ)<h2 style=";text-align:left;direction:ltr"> x <h2 style=";text-align:left;direction:ltr"> ); xcos=cos(θ x ); ysin=sin(θ y ); ycos=cos(θ y ); zsin=sin(θ z ) zcos=cos(θ z ) θ x The current pitch angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ y The current rolling angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ z The current azimuth of the unmanned ship output by the unmanned ship inertial navigation equipment; x0sin=sin(φ x ); x0cos=cos(φ x ); y0sin=sin(φ y ); y0cos=cos(φ y ); z0sin=sin(φ z ); z0cos=cos(φ z ); φ x is the pitch frame angle of the optical cabin, φ y is the optical cabin rolling frame angle, φ z is the heading frame angle of the optical cabin; Step S22: Calculate the intermediate variable t11=zcos*ycos+xsin*ysin*zsin; t12 = zsin*xcos; t13=zcos*ysin-xsin*ycos*zsin; t21=-zsin*ycos+xsin*ysin*zcos; t22 = zcos*xcos; t23=-zsin*ysin-xsin*ycos*zcos; t31 = -ysin*xcos; t32 = xsin; t33 = xcos*ycos; t111=t11*(y0cos*z0sin+y0sin*x0sin*z0cos)+t12*(x0cos*z0cos) +t13*(-y0sin*z0sin+y0cos*x0sin*z0cos); t211=t21*(y0cos*z0sin+y0sin*x0sin*z0cos)+t22*(x0cos*z0cos) +t23*(-y0sin*z0sin+y0cos*x0sin*z0cos); t311=t31*(y0cos*z0sin+y0sin*x0sin*z0cos)+t32*(x0cos*z0cos) +t33*(-y0sin*z0sin+y0cos*x0sin*z0cos); Step S23: Setting boundary points When t211 is zero and t111 is greater than zero, the azimuth coordinates of the two-axis turntable to the geodetic coordinate system are A d ＝3.1415926 / 2.0； When t211 is zero and t111 is less than zero, the azimuth coordinates of the two-axis turntable to the geodetic coordinate system are A d ＝-3.1415926 / 2.0； Step S24: Check the signs of t111 and t211 to obtain the azimuth and elevation coordinates of the two-axis turntable relative to the earth coordinate system. When t211 is greater than zero and t111 is greater than or equal to zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d =arctan(t111*1 / t211); When t211 is less than zero and t111 is greater than or equal to zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is A d =arctan(t111*1 / t211)+π; When t211 is less than zero and t111 is less than zero, the azimuth coordinates of the two-axis turntable to the geodetic coordinate system are Ad=arctan(t111*1 / t211)-π; When t211 is greater than zero and t111 is less than or equal to zero, the azimuth coordinates of the two-axis turntable to the earth coordinate system are Ad=arctan(t111*1 / t211); Step S25: Calculate the elevation coordinates E of the two-axis turntable relative to the earth coordinate system d It is d = donkey (t311). Step S3: Calculate the actual three-axis frame angle information of the azimuth, pitch, and roll of the optical cabin. Step S31: Initialize variables <h2 style=";text-align:left;direction:ltr">xsin = sin(θ)<h2 style=";text-align:left;direction:ltr"> x <h2 style=";text-align:left;direction:ltr"> ); xcos=cos(θ x ); ysin=sin(θ y ); ycos=cos(θ y ); zsin=sin(θ z ) zcos=cos(θ z ) θ x The current pitch angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ y The current rolling angle of the unmanned ship output by the unmanned ship inertial navigation equipment; θ z The current azimuth of the unmanned ship output by the unmanned ship inertial navigation equipment; A d sin=sin(a d ) A d cos=cos(α) d ) E d sin=sin(ε d ) AND d cos=cos(ε d ) Rsin=sin(γ) Rcos = cos(γ) α d is the geographic coordinate information of the unmanned ship's heading line of sight to the earth; ε d is the geographic coordinate information of the unmanned ship's pitch and elevation axis relative to the earth; γ is the rolling angle output by the optical cabin's rolling code disk; Calculate intermediate variables: t'11=zcos*ycos+xsin*ysin*zsin; t'12=-zsin*ycos+xsin*ysin*zcos; t'13 = -ysin*xcos; t'21 = xcos*zsin; t'22 = zcos*xcos; t'23 = xsin; t'31=ysin*zcos-xsin*ycos*zsin; t'32=-ysin*zsin-xsin*ycos*zcos; t'33 = xcos*ycos; t'111=t11'*E d What d sin+t12'E d What d cos+t13'*E d yes; t'211=t'21*E d cos*A d sin+t'22E d cos*A d cos+t'23*E d sin; t'311=t'31*E d cos*A d sin+t'32E d cos*A d cos+t'33*E d sin Step S32: The actual rolling frame angle information of the optical cabin is zero, that is, γ=0; Step S33: Calculate the actual azimuth frame angle information and pitch frame angle information of the optical cabin A_end=arcsin(t′111), which is the inverse sine; E_end=arctan(t′311 / t′211), which is the inverse tangent; R_end = γ; A_end is the actual azimuth frame angle information of the optical cabin, E_end is the actual pitch frame angle information of the optical cabin, and R_end is the actual roll frame angle information of the optical cabin.

6. The method according to any one of claims 2 to 3, characterized in that: The step S4 comprises: using a PID dual-loop control algorithm to set two loops, namely an azimuth loop and a pitch and roll stabilization loop loop; configuring a roll gyro output and a pitch gyro output with the obtained values ​​after differentiation of the roll frame angle information and the obtained values ​​after differentiation of the pitch frame angle information in the inner loops of the pitch and roll stabilization loop loops, respectively, for correcting optical axis drift.

7. A computer-readable storage medium, wherein a plurality of instructions are stored in the storage medium; the plurality of instructions are used for a processor to load and execute the method as claimed in any one of claims 2 to 6.

8. An electronic device, characterized in that: The electronic device comprises: A processor, which is used to execute multiple instructions; A memory for storing a plurality of instructions; The plurality of instructions are used to be stored in the memory and loaded and executed by the processor according to any one of claims 2 to 6.

Images