Pendulum-Type Spherical Robot Attitude Disturbance Compensation Control Method and System

Abstract

The present invention discloses a spherical robot attitude disturbance compensation control method and system. The control method includes: S1, the nutation angle θ and the spin angle #imgabs0#; S2, subtracting the nutation angle θ and the spin angle #imgabs1# from the real-time nutation angle y of the robot θ and the real-time spin angle #imgabs2# to obtain the error quantity e θ and #imgabs3# and differentiating them to obtain the differential quantities #imgabs4# and #imgabs5#. S3, inputting e θ and #imgabs6# and #imgabs7# and the quantity #imgabs8# into a fuzzy PID controller respectively to obtain the control quantities u θ and #imgabs9#. S4, combining u θ and #imgabs10# and transmitting them to the pendulum spherical robot for execution, collecting the attitude angle and angular velocity of the spherical robot, and outputting y θ and #imgabs11#. S5, sending the attitude angle, angular velocity and the control quantity u pid to the nonlinear disturbance observer to output the estimated disturbance #imgabs12#. S6, subtracting the disturbance #imgabs13# from the control quantity u pid to obtain the control quantity #imgabs14# and sending it to the pendulum spherical robot, and repeating steps S2 to S6 until the attitude adjustment is completed.

Description

Technical Field

[0001] The present invention relates to the technology of robot attitude compensation, and particularly to a control method and system for attitude disturbance compensation of a pendulum spherical robot. Background Art

[0002] In recent years, with the accelerated development of information and manufacturing technologies, robot technologies have begun to be applied in various industries. The concept scope of robots has become increasingly extensive, and various new forms of robots have emerged. Among them, special robots with special-shaped structures have gradually become a research hotspot.

[0003] In special environments, traditional robots cannot meet the task requirements, and various special robots suitable for complex environments have emerged. Spherical robots have attracted much attention due to their unique shapes. They have high motion flexibility and environmental adaptability and can be used for environmental reconnaissance, information collection, and other aspects. However, the control of pendulum spherical robots in complex environments becomes more difficult. At the same time, there are various disturbances in the environment, and the commonly used PID controller in engineering cannot effectively suppress the disturbances in the environment, resulting in poor motion effects and robot attitude swaying.

[0004] As an underactuated system, the spherical robot has static stability. However, when there are external disturbances, the robot will exhibit non-linear oscillation phenomena, which will have a relatively serious impact on the control system and measurement sensors, and will also reduce the motion stability of the spherical robot. Summary of the Invention

[0005] Aiming at the above deficiencies in the prior art, the control method and system for attitude disturbance compensation of the pendulum spherical robot provided by the present invention solve the problem that the motion stability of the pendulum spherical robot is poor due to external disturbances.

[0006] To achieve the above invention purpose, the technical solution adopted by the present invention is as follows:

[0007] In the first aspect, a control method for attitude disturbance compensation of a pendulum spherical robot is provided, which includes the steps of:

[0008] S1. Receive the nutation angle θ and the rotation angle transmitted from the remote control end / sensor

[0009] S2. Subtract the nutation angle θ and the rotation angle from the real-time nutation angle y θ and the real-time rotation angle of the robot to obtain the error quantity e θ and Differentiate the error quantity e θ and to obtain the differential quantity and

[0010] S3. Input the error quantity e θ and the differential quantity as well as the error quantity and the differential quantity into a fuzzy PID controller respectively to obtain the control quantities u θ and

[0011] S4. Combine u θ and and transmit the combination to the pendulum spherical robot for execution, collect the attitude angle and angular velocity of the pendulum spherical robot, and output the real-time nutation angle y and the real-time spin angle θ and

[0012] S5. Send the attitude angle, angular velocity and the control quantity u pid to the non-linear disturbance observer, and output the estimated disturbance

[0013] S6. Subtract the disturbance from the control quantity u pid to obtain the control quantity after compensating the disturbance and send it to the pendulum spherical robot, and repeat steps S2 to S6 until the attitude adjustment is completed.

[0014] Furthermore, the dynamic equation of the pendulum spherical robot in matrix form is expressed as:

[0015]

