Vehicle-mounted two-axis follow-up control method based on asymmetric cross coupling

Abstract

The invention discloses a vehicle-mounted two-axis follow-up control method based on asymmetric cross coupling. The method comprises the steps that the azimuth direction disturbance angular rate and the pitching direction disturbance angular rate of a two-axis rotary table are collected in real time; obtaining the filtered disturbance angle rates in the azimuth direction and the pitching direction by adopting a filter; an integrator is adopted to obtain disturbance angles in the azimuth direction and the pitching direction; constructing an instruction planning controller to obtain rotation angles in the azimuth direction and the pitching direction; acquiring a compensation coefficient of a pitch axis based on the target trajectory curve and constructing a cross coupling controller; an azimuth direction and pitching direction position controller is constructed, and azimuth direction and pitching direction speed control instructions are obtained; correspondingly obtaining azimuth direction and pitching direction disturbance compensation angular velocity according to the filtered azimuth direction and pitching direction disturbance angular velocity; and performing decoupling compensation on an azimuth axis speed input instruction and a pitch axis speed input instruction by adopting an asymmetric cross coupling strategy to realize following motion of a target trajectory curve. Position deviation caused by disturbance can be effectively compensated, the synchronization performance of the two shafts is improved, and the dynamic response capability and stability are good.

Description

Technical Field

[0001] The invention belongs to the technical field of multi-axis follow-up control, and in particular relates to a vehicle-mounted two-axis follow-up control method based on asymmetric cross coupling. Background Art

[0002] A two-axis turntable is a precision mechanical device that can achieve rotational motion on two mutually perpendicular axes. These two axes are usually called the azimuth axis and the elevation axis, corresponding to horizontal and vertical rotations respectively.

[0003] The two-axis turntable is a precision rotating device designed to be mounted on a vehicle and is designed to support and manipulate various types of sensors, cameras, antennas, weapon systems, and other equipment. It can provide stable and precise aiming, tracking, and smooth operation during vehicle movement, ensuring that these devices can effectively perform their functions. This type of turntable is widely used in military, security monitoring, emergency response, scientific research, and other fields.

[0004] The vehicle-mounted two-axis tracking system is introduced into the design of the two-axis turntable as a key active control system. The system ensures that the load can maintain a stable posture under various dynamic conditions and can quickly follow instructions by accurately controlling the relative motion between two key axes (usually the pitch axis and the azimuth axis).

[0005] In this type of vehicle-mounted two-axis servo system, tracking accuracy and anti-interference ability are important indicators to measure its performance, and are usually used to evaluate the dynamic response ability and stability of the servo system. In this type of vehicle-mounted two-axis system, load equipment is generally installed on the pitch axis, such as optical aiming equipment, weapon equipment, etc. The test method for this type of equipment is to send a preset target curve command through the host computer, control the load equipment to perform precise curve movement, and simultaneously apply disturbances to the base of the two-axis platform. This method is used to comprehensively evaluate the response speed and stability of the vehicle-mounted two-axis servo system to ensure that it can still maintain high performance under complex dynamic conditions.

[0006] There are two main problems that need to be solved in this type of test. The first problem is that the laser pointer on the pitch axis needs to keep its pointing stable under disturbance. The traditional approach is to install an inertial measurement unit (IMU) on the vehicle body, measure the attitude angle of the vehicle body and combine it with coordinate transformation to achieve the stability of the laser pointer's pointing in the inertial space. Although this method can provide a certain degree of stability in some application scenarios, it has obvious shortcomings in applications with high precision and real-time requirements. The communication cycle of traditional IMUs is usually limited to a few hundred hertz (Hz), which is particularly insufficient in high-precision control systems that require millisecond-level responses. The low data update rate will cause feedback lag in the control system, which in turn affects the overall control effect and stability. In order to convert the attitude information in the vehicle body coordinate system into the pointing in the inertial space, the traditional method relies on complex floating-point matrix operations. This operation not only increases the processor load, but also significantly prolongs the execution time of the control algorithm, making it difficult to meet application scenarios with extremely high real-time requirements.

[0007] In addition, for the first problem, there is a conventional approach of using a rate gyro to integrate the disturbance angular rate into an angle and compensate for it. Although this method provides a relatively simple and intuitive way to perform attitude compensation and can be effective in some scenarios, its inherent integration delay and cumulative error problems limit its performance under high precision and fast response requirements. The second problem is how to effectively reduce the contour error during the coordinated motion of the two axes. The traditional method is to use a two-axis synchronization algorithm, such as cross-coupling control, to reduce the contour error. Conventional cross-coupling algorithms do not perform well when processing complex trajectories (such as sinusoidal curves), especially at key points of the trajectory (such as crests and troughs), where large contour errors are prone to occur. This not only affects the tracking accuracy of the system, but may also cause the system to respond in a not fast and smooth manner. Therefore, a vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling is proposed. Summary of the invention

[0008] The purpose of the present invention is to address the above problems and to propose a vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling, which can improve the dynamic response capability and stability of the two-axis turntable.

[0009] To achieve the above object, the technical solution adopted by the present invention is:

[0010] The present invention proposes a vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling, which is applied to a two-axis turntable. The two-axis turntable is installed on a vehicle body and is used to drive a load to rotate around an azimuth axis and a pitch axis. The azimuth axis and the pitch axis are perpendicular. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling includes:

[0011] S1, real-time acquisition of the azimuth disturbance angular rate and the pitch disturbance angular rate of the two-axis turntable;

[0012] S2, input the output signal of the first filter at the previous moment and the azimuth disturbance angular rate, and the azimuth disturbance angular rate at the current moment into the first filter for filtering, and obtain the filtered azimuth disturbance angular rate ω at the current moment yf , the output signal of the second filter at the previous moment and the pitch disturbance angular rate, and the pitch disturbance angular rate at the current moment are input into the second filter for filtering, and the filtered pitch disturbance angular rate ω at the current moment is obtained accordingly. pf ;

[0013] S3, the azimuth disturbance angular rate and the pitch disturbance angular rate from time zero to the current time are integrated by an integrator, and the azimuth disturbance angle θ at the current time is obtained accordingly. yt and the pitch disturbance angle θ pt ;

[0014] S4. Construct an instruction planning controller, which is used to obtain the azimuth rotation angle of the two-axis turntable at the current moment according to the target trajectory curve. and pitch rotation angle β;

[0015] S5. Obtain the compensation coefficient K of the pitch axis at the current moment based on the target trajectory curve cy and build cross-coupled controllers;

[0016] S6. Construct an azimuth position controller and a pitch position controller, and rotate the current azimuth angle Azimuth disturbance angle θ yt The azimuth angular displacement is used as the input of the azimuth position controller to obtain the azimuth speed control instruction ω at the current moment. yu , the pitch rotation angle β and the pitch disturbance angle θ at the current moment pt The pitch angular displacement is used as the input of the pitch position controller to obtain the pitch velocity control instruction ω at the current moment. pu ;

