A control method, control system and storage medium for a three-axis momentum wheel self-balancing robot

Abstract

The present invention relates to a control method, control system and storage medium for a three-axis momentum wheel self-balancing robot. By obtaining the coefficient matrix of the state space equation of the three-axis momentum wheel, the three-axis momentum wheel is controlled by a single-chip microcomputer. When the three-axis momentum wheel controls the posture of the robot, the coefficient matrix of the three-axis momentum wheel is solved. By inputting the coefficient matrix into the single-chip microcomputer, the three-axis momentum wheel is controlled, and the control is achieved for achieving precise adjustment of the robot's posture, thereby improving the robot's operational accuracy.

Description

Technical Field

[0001] The present application relates to the field of robotic equipment, and specifically to a control method, control system and storage medium for a three-axis momentum wheel self-balancing robot. Background Art

[0002] A three-axis momentum wheel is a complex subsystem for posture control in robots. Due to its advantages such as long life, high precision, high agility, and high stability, it is widely used in inertial devices and products such as robots. Unlike other types of electromechanical products, momentum wheels must meet high reliability requirements to ensure precise operation of robots.

[0003] However, in the existing technology, there is no feedback mechanism during the operation of the three-axis momentum wheel, which leads to the problem of insufficient accuracy of the momentum wheel in the process of adjusting the robot's posture. Therefore, the existing technology urgently needs a control method for a three-axis momentum wheel self-balancing robot to improve the accuracy of the momentum wheel in the process of adjusting the robot's posture. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to address the deficiencies of the above technical solutions and provide a control method and control system for a three-axis momentum wheel self-balancing robot, which is used to achieve precise adjustment of the robot's posture, thereby improving the robot's operational accuracy.

[0005] In order to achieve the above object, according to one aspect of the present invention, a control method for a three-axis momentum wheel self-balancing robot is provided, comprising the following steps:

[0006] A control method for a three-axis momentum wheel self-balancing robot comprises the following steps:

[0007] Step 1: Obtain motion information of the connecting rod 1 of the three-axis momentum wheel;

[0008] Step 2: Obtain motion information of the connecting rod 2 of the three-axis momentum wheel;

[0009] Step 3: Obtaining motion information of the connecting rod 3 of the three-axis momentum wheel;

[0010] Step 4: Obtaining motion information of the connecting rod 4 of the three-axis momentum wheel;

[0011] Step 5: Obtain the force balance equation and moment balance equation of the connecting rod 1-4 according to the expressions obtained in steps 1-4;

[0012] Step 6: Create a three-rigid body model, set the zero point fit, and establish the inverted rod coordinate system in assembly mode;

[0013] Step 7: Linearize the Euler angle rotation matrix according to the inverted pole coordinate system established in step 6;

[0014] Step 8: Use a DC brush motor to drive the three-axis momentum wheel;

[0015] Step 9: Obtain the torque equations of the momentum wheel 2 and the momentum wheel 3 of the three-axis momentum wheel;

[0016] Step 10: Obtaining the state space equation of the three-axis momentum wheel;

[0017] Step 11: Solve the state space equation to obtain the value of the coefficient matrix, and input the value of the coefficient matrix into the single chip microcomputer to control the three-axis momentum wheel.

[0018] Specifically, the three-axis momentum wheel is composed of connecting rods 1-4, wherein the connecting rod 1 is the base, and the connecting rods 2-4 are momentum wheels in the x-axis, y-axis, and z-axis directions respectively;

[0019] Specifically, the Euler angles α, β, and γ of the z-axis, x-axis, and y-axis of the connecting rod 1 are obtained through an attitude sensor (IMU) to calculate the motion information of the connecting rod 1 of the three-axis momentum wheel;

[0020] Furthermore, the motion information of the connecting rod 1 of the three-axis momentum wheel includes the angular velocity of the connecting rod 1, the acceleration of the connecting rod 1, the angular acceleration of the connecting rod 1, the acceleration of the origin of the connecting rod 1, the acceleration of the center of mass of the connecting rod 1, the inertial force of the connecting rod 1, and the mathematical expression of the inertial moment of the connecting rod 1;