[0016] where Q = [0, 0, 0, τ1, τ2] T is the motor input torque of the spherical robot; τ1 and τ2 are the output torques of the drive motors inside the pendulum spherical robot respectively; is the second derivative of q; is the velocity coupling vector of the spherical robot dynamic model.

[0017] Furthermore, the expression of the second derivative of q is:

[0018]

[0019] where ω is the environmental disturbance.

[0020] Furthermore, the expression of the state equation f(x) is:

[0021]

[0022] where, is the velocity coupling vector of the spherical robot dynamics model.

[0023] The beneficial effect of the above technical solution is: a non - linear control model of the spherical robot is constructed, providing a basis for the subsequent establishment of a disturbance observer.

[0024] Furthermore, the mathematical expression of the non - linear disturbance observer is:

[0025]

[0026]

[0027] where, is the state equation; z is the real - time state, which is equal to the integral of ; and are both coefficient functions of the disturbance observer; K s is a symmetric coefficient matrix; g2(x) = g1(x) are both intermediate parameters; M(q) is the inertia matrix of the pendulum - type spherical robot dynamics model; is the disturbance; is the angular velocity, q is the generalized coordinate of the pendulum - type spherical robot; θ, ψ, α, γ are the nutation angle, spin angle, precession angle, pendulum angle of the spherical robot's spherical shell and the rotation angle of the internal frame respectively; [.] T is the transpose; is the first - order derivative of q.

[0028] The beneficial effect of the above technical solution is: a disturbance observer based on the spherical robot dynamics equation is constructed for the motion of the spherical robot, which can eliminate the external disturbance received during the motion of the spherical robot in real - time, improve the attitude stability performance of the spherical robot, and make the spherical robot easy to control.

[0029] Furthermore, the method of obtaining the control quantity u θ / by using a fuzzy PID controller includes:

[0030] The fuzzy controller receives the error quantity e θ and the differential quantity or the error quantity and the differential quantity and performs fuzzy processing;

[0031] Combined with fuzzy rule reasoning, defuzzify the data after fuzzy processing, and output the change values of the three parameters KP, KI, and KD in the PID controller;

[0032] The PID controller performs error control based on the change value of the fuzzy controller and outputs the control quantity u θ or the control quantity

[0033] The beneficial effects of the above technical solution are as follows: By introducing a fuzzy controller and a PID controller, the adaptability of the spherical robot in different environments is effectively improved, and the control effect is enhanced.

[0034] In the second aspect, an attitude disturbance control system for an attitude disturbance compensation control method of a pendulum spherical robot is provided, which includes a non-linear disturbance observer and at least one PID fuzzy controller; the design method of the non-linear disturbance observer is as follows:

[0035] According to the Lagrange method, the dynamic equation of the pendulum spherical robot is constructed, and the expression in matrix form is:

[0036]

[0037] where Q = [0, 0, 0, τ1, τ2] T is the motor input torque of the spherical robot; τ1 and τ2 are the output torques of the driving motors inside the pendulum spherical robot respectively; is the second derivative of q; is the velocity coupling vector of the spherical robot dynamic model;

[0038] Based on the dynamic equation of the pendulum spherical robot, a non-linear disturbance observer is designed:

[0039]

[0040]

[0041] where, is the state equation; z is the real-time state, which is equal to the integral of; and are both coefficient functions of the disturbance observer; K s is a symmetric coefficient matrix; g2(x) = g1(x) are both intermediate parameters; M(q) is the inertia matrix of the pendulum spherical robot dynamic model; is the disturbance; is the angular velocity, q is the generalized coordinate of the pendulum spherical robot; θ, ψ, α, γ are the nutation angle, spin angle, precession angle, pendulum angle of the pendulum and the rotation angle of the internal frame of the spherical robot respectively; [.] T is the transpose; is the first derivative of q;

[0042] Design a fuzzy PID controller for a pendulum spherical robot. The fuzzy PID controller includes a fuzzy controller and a PID controller.

[0043] The fuzzy controller is used to delimit the error range and the range of the differential of the error. The high-order membership function is used to fuzzify the ranges of the error and its differential, which are delimited into seven categories: 'NB', 'NM', 'NS', 'ZO', 'PS', 'PM', and 'PB'.

