A high-precision three-axis turntable control device and control method

Abstract

The present invention relates to a high-precision three-axis turntable control device and control method, which belongs to the field of three-axis turntable control technology and solves the problem that the existing three-axis turntable control method is not suitable for scenes with large external environmental disturbances. The high-precision three-axis turntable control device includes a pitch control loop, a roll control loop and an azimuth control loop; each control loop includes: a feedforward controller: used to process the data collected by the end IMU sensor and the base IMU sensor of the three-axis turntable to obtain the feedforward result of the current control loop; an ADRC controller: used to obtain the axial angle of the current control loop according to the deviation between the given value of the current control loop and the feedforward result; an axis driver: used to drive the corresponding axis to rotate to the axial angle according to the axial angle of the current control loop; the pitch control loop, the roll control loop and the azimuth control loop are used to adjust the pitch axis angle, the roll axis angle and the azimuth axis angle of the three-axis turntable respectively.

Description

Technical Field

[0001] The present invention relates to the technical field of three-axis turntable control, and in particular to a high-precision three-axis turntable control device and control method. Background Art

[0002] In the field of target tracking and scanning, servo turntables are widely used for load end-to-end stabilization. A three-axis turntable, a type of servo turntable, features three drive axes: roll, pitch, and azimuth. Compared to a two-axis turntable, a three-axis turntable offers redundant degrees of freedom and better dynamic attitude adjustment and compensation performance in highly dynamic disturbance environments. This makes it more suitable for the drive configuration of target tracking and scanning devices that maintain high-precision end-to-end attitude control in complex disturbance environments, such as those on vehicles and ships. However, the increased number of redundant degrees of freedom significantly increases the difficulty of high-precision control of a three-axis turntable. The quality of the control method directly determines the end-to-end accuracy of the three-axis turntable and is the key to achieving high-precision end-to-end tracking and scanning.

[0003] The currently commonly used three-axis turntable control method is to use kinematic inverse solution to obtain the joint angles of each axis based on the desired value of the end-point attitude control and the attitude value of the three-axis attitude sensor fixed on the base, thereby realizing attitude tracking and control. This control method is essentially a feedforward open-loop control. Due to the uncertain step disturbances in the vehicle environment, it is difficult to achieve high-precision end-point attitude control for this open-loop control. Therefore, it is only applicable to the field of three-axis turntable control with relatively small external environmental disturbances. Summary of the Invention

[0004] In view of the above analysis, the embodiments of the present invention aim to provide a high-precision three-axis turntable control device and control method to solve the problem that the existing three-axis turntable control method is not suitable for scenarios with large external environmental disturbances.

[0005] In one aspect, the present invention provides a high-precision three-axis turntable control device, the device comprising a pitch control circuit, a roll control circuit, and an azimuth control circuit; wherein each control circuit comprises:

[0006] Feedforward controller: used to process the data collected by the end IMU sensor and the base IMU sensor of the three-axis turntable to obtain the feedforward result of the current control loop;

[0007] ADRC controller: used to obtain the axial angle of the current control loop based on the deviation between the given value of the current control loop and the feedforward result;

[0008] Axis driver: used to drive the corresponding axis to rotate to the axial angle according to the axial angle of the current control loop;

[0009] The pitch control loop is used to adjust the pitch axis angle of the three-axis turntable, the roll control loop is used to adjust the roll axis angle of the three-axis turntable, and the azimuth control loop is used to adjust the azimuth axis angle of the three-axis turntable.

[0010] On the basis of the above solution, the present invention also makes the following improvements:

[0011] Furthermore, the feedforward controller in the pitch control loop is a pitch feedforward controller, which is used to obtain the pitch feedforward result β of the pitch control loop according to formula (1): f :

[0012] β f =arcsin[|sinφsin(ψ+β)-cosφsinθcos(ψ+β)|] (1)

[0013] Among them, β and θ represent the pitch angle and roll angle of the end IMU sensor respectively; ψ and φ represent the pitch angle and roll angle of the base IMU sensor respectively.

[0014] Furthermore, the ADRC controller in the pitch control loop is a pitch ADRC controller, which is used to obtain the pitch axial angle q of the pitch control loop according to formula (2): βd :

[0015] q βd =b1e β -fhan(e β ,r1,h1) (2)

[0016] Among them, e β =β d +β f -β,β d Indicates the given value of the pitch control loop; b1 indicates the pitch proportional coefficient; fhan(e β ,r1,h1) represents the tracking differentiator of pitch control, r1 and h1 are both pitch control parameters.