[0017] S7, according to the filtered azimuth disturbance angular rate ω at the current moment yf and the filtered pitch disturbance angular rate ω pf , corresponding to the azimuth disturbance compensation angular velocity ω at the current moment yc and the pitch disturbance compensation angular velocity ω pc ;

[0018] S8, using an asymmetric cross-coupling strategy to decouple and compensate the azimuth axis velocity input command and the pitch axis velocity input command at the current moment to achieve the following motion of the target trajectory curve. The asymmetric cross-coupling strategy is to decouple the azimuth axis decoupling compensation amount ω ys As the azimuth axis speed input command, the pitch axis decoupling compensation amount ω ps As the pitch axis speed input command, where:

[0019] Azimuth axis decoupling compensation ω ys , the formula is as follows:

[0020] ω ys =ω yu -ω yc +C x *u

[0021] Pitch axis decoupling compensation ω ps , the formula is as follows:

[0022] ω ps =ω pu -ω pc +K cy *u

[0023] In the formula, C x is the azimuthal cross-coupling factor, and u is the output of the cross-coupling controller.

[0024] Preferably, the two-axis turntable includes a carrier, an azimuth seat, a pitch bracket, a pitch seat, a first fiber optic gyroscope, a second fiber optic gyroscope, a first motor, a second motor, a first driver and a second driver. The carrier is connected to the vehicle body, the first motor is mounted on the carrier and is used to drive the azimuth seat to rotate around the azimuth axis, the pitch bracket is connected to the azimuth seat, the second motor is mounted on the pitch bracket and is used to drive the pitch seat to rotate around the pitch axis, the first fiber optic gyroscope is connected to the carrier, the second fiber optic gyroscope is connected to the pitch bracket, the first driver is electrically connected to the first motor, the second driver is electrically connected to the second motor, the detection result of the first fiber optic gyroscope is the azimuth axis disturbance angular velocity, and the detection result of the second fiber optic gyroscope is the pitch axis disturbance angular velocity.

[0025] Preferably, the first filter and the second filter are both bilinear low-pass filters, and perform the following operations:

[0026] S21. Use a first-order Butterworth low-pass filter to establish the analog transfer function H(s), the formula is as follows:

[0027]

[0028] In the formula, ω c is the cut-off angular frequency, and ω c =2πf c , fc is the cut-off frequency, s is the first complex variable;

[0029] S22, calculate frequency predistortion ω a , the formula is as follows:

[0030]

[0031] In the formula, ω d is the ideal discrete time angular frequency, and ω d =2πf c , T is the sampling period;

[0032] S23, replace the cutoff angular frequency ω in the analog transfer function H(s) c is the frequency predistortion ω a , the first complex variable s is After completing the bilinear transformation, the discrete transfer function H(z) is obtained, and the formula is as follows:

[0033]

[0034] Where z is the second complex variable;

[0035] S24. Establish the difference equation:

[0036] Let the first intermediate variable Then the discrete transfer function H(z) is expressed as:

[0037]

[0038] because Among them, X(z) is the input signal after z transformation, and Y(z) is the output signal after z transformation, so:

[0039]

[0040] Then perform the z inverse transform, that is, replace Y(z) with y[t], replace X(z) with x[t], and replace X(z) with x(t-1) -1 , y(t-1) replaces Y(z)z -1 , we can get the difference equation, the formula is as follows:

[0041]

[0042] Where y[t] is the output signal of the filter at time t, y[t-1] is the output signal of the filter at time t-1, x[t] is the input signal of the filter at time t, x[t-1] is the input signal of the filter at time t-1, and z -1 is the second complex variable z raised to the power of -1;

[0043] S25, respectively obtaining the filtered azimuth disturbance angular rate and the filtered pitch disturbance angular rate at the current moment according to the differential equation as output signals of the corresponding filters, wherein:

[0044] The filtered azimuth disturbance angular rate ω at time t yf [t], the formula is as follows:

[0045]

[0046] The filtered pitch disturbance angular rate ω at time t pf [t], the formula is as follows:

[0047]

[0048] In the formula, ω yf [t-1] is the filtered azimuth disturbance angular rate at time t-1, ω yt [t] is the azimuth disturbance angular velocity at time t, ω yt [t-1] is the azimuth disturbance angular rate at time t-1, ω pf [t-1] is the filtered pitch disturbance angular rate at time t-1, ω pt [t] is the pitch disturbance angular rate at time t, ω pt [t-1] is the pitch disturbance angular rate at time t-1.

[0049] Preferably, the azimuth disturbance angular rate and the pitch disturbance angular rate from time zero to the current time are integrated by an integrator, and the azimuth disturbance angle θ at the current time is obtained accordingly. yt and the pitch disturbance angle θ pt , as follows:

[0050] The azimuth disturbance angle θ at time t yt (t), the formula is as follows:

[0051]

[0052] The pitch disturbance angle θ at time t pt (t), the formula is as follows:

[0053]

[0054] In the formula, ω yt [n] is the azimuth disturbance angular velocity at time n, ω pt [n] is the pitch disturbance angular rate at time n, n = 0 ~ t, and f is the sampling frequency.

[0055] Preferably, the instruction planning controller performs the following operations:

[0056] S41, establishing a target coordinate system o1x1y1 on the projection plane where the target trajectory curve is located, the origin o1 of the target coordinate system is the point on the projection plane to which the load of the two-axis turntable points in the initial state, with the vertical upward axis being the y1 axis and the horizontal rightward axis being the x1 axis;

[0057] S42, sampling the target trajectory curve using a fixed period sampling method to obtain a number of sampling points and the coordinates of the corresponding sampling points;

[0058] S43, according to the coordinates of the current sampling point P, solve the azimuth rotation angle at the current moment And the pitch rotation angle β, the formula is as follows:

[0059]

[0060] l′ 2 =x 2 +l 2

[0061] Wherein, l is the distance between the load and the projection plane and is a constant, l′ is the second intermediate variable, (x0, y0) is the coordinate of the current sampling point P, x0 is the abscissa of the current sampling point P, y0 is the ordinate of the current sampling point P, and the current moment is the time to reach the current sampling point P, and the current sampling point P is also the target point at the current moment.

[0062] Preferably, according to the filtered azimuth disturbance angular rate ω at the current moment yf and the filtered pitch disturbance angular rate ω pf , corresponding to the azimuth disturbance compensation angular velocity ω at the current moment yc and the pitch disturbance compensation angular velocity ω pc ,in:

[0063] The azimuth disturbance compensation angular velocity ω at the current moment yc , the formula is as follows:

[0064] ω yc =K u1 *ω yf

[0065]

[0066] In the formula, K u1 is the azimuth angular rate compensation coefficient, R y is the gear ratio of the first motor, K1 is the azimuth compensation proportional control coefficient, the unit of the azimuth disturbance compensation angular velocity is 0.0001rps, and the unit of the filtered azimuth disturbance angular velocity is ° / s;

[0067] The current pitch disturbance compensation angular velocity ω pc , the formula is as follows:

[0068] ω pc =K u2 *ω pf

