A servo mechanism for probing

Abstract

The application discloses a servo mechanism for detection, comprising: a collection module, which is used for collecting rate data of an optical cabin in a rolling direction and a direction of azimuth, and collecting speed data of the optical cabin in a pitching direction; an integration module, which is used for calculating azimuth coordinates and pitching coordinates of a two-axis turntable relative to a geodetic coordinate system based on current pitching angle, rolling angle and azimuth angle of an unmanned ship output by an inertial navigation equipment of the unmanned ship, and frame angle information of the optical cabin in the pitching direction, the rolling direction and the azimuth direction obtained from a rolling code disc and a pitching code disc; a calculation module, which is used for calculating actual three-axis frame angle information of the optical cabin in the azimuth direction, the pitching direction and the rolling direction; and a compensation module, which is used for respectively differentiating the three-axis frame angle information and performing drift compensation of gyro data. The application realizes three-axis stabilization by using a two-axis stabilized platform, and meanwhile, performance indexes of the optical cabin remain unchanged. Hardware cost can be saved, and redesign time can be reduced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of servo control, in particular to a servo mechanism for detection. BACKGROUND

[0002] Unmanned super high-speed warship is a kind of unmanned warship, mainly including unmanned surface warship and unmanned underwater vehicle, and is mainly used for executing dangerous and unsuitable tasks for manned ships.

[0003] Since the 1990s, the progress of artificial intelligence and automation technology has brought the unmanned warship on water and underwater into a real active period, and more complex underwater robots and surface robots have also gone from concept to reality. The unmanned warships of western countries mostly perform reconnaissance tasks, and the modified versions with attack capabilities have also begun to be deployed.

[0004] Unmanned super high-speed warship has high speed and good maneuverability, and requires the optical cabin installed on the unmanned super high-speed warship to have the advantages of light weight, large field of view, and good stability.

[0005] At present, the photoelectric detection optical cabin installed on the warship is mostly a two-axis stable platform, and in most cases, due to the actual scene needs, the function of a three-axis stable platform needs to be realized, but for the optical cabin of the already deployed warship, it is difficult to ensure the performance indicators of the original optical cabin due to the huge cost of redesigning a three-axis stable platform. SUMMARY

[0006] Therefore, the present application provides a servo mechanism for detection, which can solve the technical problem of realizing three-axis stabilization of a two-axis turntable of an optical cabin.

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

[0008] A servo mechanism for detection of a two-axis turntable of an optical cabin of an unmanned warship realizing three-axis stabilization, comprising:

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

[0010] The integration module is configured to solve the azimuth coordinate and the pitching coordinate of the two-axis turntable to the geodetic coordinate system based on the current pitching angle, rolling angle, and azimuth angle of the unmanned warship output by the inertial navigation equipment of the unmanned warship, and the pitching, rolling, and azimuth frame angle information of the optical cabin obtained from the rolling code disc and the pitching code disc; wherein the pitching frame angle information is obtained by integrating the single-axis gyroscope, and the pitching and rolling frame angle information is obtained by integrating the two-axis gyroscope in the corresponding direction;

[0011] A solving module configured to solve actual azimuth, pitch, and roll three-axis frame angle information of the optical pod;

[0012] A compensation module configured to respectively differentiate the three-axis frame angle information, take a value after differentiating the azimuth frame angle information as an azimuth ring speed value to perform closed loop, and take values after differentiating the pitch frame angle information and the roll frame angle information as compensation speeds to perform drift compensation of pitch and roll stable loop gyro data.

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

[0014] Step S1: configuring a two-axis gyro at a roll position of the two-axis turntable to collect rate data of the optical pod in the roll and azimuth directions, and configuring a single-axis gyro at a pitch position to collect speed data of the optical pod in the pitch direction;