[0044] The triangular membership function is used to construct the fuzzy rules of KP, KI, and KD, and the corresponding variables and membership functions of the three parameters KP, KI, and KD are obtained according to the fuzzy rules. The centroid method is used for defuzzification to obtain the change values of the three parameters KP, KI, and KD in the PID controller.

[0045] The PID controller is used to perform error control according to the change value output by the fuzzy controller and output the control quantity for controlling the movement of the pendulum spherical robot.

[0046] The beneficial effects of the present invention are as follows: (1) The disturbance observer system provided by this solution is used for disturbance compensation in the control process, which can effectively reduce the influence of system and external disturbances on the pendulum spherical robot during movement, improve the control accuracy, and enhance the movement control effect.

[0047] (2) The motion and attitude control system provided by this solution effectively combines the fast response ability of the PID controller and the anti-interference ability of the disturbance observer. In a complex environment, it can efficiently perform motion attitude adjustment and motion control, ensuring the stability of motion control. Description of the Drawings

[0048] Figure 1 It is a flowchart of the attitude disturbance compensation control method for the pendulum spherical robot.

[0049] Figure 2 It is a schematic structural diagram of the attitude disturbance compensation control for the pendulum spherical robot.

[0050] Figure 3 It is a schematic diagram of the spherical robot coordinate system applied in this solution.

[0051] Figure 4 It is a schematic diagram of the simulation results in the embodiment provided by this solution. Detailed Embodiment

[0052] The following describes the specific embodiments of the present invention to facilitate the understanding of those skilled in the art of this technology. It should be clear that the present invention is not limited to the scope of the specific embodiments. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention defined and determined by the appended claims, these changes are obvious, and all inventions created using the concept of the present invention are within the scope of protection.

[0053] In a first aspect, a method for compensating attitude disturbance control of a pendulum spherical robot is provided, which includes the steps of:

[0054] In step S1, receive the nutation angle θ and the rotation angle transmitted from the remote control end / sensor

[0055] In step S2, subtract the nutation angle θ and the rotation angle from the real-time nutation angle y θ and the real-time rotation angle of the robot to obtain the error quantity e θ and Differentiate the error quantity e θ and to obtain the differential quantity and

[0056] In step S3, input the error quantity e θ and the differential quantity and the error quantity and the differential quantity into a fuzzy PID controller respectively to obtain the control quantities u θ and

[0057] In step S4, combine u θ and into and transmit it to the pendulum spherical robot for execution, collect the attitude angle and angular velocity of the pendulum spherical robot, and output the real-time nutation angle y θ and the real-time rotation angle

[0058] In step S5, send the attitude angle, angular velocity and the control quantity u pid to the nonlinear disturbance observer, and output the estimated disturbance

[0059] During implementation, the preferred mathematical expression of the nonlinear disturbance observer in this solution is:

[0060]

[0061]

[0062] wherein, is the state equation; z is the real-time state, which is equal to the integral of ; and are both coefficient functions of the disturbance observer; K s is a symmetric coefficient matrix; g2(x) = g1(x) are both intermediate parameters; M(q) is the inertia matrix of the dynamic model of the pendulum spherical robot; is the disturbance; is the angular velocity, q is the generalized coordinate of the pendulum spherical robot; θ, ψ, α, γ are respectively the nutation angle, the spin angle, the precession angle of the spherical shell of the spherical robot, the pendulum angle of the pendulum and the rotation angle of the internal frame; [.] T is the transpose; is the first derivative of q.

[0063] In step S6, subtract the disturbance from the control quantity u pid to obtain the control quantity after compensating for the disturbance, and send it to the pendulum spherical robot, and repeat steps S2 to S6 until the attitude adjustment is completed.

[0064] The following is an explanation of the detailed design process of the nonlinear disturbance observer provided by this solution:

[0065] Based on the dynamic model of the spherical robot, design the coefficient function L(x) of the disturbance observer. For the dynamic model, add the known environmental disturbance ω and perform matrix operations to obtain:

[0066]

[0067] Select the state variable to construct the nonlinear state space equation of the spherical robot. It can be known that the mathematical expression of the state space equation is:

[0068]