[0069]

[0070] In the formula, K u2 is the pitch angular rate compensation coefficient, R p is the gear ratio of the second motor, K2 is the pitch compensation proportional control coefficient, the unit of the pitch disturbance compensation angular velocity is 0.0001rps, and the unit of the filtered pitch disturbance angular velocity is ° / s.

[0071] Preferably, the cross-coupling controller construction process is as follows:

[0072] S51, calculate the contour error ε at the current moment, the formula is as follows:

[0073] ε=-sinθ*e y +cosθ*e p

[0074] Where ε is the actual arrival point P at the current moment r To the target point P t The distance between the secants, e y is the azimuth tracking error, e p is the pitch tracking error, θ is the target point P at the current moment t The angle between the secant line of and the horizontal axis of the target coordinate system;

[0075] S52, assuming ε = -C x *e y +C y *e p , and based on the formula in step S51, the mathematical expression of the cross-coupling factor is obtained:

[0076] C x = sinθ,C y = cosθ

[0077] In the formula, the cross-coupling factor includes the azimuthal cross-coupling factor C x and the pitch cross-coupling factor C y ;

[0078] S53, obtaining the output of the cross-coupling controller at the corresponding time according to the cross-coupling control algorithm, the cross-coupling control algorithm adopts the PID control algorithm, and the mathematical expression is as follows:

[0079]

[0080] Where u(t) is the output of the cross-coupling controller at time t, ε(t) is the input of the cross-coupling controller at time t, that is, the contour error at time t. Similarly, ε(i) is the input of the cross-coupling controller at time i, i = 0 ~ t-1, ε(t-1) is the input of the cross-coupling controller at time t-1, k p is the cross-coupled controller proportional coefficient, k i is the cross-coupled controller integral coefficient, k d is the differential coefficient of the cross-coupled controller.

[0081] Preferably, the compensation coefficient K of the pitch axis at the current moment is obtained based on the target trajectory curve cy , the formula is as follows:

[0082]

[0083] In the formula, y t is the target point P at time t t The vertical coordinate, y t-1 is the target point P at time t-1 t-1 The vertical coordinate, H is the proportional adjustment coefficient, and the target point P at time t t and the target point P at time t-1 t-1 are all points on the target trajectory curve.

[0084] Preferably, both the azimuth position controller and the pitch position controller are PID controllers.

[0085] Preferably, the current orientation is rotated by an angle Azimuth disturbance angle θ yt The azimuth angular displacement is used as the input of the azimuth position controller to obtain the azimuth speed control instruction ω at the current moment. yu , the pitch rotation angle β and the pitch disturbance angle θ at the current moment pt The pitch angular displacement is used as the input of the pitch position controller to obtain the pitch velocity control instruction ω at the current moment. pu , the formula is as follows:

[0086] ω yu =K yp *K yu (θ yr -θ yt )-K yp *E y

[0087] ω pu =K pp *K pu (θpr -θ pt )-K pp *E p

[0088] In the formula, K yp is the proportional control parameter of the azimuth position controller, K yu K is the conversion factor from azimuth rotation angle units to azimuth angular displacement units. pp is the proportional control parameter of the pitch position controller, K pu is the conversion coefficient from pitch rotation angle units to pitch angular displacement units, θ yr The output command of the command planning controller at the current moment, that is, the azimuth rotation angle at the current moment E y is the azimuth angular displacement, obtained by the encoder feedback of the first motor, θ pr The output command of the command planning controller at the current moment, that is, the pitch rotation angle β at the current moment, E p It is the pitch angular displacement, obtained by the encoder feedback of the second motor.

[0089] Compared with the prior art, the present invention has the following beneficial effects:

[0090] This method uses a fiber optic gyroscope to sense the azimuth and pitch disturbance angular rates of a two-axis turntable, achieves stable control through disturbance angular rate compensation, and optimizes the synchronous control of the two-axis turntable based on an asymmetric cross-coupling strategy, where:

[0091] Achieving stable control through disturbance angular rate compensation specifically includes using filters to obtain filtered azimuth disturbance angular rate and filtered pitch disturbance angular rate for the azimuth and pitch disturbance angular rates respectively, using integrators to obtain azimuth disturbance angle and pitch disturbance angle respectively, and compensating the filtered azimuth disturbance angular rate and filtered pitch disturbance angular rate to the input end of the corresponding driver after proportional control, and at the same time, feeding back the azimuth disturbance angle and the pitch disturbance angle to the input end of the corresponding position controller respectively, which can effectively compensate for the position deviation caused by disturbance in the two-axis turntable, and at the same time can improve the synchronization performance of the two axes, and further reduce the generation of contour errors;

[0092] The synchronous control of the two-axis turntable based on the asymmetric cross-coupling strategy optimization includes constructing an instruction planning controller to obtain the azimuth rotation angle and the pitch rotation angle, obtaining the compensation coefficient of the pitch axis based on the target trajectory curve and constructing a cross-coupling controller; constructing an azimuth position controller and a pitch position controller, and taking the azimuth rotation angle, the azimuth disturbance angle and the azimuth angular displacement as the input of the azimuth position controller to obtain the azimuth speed control instruction, and taking the pitch rotation angle, the pitch disturbance angle and the pitch angular displacement as the input of the pitch position controller to obtain the pitch speed control instruction; according to the filtered azimuth The disturbance angular rate and the filtered pitch disturbance angular rate are used to obtain the azimuth disturbance compensation angular velocity and the pitch disturbance compensation angular velocity respectively; an asymmetric cross-coupling strategy is adopted for the azimuth axis velocity input command and the pitch axis velocity input command at the current moment, and the azimuth axis decoupling compensation amount and the pitch axis decoupling compensation amount are used as the azimuth axis velocity input command and the pitch axis velocity input command for decoupling compensation respectively, so as to realize the following motion of the target trajectory curve, that is, the azimuth axis velocity input command and the pitch axis velocity input command are used as the input of the driver of the azimuth axis and the pitch axis to complete the compensation, which can improve the dynamic response capability, stability and accuracy of the two-axis turntable. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] Figure 1 It is a flow chart of the vehicle-mounted two-axle follow-up control method based on asymmetric cross coupling of the present invention;

[0094] Figure 2 It is a structural schematic diagram of a two-axis turntable of the present invention;

[0095] Figure 3 It is a schematic diagram of the principle of solving the azimuth rotation angle and the pitch rotation angle of the present invention;

[0096] Figure 4 It is a control block diagram of the two-axis turntable of the present invention;

[0097] Figure 5 It is a schematic diagram of the contour error solving principle of the present invention;

[0098] Figure 6 This is a comparison diagram of the effects of the single-axis disturbance angular rate compensation and the disturbance angle compensation of the present invention;

[0099] Figure 7 It is a comparison diagram of contour errors between the prior art method and the method of the present invention.

[0100] Figure numerals: 1, carrier; 2, azimuth seat ring; 3, pitch bracket; 4, pitch seat; 5, first fiber optic gyroscope; 6, second fiber optic gyroscope. DETAILED DESCRIPTION