[0017] Furthermore, the feedforward controller in the roll control loop is a roll feedforward controller, which is used to obtain the roll feedforward result α of the roll control loop according to formula (3): f :

[0018] α f =arcsin[|sinφcos(ψ+β)+cosφsinθcos(ψ+β)|] (3).

[0019] Furthermore, the ADRC controller in the roll control loop is a roll ADRC controller, which is used to obtain the roll axial angle q of the roll control loop according to formula (4): αd :

[0020] q αd =b2e α -fhan(e α ,r2,h2) (4)

[0021] Among them, e α =α d +α f -θ,e α Indicates the given value of the roll control loop; b2 indicates the roll proportional coefficient; fhan(e α ,r2,h2) represents the tracking differentiator of roll control, r2 and h2 are both roll control parameters.

[0022] Furthermore, the feedforward controller in the azimuth control loop is an azimuth feedforward controller, which is used to obtain the azimuth feedforward result ω of the azimuth control loop according to formula (5): f :

[0023] ω f =(ω ψ sinφ+ω θ )·sinα+(ω φ ·cosθ-ω ψ ·cosφsinθ)·cosα (5)

[0024] Among them, ω θ Represents the roll angular velocity of the end IMU sensor, ω ψ Indicates the pitch angular velocity of the base IMU sensor, ω φ Indicates the roll angular velocity of the base IMU sensor.

[0025] Furthermore, the ADRC controller in the azimuth control loop is an azimuth ADRC controller, which is used to obtain the azimuth axial angle ω of the azimuth control loop according to formula (6): dd :

[0026] ω dd =b3e ω -fhan(e ω ,r3,h3) (6)

[0027] Among them, e ω =ω d +ω f -ω θ ,ω d Indicates the given value of the azimuth control loop; b3 indicates the azimuth proportional coefficient; fhan(e ω ,r3,h3) represents the tracking differentiator of azimuth control, r3 and h3 are both azimuth control parameters.

[0028] Furthermore, the fhan function satisfies:

[0029]

[0030] in,

[0031]

[0032] The fsg function is defined as follows:

[0033] fsg(a,d)=(sign(a+d)-sign(ad)) / 2 (9).

[0034] Furthermore, the given value of the pitch control loop is a desired pitch instruction;

[0035] The given value of the roll control loop is the desired roll angle;

[0036] The given value of the azimuth control loop is the azimuth speed instruction.

[0037] On the other hand, the present invention also provides a high-precision three-axis turntable control method, comprising:

[0038] The control method is divided into a pitch control loop control method, a roll control loop control method and an azimuth control loop control method; wherein the control method of each control loop includes:

[0039] Process the data collected by the end IMU sensor and the base IMU sensor of the three-axis turntable to obtain the feedforward result of the current control loop;

[0040] According to the axial angle of the current control loop, the corresponding shaft is driven to rotate to the axial angle;

[0041] Drive the axial angle of the current control loop to achieve angle adjustment of the corresponding axial direction of the three-axis turntable;

[0042] The pitch control loop is used to adjust the pitch axis angle of the three-axis turntable, the roll control loop is used to adjust the roll axis angle of the three-axis turntable, and the azimuth control loop is used to adjust the azimuth axis angle of the three-axis turntable.

[0043] Compared with the prior art, the present invention can achieve at least one of the following beneficial effects:

[0044] The high-precision three-axis turntable control device and control method provided by the present invention have the following advantages:

[0045] First, by decoupling the pitch control loop, roll control loop, and azimuth control loop, and adopting a control method combining feedforward control and closed-loop control, the terminal control accuracy and response timeliness are effectively improved.

[0046] Second, this control method has strong adaptability and can solve the problem that the existing three-axis turntable control method is not suitable for scenarios with large external environmental disturbances.

[0047] Third, by selecting the ADRC controller, the control accuracy can be better improved, providing a theoretical basis for the development of high-precision three-axis turntable.

[0048] In the present invention, the above-mentioned technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of the present invention will be described in the following description, and some advantages will become apparent from the description or be learned through practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] The accompanying drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like parts throughout the drawings.

[0050] Figure 1 A schematic structural diagram of a high-precision three-axis turntable control device provided in Example 1 of the present invention;

[0051] Figure 2 This is a flow chart of a high-precision three-axis turntable control method provided in Example 2 of the present invention. DETAILED DESCRIPTION

[0052] The preferred embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, and are not used to limit the scope of the present invention.

[0053] Example 1