[0015] Step S2: based on current pitch angle, roll angle, and azimuth angle of the unmanned ship output by an inertial navigation device of the unmanned ship, and pitch, roll, and azimuth frame angle information of the optical pod obtained from a roll encoder and a pitch encoder, solving azimuth coordinates and pitch coordinates of the two-axis turntable in a geodetic coordinate system; wherein the pitch frame angle information is obtained by integrating the single-axis gyro, and the pitch and roll frame angle information is obtained by integrating the two-axis gyro in corresponding directions;

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

[0017] Step S4: respectively differentiating the three-axis frame angle information, taking a value after differentiating the azimuth frame angle information as an azimuth ring speed value to perform closed loop, and taking values after differentiating the pitch frame angle information and the roll frame angle information as compensation speeds to perform drift compensation of pitch and roll stable loop gyro data.

[0018] Preferably, the two-axis gyro is a roll gyro, and the single-axis gyro is a pitch gyro.

[0019] Preferably, in the step S1, the collected data are output in the form of angles through the roll encoder and the pitch encoder.

[0020] Preferably, the step S2: based on current pitch angle, roll angle, and azimuth angle of the unmanned ship output by an inertial navigation device of the unmanned ship, and pitch, roll, and azimuth frame angle information of the optical pod obtained from a roll encoder and a pitch encoder, solving azimuth coordinates and pitch coordinates of the two-axis turntable in a geodetic coordinate system; wherein the pitch frame angle information is obtained by integrating the single-axis gyro, and the pitch and roll frame angle information is obtained by integrating the two-axis gyro in corresponding directions, comprises:

[0021] Step S21: Initialize variables based on the current pitch, roll, and azimuth angles of the unmanned vessel output by its inertial navigation system.