[0021] Furthermore, the angular velocity of the connecting rod 1 is obtained by the angular velocity superposition principle, which specifically includes:

[0022] Step 1.1: Obtain the mathematical expression of the angular velocity of the connecting rod 1:

[0023]

[0024] Where, 1 ω i is the angular velocity of the connecting rod 1, α is the Euler angle of the Z-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, is the Euler angle angular velocity of the Z-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, e1 is a unit vector, and its expression is e1=(100) T , is the attitude matrix of the three-axis momentum wheel on the x-axis, is the Euler angle angular velocity of the attitude sensor (IMU) of the three-axis momentum wheel in the X-axis direction, e2 is a unit vector, and its expression is e2=(010) T , is the attitude matrix of the three-axis momentum wheel on the y-axis, is the Euler angle angular velocity of the Y-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, e3 is a unit vector, and its expression is e3=(001) T ;

[0025] Step 1.2: Expand the angular velocity of the connecting rod 1 to obtain the angular velocity expression of each direction of the connecting rod 1;

[0026]

[0027] Where, 1 ω 1x is the angular velocity component of the connecting rod 1 on the x-axis, 1 ω 1y is the angular velocity component of the connecting rod 1 on the y-axis, 1 ω 1z is the angular velocity component of the connecting rod 1 on the z axis, c α is the abbreviation of cosα, c β is the abbreviation of cosβ, s α is the abbreviation of sinα, s β is the abbreviation of sinβ; α is the Euler angle of the attitude sensor (IMU) of the three-axis momentum wheel in the Z-axis direction, and β is the Euler angle of the attitude sensor (IMU) of the three-axis momentum wheel in the Y-axis direction;

[0028] Furthermore, the acceleration expression of the connecting rod 1 is:

[0029]

[0030] Furthermore, the angular acceleration expression of the connecting rod 1 is:

[0031]

[0032] Furthermore, the origin acceleration expression of the connecting rod 1 is:

[0033]

[0034] Furthermore, the center-of-mass acceleration expression of the connecting rod 1 is:

[0035]

[0036] Furthermore, the inertia force expression of the connecting rod 1 is:

[0037] 1 f C1 =m1 1 a C1

[0038] Where,1 f C1 is the inertia force of connecting rod 1, m1 is the mass of connecting rod 1, 1 a C1 is the origin acceleration of connecting rod 1;

[0039] Furthermore, the inertia moment of the connecting rod 1 is expressed as:

[0040]

[0041] 1 n C1 is the inertia moment of connecting rod 1, C1 I1 is the inertia matrix of the center-of-mass coordinate system of connecting rod 1;

[0042] Specifically, the step 2 includes obtaining the angular velocity of the connecting rod 2 of the three-axis momentum wheel, the angular velocity of the connecting rod 2, the origin acceleration of the connecting rod 2, the center of mass acceleration of the connecting rod 2, the inertia force of the connecting rod 2, and the inertia moment expression of the connecting rod 2;

[0043] Furthermore, the angular velocity expression of the connecting rod 2 of the three-axis momentum wheel is:

[0044]

[0045] Furthermore, the angular acceleration expression of the connecting rod 2 of the three-axis momentum wheel is:

[0046]

[0047] Furthermore, the origin acceleration expression of the connecting rod 2 of the three-axis momentum wheel is:

[0048]

[0049] Furthermore, the center-of-mass acceleration expression of the connecting rod 2 of the three-axis momentum wheel is:

[0050] 2 a C2 = 2 a2

[0051] Furthermore, the inertial force expression of the connecting rod 2 of the three-axis momentum wheel is:

[0052] 2 f C2 =m2 2 a C2

[0053] Furthermore, the inertia moment of the connecting rod 2 of the three-axis momentum wheel is expressed as:

[0054]

[0055] Specifically, the step 3 includes obtaining the angular velocity of the connecting rod 3 of the three-axis momentum wheel, the angular velocity of the connecting rod 3, the origin acceleration of the connecting rod 3, the center of mass acceleration of the connecting rod 3, the inertia force of the connecting rod 3, and the inertia moment expression of the connecting rod 3;