[0054] Specific embodiment 1 of the present invention discloses a high-precision three-axis turntable control device, the structural diagram of which is shown in FIG. Figure 1 As shown, the device includes a pitch control loop, a roll control loop, and an azimuth control loop; wherein each control loop includes:

[0055] Feedforward controller: used to process the data collected by the end IMU sensor and the base IMU sensor of the three-axis turntable to obtain the feedforward result of the current control loop;

[0056] ADRC controller: used to obtain the axial angle of the current control loop based on the deviation between the given value of the current control loop and the feedforward result;

[0057] Axis driver: used to drive the corresponding axis to rotate to the axial angle according to the axial angle of the current control loop;

[0058] The pitch control loop is used to adjust the pitch axis angle of the three-axis turntable, the roll control loop is used to adjust the roll axis angle of the three-axis turntable, and the azimuth control loop is used to adjust the azimuth axis angle of the three-axis turntable.

[0059] To help those skilled in the art better achieve precise control of the three-axis turntable, the following is a detailed introduction to the structure of each control loop:

[0060] like Figure 1 As shown, in the pitch control loop, the feedforward controller, ADRC controller, and axis driver are described as the pitch feedforward controller, pitch ADRC controller, and pitch axis driver, respectively. Specifically,

[0061] The pitch feedforward controller obtains the pitch feedforward result β of the pitch control loop according to formula (1): f :

[0062] β f =arcsin[|sinφsin(ψ+β)-cosφsinθcos(ψ+β)|] (1)

[0063] Among them, β and θ represent the pitch angle and roll angle of the end IMU sensor respectively; ψ and φ represent the pitch angle and roll angle of the base IMU sensor respectively.

[0064] The pitch ADRC controller obtains the pitch axial angle q of the pitch control loop according to formula (2): βd :

[0065] q βd =b1e β -fhan(e β ,r1,h1) (2)

[0066] Among them, e β =β d +β f -β,β d Indicates the given value of the pitch control loop; the given value of the pitch control loop is the pitch desired instruction; b1 represents the pitch proportional coefficient; fhan(e β ,r1,h1) represents the tracking differentiator of pitch control, r1 and h1 are both pitch control parameters.

[0067] After the pitch axis angle is acquired, the pitch axis angle is driven by the pitch axis driver to achieve adjustment of the pitch axis angle of the three-axis turntable.

[0068] like Figure 1As shown, in the roll control loop, the feedforward controller, ADRC controller, and axis driver are described as the roll feedforward controller, roll ADRC controller, and roll axis driver, respectively. Specifically,

[0069] The roll feedforward controller obtains the roll feedforward result α of the roll control loop according to formula (3): f :

[0070] α f =arcsin[|sinφcos(ψ+β)+cosφsinθcos(ψ+β)|] (3)

[0071] The roll ADRC controller obtains the roll axial angle q of the roll control loop according to formula (4): αd :

[0072] q αd =b2e α -fhan(e α ,r2,h2) (4)

[0073] Among them, e α =α d +α f -θ,e α Indicates the given value of the roll control loop; the given value of the roll control loop is the expected roll angle; b2 represents the roll proportional coefficient; fhan(e α ,r2,h2) represents the tracking differentiator of roll control, r2 and h2 are both roll control parameters.

[0074] After the roll axial angle is obtained, the roll axial angle is driven by the roll axis driver to achieve adjustment of the roll axial angle of the three-axis turntable.

[0075] like Figure 1 As shown, in the azimuth control loop, the feedforward controller, ADRC controller, and axis driver are described as the azimuth feedforward controller, azimuth ADRC controller, and azimuth axis driver, respectively. Specifically,

[0076] The azimuth feedforward controller obtains the azimuth feedforward result of the azimuth control loop according to formula (5): f :

[0077] ω f =(ω ψ sinφ+ω θ )·sinα+(ω φ ·cosθ-ω ψ ·cosφsinθ)·cosα (5)

[0078] Among them, ω θRepresents the roll angular velocity of the end IMU sensor, ω ψ Indicates the pitch angular velocity of the base IMU sensor, ω φ Indicates the roll angular velocity of the base IMU sensor.

[0079] The azimuth ADRC controller obtains the azimuth axial angle ω of the azimuth control loop according to formula (6): dd :

[0080] ω dd =b3e ω -fhan(e ω ,r3,h3) (6)

[0081] Among them, e ω =ω d +ω f -ω θ ,ω d Indicates the given value of the azimuth control loop, which is the azimuth speed instruction; b3 indicates the azimuth proportional coefficient; fhan(e ω ,r3,h3) represents the tracking differentiator of azimuth control, r3 and h3 are both azimuth control parameters.