[0069] wherein, is the state equation, y = h(x) is the output equation. Further, simplifying the expression of formula 3 shows that:

[0070]

[0071] wherein,

[0072] Thus, a set of nonlinear state space equations of the spherical robot is established. Based on the set of nonlinear state space equations, construct the relationship between the estimated disturbance and its differential component, and design the estimated disturbance as:

[0073]

[0074] Among them, is the observed value of the disturbance, and it is set that K s is a symmetric coefficient matrix, specifically K s = diag(a, b,...), where a and b are both constants. Considering that the main disturbance in the movement of the spherical robot is the interference of the external movement environment, and the disturbance changes slowly relative to the system, then Define the auxiliary function Among them, p(x) is the coefficient function of the disturbance observer, and its value is Taking the derivative of the auxiliary function, we can get:

[0075]

[0076] Combining the auxiliary function and performing simple transposition, a disturbance observer based on the nonlinear model of the spherical robot can be constructed:

[0077]

[0078] Among them, the dynamic equation of the disturbance observer is

[0079] During implementation, the expression of the dynamic equation of the pendulum spherical robot involved in this solution in matrix form is:

[0080]

[0081] Among them, Q = [0, 0, 0, τ1, τ2] T is the motor input torque of the spherical robot; τ1 and τ2 are the output torques of the drive motors inside the pendulum spherical robot respectively; is the second derivative of q; is the velocity coupling vector of the spherical robot dynamic model.

[0082] During the construction of the dynamic equation of the pendulum spherical robot, three Euler angles of the spherical shell of the pendulum spherical robot and two rotation angles of the internal drive pendulum are selected as the generalized coordinates, that is The schematic diagram of the spherical robot coordinate system can be referred to Figure 2 .

[0083] In an embodiment of the present invention, the method for the fuzzy PID controller to obtain the control quantity u θ / includes:

[0084] The fuzzy controller receives the error quantity e θ and the differential quantity or error quantity and differential quantity and perform fuzzification;

[0085] Combine fuzzy rule inference to defuzzify the fuzzified data, and output the change values of the three parameters KP, KI, and KD in the PID controller;

[0086] The PID controller performs error control according to the change value of the fuzzy controller and outputs the control quantity u θ or control quantity

[0087] such as Figure 3 As shown, this solution also provides an attitude disturbance control system applied to the attitude disturbance compensation control method of a pendulum spherical robot, which includes a non-linear disturbance observer and at least one PID fuzzy controller; if multiple attitude angle controls need to be realized, multiple PID fuzzy controllers are connected in parallel, and the multiple output control quantities perform subtraction operations with the multi-dimensional observation errors output by the non-linear disturbance observer and are sent to the pendulum spherical robot to achieve the purpose of eliminating disturbances.

[0088] Among them, the design method of the non-linear disturbance observer is:

[0089] According to the Lagrange method, construct the dynamic equation of the pendulum spherical robot, and the expression in matrix form is:

[0090]

[0091] Among them, Q = [0, 0, 0, τ1, τ2] T is the motor input torque of the spherical robot; τ1 and τ2 are the output torques of the drive motors inside the pendulum spherical robot respectively; is the second derivative of q; is the velocity coupling vector of the spherical robot dynamic model;

[0092] Based on the dynamic equation of the pendulum spherical robot, design a non-linear disturbance observer:

[0093]

[0094]

[0095] Among them, is the state equation; z is the real-time state, which is equal to the integral of; and are both coefficient functions of the disturbance observer; K sis a symmetric coefficient matrix; g2(x) = g1(x) are both intermediate parameters; M(q) is the inertia matrix of the dynamic model of the pendulum spherical robot; is a perturbation; is the angular velocity, q is the generalized coordinate of the pendulum spherical robot; θ, ψ, α, γ are respectively the nutation angle, spin angle, precession angle of the spherical robot's spherical shell, the pendulum angle of the pendulum weight, and the rotation angle of the internal frame; [.] T is the transpose; is the first derivative of q;

[0096] Design a fuzzy PID controller for the pendulum spherical robot. The fuzzy PID controller includes a fuzzy controller and a PID controller.