[0056] Furthermore, the angular velocity expression of the connecting rod 3 of the three-axis momentum wheel is:

[0057]

[0058] Furthermore, the angular acceleration expression of the connecting rod 3 of the three-axis momentum wheel is:

[0059]

[0060] Furthermore, the origin acceleration expression of the connecting rod 3 of the three-axis momentum wheel is:

[0061]

[0062] Furthermore, the center-of-mass acceleration expression of the connecting rod 3 of the three-axis momentum wheel is:

[0063] 3 a C3 = 3 a3

[0064] Furthermore, the inertial force expression of the connecting rod 3 of the three-axis momentum wheel is:

[0065] 3 f C3 =m3 3 a C3

[0066] Furthermore, the inertia moment of the connecting rod 3 of the three-axis momentum wheel is expressed as:

[0067]

[0068] Specifically, the step 4 includes obtaining the angular velocity of the connecting rod 4 of the three-axis momentum wheel, the angular velocity of the connecting rod 4, the origin acceleration of the connecting rod 4, the center of mass acceleration of the connecting rod 4, the inertia force of the connecting rod 4, and the inertia moment expression of the connecting rod 4;

[0069] Furthermore, the angular velocity expression of the connecting rod 4 of the three-axis momentum wheel is:

[0070]

[0071] Furthermore, the angular acceleration of the connecting rod 4 of the three-axis momentum wheel is expressed as:

[0072]

[0073] Furthermore, the origin acceleration expression of the connecting rod 4 of the three-axis momentum wheel is:

[0074]

[0075] Furthermore, the center-of-mass acceleration expression of the connecting rod 4 of the three-axis momentum wheel is:

[0076] 4 a C4 = 4 a4

[0077] Furthermore, the inertial force expression of the connecting rod 4 of the three-axis momentum wheel is:

[0078] 4 f C4 =m4 4 a C4

[0079] Furthermore, the inertia moment of the connecting rod 4 of the three-axis momentum wheel is expressed as:

[0080]

[0081] Specifically, the force balance equation of the connecting rods 1-4 is:

[0082] 2 f2= 2 f C2

[0083] 3 f3= 3 f C3

[0084] 4 f4= 4 f C4

[0085]

[0086] Specifically, the moment balance equation of the connecting rod 1-4 is:

[0087] 2 n2= 2 n C2

[0088] 3 n3= 3 n C3

[0089] 4 n4= 4 n C4

[0090]

[0091] Specifically, in the assembly mode, the subassembly's connected coordinate system cannot be used as a reference coordinate system for mass property measurement. Therefore, this embodiment establishes an inverted rod coordinate system in the assembly mode.

[0092] Specifically, based on the simplification of angular velocity and angular velocity, the Euler angle rotation matrix is ​​linearized to obtain the following formula:

[0093]

[0094] At the same time, the angular velocity of link 1 can be converted into:

[0095]

[0096] At the same time, the angular acceleration of link 1 can be converted into:

[0097]

[0098] Specifically, the torque model of the DC brushed motor drive is:

[0099]

[0100] Specifically, the torque equations of the momentum wheel 2 and the momentum wheel 3 of the three-axis momentum wheel are:

[0101]

[0102] Specifically, the five motion parameters of the three-axis momentum wheel are selected The angular velocities (θ2, θ3) (θ2, θ3) of the momentum wheel 2 and the momentum wheel 3 of the three-axis momentum wheel are used as inputs of the state-space equation, and the outputs are (α, β), thereby obtaining the state-space equation of the three-axis momentum wheel;

[0103] Furthermore, the state equation is:

[0104]

[0105] y=Cx+D

[0106] Where A, B, C, and D are coefficient matrices;

[0107]

[0108] u=(u2 u3) T

[0109] Specifically, the value of the coefficient matrix is ​​obtained by the state space equation of the three-axis momentum wheel established in step 10, and the value can realize negative feedback of the three-axis momentum wheel, and the three-axis momentum wheel is controlled according to the value of the negative feedback.