[0082] After the azimuth axial angle is acquired, the azimuth axial angle is driven by the azimuth axis driver to achieve adjustment of the azimuth axial angle of the three-axis turntable.

[0083] It should be noted that the fhan function is used in the relevant calculation processes of the above three control loops. The fhan function is explained as follows:

[0084]

[0085] in,

[0086]

[0087] The fsg function is defined as follows:

[0088] fsg(a,d)=(sign(a+d)-sign(ad)) / 2 (9)

[0089] The sign function is defined as follows:

[0090]

[0091] When the fhan function is used for the above loop calculation, the corresponding values are assigned to e, r, and h in the function to calculate the fhan function result, and on this basis, the axial angle of each control loop can be obtained. For example, when e, r, and h in the function are assigned to e β,r1,h1, we can get fhan(e β ,r1,h1), and then according to formula (2), the pitch axial angle q of the pitch control loop can be calculated βd When e, r, and h in the function are assigned to e α ,r2,h2, we can get fhan(e α ,r2,h2), and then according to formula (4), the roll axial angle q of the roll control loop can be calculated αd When e, r, and h in the function are assigned to e ω ,r3,h3, we can get fhan(e ω ,r3,h3), and then according to formula (6), the azimuth axial angle ω of the azimuth control loop can be calculated dd .

[0092] Compared with the prior art, the high-precision three-axis turntable control device provided in this embodiment has the following advantages:

[0093] Third, by decoupling the pitch control loop, roll control loop and azimuth control loop, and adopting a control method combining feedforward control and closed-loop control, the terminal control accuracy and response timeliness are effectively improved.

[0094] Fourth, this control method has strong adaptability and can solve the problem that the existing three-axis turntable control method is not suitable for scenarios with large external environmental disturbances.

[0095] Third, by selecting the ADRC controller, the control accuracy can be better improved, providing a theoretical basis for the development of high-precision three-axis turntable.

[0096] Example 2

[0097] Specific embodiment 2 of the present invention discloses a high-precision three-axis turntable control method, the method flow chart is as follows Figure 2 Shown, including:

[0098] The control method is divided into a pitch control loop control method, a roll control loop control method and an azimuth control loop control method; wherein the control method of each control loop includes:

[0099] Step S1: Process the data collected by the end IMU sensor and the base IMU sensor of the three-axis turntable to obtain the feedforward result of the current control loop;

[0100] Step S2: Obtain the axial angle of the current control loop according to the deviation between the given value of the current control loop and the feedforward result;

[0101] Step S3: driving the corresponding shaft to rotate to the axial angle according to the axial angle of the current control loop;

[0102] The pitch control loop is used to adjust the pitch axis angle of the three-axis turntable, the roll control loop is used to adjust the roll axis angle of the three-axis turntable, and the azimuth control loop is used to adjust the azimuth axis angle of the three-axis turntable.

[0103] The specific implementation process of the method embodiment of the present invention can be referred to the above device embodiment, and this embodiment will not be repeated here. Since the principle of this method embodiment is the same as that of the above device embodiment, this method also has the corresponding technical effects of the above method embodiment.

[0104] Those skilled in the art will appreciate that all or part of the process steps of the above-described embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, such as a magnetic disk, an optical disk, a read-only memory, or a random access memory.

[0105] The above description is only a preferred specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by any technician familiar with this technical field within the technical scope disclosed by the present invention should be covered by the scope of protection of the present invention.

Claims