[0097] The fuzzy controller is used to delimit the error range and the error differential range, and uses a high-order membership function to fuzzify the ranges of the error and its differential, delimiting them into seven categories: 'NB', 'NM', 'NS', 'ZO', 'PS', 'PM', 'PB';

[0098] Use a triangular membership function to construct fuzzy rules for KP, KI, and KD, and obtain the corresponding variables and membership functions of the membership functions of the three parameters KP, KI, and KD according to the fuzzy rules. Use the centroid method for defuzzification to obtain the change values of the three parameters KP, KI, and KD in the PID controller;

[0099] The PID controller is used to perform error control according to the change values output by the fuzzy controller, and output a control quantity for controlling the movement of the pendulum spherical robot.

[0100] The following is a detailed description of the design process of the fuzzy PID controller:

[0101] After obtaining the error quantity and its differential, delimit the error range according to experimental experience, e k ∈[-50,50], k = θ, The membership function is selected as Gaussian type, and its expression is g(x; c, σ), where c represents the center of the membership function and σ represents the width of the membership function. Based on the Gaussian membership function, it is divided into seven parts: 'NB', 'NM', 'NS', 'ZO', 'PS', 'PM', 'PB' to fuzzify the error. According to the change amounts of the three parameters KP, KI, KD ∈ [-1,1], design three groups of fuzzy rules KP, KI, and KD using a triangular membership function, as shown in Tables 1 - 3.

[0102] Table 1 Fuzzy rule KP

[0103]

[0104] Table 2 Fuzzy Rule KI

[0105]

[0106] Table 3 Fuzzy Rule KD

[0107]

[0108] The membership functions of the three parameters obtained according to the three rules correspond to the variable y p 、y i 、y d and the membership function u p (y), u i (y), u d (y). The centroid method is used for defuzzification. For the pendulum spherical robot, it can be used to obtain the output value, where u(y) is the membership function of the parameter, y is the variable corresponding to the membership function of the parameter, and then the output value is assigned to the three parameters of the PID controller, and finally the three control parameters of the PID controller are obtained.

[0109] The following is an illustration of the effect of the attitude disturbance control method of this solution in combination with simulation:

[0110] For the angular velocity control of the nutation angle θ, a random disturbance ω is added to the spherical robot model, specifically a continuous random quantity with a mean of 0 and a variance of 5. Simulate the attitude control of the spherical robot under strong wind disturbance and bumpy road disturbance. Verify the control effect of the combination of fuzzy PID and nonlinear disturbance observer (NDOB), and use active disturbance rejection control (ADRC) and fuzzy PID (FPID) as the control groups respectively. The specific simulation can be referred to Figure 4 .

[0111] In Figure 4 , the 4 curves are the angular velocity control of the spherical robot θ, which are respectively represented as the ideal control curve, the velocity control curves under NDOB, ADRC, and FPID control; through the comparison of the 4 curves, it can be seen that the control system combining fuzzy PID and nonlinear disturbance observer has a higher disturbance suppression effect, and its performance is better than that of the ADRC controller and the fuzzy PID controller. Therefore, it can be known that this solution has advantages such as real-time and fast compensation for the attitude disturbance of the spherical robot.

Claims