[0110] Therefore, through the setting of the above steps, this scheme enables the three-axis momentum wheel to solve the coefficient matrix of the three-axis momentum wheel when controlling the posture of the robot, and input the coefficient matrix into the single-chip microcomputer to realize the control of the three-axis momentum wheel, which is used to achieve precise adjustment of the robot's posture, thereby improving the robot's operating accuracy.

[0111] According to another aspect of the present invention, the present invention also includes a control system for a three-axis momentum wheel self-balancing robot.

[0112] According to another aspect of the present invention, the present invention also includes a computer-readable storage medium, on which a data processing program is stored, and the data processing program is executed by a processor to implement the above-mentioned control method for a three-axis momentum wheel self-balancing robot.

[0113] Based on the above technical solution, the present application provides a control method and system for a three-axis momentum wheel self-balancing robot, which has the following technical effects:

[0114] The present invention obtains the coefficient matrix of the state space equation of the three-axis momentum wheel, thereby controlling the three-axis momentum wheel through a single-chip microcomputer, so that when the three-axis momentum wheel controls the posture of the robot, the coefficient matrix of the three-axis momentum wheel is solved. By inputting the coefficient matrix into the single-chip microcomputer, the control of the three-axis momentum wheel is realized, which is used to achieve precise adjustment of the robot's posture, thereby improving the robot's operating accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0115] The accompanying drawings, which constitute part of this document, are intended to provide a further understanding of this document. The exemplary embodiments and descriptions herein are intended to explain this document and do not constitute an improper limitation on this document. In the accompanying drawings:

[0116] Figure 1 A schematic diagram of a three-axis momentum wheel provided in an embodiment of the present application;

[0117] Figure 2 A flow chart of a control method for a three-axis momentum wheel self-balancing robot provided in an embodiment of the present application;

[0118] Figure 3 This is a flow chart of a method for obtaining the angular velocity of the connecting rod 1 provided in an embodiment of the present application. DETAILED DESCRIPTION

[0119] In order to make the purpose, technical solutions and advantages of the embodiments of this document clearer, the technical solutions in the embodiments of this document will be clearly and completely described below in conjunction with the drawings in the embodiments of this document. Obviously, the described embodiments are part of the embodiments of this document, not all of the embodiments. Based on the embodiments of this document, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this document. It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of this document can be combined with each other in any way.

[0120] Unless the context clearly requires otherwise, throughout the specification and claims, the words "include," "comprising," and similar words should be construed in an inclusive sense rather than an exclusive or exhaustive sense; that is, in the sense of "including but not limited to."

[0121] Figure 1 A schematic diagram of a three-axis momentum wheel provided in an embodiment of the present application.

[0122] Example 1

[0123] like Figure 2 As shown, a control method for a three-axis momentum wheel self-balancing robot includes the following steps:

[0124] The three-axis momentum wheel is composed of connecting rods 1-4, wherein the connecting rod 1 is the base, and the connecting rods 2-4 are momentum wheels in the x-axis, y-axis, and z-axis directions respectively;

[0125] Step 1: Obtain motion information of the connecting rod 1 of the three-axis momentum wheel;

[0126] Specifically, the Euler angles α, β, and γ of the z-axis, x-axis, and y-axis of the connecting rod 1 are obtained through an attitude sensor (IMU) to calculate the motion information of the connecting rod 1 of the three-axis momentum wheel;

[0127] Furthermore, the motion information of the connecting rod 1 of the three-axis momentum wheel includes the angular velocity of the connecting rod 1, the acceleration of the connecting rod 1, the angular acceleration of the connecting rod 1, the acceleration of the origin of the connecting rod 1, the acceleration of the center of mass of the connecting rod 1, the inertial force of the connecting rod 1, and the mathematical expression of the inertial moment of the connecting rod 1;

[0128] Furthermore, the angular velocity of the connecting rod 1 is obtained by the principle of angular velocity superposition, as Figure 3 As shown, it specifically includes:

[0129] Step 1.1: Obtain the mathematical expression of the angular velocity of the connecting rod 1:

[0130]