[0101] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0102] It should be noted that, unless otherwise defined, all technical and scientific terms used herein have the same meaning as those commonly understood by those skilled in the art in the technical field of this application. The terms used herein in the specification of this application are only for the purpose of describing specific embodiments and are not intended to limit this application.

[0103] like Figure 1-Figure 7 As shown, a vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling is applied to a two-axis turntable, which is installed on a vehicle body and used to drive a load to rotate around an azimuth axis and a pitch axis, where the azimuth axis and the pitch axis are perpendicular. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling includes:

[0104] S1. Collect the azimuth disturbance angular rate and pitch disturbance angular rate of the two-axis turntable in real time.

[0105] In one embodiment, the two-axis turntable includes a carrier 1, an azimuth seat ring 2, a pitch bracket 3, a pitch seat 4, a first fiber optic gyroscope 5, a second fiber optic gyroscope 6, a first motor, a second motor, a first driver and a second driver. The carrier 1 is connected to a vehicle body, the first motor is mounted on the carrier 1 and is used to drive the azimuth seat ring 2 to rotate around the azimuth axis, the pitch bracket 3 is connected to the azimuth seat ring 2, the second motor is mounted on the pitch bracket 3 and is used to drive the pitch seat 4 to rotate around the pitch axis, the first fiber optic gyroscope 5 is connected to the carrier 1, the second fiber optic gyroscope 6 is connected to the pitch bracket 3, the first driver is electrically connected to the first motor, the second driver is electrically connected to the second motor, the detection result of the first fiber optic gyroscope 5 is the azimuth axis disturbance angular velocity, and the detection result of the second fiber optic gyroscope 6 is the pitch axis disturbance angular velocity.

[0106] like Figure 2As shown, the two-axis turntable of this embodiment includes a carrier 1, an azimuth seat ring 2, a pitch bracket 3, a pitch seat 4, a first fiber optic gyroscope 5, a second fiber optic gyroscope 6, a first motor, a second motor, a first driver and a second driver. The carrier 1 is used to provide support for structures such as the azimuth seat ring 2, the pitch bracket 3, the pitch seat 4, and the fiber optic gyroscope. It can also be used to carry a control board, a driver (such as a servo driver) and a motor, and can be fixed on a movable carrier such as a vehicle body through a mounting hole. The carrier 1 is rigidly connected to the vehicle body, and the azimuth seat ring 2 can be driven by the first motor to rotate around the azimuth axis, and its rotation range is generally 0°-360°. The pitch bracket 3 is connected to the azimuth seat ring 2, and the pitch seat 4 can be driven by the second motor to rotate around the pitch axis, and its rotation range is generally -10°-70°. The rotation of the azimuth seat ring 2 and the pitch seat 4 is generally driven by the motor rotating to drive the mechanical gear meshing. In this embodiment, the movements of the two rotating axes (azimuth axis and pitch axis which are perpendicular to each other) of the two-axis turntable are all controlled by the control board sending control instructions to the servo driver. After receiving the control instructions, the servo driver drives the motor to rotate, so that the rotation of the motor drives the gears to engage, and finally the rotation is transmitted to the rotating axis of the two-axis turntable. The first fiber optic gyroscope 5 is used to detect the azimuth axis disturbance angular velocity of the carrier 1, and the second fiber optic gyroscope 6 is used to detect the pitch axis disturbance angular velocity of the pitch bracket 3, that is, the azimuth disturbance angular velocity and pitch disturbance angular velocity of the two-axis turntable, that is, the azimuth disturbance angular velocity and pitch disturbance angular velocity of the vehicle body.

[0107] Figure 2 In the figure, the vehicle body coordinate system o′x′y′z′ is a moving coordinate system fixed on the platform 1, and the earth coordinate system OXYZ is a fixed coordinate system. The posture of the platform 1 will change due to disturbances, and stability means that when the platform 1 is disturbed, the angles of the azimuth seat 2 and the pitch seat 4 are adjusted so that the load mounted on the pitch seat 4 can maintain stability in the azimuth and pitch directions in the earth coordinate system OXYZ. The azimuth and pitch directions refer to the two rotation directions parallel to the OZ rotation axis and the OX rotation axis in the earth coordinate system OXYZ, such as Figure 2 Where L1 represents the azimuth axis, L2 represents the pitch axis, L3 represents the azimuth direction, and L4 represents the pitch direction. In the initial state, the vehicle coordinate system o′x′y′z′ and the earth coordinate system OXYZ can be overlapped by translation.

[0108] Fiber optic gyroscopes (fiber optic rate gyroscopes) are installed on the stage 1 and the pitch bracket 3 respectively to sense the disturbance angular rates in the azimuth and pitch directions of the two-axis turntable. The azimuth disturbance angular rate of the two-axis turntable refers to the rotation angular rate of the stage 1 when the o′z′ of the moving coordinate system on the stage 1 is the rotation axis. The pitch disturbance angular rate of the two-axis turntable refers to the rotation angular rate of the stage 1 when the o′x′ of the moving coordinate system on the stage 1 is the rotation axis.

[0109] When examining the stability and following performance of the vehicle-mounted two-axis follow-up system (two-axis turntable), the load on the pitch seat 4 can be replaced with a visible light source (such as a laser pen, etc.), and the performance can be measured by recording the motion trajectory of the visible light source projected on the projection plane. Generally, a sinusoidal signal with a preset amplitude and frequency is used as the command trajectory (target trajectory curve), and the stability index and following index of the two-axis turntable can be obtained by comparing the motion trajectory recorded in the projection plane.

[0110] S2, input the output signal of the first filter at the previous moment and the azimuth disturbance angular rate, and the azimuth disturbance angular rate at the current moment into the first filter for filtering, and obtain the filtered azimuth disturbance angular rate ω at the current moment yf , the output signal of the second filter at the previous moment and the pitch disturbance angular rate, and the pitch disturbance angular rate at the current moment are input into the second filter for filtering, and the filtered pitch disturbance angular rate ω at the current moment is obtained accordingly. pf .

[0111] In one embodiment, the first filter and the second filter are both bilinear low-pass filters, and perform the following operations:

[0112] S21. Use a first-order Butterworth low-pass filter to establish the analog transfer function H(s), the formula is as follows:

[0113]

[0114] In the formula, ω c is the cut-off angular frequency, and ω c =2πf c , f c is the cut-off frequency, s is the first complex variable;

[0115] S22, calculate frequency predistortion ω a , the formula is as follows:

[0116]

[0117] In the formula, ω d is the ideal discrete time angular frequency, and ω d =2πf c , T is the sampling period;

[0118] S23, replace the cutoff angular frequency ω in the analog transfer function H(s) c is the frequency predistortion ω a , the first complex variable s is After completing the bilinear transformation, the discrete transfer function H(z) is obtained, and the formula is as follows:

[0119]

[0120] Where z is the second complex variable;

[0121] S24. Establish the difference equation:

[0122] Let the first intermediate variable Then the discrete transfer function H(z) is expressed as:

[0123]

[0124] because Among them, X(z) is the input signal after z transformation, and Y(z) is the output signal after z transformation, so:

[0125]

[0126] Then perform the z inverse transform, that is, replace Y(z) with y[t], replace X(z) with x[t], and replace X(z) with x(t-1) -1 , y(t-1) replaces Y(z)z -1 , we can get the difference equation, the formula is as follows:

[0127]

[0128] Where y[t] is the output signal of the filter at time t, y[t-1] is the output signal of the filter at time t-1, x[t] is the input signal of the filter at time t, x[t-1] is the input signal of the filter at time t-1, and z -1 is the second complex variable z raised to the power of -1;

[0129] S25, respectively obtaining the filtered azimuth disturbance angular rate and the filtered pitch disturbance angular rate at the current moment according to the differential equation as output signals of the corresponding filters, wherein:

[0130] The filtered azimuth disturbance angular rate ω at time t yf [t], the formula is as follows:

[0131]

[0132] The filtered pitch disturbance angular rate ω at time t pf [t], the formula is as follows:

[0133]

[0134] In the formula, ω yf [t-1] is the filtered azimuth disturbance angular rate at time t-1, ω yt [t] is the azimuth disturbance angular velocity at time t, ω yt[t-1] is the azimuth disturbance angular rate at time t-1, ω pf [t-1] is the filtered pitch disturbance angular rate at time t-1, ω pt [t] is the pitch disturbance angular rate at time t, ω pt [t-1] is the pitch disturbance angular rate at time t-1.

[0135] Wherein, the first filter and the second filter are both digital filters. For example, a bilinear low-pass filter is selected, and its construction method is to convert a continuous time domain filter, such as a first-order Butterworth low-pass filter, into a time domain.

[0136] S3, the azimuth disturbance angular rate and the pitch disturbance angular rate from time zero to the current time are integrated by an integrator, and the azimuth disturbance angle θ at the current time is obtained accordingly. yt and the pitch disturbance angle θ pt .

[0137] In one embodiment, the azimuth disturbance angular rate and the pitch disturbance angular rate from time zero to the current time are integrated by an integrator, and the azimuth disturbance angle θ at the current time is obtained. yt and the pitch disturbance angle θ pt , as follows:

[0138] The azimuth disturbance angle θ at time t yt (t), the formula is as follows:

[0139]

[0140] The pitch disturbance angle θ at time t pt (t), the formula is as follows:

[0141]

[0142] In the formula, ω yt [n] is the azimuth disturbance angular velocity at time n, ω pt [n] is the pitch disturbance angular rate at time n, n = 0 ~ t, and f is the sampling frequency.

[0143] Among them, Figure 4 As shown, the azimuth disturbance angle θ at the current moment yt The first integrator integrates the azimuth disturbance angular rate from time zero to the current time, and the pitch disturbance angle θ at the current time is pt The second integrator integrates the pitch disturbance angular rate from time zero to the current time, ω yt represents the azimuth disturbance angular velocity at the corresponding moment, ω ptrepresents the pitch disturbance angular rate at the corresponding moment.

[0144] S4. Construct an instruction planning controller, which is used to obtain the azimuth rotation angle of the two-axis turntable at the current moment according to the target trajectory curve. and the pitch rotation angle β.

[0145] In one embodiment, the instruction planning controller performs the following operations:

[0146] S41, establishing a target coordinate system o1x1y1 on the projection plane where the target trajectory curve is located, the origin o1 of the target coordinate system is the point on the projection plane to which the load of the two-axis turntable points in the initial state, with the vertical upward axis being the y1 axis and the horizontal rightward axis being the x1 axis;

[0147] S42, sampling the target trajectory curve using a fixed period sampling method to obtain a number of sampling points and the coordinates of the corresponding sampling points;

[0148] S43, according to the coordinates of the current sampling point P, solve the azimuth rotation angle at the current moment And the pitch rotation angle β, the formula is as follows:

[0149]

[0150] l′ 2 =x 2 +l 2

[0151] Wherein, l is the distance between the load and the projection plane and is a constant, l′ is the second intermediate variable, (x0, y0) is the coordinate of the current sampling point P, x0 is the abscissa of the current sampling point P, y0 is the ordinate of the current sampling point P, and the current moment is the time to reach the current sampling point P, and the current sampling point P is also the target point at the current moment.

[0152] The instruction planning controller is designed according to the target trajectory curve. For example, in this embodiment, the target trajectory curve is a sine curve. Figure 3 As shown, a target coordinate system o1x1y1 is established on the projection plane S. The origin o1 of the target coordinate system is the point on the projection plane S pointed to by the load (laser pen) of the two-axis turntable in the initial state. The vertical upward is the y1 axis, and the horizontal rightward is the x1 axis. When the pitch axis rotates by an angle β and the azimuth axis is in the initial position, the point where the light beam o0 of the visible light source on the pitch seat 4 hits the projection plane S is P", and its distance from the x1 axis is h. When the azimuth axis rotates by an angle After that, the light spot is not point P' which is at a distance h from the x1 axis, but point P which is farther from the x1 axis. It can be seen from this that further geometric analysis is needed to obtain the azimuth rotation angle and the pitch rotation angle from the points on the target trajectory curve in order to achieve more accurate tracking accuracy.

[0153] Specifically, the distance between the light beam emitting point o0 of the visible light source on the pitch seat 4 and the projection plane S is l, and l is a constant during the test. The target trajectory curve is decomposed into a series of points on the projection plane S by adopting a fixed period sampling method, and the horizontal and vertical coordinates of the sampled points are known quantities. The azimuth rotation angle and the pitch rotation angle are solved according to the coordinates of the sampled points. The azimuth rotation angle is then calculated. (i.e. the angle that the direction seat ring 2 needs to rotate) is used as the target instruction θ output by the instruction planning controller to the azimuth position controller yr , the pitch rotation angle β (the angle that the pitch seat 4 needs to rotate) is used as the target instruction θ output by the instruction planning controller to the pitch position controller pr .

[0154] S5. Obtain the compensation coefficient K of the pitch axis at the current moment based on the target trajectory curve cy and build cross-coupled controllers.

[0155] In one embodiment, the compensation coefficient K of the pitch axis at the current moment is obtained based on the target trajectory curve. cy , the formula is as follows:

[0156]

[0157] In the formula, y t is the target point P at time t t The vertical coordinate, y t-1 is the target point P at time t-1 t-1 The vertical coordinate, H is the proportional adjustment coefficient, and the target point P at time t t and the target point P at time t-1 t-1 are all points on the target trajectory curve.

[0158] In one embodiment, the cross-coupling controller construction process is as follows:

[0159] S51, calculate the contour error ε at the current moment, the formula is as follows:

[0160] ε=-sinθ*e y +cosθ*e p

[0161] Where ε is the actual arrival point P at the current moment r To the target point P tThe distance between the secants, e y is the azimuth tracking error, e p is the pitch tracking error, θ is the target point P at the current moment t The angle between the secant line of and the horizontal axis of the target coordinate system;