1. A control method for attitude disturbance compensation of a pendulum spherical robot, characterized in that, Including the steps: S1. Receive the nutation angle sent from the remote control end / transmitted by the sensor θ and the rotation angle φ ; S2. Calculate the difference between the nutation angle θ and the spin angle φ of the robot and the real-time nutation angle y θ and the real-time spin angle y φ to obtain the error quantity e θ and e φ . Differentiate the error quantity e θ and e φ to obtain the differential quantity and ; S3. Input the error quantity e θ and the differential quantity as well as the error quantity e φ and the differential quantity into a fuzzy PID controller respectively to obtain the control quantities u θ and u φ ; S4. Combine u θ and u φ to form u pid = [0, 0, …, u θ , u φ T and transmit it to the pendulum spherical robot for execution, collect the attitude angle and angular velocity of the pendulum spherical robot, and output the real-time nutation angle y θ and the real-time spin angle y φ ;​ S5. Send the attitude angle, angular velocity, and control quantity u pid to the nonlinear disturbance observer, and output the estimated disturbance ; The mathematical expression of the non - linear disturbance observer is: ， Among them, is the state equation; is the real-time state, which is equal to the integral of; and are both coefficient functions of the disturbance observer; is a symmetric coefficient matrix; are both intermediate parameters; is the inertia matrix of the dynamic model of the pendulum spherical robot; is the disturbance; is the angular velocity, is the generalized coordinate of the pendulum spherical robot; are respectively the nutation angle, the spin angle, the precession angle, the pendulum angle of the pendulum and the rotation angle of the inner frame of the spherical robot;[.] T is the transpose; is the first derivative of; is the state equation; S6. Subtract the disturbance from the control quantity u pid to obtain the control quantity after compensating for the disturbance , and send it to the pendulum spherical robot, and repeat steps S2 to S6 until the attitude adjustment is completed.

2. The attitude disturbance compensation control method of the pendulum spherical robot according to claim 1, characterized in that The dynamic equation of the pendulum - type spherical robot in matrix form is: ， Among them, is the motor input torque of the spherical robot; are respectively the output torques of the driving motors inside the pendulum spherical robot; is the second derivative of; is the velocity coupling vector of the dynamic model of the spherical robot; is the environmental disturbance.

3. The attitude disturbance compensation control method for the pendulum spherical robot according to claim 2, characterized in that, The second derivative of is expressed as: Among them, is the environmental disturbance.

4. The attitude disturbance compensation control method of the pendulum spherical robot according to claim 1, characterized in that The state equation has the following expression: Among them, is the velocity coupling vector of the spherical robot dynamics model.

5. The attitude disturbance compensation control method for a pendulum spherical robot according to any one of claims 1-4, characterized in that, The method of obtaining the control quantity by using a fuzzy PID controller / includes: The fuzzy controller receives the error quantity and the differential quantity or the error quantity and the differential quantity , and performs fuzzification processing; Combining fuzzy rule reasoning to defuzzify the data after fuzzy processing, and outputting the variation values of the three parameters KP, KI, and KD in the PID controller; The PID controller performs error control based on the change value of the fuzzy controller and outputs a control quantity or a control quantity .

6. An attitude disturbance control system for the attitude disturbance compensation control method of the pendulum spherical robot according to any one of claims 1-5, characterized in that, Including a non - linear disturbance observer and at least one PID fuzzy controller; the design method of the non - linear disturbance observer is: According to the Lagrange method, construct the dynamic equation of the pendulum - type spherical robot, and its expression in matrix form is: ， Among them, is the motor input torque of the spherical robot; are the output torques of the driving motors inside the pendulum spherical robot respectively; is the second derivative of; is the velocity coupling vector of the spherical robot dynamics model; Based on the dynamic equation of the pendulum - type spherical robot, design a non - linear disturbance observer: ， Among them, is the state equation; is the real-time state, which is equal to the integral of ; and are both coefficient functions of the disturbance observer; is a symmetric coefficient matrix; are both intermediate parameters; is the inertia matrix of the dynamic model of the pendulum spherical robot; is the disturbance; is the angular velocity, is the generalized coordinate of the pendulum spherical robot; are respectively the nutation angle, the spin angle, the precession angle of the spherical shell of the spherical robot, the pendulum angle of the pendulum and the rotation angle of the internal frame;[.] T is the transpose; is 's first derivative; Design the fuzzy PID controller of the pendulum - type spherical robot. The fuzzy PID controller includes a fuzzy controller and a PID controller, The fuzzy controller is used to delimit the error range and the error differential range, and uses a high - order membership function to perform fuzzy processing on the ranges of the error and its differential, delimiting them into seven categories: 'NB', 'NM', 'NS', 'ZO', 'PS', 'PM', 'PB'; Construct the fuzzy rules of KP, KI, and KD using triangular membership functions, and obtain the corresponding variables and membership functions of the three parameters KP, KI, and KD according to the fuzzy rules. Use the centroid method for defuzzification to obtain the variation values of the three parameters KP, KI, and KD in the PID controller; The PID controller is used to perform error control according to the variation values output by the fuzzy controller, and output a control quantity for controlling the motion of the pendulum - type spherical robot.

Images