1. A high-precision three-axis turntable control device, characterized in that: The device includes a pitch control loop, a roll control loop, and an azimuth control loop; wherein each control loop includes: Feedforward controller: used to process the data collected by the end IMU sensor and the base IMU sensor of the three-axis turntable to obtain the feedforward result of the current control loop; ADRC controller: used to obtain the axial angle of the current control loop based on the deviation between the given value of the current control loop and the feedforward result; Axis driver: used to drive the corresponding axis to rotate to the axial angle according to the axial angle of the current control loop; The pitch control circuit is used to adjust the pitch axis angle of the three-axis turntable, the roll control circuit is used to adjust the roll axis angle of the three-axis turntable, and the azimuth control circuit is used to adjust the azimuth axis angle of the three-axis turntable; The feedforward controller in the pitch control loop is a pitch feedforward controller, which is used to obtain the pitch feedforward result β of the pitch control loop according to formula (1): f : b f =arcsin[|sinφsin(ψ+β)-cosφsinθcos(ψ+β)|] (1) Among them, β and θ represent the pitch angle and roll angle of the end IMU sensor respectively; ψ and φ represent the pitch angle and roll angle of the base IMU sensor respectively; The ADRC controller in the pitch control loop is a pitch ADRC controller, which is used to obtain the pitch axial angle q of the pitch control loop according to formula (2): βd : q βd =b1e β -stay(it) β ,r1,h1) (2) Among them, e β =β d +β f -β,β d Indicates the given value of the pitch control loop; b1 indicates the pitch proportional coefficient; fhan(e β ,r1,h1) represents the tracking differentiator of pitch control, r1 and h1 are both pitch control parameters; The feedforward controller in the roll control loop is a roll feedforward controller, which is used to obtain the roll feedforward result α of the roll control loop according to formula (3): f : a f =arcsin[|sinφcos(ψ+β)+cosφsinθcos(ψ+β)|] (3); The ADRC controller in the roll control loop is a roll ADRC controller, which is used to obtain the roll axial angle q of the roll control loop according to formula (4): αd : q αd =b2e α -stay(it) α ,r2,h2) (4) Among them, e α =α d +α f -θ,e α Indicates the given value of the roll control loop; b2 indicates the roll proportional coefficient; fhan(e α ,r2,h2) represents the tracking differentiator of roll control, r2 and h2 are both roll control parameters; The feedforward controller in the azimuth control loop is an azimuth feedforward controller, which is used to obtain the azimuth feedforward result ω of the azimuth control loop according to formula (5): f : oh f =(ω ψ ·sinφ+ω θ )·sinα+(ω φ ·cosθ-ω ψ ·cosφsinθ)·cosα (5) Among them, ω θ Represents the roll angular velocity of the end IMU sensor, ω ψ Indicates the pitch angular velocity of the base IMU sensor, ω φ Indicates the roll angular velocity of the base IMU sensor; The ADRC controller in the azimuth control loop is an azimuth ADRC controller, which is used to obtain the azimuth axial angle ω of the azimuth control loop according to formula (6): dd : <h2 style=";text-align:left;direction:ltr">ω<h2 style=";text-align:left;direction:ltr"> dd <h2 style=";text-align:left;direction:ltr"> =b3e<h2 style=";text-align:left;direction:ltr"> ω <h2 style=";text-align:left;direction:ltr"> -fhan(e<h2 style=";text-align:left;direction:ltr"> ω <h2 style=";text-align:left;direction:ltr"> ,r3,h3) (6) Among them, e ω =ω d +ω f -ω θ ,ω d Indicates the given value of the azimuth control loop; b3 indicates the azimuth proportional coefficient; fhan(e ω ,r3,h3) represents the tracking differentiator of azimuth control, r3 and h3 are both azimuth control parameters.

2. The high-precision three-axis turntable control device according to claim 1, characterized in that: The fhan function satisfies: in, The fsg function is defined as follows: fsg(a,d)=(sign(a+d)-sign(ad)) / 2 (9) When e, r, and h in the fhan function are assigned to e β ,r1,h1, we get fhan(e β ,r1,h1); When e, r, and h in the fhan function are assigned to e α ,r2,h2, we get fhan(e α ,r2,h2); When e, r, and h in the fhan function are assigned to e ω ,r3,h3, we get fhan(e ω ,r3,h3).

3. The high-precision three-axis turntable control device according to claim 2, characterized in that: The given value of the pitch control loop is the pitch desired instruction; The given value of the roll control loop is the desired roll angle; The given value of the azimuth control loop is the azimuth speed instruction.

4. A high-precision three-axis turntable control method, characterized in that: The control method is implemented based on the high-precision three-axis turntable control device according to any one of claims 1 to 3, and the control method includes: The control method is divided into a pitch control loop control method, a roll control loop control method and an azimuth control loop control method; wherein the control method of each control loop includes: Process the data collected by the end IMU sensor and the base IMU sensor of the three-axis turntable to obtain the feedforward result of the current control loop; According to the deviation between the given value of the current control loop and the feedforward result, the axial angle of the current control loop is obtained; According to the axial angle of the current control loop, the corresponding shaft is driven to rotate to the axial angle; The pitch control loop is used to adjust the pitch axis angle of the three-axis turntable, the roll control loop is used to adjust the roll axis angle of the three-axis turntable, and the azimuth control loop is used to adjust the azimuth axis angle of the three-axis turntable.

Images