[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 surface vessel (USV) output by its inertial navigation system; θ y The current roll angle of the unmanned vessel, output by its inertial navigation system; θ z The current azimuth angle of the unmanned vessel output by its inertial navigation system;

[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 For the pitch frame angle of the optical cabin, φ y For the rolling frame angle of the optical cabin, φ z For the optical cabin heading frame angle; Step S22: Calculate intermediate variables

[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 * (-y0 sin * 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 * (-y0 sin * 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 * (-y0 sin * 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, t111 is less than zero, the azimuth of the two-axis turntable to the earth coordinate system is A d = -3.1415926 / 2.0;

[0054] Step S24: judging the sign of t111 and t211, the azimuth and the elevation of the two-axis turntable to the earth coordinate system are solved

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

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

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

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

[0059] E d = asin(t311).

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

[0061] Step S31: initializing variables

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

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

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

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

[0066] z sin = sin(θ z )

[0067] z cos = cos(θ z )

[0068] θ x is the current pitch angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle; θ y is the current roll angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle; θ z is the current yaw angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle;

[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] R sin = sin(γ)

[0074] R cos = cos(γ)

[0075] α d is the heading boresight to geodetic geographic coordinate information of the unmanned surface vehicle; εd is the pitch boresight to geodetic geographic coordinate information of the unmanned surface vehicle; γ is the roll angle output by the optical cabin roll encoder;

[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'22 * E d cos * A d cos + t'23 * E d sin;

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

[0089] Step S32: the actual roll frame angle information of the optical cabin is zero, i.e.

[0090] γ = 0;

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

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

[0093] E_end = arctan (t'311 / t'211), which is an 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, the step S4 comprises: using a PID double loop control algorithm to set two loops, namely an azimuth loop, a pitch and roll stable loop; and configuring the roll gyroscope output and the pitch gyroscope output with the differential values of the obtained roll frame angle information and the pitch frame angle information in the inner loop of the pitch and roll stable loop, for correction of the optical axis drift.

[0097] A computer readable storage medium, the storage medium has a plurality of instructions stored therein; the plurality of instructions are used for loading and executing the method as described above by a processor.

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

[0099] A processor for executing a plurality of instructions;

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

[0101] The plurality of instructions are used for storing in the memory and loading and executing the method as described above by the processor.

[0102] The beneficial technical effects brought by the present application are:

[0103] (1) The present application improves the existing hardware platform, i.e. the two-axis turntable of the optical cabin of the unmanned ship, applies the two-axis stable platform to realize the function of three-axis stabilization, and maintains the performance indicators of the optical cabin unchanged. It can save hardware cost and reduce redesign time.

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

[0105] (3) The present application solves the problem of optical axis drift of the optical cabin on the unmanned ship, realizes the solution of the optical cabin drift problem, and meets the performance indicator requirements of the product.

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

[0107] Figure 1 The structure diagram of the two-axis turntable of the optical cabin of the unmanned ship of the present application realizing the three-axis stable detection servo mechanism.

[0108] Figure 2 The method flowchart of the two-axis turntable of the optical cabin of the unmanned ship of the present application realizing the three-axis stabilization. DETAILED DESCRIPTION

[0109] The present application will be described in detail below in combination with the drawings and examples.

[0110] AsFigure 1 As shown, this invention proposes a servo mechanism for three-axis stable detection using a two-axis turntable in the optical cabin of an unmanned surface vessel, comprising:

[0111] The acquisition module is configured to have a dual-axis gyroscope at the rolling position of the two-axis turntable to acquire the speed data of the optical cabin in the rolling and azimuth directions, and a single-axis gyroscope at the pitch position to acquire the speed data of the optical cabin in the pitch direction.

[0112] Integration module: Configured to calculate the azimuth and pitch coordinates of the two-axis turntable relative to the geodetic coordinate system based on the current pitch, roll, and azimuth angles of the unmanned vessel output by the unmanned vessel's inertial navigation equipment, as well as the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the roll encoder and pitch encoder; wherein, the pitch frame angle information is obtained by integration of a single-axis gyroscope, and the pitch and roll frame angle information is obtained by integration of a dual-axis gyroscope in the corresponding directions;

[0113] Solution module: configured to solve the actual azimuth, pitch, and roll three-axis frame angle information 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, and use the differentiated values ​​of the pitch frame angle information and the differentiated values ​​of the roll frame angle information as compensation velocities, as drift compensation for the pitch and roll stabilization loop gyroscope data.

[0115] like Figure 2 As shown, this invention proposes a method for achieving three-axis stabilization using a two-axis turntable in the optical cabin of an unmanned surface vessel. The method includes:

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

[0117] Step S2: Based on the current pitch, roll, and azimuth angles of the unmanned vessel output by the unmanned vessel's inertial navigation equipment, and the pitch, roll, and azimuth frame angle information of the optical cabin obtained from the roll encoder and pitch encoder, calculate the azimuth and pitch coordinates of the two-axis turntable relative to the geodetic coordinate system; wherein, the pitch frame angle information is obtained by integration of a single-axis gyroscope, and the pitch and roll frame angle information is obtained by integration of a dual-axis gyroscope in the corresponding directions;

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

[0119] Step S4: differentiating the three-axis frame angle information respectively, taking the differentiated value of the azimuth frame angle information as an azimuth ring velocity value for closed loop, and taking the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation velocities as drift compensation of pitch and roll stable loop gyro data.

[0120] Further, the two-axis gyro is a roll gyro, and the single-axis gyro is a pitch gyro.

[0121] The step S1, wherein the collected data are output in the form of angles through the roll code disc and the pitch code disc respectively.

[0122] The step S2: based on the current pitch angle, roll angle and azimuth angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship, and the optical cabin pitch, roll and azimuth frame angle information obtained from the roll code disc and the pitch code disc, the azimuth coordinate and the pitch coordinate 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 gyro, and the pitch and roll frame angle information are obtained by integrating the two-axis gyro in the corresponding direction, including:

[0123] Step S21: based on the current pitch angle, roll angle and azimuth angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship, performing variable initialization

[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 is the current pitch angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship; θ y is the current roll angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship; θ z is the current azimuth angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship;

[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 optical pod pitch frame angle, φ y is the optical pod roll frame angle, and φ z is the optical pod heading frame angle; step S22: calculating intermediate variables

[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 * (-y0 sin * 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) + t23 * (-y0 sin * z0 sin + y0 cos * x0 sin * z0 cos);

[0150] t311 = t31 * (y0 cos * z0 sin + y0 sin * x0 sin * z0 cos) + t32 * (x0 cos * z0 cos) + t33 * (-y0 sin * 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) + t33 * (-y0 sin * z0 sin + y0 cos * x0 sin * z0 cos);

[0152] t311 = t31 * (y0 cos * z0 sin + y0 sin * x0 sin * z0 cos) + t32 * (x0 cos * z0 cos) + t33 * (-y0 sin * 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 coordinate of the two-axis turntable pair to the geodetic coordinate system is

[0155] A = 3.1415926 / 2.0; d

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

[0157] A = -3.1415926 / 2.0; d

[0158] Step S24: symbol judgment is performed on t111 and t211, and the azimuth coordinate and the pitch coordinate of the two-axis turntable pair to the geodetic coordinate system are solved

[0159] When t211 is greater than zero and t111 is greater than or equal to zero, the azimuth coordinate of the two-axis turntable pair 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 pair 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 coordinate of the two-axis turntable pair to the geodetic coordinate system is 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 pair to the geodetic coordinate system is A d ​​= arctan (t111 * 1 / t211); Step S25: the two-axis turntable to the earth coordinate system of the pitch coordinates E d

[0163] E d = asin (t311).

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

[0165] Step S31: variable initialization

[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 is the current pitch angle of the unmanned ship inertial navigation equipment output by the unmanned ship; θ y is the current roll angle of the unmanned ship inertial navigation equipment output by the unmanned ship; θ z is the current azimuth angle of the unmanned ship inertial navigation equipment output by the unmanned ship;

[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 heading axis of the unmanned ship to the earth; ε d is the geographic coordinate information of the pitch axis of the unmanned ship to the earth; γ is the roll angle output by the optical cabin roll encoder;

[0180] Calculate the 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 roll frame angle information of the optical cabin is zero, i.e.

[0194] γ=0;

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

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

[0197] E_end=arctan(t'311 / t'211), which is an 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: the three-axis frame angle information is respectively differentiated, the differentiated value of the azimuth frame angle information is taken as the azimuth ring speed value for closed loop, and the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information are taken as compensation speed, which is used as drift compensation of the pitch and roll stable loop gyro data.

[0201] Further, a PID double-loop control algorithm is used to set two loops, which are the azimuth loop and the pitch and roll stable loop. The differentiated value of the roll frame angle information and the differentiated value of the pitch frame angle information are respectively configured as the roll gyro output and the pitch gyro output in the inner loop of the pitch and roll stable loop, which are used for correcting the drift of the stable loop optical axis. A direct current 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 for driving the motor according to the control amount output by the controller to correct the optical axis.

[0202] In the application, the optical cabin has a pitch and roll two-axis frame, the azimuth, pitch and roll frame angles provided by the inertial navigation of the unmanned ship and the roll frame angle and the pitch frame angle of the optical cabin itself are applied to calculate the frame angle of the azimuth of the optical cabin, the azimuth frame angle is differentiated to obtain the azimuth speed, so that the speed loop control and the position loop control in the azimuth axis direction can be performed, and the two-axis frame to three-axis control is realized. Meanwhile, the roll and pitch frame angles are calculated, the roll and pitch speed compensation values are obtained by differentiating the frame angles, so that the optical axis drift compensation of the roll axis and the pitch axis is realized.

[0203] The application provides an embodiment of a method for realizing three-axis stabilization of a two-axis turntable of a ship optical cabin.

[0204] Step S1: a two-axis gyro is configured in the rolling position of the two-axis turntable for collecting rate data in the rolling and azimuth axes, and a single-axis gyro is configured in the pitching position for collecting speed data in the pitching axis; the two-axis gyro is a rolling gyro, and the single-axis gyro is a pitching gyro; the angle output values of the rolling code disc and the pitching code disc;

[0205] Step S2: based on the current pitching angle, rolling angle and heading angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship, and the pitching, rolling and heading frame angle information output by the optical cabin position sensor code disc, the heading frame angle at this time is obtained by the integral value of the rolling gyro output in the heading, and the heading coordinate and the pitching coordinate of the two-axis turntable in the geodetic coordinate system are calculated;

[0206] Step S3: based on the current pitching angle, rolling angle and heading angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship, and the heading coordinate and the pitching coordinate of the two-axis turntable in the geodetic coordinate system, the three-axis frame angle information of the azimuth, the pitching and the rolling of the two-axis turntable is calculated;

[0207] Step S4: the three-axis frame angle information is respectively differentiated, the value after the differentiation of the azimuth frame angle is taken as the azimuth ring speed value for closed loop, and the differentiation results of the pitching and rolling frame angles are taken as compensation speed, as the drift compensation of the gyro data of the pitching and rolling stable loop.

[0208] The above specific embodiments only describe the design principles of the present application, and the shapes and names of the components in the description can be different and are not limited. Therefore, the person skilled in the art of the present application can modify or equivalently replace the technical solutions described in the foregoing embodiments; and these modifications and replacements do not deviate from the purpose and technical solution of the present application, and should all belong to the protection scope of the present application.

Claims

1. A method for three-axis stabilization of a two-axis turntable of an unmanned ship optical pod, characterized in that, Comprise: Step S1: a two-axis gyro is configured at the rolling position of the two-axis turntable for collecting rate data of the optical cabin in the rolling and azimuth directions, and a single-axis gyro is configured at the pitching position for collecting speed data of the optical cabin in the pitching direction; wherein the collected data are output in the form of angle through the rolling encoder and the pitching encoder; Step S2: based on the current pitching angle, rolling angle and azimuth angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship, and the pitching, rolling and azimuth frame angle information of the optical cabin obtained from the rolling encoder and the pitching encoder, the azimuth coordinate and the pitching coordinate of the two-axis turntable to the geodetic coordinate system are calculated; wherein the pitching frame angle information is obtained by integrating the single-axis gyro, and the pitching and rolling frame angle information is obtained by integrating the two-axis gyro in the corresponding direction, comprising: Step S21: based on the current pitching angle, rolling angle and azimuth angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship, the variables are initialized xsin = sin(θ x ); xcos = cos(θ x ); ysin = sin(θ y ); ycos = cos(θ y ); zsin = sin(θ z ) zcos = cos(θ z ) θ x is the current pitch angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle; θ y is the current roll angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle; θ z is the current yaw angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle; x0sin = sin(φ x ); x0cos = cos(φ x ); y0sin = sin(φ y ); y0cos = cos(φ y ); z0sin = sin(φ z ); z0cos = cos(φ z ); φ x is the optical bay pitch frame angle, φ y is the optical bay roll frame angle, φ z is the optical bay heading frame angle; step S22: calculating intermediate variables 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+y0 sin*x0 sin*z0cos)+t22*(x0cos*z0cos)+t23*(-y0sin*z0sin+y0cos*x0 sin*z0cos); t311=t31*(y0cos*z0 sin+y0sin*x0sin*z0cos)+t32*(x0cos*z0cos)+t33*(-y0sin*z0sin+y0cos*x0 sin*z0cos); Step S23: set the boundary point 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； 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； Step S24: symbol judgment is performed on t111 and t211, and the azimuth coordinate and the pitching coordinate of the two-axis turntable to the geodetic coordinate system are calculated When t211 is greater than zero, and t111 is greater than or equal to zero, the azimuth of the two-axis turntable to the earth 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 of the two-axis turntable to the earth coordinate system is A d = arctan(t111 * 1 / t211) + π; When t211 is less than zero and t111 is less than zero, the azimuth coordinate of the two-axis turntable to the geodetic coordinate system is Ad=arctan(t111*1 / t211)-π; 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 earth coordinate system is Ad = arctan(t111*1 / t211); Step S25: the two-axis turntable's pitch coordinate E in the geodetic coordinate system is calculated d E d = asin(t311); Step S3: calculating the actual azimuth, pitch and roll three-axis frame angle information of the optical cabin, comprising: Step S31: variable initialization xsin = sin(θ x ); xcos = cos(θ x ); ysin = sin(θ y ); ycos = cos(θ y ); zsin = sin(θ z ) zcos = cos(θ z ) θ x is the current pitch angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle; θ y is the current roll angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle; θ z is the current yaw angle of the unmanned surface vehicle output by the inertial navigation equipment of the unmanned surface vehicle; A d sin = sin(a d ) A d cos = cos(a d ) E d sin = sin(ε d ) E d cos = cos(ε d ) Rsin = sin(gamma) Rcos = cos(gamma) α d is the geographic coordinate information of the heading axis of the unmanned ship to the earth; ε d is the geographic coordinate information of the pitch axis of the unmanned ship to the earth; γ is the roll angle output by the roll code disc of the optical cabin; Intermediate variables are calculated: 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 cos * A d sin + t12 * E d cos * A d cos + t13 * E d sin; t'21 = t'2 * E d cos * A d sin + t'22 * E d cos * A d cos + t'23 * E d sin t'311 = t'31 * E d cos * A d sin + t'32 * E d cos * A d cos + t'33 * E d sin Step S32: the actual roll frame angle information of the optical cabin is zero, that is Gamma = 0; Step S33: calculating the actual azimuth frame angle information and the 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 = gamma; 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; Step S4: differentiating the three-axis frame angle information, taking the differentiated value of the azimuth frame angle information as the azimuth ring speed value for closed loop, and taking the differentiated value of the pitch frame angle information and the differentiated value of the roll frame angle information as compensation speed, as drift compensation of the pitch and roll stable loop gyro data.

2. The method of claim 1, wherein, The two-axis gyro is a roll gyro, and the single-axis gyro is a pitch gyro.

3. The method of any one of claims 1-2, wherein, The step S4 comprises: using a PID double-loop control algorithm to set two loops, which are the azimuth loop, the pitch and roll stable loop; and configuring the differentiated value of the roll frame angle information and the differentiated value of the pitch frame angle information obtained in the inner loop of the pitch and roll stable loop to the roll gyro output and the pitch gyro output, respectively, for correction of the optical axis drift.

4. A two-axis turntable of an optical pod of an unmanned ship for implementing three-axis stabilization of a detection servo mechanism for performing the method according to any one of claims 1 to 3, characterized in that, The detection servo mechanism comprises: A collection module is configured to configure a two-axis gyro at a roll position of the two-axis turntable for collecting rate data of the optical cabin in the roll and azimuth directions, and configure a single-axis gyro at a pitch position for collecting speed data of the optical cabin in the pitch direction; An integration module is configured to calculate the azimuth coordinate and the pitch coordinate of the two-axis turntable to the earth coordinate system based on the current pitch angle, roll angle and azimuth angle of the unmanned ship output by the inertial navigation equipment of the unmanned ship, and the pitch, roll and azimuth frame angle information of the optical cabin obtained from the roll encoder and the pitch encoder; wherein the pitch frame angle information is obtained by integrating the single-axis gyro, and the pitch and roll frame angle information is obtained by integrating the two-axis gyro in the corresponding direction. The solving module is configured to solve actual azimuth, pitch and roll three-axis frame angle information of the optical cabin; The compensation module is configured to respectively differentiate the three-axis frame angle information, take a value after differentiating the azimuth frame angle information as an azimuth ring speed value to perform closed loop, and take values after differentiating the pitch frame angle information and the roll frame angle information as compensation speeds to perform drift compensation on pitch and roll stable loop gyro data. 5.A computer readable storage medium, the storage medium storing a plurality of instructions; the plurality of instructions are used to be loaded and executed by a processor to perform the method of any one of claims 1-3.

6. An electronic device, comprising: The electronic device comprises: a processor for executing a plurality of instructions; a memory for storing a plurality of instructions; wherein the plurality of instructions are used to be stored by the memory, and loaded and executed by the processor to perform the method of any one of claims 1-3.

Images