[0131] Where, 1 ω iis the angular velocity of the connecting rod 1, α is the Euler angle of the Z-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, is the Euler angle angular velocity of the Z-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, e1 is a unit vector, and its expression is e1=(100) T , is the attitude matrix of the three-axis momentum wheel on the x-axis, is the Euler angle angular velocity of the attitude sensor (IMU) of the three-axis momentum wheel in the X-axis direction, e2 is a unit vector, and its expression is e2=(010) T , is the attitude matrix of the three-axis momentum wheel on the y-axis, is the Euler angle angular velocity of the Y-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, e3 is a unit vector, and its expression is e3=(001) T ;

[0132] Step 1.2: Expand the angular velocity of the connecting rod 1 to obtain the angular velocity expression of each direction of the connecting rod 1;

[0133]

[0134] Where, 1 ω 1x is the angular velocity component of the connecting rod 1 on the x-axis, 1 ω 1y is the angular velocity component of the connecting rod 1 on the y-axis, 1 ω 1z is the angular velocity component of the connecting rod 1 on the z axis, c α is the abbreviation of cosα, c β is the abbreviation of cosβ, s α is the abbreviation of sinα, s β is the abbreviation of sinβ; α is the Euler angle of the attitude sensor (IMU) of the three-axis momentum wheel in the Z-axis direction, and β is the Euler angle of the attitude sensor (IMU) of the three-axis momentum wheel in the Y-axis direction;

[0135] Furthermore, the acceleration expression of the connecting rod 1 is:

[0136]

[0137] Furthermore, the angular acceleration expression of the connecting rod 1 is:

[0138]

[0139] Furthermore, the origin acceleration expression of the connecting rod 1 is:

[0140]

[0141] Furthermore, the center-of-mass acceleration expression of the connecting rod 1 is:

[0142]

[0143] Furthermore, the inertia force expression of the connecting rod 1 is:

[0144] 1 f C1 =m1 1 a C1

[0145] Where, 1 f C1 is the inertia force of connecting rod 1, m1 is the mass of connecting rod 1, 1 a C1 is the origin acceleration of connecting rod 1;

[0146] Furthermore, the inertia moment of the connecting rod 1 is expressed as:

[0147]

[0148] 1 n C1 is the inertia moment of connecting rod 1, C1 I1 is the inertia matrix of the center-of-mass coordinate system of connecting rod 1;

[0149] Step 2: Obtain motion information of the connecting rod 2 of the three-axis momentum wheel;

[0150] Specifically, the step 2 includes obtaining the angular velocity of the connecting rod 2 of the three-axis momentum wheel, the angular velocity of the connecting rod 2, the origin acceleration of the connecting rod 2, the center of mass acceleration of the connecting rod 2, the inertia force of the connecting rod 2, and the inertia moment expression of the connecting rod 2;

[0151] Furthermore, the angular velocity expression of the connecting rod 2 of the three-axis momentum wheel is:

[0152]

[0153] Furthermore, the angular acceleration expression of the connecting rod 2 of the three-axis momentum wheel is:

[0154]

[0155] Furthermore, the origin acceleration expression of the connecting rod 2 of the three-axis momentum wheel is:

[0156]

[0157] Furthermore, the center-of-mass acceleration expression of the connecting rod 2 of the three-axis momentum wheel is:

[0158] 2 a C2 = 2 a2

[0159] Furthermore, the inertial force expression of the connecting rod 2 of the three-axis momentum wheel is:

[0160] 2 f C2 =m2 2 a C2

[0161] Furthermore, the inertia moment of the connecting rod 2 of the three-axis momentum wheel is expressed as:

[0162]

[0163] Step 3: Obtaining motion information of the connecting rod 3 of the three-axis momentum wheel;

[0164] Specifically, the step 3 includes obtaining the angular velocity of the connecting rod 3 of the three-axis momentum wheel, the angular velocity of the connecting rod 3, the origin acceleration of the connecting rod 3, the center of mass acceleration of the connecting rod 3, the inertia force of the connecting rod 3, and the inertia moment expression of the connecting rod 3;

[0165] Furthermore, the angular velocity expression of the connecting rod 3 of the three-axis momentum wheel is:

[0166]

[0167] Furthermore, the angular acceleration expression of the connecting rod 3 of the three-axis momentum wheel is:

[0168]

[0169] Furthermore, the origin acceleration expression of the connecting rod 3 of the three-axis momentum wheel is:

[0170]

[0171] Furthermore, the center-of-mass acceleration expression of the connecting rod 3 of the three-axis momentum wheel is:

[0172] 3 a C3 = 3 a3

[0173] Furthermore, the inertial force expression of the connecting rod 3 of the three-axis momentum wheel is:

[0174] 3 f C3 =m 33 a C3

[0175] Furthermore, the inertia moment of the connecting rod 3 of the three-axis momentum wheel is expressed as:

[0176]

[0177] Step 4: Obtaining motion information of the connecting rod 4 of the three-axis momentum wheel;

[0178] Specifically, the step 4 includes obtaining the angular velocity of the connecting rod 4 of the three-axis momentum wheel, the angular velocity of the connecting rod 4, the origin acceleration of the connecting rod 4, the center of mass acceleration of the connecting rod 4, the inertia force of the connecting rod 4, and the inertia moment expression of the connecting rod 4;

[0179] Furthermore, the angular velocity expression of the connecting rod 4 of the three-axis momentum wheel is:

[0180]

[0181] Furthermore, the angular acceleration of the connecting rod 4 of the three-axis momentum wheel is expressed as:

[0182]

[0183] Furthermore, the origin acceleration expression of the connecting rod 4 of the three-axis momentum wheel is:

[0184]

[0185] Furthermore, the center-of-mass acceleration expression of the connecting rod 4 of the three-axis momentum wheel is:

[0186] 4 a C4 = 4 a4 Going a step further, the inertial force expression of the connecting rod 4 of the three-axis momentum wheel is:

[0187] 4 f C4 =m 4 4 a C4

[0188] Furthermore, the inertia moment of the connecting rod 4 of the three-axis momentum wheel is expressed as:

[0189]

[0190] Step 5: Obtain the force balance equation and moment balance equation of the connecting rod 1-4 according to the expressions obtained in steps 1-4;

[0191] Specifically, the force balance equation of the connecting rods 1-4 is:

[0192] 2 f2= 2 f C2

[0193] 3 f3= 3 f C3

[0194] 4 f4= 4 f C4

[0195]

[0196] Specifically, the moment balance equation of the connecting rod 1-4 is:

[0197] 2 n2= 2 n C2

[0198] 3 n3= 3 n C3

[0199] 4 n4= 4 n C4

[0200]

[0201] Step 6: Create a three-rigid body model, set the zero point fit, and establish the inverted rod coordinate system in assembly mode;

[0202] Specifically, in the assembly mode, the subassembly's connected coordinate system cannot be used as a reference coordinate system for mass property measurement. Therefore, this embodiment establishes an inverted rod coordinate system in the assembly mode.

[0203] Step 7: Linearize the Euler angle rotation matrix according to the inverted pole coordinate system established in step 6;

[0204] Specifically, based on the simplification of angular velocity and angular velocity, the Euler angle rotation matrix is ​​linearized to obtain the following formula:

[0205]

[0206] At the same time, the angular velocity of link 1 can be converted into:

[0207]

[0208] At the same time, the angular acceleration of link 1 can be converted into:

[0209]

[0210] Step 8: Use a DC brush motor to drive the three-axis momentum wheel;

[0211] Specifically, the torque model of the DC brushed motor drive is:

[0212]

[0213] Step 9: Obtain the torque equations of the momentum wheel 2 and the momentum wheel 3 of the three-axis momentum wheel;

[0214] Specifically, the torque equations of the momentum wheel 2 and the momentum wheel 3 of the three-axis momentum wheel are:

[0215]

[0216] Step 10: Obtaining the state space equation of the three-axis momentum wheel;

[0217] Specifically, the five motion parameters of the three-axis momentum wheel are selected The angular velocities (θ2, θ3) of the momentum wheel 2 and the momentum wheel 3 of the three-axis momentum wheel are used as inputs of the state space equation, and the outputs are (α, β), thereby obtaining the state space equation of the three-axis momentum wheel;