[0162] S52, assuming ε = -C x *e y +C y *e p , and based on the formula in step S51, the mathematical expression of the cross-coupling factor is obtained:

[0163] C x = sinθ,C y = cosθ

[0164] In the formula, the cross-coupling factor includes the azimuthal cross-coupling factor C x and the pitch cross-coupling factor C y ;

[0165] S53, obtaining the output of the cross-coupling controller at the corresponding time according to the cross-coupling control algorithm, the cross-coupling control algorithm adopts the PID control algorithm, and the mathematical expression is as follows:

[0166]

[0167] Where u(t) is the output of the cross-coupling controller at time t, ε(t) is the input of the cross-coupling controller at time t, that is, the contour error at time t. Similarly, ε(i) is the input of the cross-coupling controller at time i, i = 0 ~ t-1, ε(t-1) is the input of the cross-coupling controller at time t-1, k p is the cross-coupled controller proportional coefficient, k i is the cross-coupled controller integral coefficient, k d is the differential coefficient of the cross-coupled controller.

[0168] Among them, a cross-coupling controller (CCC) is designed based on the target trajectory curve, and an asymmetric cross-coupling strategy is used to multiply the output of the cross-coupling controller by the corresponding coupling coefficient to compensate the input ends of the actuators (first actuator and second actuator) of the azimuth axis and pitch axis. Specifically, Figure 4 As shown, P t Point is the point expected to be reached at time t, P t-1 Point is the point expected to be reached at time t-1, P t , P t-1 All points are points on the expected trajectory (target trajectory curve), P r Point is the point actually reached at time t. In this embodiment, the point P expected to be reached is used.t The secant line replaces the tangent line, that is, use P t P t-1 As the expected arrival point P t The tangent of the contour is defined by the contour error. The contour error ε at time t is defined as the actual arrival point P at the current time. r To the target point P t Secant P t P t-1 The distance between them, θ is the target point P at the current moment t Secant P t P t-1 The angle between the horizontal axis x1 and the target coordinate system. The output u of the cross-coupling controller is decoupled and compensated to the corresponding axis through a cross-coupling control algorithm to reduce the contour error. The cross-coupling control algorithm is preferably a PID control algorithm, or other cross-coupling control algorithms well known to those skilled in the art.

[0169] S6. Construct an azimuth position controller and a pitch position controller, and rotate the current azimuth angle Azimuth disturbance angle θ yt The azimuth angular displacement is used as the input of the azimuth position controller to obtain the azimuth speed control instruction ω at the current moment. yu , the pitch rotation angle β and the pitch disturbance angle θ at the current moment pt The pitch angular displacement is used as the input of the pitch position controller to obtain the pitch velocity control instruction ω at the current moment. pu .

[0170] In one embodiment, both the azimuth position controller and the pitch position controller are PID controllers.

[0171] In one embodiment, the current orientation is rotated by an angle Azimuth disturbance angle θ yt The azimuth angular displacement is used as the input of the azimuth position controller to obtain the azimuth speed control instruction ω at the current moment. yu , the pitch rotation angle β and the pitch disturbance angle θ at the current moment pt The pitch angular displacement is used as the input of the pitch position controller to obtain the pitch velocity control instruction ω at the current moment. pu , the formula is as follows:

[0172] ω yu =K yp *K yu (θ yr -θ yt )-K yp *E y

[0173] ω pu =K pp *K pu (θ pr -θ pt )-K pp *E p

[0174] In the formula, K yp is the proportional control parameter of the azimuth position controller, K yu K is the conversion factor from azimuth rotation angle units to azimuth angular displacement units. pp is the proportional control parameter of the pitch position controller, K pu is the conversion coefficient from pitch rotation angle units to pitch angular displacement units, θ yr The output command of the command planning controller at the current moment, that is, the azimuth rotation angle at the current moment E y is the azimuth angular displacement, obtained by the encoder feedback of the first motor, θ pr The output command of the command planning controller at the current moment, that is, the pitch rotation angle β at the current moment, E p It is the pitch angular displacement, obtained by the encoder feedback of the second motor.

[0175] Specifically, both the azimuth position controller and the pitch position controller are implemented based on the PID controller, and can suppress the corresponding position controller output caused by disturbance.

[0176] S7, according to the filtered azimuth disturbance angular rate ω at the current moment yf and the filtered pitch disturbance angular rate ω pf , corresponding to the azimuth disturbance compensation angular velocity ω at the current moment yc and the pitch disturbance compensation angular velocity ω pc .

[0177] In one embodiment, according to the filtered azimuth disturbance angular rate ω at the current moment yf and the filtered pitch disturbance angular rate ω pf , corresponding to the azimuth disturbance compensation angular velocity ω at the current moment yc and the pitch disturbance compensation angular velocity ω pc ,in:

[0178] The azimuth disturbance compensation angular velocity ω at the current moment yc , the formula is as follows:

[0179] ω yc =K u1 *ω yf

[0180]

[0181] In the formula, K u1 is the azimuth angular rate compensation coefficient, R y is the gear ratio of the first motor, K1 is the azimuth compensation proportional control coefficient, the unit of the azimuth disturbance compensation angular velocity is 0.0001rps, and the unit of the filtered azimuth disturbance angular velocity is ° / s;

[0182] The current pitch disturbance compensation angular velocity ω pc , the formula is as follows:

[0183] ω pc =K u2 *ω pf

[0184]

[0185] In the formula, K u2 is the pitch angular rate compensation coefficient, R p is the gear ratio of the second motor, K2 is the pitch compensation proportional control coefficient, the unit of the pitch disturbance compensation angular velocity is 0.0001rps, and the unit of the filtered pitch disturbance angular velocity is ° / s.

[0186] The filtered azimuth disturbance angular rate and the filtered pitch disturbance angular rate are converted into units, multiplied by corresponding proportional control coefficients, and then compensated to the input end of the corresponding driver.

[0187] S8, using an asymmetric cross-coupling strategy to decouple and compensate the azimuth axis velocity input command and the pitch axis velocity input command at the current moment to achieve the following motion of the target trajectory curve. The asymmetric cross-coupling strategy is to decouple the azimuth axis decoupling compensation amount ω ys As the azimuth axis speed input command, the pitch axis decoupling compensation amount ω ps As the pitch axis speed input command, where:

[0188] Azimuth axis decoupling compensation ω ys , the formula is as follows:

[0189] ω ys =ω ys -ω yc +C x *u

[0190] Pitch axis decoupling compensation ω ps , the formula is as follows:

[0191] ω ps =ω pu -ω pc +Kcy *u

[0192] In the formula, C x is the azimuthal cross-coupling factor, and u is the output of the cross-coupling controller.

[0193] Among them, this method adopts an asymmetric cross-coupling strategy, namely an asymmetric cross-coupling strategy to complete the control of the two-axis turntable. The output of the position controller, the disturbance compensation angular velocity and the output of the cross-coupling controller are used as the input instructions of the driver. After linear superposition, the three are used as the reference instructions of the driver to complete the control of the two-axis turntable.

