Robot and its line-of-sight stabilization control method and control device
By constructing an extended state observer in the horizontal direction of the robot's line of sight and fusing it with magnetometer data, the problems of gyroscope compensation lag and magnetometer susceptibility to interference were solved, and high-precision stable control of the robot's line of sight was achieved.
Patent Information
- Application Number
- CN202210909630.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2022-07-29
- Publication Date
- 2026-11-17
- Estimated Expiration
- 2042-07-29
AI Technical Summary
In existing robot line-of-sight stabilization systems, gyroscope compensation suffers from lag and is not ideal for handling high-frequency disturbances. Furthermore, magnetometers are susceptible to interference from external magnetic fields, leading to large errors in line-of-sight orientation compensation and making it difficult to achieve precise control.
An extended state observer based on the H∞ principle is designed. Combined with magnetometer data, the data is fused after low-pass and high-pass filtering to obtain the total disturbance of the carrier. Feedforward compensation is then performed to reduce external interference and suppress high-frequency interference.
It improves the accuracy of robot line-of-sight control, reduces the impact of external interference and high-frequency interference, and achieves timely compensation and precise control of the line of sight.
Smart Images

Figure CN117518790B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of data processing technology, specifically to a method and device for stabilizing the line of sight of a robot. Background Technology
[0002] Currently, in robot line-of-sight stabilization systems, gyroscopes are typically used to sense disturbances in the three-dimensional space experienced by the robot. The transformation between the geodetic coordinate system and the robot coordinate system is used as error information for feedback adjustment, thereby ensuring the stability of the line of sight.
[0003] However, compensation using a gyroscope has a lag, always adjusting only after an error occurs. Furthermore, due to bandwidth limitations, gyroscopes are generally not ideal for compensating for high-frequency disturbances. In addition, the gyroscope's components (magnetometers) are susceptible to interference from external magnetic fields, causing deviations in the magnetometer data. This, in turn, leads to errors in the final line-of-sight orientation compensation, making it difficult to adjust. Summary of the Invention
[0004] To solve the above-mentioned technical problems, the present invention provides a method for stabilizing the line of sight of a robot.
[0005] The present invention also provides a line-of-sight stabilization control device for a robot.
[0006] The technical solution adopted in this invention is as follows:
[0007] A first aspect of the present invention provides a method for stabilizing the visual axis of a robot, comprising the following steps: designing an extended state observer based on the H∞ principle; observing the total disturbance in the horizontal direction of the robot's visual axis using the extended state observer; acquiring the horizontal disturbance in the horizontal direction of the robot's visual axis measured by a magnetometer in the robot; performing data fusion processing on the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer based on the complementarity principle to obtain the total disturbance of the carrier; and inputting the total disturbance of the carrier into the input of the robot's visual axis control system for feedforward compensation.
[0008] The line-of-sight stabilization control method for robots proposed in this invention also has the following additional technical features:
[0009] According to one embodiment of the present invention, the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer are fused based on the complementarity principle to obtain the total disturbance of the carrier. Specifically, this includes: inputting the total disturbance in the horizontal direction of the robot's line of sight observed by the extended state observer into a low-pass filter for low-pass filtering; inputting the horizontal disturbance in the horizontal direction of the robot's line of sight measured by the magnetometer into a high-pass filter for high-pass filtering; and fusing the total disturbance w1 after low-pass filtering and the horizontal disturbance w2 after high-pass filtering to obtain the total disturbance of the carrier.
[0010] According to one embodiment of the present invention, the total disturbance of the carrier is obtained according to the following formula: total disturbance of the carrier = a1*w1 + a2*w2; where a1 is a first coefficient, a2 is a second coefficient, and a1+a2=1, w1 is the total disturbance after low-pass filtering, and w2 is the horizontal disturbance after high-pass filtering.
[0011] A second aspect of the present invention provides a visual axis stabilization control device for a robot, comprising: a design module for designing an extended state observer based on the H∞ principle; an observation module for observing the total disturbance in the horizontal direction of the robot's visual axis using the extended state observer; an acquisition module for acquiring the horizontal disturbance in the horizontal direction of the robot's visual axis measured by a magnetometer in the robot; a fusion module for performing data fusion processing on the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer based on the complementarity principle to obtain the total disturbance of the carrier; and a compensation module for inputting the total disturbance of the carrier into the input of the robot's visual axis control system for feedforward compensation.
[0012] The robot's line-of-sight stabilization control device proposed above in this invention also has the following additional technical features:
[0013] According to one embodiment of the present invention, the fusion module is specifically used to: input the total disturbance in the horizontal direction of the robot's line of sight observed by the extended state observer into a low-pass filter for low-pass filtering; input the horizontal disturbance in the horizontal direction of the robot's line of sight measured by the magnetometer into a high-pass filter for high-pass filtering; and fuse the total disturbance w1 after low-pass filtering and the horizontal disturbance w2 after high-pass filtering to obtain the total disturbance of the carrier.
[0014] According to one embodiment of the present invention, the fusion module specifically obtains the total carrier perturbation according to the following formula: total carrier perturbation = a1*w1 + a2*w2; where a1 is a first coefficient, a2 is a second coefficient, and a1+a2=1, w1 is the total perturbation after low-pass filtering, and w2 is the horizontal perturbation after high-pass filtering.
[0015] A third aspect of the present invention provides a robot including the line-of-sight stabilization control device of the robot of the second aspect of the present invention.
[0016] The beneficial effects of this invention are:
[0017] This invention fuses interference observer and magnetometer data in the horizontal direction of the robot's line of sight, and uses feedforward compensation to compensate the robot's line of sight in time before errors occur based on the fused data. This can better reduce external interference, suppress high-frequency interference, reduce line of sight control errors, and thus improve the robot's control accuracy. Attached Figure Description
[0018] Figure 1 This is a flowchart of a line-of-sight stabilization control method for a robot according to an embodiment of the present invention;
[0019] Figure 2 This is a schematic block diagram of an extended state observer according to an embodiment of the present invention;
[0020] Figure 3 This is a block diagram illustrating the data fusion principle according to an embodiment of the present invention;
[0021] Figure 4 A block diagram of a robot's line-of-sight stabilization control device according to an embodiment of the present invention. Detailed Implementation
[0022] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.
[0023] Figure 1 This is a flowchart of a robot's line-of-sight stabilization control method according to an embodiment of the present invention, as follows: Figure 1 As shown, the method includes the following steps:
[0024] S1, a robust extended state observer designed based on the H∞ principle.
[0025] Specifically, H∞ control refers to a system where, when system parameters are subject to perturbations within a certain range (note that the transfer function is not a fixed value but fluctuates within a certain range), the system can be described by a family of transfer functions, called the transfer function set (with infinitely many elements). The input and output gains are also not fixed values, but we can choose the largest gain among them as the gain of this function set. H∞ control achieves the goal of noise suppression by maximizing the gain of the transfer function set between noise and the desired output.
[0026] The principle of designing a robust extended state observer based on the H∞ principle can be found in [reference needed]. Figure 2 As shown, where,
[0027]
[0028] z represents the output signal, w represents the external input signal, u is the disturbance observed by the extended state observer, y is the input of the extended state observer, P represents the state equation (controlled object) of the extended state observer, d is the external disturbance, and K is the gain matrix of the extended state observer.
[0029] The K parameter, or β parameter, obtained when the infinite norm of the matrix shown above is minimized is the gain matrix (design parameter) of the extended state observer. As long as the gain matrix K of the extended state observer is designed appropriately in advance, the required total disturbance can be obtained based on the disturbance observed by the extended state observer.
[0030] Furthermore, the gain matrix K of the extended state observer satisfies the following formula:
[0031] min||T wz || ∞ =min||(1+PK) -1 || ∞ ;
[0032] Where, min|||| ∞ T represents the minimum value of the infinity norm, P represents the state equation (controlled object) of the extended state observer, and T represents the minimum value of the infinity norm. wz The transfer function matrix from w to z represents the controller of the line-of-sight control system, where w represents the external input signal, z represents the output signal, and K represents the gain matrix of the extended state observer (the design parameter β of the extended state observer), which is designed in advance.
[0033] S2, using an extended state observer to observe the total disturbance in the horizontal direction of the robot's line of sight.
[0034] S3, acquire the horizontal disturbance in the horizontal direction of the robot's line of sight as measured by the magnetometer in the robot.
[0035] Specifically, the magnetometer is usually located on the control board of the robot's gyroscope and is a component inherent to most robots.
[0036] S4, based on the principle of complementarity, performs data fusion processing on the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer to obtain the total disturbance of the carrier.
[0037] Furthermore, based on the principle of complementarity, the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer are fused to obtain the total disturbance of the carrier. Specifically, this includes: inputting the total disturbance in the horizontal direction of the robot's line of sight observed by the extended state observer into a low-pass filter for low-pass filtering; inputting the horizontal disturbance in the horizontal direction of the robot's line of sight measured by the magnetometer into a high-pass filter for high-pass filtering; and fusing the total disturbance w1 after low-pass filtering and the horizontal disturbance w2 after high-pass filtering to obtain the total disturbance of the carrier.
[0038] Specifically, such as Figure 3 As shown, the total disturbance in the horizontal direction of the robot's line of sight observed by the extended state observer is input into a low-pass filter and processed to obtain w1. The horizontal disturbance in the horizontal direction of the robot's line of sight measured by the magnetometer is input into a high-pass filter and processed to obtain w2. Then, w1 and w2 are fused to obtain the total disturbance of the carrier.
[0039] In a specific embodiment of the present invention, the total carrier disturbance is obtained according to the following formula:
[0040] Total disturbance of the carrier = a1*w1 + a2*w2;
[0041] Where a1 is the first coefficient, a2 is the second coefficient, and a1+a2=1, w1 is the total perturbation after low-pass filtering, and w2 is the horizontal perturbation after high-pass filtering.
[0042] a1 and a2 are set in advance according to the actual situation, as long as a1+a2=1.
[0043] In another embodiment of the present invention, data fusion can also be achieved by methods such as Kalman filtering.
[0044] S5 inputs the total disturbance of the carrier into the input of the robot's line-of-sight control system for feedforward compensation.
[0045] Specifically, by fusing data from the interference observer and magnetometer in the horizontal direction, the problem of magnetometers being easily affected by external magnetic fields can be solved. The fused data, i.e., the total disturbance of the carrier, is then input to the input of the robot's line-of-sight control system for feedforward compensation, which can solve the problem of delayed compensation information from the gyroscope.
[0046] In summary, the robot's line-of-sight stabilization control method according to embodiments of the present invention, by constructing an interference observer and fusing magnetometer data in the horizontal direction of the robot's line of sight, and using feedforward compensation based on the fused data to compensate the robot's line of sight in a timely manner before errors occur, can better reduce external interference, suppress high-frequency interference, reduce line-of-sight control errors, and thus improve the robot's control accuracy.
[0047] Corresponding to the above-described robot line-of-sight stabilization control method, this invention also proposes a robot line-of-sight stabilization control device. Since the device embodiments of this invention correspond to the method embodiments described above, details not disclosed in the device embodiments can be referred to in the method embodiments, and will not be repeated here.
[0048] Figure 4 A block diagram of a robot's line-of-sight stabilization control device according to an embodiment of the present invention, as shown below. Figure 4 As shown, the device includes: a design module 1, an observation module 2, an acquisition module 3, a fusion module 4, and a compensation module 5.
[0049] The design module 1 is used to design an extended state observer based on the H∞ principle; the observation module 2 is used to observe the total disturbance in the horizontal direction of the robot's line of sight using the extended state observer; the acquisition module 3 is used to acquire the horizontal disturbance in the horizontal direction of the robot's line of sight measured by the magnetometer in the robot; the fusion module 4 is used to perform data fusion processing on the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer based on the complementarity principle to obtain the total disturbance of the carrier; and the compensation module 5 is used to input the total disturbance of the carrier into the input of the robot's line of sight control system for feedforward compensation.
[0050] According to one embodiment of the present invention, the fusion module 4 is specifically used to: input the total disturbance in the horizontal direction of the robot's line of sight observed by the extended state observer into a low-pass filter for low-pass filtering; input the horizontal disturbance in the horizontal direction of the robot's line of sight measured by the magnetometer into a high-pass filter for high-pass filtering; and fuse the total disturbance w1 after low-pass filtering and the horizontal disturbance w2 after high-pass filtering to obtain the total disturbance of the carrier.
[0051] According to an embodiment of the present invention, the fusion module 4 specifically obtains the total carrier perturbation according to the following formula: total carrier perturbation = a1*w1 + a2*w2; where a1 is the first coefficient, a2 is the second coefficient, and a1+a2=1, w1 is the total perturbation after low-pass filtering, and w2 is the horizontal perturbation after high-pass filtering.
[0052] The robot's line-of-sight stabilization control device according to an embodiment of the present invention, by constructing an interference observer and fusing magnetometer data in the horizontal direction of the robot's line of sight, and using feedforward compensation based on the fused data to compensate the robot's line of sight in a timely manner before errors occur, can better reduce external interference, suppress high-frequency interference, reduce line-of-sight control errors, and thus improve the robot's control accuracy.
[0053] In the description of this invention, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. "A plurality of" means two or more, unless otherwise explicitly specified.
[0054] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Furthermore, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.
[0055] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.
Claims
1. A method for stabilizing the line of sight of a robot, characterized in that, Includes the following steps: Design of an extended state observer based on the H∞ principle; The total disturbance in the horizontal direction of the robot's line of sight is observed using the extended state observer; Obtain the horizontal perturbation in the horizontal direction of the robot's line of sight as measured by the magnetometer in the robot; Based on the principle of complementarity, the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer are fused together to obtain the total disturbance of the carrier. The total disturbance of the carrier is input into the input of the robot's line-of-sight control system for feedforward compensation; Based on the complementarity principle, the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer are fused to obtain the total disturbance of the carrier, specifically including: The total disturbance input to the horizontal direction of the robot's line of sight by the extended state observer is low-pass filtered. The horizontal disturbance in the horizontal direction of the robot's line of sight, measured by the magnetometer, is input into a high-pass filter for high-pass filtering. The total disturbance w1 after low-pass filtering and the horizontal disturbance w2 after high-pass filtering are fused to obtain the total disturbance of the carrier.
2. The line-of-sight stabilization control method for a robot according to claim 1, characterized in that, The total disturbance of the carrier is obtained specifically according to the following formula: Total disturbance of the carrier = a1 w1+a2 w2; Where a1 is the first coefficient, a2 is the second coefficient, and a1+a2=1, w1 is the total disturbance after low-pass filtering, and w2 is the horizontal disturbance after high-pass filtering.
3. A line-of-sight stabilization control device for a robot, characterized in that, include: Design module, which is used to design an extended state observer based on the H∞ principle; An observation module is used to observe the total disturbance in the horizontal direction of the robot's line of sight using the extended state observer; The acquisition module is used to acquire the horizontal disturbance in the horizontal direction of the robot's line of sight as measured by the magnetometer in the robot; The fusion module is used to perform data fusion processing on the total disturbance observed by the extended state observer and the horizontal disturbance measured by the magnetometer based on the complementarity principle, so as to obtain the total disturbance of the carrier. A compensation module is used to input the total disturbance of the carrier into the input of the robot's line-of-sight control system for feedforward compensation. The fusion module is specifically used to: perform low-pass filtering on the total disturbance input to the horizontal direction of the robot's line of sight observed by the extended state observer; The horizontal disturbance in the horizontal direction of the robot's line of sight, measured by the magnetometer, is input into a high-pass filter for high-pass filtering. The total disturbance w1 after low-pass filtering and the horizontal disturbance w2 after high-pass filtering are fused to obtain the total disturbance of the carrier.
4. The line-of-sight stabilization control device for a robot according to claim 3, characterized in that, The fusion module specifically obtains the total perturbation of the carrier according to the following formula: Total disturbance of the carrier = a1 w1+a2 w2; Where a1 is the first coefficient, a2 is the second coefficient, and a1+a2=1, w1 is the total disturbance after low-pass filtering, and w2 is the horizontal disturbance after high-pass filtering.
5. A robot, characterized in that, Includes the line-of-sight stabilization control device for the robot according to claim 3 or 4.
Citation Information
Patent Citations
Disturbance decoupling and suppression method based on gyroscope and fine television signal fusion
CN113358114A