[0218] Furthermore, the state equation is:

[0219]

[0220] y=Cx+D

[0221] Where A, B, C, and D are coefficient matrices;

[0222]

[0223] u=(u2 u3) T

[0224] Step 11: Solve the state space equation to obtain the value of the coefficient matrix, and input the value of the coefficient matrix into the single chip microcomputer to control the three-axis momentum wheel;

[0225] Specifically, the value of the coefficient matrix is ​​obtained by the state space equation of the three-axis momentum wheel established in step 10, and the value can realize negative feedback of the three-axis momentum wheel, and the three-axis momentum wheel is controlled according to the value of the negative feedback.

[0226] Therefore, through the setting of the above steps, this embodiment enables the three-axis momentum wheel to solve the coefficient matrix of the three-axis momentum wheel when controlling the posture of the robot, and inputs the coefficient matrix into the single-chip microcomputer to realize the control of the three-axis momentum wheel, which is used to achieve precise adjustment of the robot's posture, thereby improving the robot's operating accuracy.

[0227] Example 2

[0228] This embodiment also includes a control system for a three-axis momentum wheel self-balancing robot, wherein the system includes a processor for executing the control method of the three-axis momentum wheel self-balancing robot of the first embodiment.

[0229] Example 3

[0230] This embodiment includes a computer-readable storage medium having a data processing program stored thereon. The data processing program is executed by a processor to implement a control method for a three-axis momentum wheel self-balancing robot according to the first embodiment.

[0231] Those skilled in the art will appreciate that the embodiments herein may be provided as methods, devices (equipment), or computer program products. Therefore, this document may take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Furthermore, this document may take the form of a computer program product implemented on one or more computer-usable storage media containing computer-usable program code. Computer storage media include volatile and non-volatile, removable and non-removable media implemented in any method or technology for storing information (such as computer-readable instructions, data structures, program modules, or other data), including but not limited to RAM, ROM, EEPROM, flash memory or other memory technology, CD-ROM, digital versatile disks (DVDs) or other optical disk storage, magnetic cassettes, magnetic tape, magnetic disk storage or other magnetic storage devices, or any other medium that can be used to store desired information and can be accessed by a computer. Furthermore, it is well known to those skilled in the art that communication media typically contain computer-readable instructions, data structures, program modules, or other data in a modulated data signal such as a carrier wave or other transmission mechanism, and may include any information delivery medium.

[0232] This document is described with reference to flowcharts and / or block diagrams of methods, apparatus (devices) and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0233] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0234] Although the preferred embodiments of this invention have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concepts. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of this invention.

[0235] Obviously, those skilled in the art may make various changes and modifications to this disclosure without departing from the spirit and scope of this disclosure. Thus, if such changes and modifications fall within the scope of the claims herein and their equivalents, then this disclosure is intended to include such changes and modifications.

Claims

1. A control method for a three-axis momentum wheel self-balancing robot, comprising the following steps: Step 1: Obtain motion information of the connecting rod 1 of the three-axis momentum wheel; Step 2: Obtain motion information of the connecting rod 2 of the three-axis momentum wheel; Step 3: Obtaining motion information of the connecting rod 3 of the three-axis momentum wheel; Step 4: Obtaining motion information of the connecting rod 4 of the three-axis momentum wheel; Step 5: Obtain force balance equations and moment balance equations for the connecting rod 1, the connecting rod 2, the connecting rod 3, and the connecting rod 4 based on the motion information obtained in steps 1-4; the three-axis momentum wheel is composed of the connecting rod 1, the connecting rod 2, the connecting rod 3, and the connecting rod 4, wherein the connecting rod 1 is the base, and the connecting rod 2, the connecting rod 3, and the connecting rod 4 are momentum wheels in the x-axis, y-axis, and z-axis directions, respectively; Step 6: Create a three-rigid body model, set the zero point fit, and establish the inverted rod coordinate system in assembly mode; Step 7: Linearize the Euler angle rotation matrix according to the inverted pole coordinate system established in step 6; Step 8: Use a DC brush motor to drive the three-axis momentum wheel; Step 9: Obtain the torque equations of the connecting rod 2 and the connecting rod 3 of the three-axis momentum wheel; Step 10: Obtaining the state space equation of the three-axis momentum wheel; Step 11: Solve the state space equation to obtain the value of the coefficient matrix, and input the value of the coefficient matrix into the single chip microcomputer to control the three-axis momentum wheel.