[0194] like Figure 6 As shown, when the disturbance with an amplitude of 2° and a frequency of 1Hz is loaded on the platform 1, the prior art disturbance angle compensation method and the disturbance angular rate compensation method of the present invention are used respectively. The disturbance angle obtained by the prior art disturbance angle compensation method can be obtained by the integrator in step S3. After comparison and simulation experiments, it is verified that the disturbance angular rate compensation method provided by the present invention has a lower angle error and higher stability accuracy than the prior art disturbance angle compensation method. For example, for the disturbance experiment of a single axis (azimuth axis or pitch axis), the figure shows the disturbance loaded in the azimuth direction, and the angle error is the difference between the azimuth rotation angle and the azimuth angle of the platform 1. The azimuth angle of the platform 1 is specifically the angle between the o'x' axis of the vehicle body coordinate system o'x'y'z' and the OX axis of the earth coordinate system OXYZ, or the angle between the o'y' axis of the vehicle body coordinate system o'x'y'z' and the OY axis of the earth coordinate system OXYZ. If there is no error in an ideal state, it should be 0.

[0195] like Figure 7 As shown, it is verified by simulation experiments that the method provided by the present invention (improved cross-coupling, i.e. asymmetric cross-coupling) can further reduce the contour error compared with the prior art method (conventional cross-coupling).

[0196] The technical features of the above-described embodiments may be arbitrarily combined. To make the description concise, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0197] The above-described embodiments only express the more specific and detailed embodiments described in this application, but they cannot be understood as limiting the scope of the application. It should be pointed out that for ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of this application, which all belong to the protection scope of this application. Therefore, the protection scope of this application shall be based on the attached claims.

Claims

1. A vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling, applied to a two-axis turntable, the two-axis turntable is installed on the vehicle body and is used to drive the load to rotate around an azimuth axis and a pitch axis, the azimuth axis and the pitch axis are perpendicular, and is characterized by: The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling includes: S1, real-time acquisition of the azimuth disturbance angular rate and the pitch disturbance angular rate of the two-axis turntable; S2, input the output signal of the first filter at the previous moment and the azimuth disturbance angular rate, and the azimuth disturbance angular rate at the current moment into the first filter for filtering, and obtain the filtered azimuth disturbance angular rate ω at the current moment yf , the output signal of the second filter at the previous moment and the pitch disturbance angular rate, and the pitch disturbance angular rate at the current moment are input into the second filter for filtering, and the filtered pitch disturbance angular rate ω at the current moment is obtained accordingly. pf ; S3, the azimuth disturbance angular rate and the pitch disturbance angular rate from time zero to the current time are integrated by an integrator, and the azimuth disturbance angle θ at the current time is obtained accordingly. yt and the pitch disturbance angle θ pt ; S4. Construct an instruction planning controller, which is used to obtain the azimuth rotation angle of the two-axis turntable at the current moment according to the target trajectory curve. and pitch rotation angle β; S5. Obtain the compensation coefficient K of the pitch axis at the current moment based on the target trajectory curve cy and constructing cross-coupled controllers; S6. Construct the azimuth position controller and the pitch position controller, and rotate the current azimuth angle Azimuth disturbance angle θ yt The azimuth angular displacement is used as the input of the azimuth position controller to obtain the azimuth speed control instruction ω at the current moment. yu , the pitch rotation angle β and the pitch disturbance angle θ at the current moment pt The pitch angular displacement is used as the input of the pitch position controller to obtain the pitch velocity control instruction ω at the current moment. pu ; S7, according to the filtered azimuth disturbance angular rate ω at the current moment yf and the filtered pitch disturbance angular rate ω pf , corresponding to the azimuth disturbance compensation angular velocity ω at the current moment yc and the pitch disturbance compensation angular velocity ω pc ; S8, using an asymmetric cross-coupling strategy to decouple and compensate the azimuth axis velocity input command and the pitch axis velocity input command at the current moment to achieve the following motion of the target trajectory curve, wherein the asymmetric cross-coupling strategy is to decouple the azimuth axis decoupling compensation amount ω ys As the azimuth axis speed input command, the pitch axis decoupling compensation amount ω ps As the pitch axis speed input command, where: Azimuth axis decoupling compensation ω ys , the formula is as follows: oh ys =ω yu -oh yc +C x *u Pitch axis decoupling compensation ω ps , the formula is as follows: oh ps =ω pu -oh pc +K cy *u In the formula, C x is the azimuthal cross-coupling factor, and u is the output of the cross-coupling controller.

2. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling as claimed in claim 1, characterized in that: The two-axis turntable comprises a carrier (1), an azimuth seat ring (2), a pitch bracket (3), a pitch seat (4), a first fiber optic gyroscope (5), a second fiber optic gyroscope (6), a first motor, a second motor, a first driver and a second driver. The carrier (1) is connected to a vehicle body. The first motor is mounted on the carrier (1) and is used to drive the azimuth seat ring (2) to rotate around an azimuth axis. The pitch bracket (3) is connected to the azimuth seat ring (2). The second motor is mounted on the pitch bracket (3) and is used to drive the pitch seat (4) to rotate around the pitch axis. The first fiber optic gyroscope (5) is connected to the carrier (1). The second fiber optic gyroscope (6) is connected to the pitch bracket (3). The first driver is electrically connected to the first motor. The second driver is electrically connected to the second motor. The detection result of the first fiber optic gyroscope (5) is the azimuth axis disturbance angular velocity. The detection result of the second fiber optic gyroscope (6) is the pitch axis disturbance angular velocity.

3. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling as claimed in claim 2, characterized in that: The first filter and the second filter are both bilinear low-pass filters, and perform the following operations: S21. Use a first-order Butterworth low-pass filter to establish the analog transfer function H(s), the formula is as follows: In the formula, ω c is the cut-off angular frequency, and ω c =2πf c , f c is the cut-off frequency, s is the first complex variable; S22, calculate frequency predistortion ω a , the formula is as follows: In the formula, ω d is the ideal discrete time angular frequency, and ω d =2πf c , T is the sampling period; S23, replace the cutoff angular frequency ω in the analog transfer function H(s) c is the frequency predistortion ω a , the first complex variable s is After completing the bilinear transformation, the discrete transfer function H(z) is obtained, and the formula is as follows: Where z is the second complex variable; S24. Establish the difference equation: Let the first intermediate variable Then the discrete transfer function H(z) is expressed as: because Among them, X(z) is the input signal after z transformation, and Y(z) is the output signal after z transformation, so: Then perform the z inverse transform, that is, replace Y(z) with y[t], replace X(z) with x[t], and replace X(z) with x(t-1) -1 , y(t-1) replaces Y(z)z -1 , we can get the difference equation, the formula is as follows: Where y[t] is the output signal of the filter at time t, y[t-1] is the output signal of the filter at time t-1, x[t] is the input signal of the filter at time t, x[t-1] is the input signal of the filter at time t-1, and z -1 is the second complex variable z raised to the power of -1; S25, respectively obtaining the filtered azimuth disturbance angular rate and the filtered pitch disturbance angular rate at the current moment according to the differential equation as output signals of the corresponding filters, wherein: The filtered azimuth disturbance angular rate ω at time t yf [t], the formula is as follows: The filtered pitch disturbance angular rate ω at time t pf [t], the formula is as follows: In the formula, ω yf [t-1] is the filtered azimuth disturbance angular rate at time t-1, ω yt [t] is the azimuth disturbance angular velocity at time t, ω yt [t-1] is the azimuth disturbance angular rate at time t-1, ω pf [t-1] is the filtered pitch disturbance angular rate at time t-1, ω pt [t] is the pitch disturbance angular rate at time t, ω pt [t-1] is the pitch disturbance angular rate at time t-1.

4. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling as claimed in claim 1, characterized in that: The azimuth disturbance angular rate and the pitch disturbance angular rate are integrated by integrators respectively, and the azimuth disturbance angle θ at the current moment is obtained accordingly. yt and the pitch disturbance angle θ pt , as follows: The azimuth disturbance angle θ at time t yt (t), the formula is as follows: The pitch disturbance angle θ at time t pt (t), the formula is as follows: In the formula, ω yt [n] is the azimuth disturbance angular velocity at time n, ω pt [n] is the pitch disturbance angular rate at time n, n = 0 ~ t, and f is the sampling frequency.

5. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling as claimed in claim 1, characterized in that: The instruction planning controller performs the following operations: S41, establishing a target coordinate system o1x1y1 on the projection plane where the target trajectory curve is located, wherein the origin o1 of the target coordinate system is the point on the projection plane to which the load of the two-axis turntable points in the initial state, with the vertical upward axis being the y1 axis and the horizontal rightward axis being the x1 axis; S42, sampling the target trajectory curve using a fixed period sampling method to obtain a number of sampling points and the coordinates of the corresponding sampling points; S43, according to the coordinates of the current sampling point P, solve the azimuth rotation angle at the current moment And the pitch rotation angle β, the formula is as follows: l′ 2 ＝x 2 +l 2 Wherein, l is the distance between the load and the projection plane and is a constant, l′ is the second intermediate variable, (x0, y0) is the coordinate of the current sampling point P, x0 is the abscissa of the current sampling point P, y0 is the ordinate of the current sampling point P, and the current moment is the time to reach the current sampling point P, and the current sampling point P is also the target point at the current moment.

6. The vehicle-mounted two-axle follow-up control method based on asymmetric cross coupling as claimed in claim 2, characterized in that: The azimuth disturbance angular rate ω after filtering at the current moment is respectively yf and the filtered pitch disturbance angular rate ω pf , corresponding to the azimuth disturbance compensation angular velocity ω at the current moment yc and the pitch disturbance compensation angular velocity ω pc ,in: The azimuth disturbance compensation angular velocity ω at the current moment yc , the formula is as follows: oh yc =K u1 *oh yf In the formula, K u1 is the azimuth angular rate compensation coefficient, R y is the gear ratio of the first motor, K1 is the azimuth compensation proportional control coefficient, the unit of the azimuth disturbance compensation angular velocity is 0.0001rps, and the unit of the filtered azimuth disturbance angular velocity is ° / s; The current pitch disturbance compensation angular velocity ω pc , the formula is as follows: oh pc =K u2 *oh pf In the formula, K u2 is the pitch angular rate compensation coefficient, R p is the gear ratio of the second motor, K2 is the pitch compensation proportional control coefficient, the unit of the pitch disturbance compensation angular velocity is 0.0001rps, and the unit of the filtered pitch disturbance angular velocity is ° / s.

7. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling as claimed in claim 1, characterized in that: The cross-coupling controller construction process is as follows: S51, calculate the contour error ε at the current moment, the formula is as follows: ε=-sinθ*e y +cosθ*e p Where ε is the actual arrival point P at the current moment r To the target point P t The distance between the secants, e y is the azimuth tracking error, e p is the pitch tracking error, θ is the target point P at the current moment t The angle between the secant line of and the horizontal axis of the target coordinate system; S52, assuming ε = -C x *e y +C y *e p , and based on the formula in step S51, the mathematical expression of the cross-coupling factor is obtained: C x =sinθ,C y =cosθ In the formula, the cross-coupling factor includes the azimuthal cross-coupling factor C x and the pitch cross-coupling factor C y ; S53, obtaining the output of the cross-coupling controller at the corresponding time according to the cross-coupling control algorithm, wherein the cross-coupling control algorithm adopts the PID control algorithm, and the mathematical expression is as follows: Where u(t) is the output of the cross-coupling controller at time t, ε(t) is the input of the cross-coupling controller at time t, that is, the contour error at time t. Similarly, ε(i) is the input of the cross-coupling controller at time i, i = 0 ~ t-1, ε(t-1) is the input of the cross-coupling controller at time t-1, k p is the cross-coupled controller proportional coefficient, k i is the cross-coupled controller integral coefficient, k d is the differential coefficient of the cross-coupled controller.

8. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling as claimed in claim 1, characterized in that: The compensation coefficient K of the pitch axis at the current moment is obtained based on the target trajectory curve cy , the formula is as follows: In the formula, y t is the target point P at time t t The vertical coordinate, y t-1 is the target point P at time t-1 t-1 The vertical coordinate, H is the proportional adjustment coefficient, and the target point P at time t t and the target point P at time t-1 t-1 are all points on the target trajectory curve.

9. The vehicle-mounted two-axis follow-up control method based on asymmetric cross-coupling as claimed in claim 2, characterized in that: The azimuth position controller and the pitch position controller are both PID controllers.

10. The vehicle-mounted two-axle follow-up control method based on asymmetric cross coupling as claimed in claim 9, characterized in that: The angle of rotation of the current moment Azimuth disturbance angle θ yt The azimuth angular displacement is used as the input of the azimuth position controller to obtain the azimuth speed control instruction ω at the current moment. yu , the pitch rotation angle β and the pitch disturbance angle θ at the current moment pt The pitch angular displacement is used as the input of the pitch position controller to obtain the pitch velocity control instruction ω at the current moment. pu , the formula is as follows: oh yu =K yp *K yu (i yr -θ yt )-K yp *E y oh pu =K pp *K pu (i pr -θ pt )-K pp *E p In the formula, K yp is the proportional control parameter of the azimuth position controller, K yu K is the conversion factor from azimuth rotation angle units to azimuth angular displacement units. pp is the proportional control parameter of the pitch position controller, K pu is the conversion coefficient from pitch rotation angle units to pitch angular displacement units, θ yr The output command of the command planning controller at the current moment, that is, the azimuth rotation angle at the current moment E y is the azimuth angular displacement, obtained by the encoder feedback of the first motor, θ pr The output command of the command planning controller at the current moment, that is, the pitch rotation angle β at the current moment, E p It is the pitch angular displacement, obtained by the encoder feedback of the second motor.

Images