2. The control method of a three-axis momentum wheel self-balancing robot according to claim 1, characterized in that: The Euler angles of the z-axis, x-axis, and y-axis of the connecting rod 1 are obtained through a posture sensor.

3. The control method of a three-axis momentum wheel self-balancing robot according to claim 1, characterized in that: The motion information of the connecting rod 1 of the three-axis momentum wheel includes the angular velocity of the connecting rod 1, the acceleration of the connecting rod 1, the angular acceleration of the connecting rod 1, the origin acceleration of the connecting rod 1, the center of mass acceleration of the connecting rod 1, the inertia force of the connecting rod 1, and the inertia moment of the connecting rod 1.

4. The control method of a three-axis momentum wheel self-balancing robot according to claim 1, characterized in that: The angular velocity of connecting rod 1 is obtained by the principle of angular velocity superposition.

5. The control method of a three-axis momentum wheel self-balancing robot according to claim 4, characterized in that: The angular velocity of the connecting rod 1 is obtained by the angular velocity superposition principle, specifically including: Step 1.1: Obtain the mathematical expression of the angular velocity of the connecting rod 1: Where, is the angular velocity of connecting rod 1, is the Euler angle of the Z-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, is the Euler angle angular velocity in the Z-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, is a unit vector, and its expression is , is the attitude matrix of the three-axis momentum wheel on the x-axis, is the Euler angle angular velocity in the X-axis direction of the attitude sensor (IMU) of the three-axis momentum wheel, is a unit vector, and its expression is , is the attitude matrix of the three-axis momentum wheel on the y-axis, is the Euler angle angular velocity in the Y-axis direction of the attitude sensor of the three-axis momentum wheel, is a unit vector, and its expression is ; Step 1.2: Expand the angular velocity of the connecting rod 1 to obtain the angular velocity expression of each direction of the connecting rod 1; Where, is the angular velocity component of the connecting rod 1 on the x-axis, is the angular velocity component of the connecting rod 1 on the y-axis, is the angular velocity component of the connecting rod 1 along the z-axis, for The abbreviation of for The abbreviation of for The abbreviation of for abbreviation of; is the Euler angle of the Z-axis direction of the attitude sensor of the three-axis momentum wheel, is the Euler angle in the Y-axis direction of the attitude sensor of the three-axis momentum wheel.

6. The control method of a three-axis momentum wheel self-balancing robot according to claim 5, characterized in that: The acceleration expression of the connecting rod 1 is: The angular acceleration expression of the connecting rod 1 is: The origin acceleration expression of the connecting rod 1 is: The center-of-mass acceleration expression of the connecting rod 1 is: The inertia force expression of the connecting rod 1 is: Where, is the mass of connecting rod 1; The inertia moment expression of the connecting rod 1 is: is the inertia moment of connecting rod 1, is the inertia matrix of the center of mass coordinate system of connecting rod 1.

7. The control method of a three-axis momentum wheel self-balancing robot according to claim 1, characterized in that: The step 2 includes obtaining the angular velocity of the connecting rod 2 of the three-axis momentum wheel, the angular acceleration of the connecting rod 2, the origin acceleration of the connecting rod 2, the center of mass acceleration of the connecting rod 2, the inertia force of the connecting rod 2, and the inertia moment of the connecting rod 2.

8. A control system for a three-axis momentum wheel self-balancing robot, characterized in that: The system includes a processor for executing the control method of the three-axis momentum wheel self-balancing robot according to any one of claims 1 to 7.

9. A computer-readable storage medium having a data processing program stored thereon, wherein the data processing program is executed by a processor to implement the control method of the three-axis momentum wheel self-balancing robot according to any one of claims 1 to 7.

Images