Two-dimensional stable motion control method for a multi-degree-of-freedom wheeled inverted pendulum robot

Abstract

The present invention discloses a two-dimensional stable movement control method for a multi-degree-of-freedom wheeled inverted pendulum type robot, belonging to the field of robotics. The present invention divides the multi-degree-of-freedom wheeled inverted pendulum into an equivalent wheeled inverted pendulum and a multi-link part, and designs controllers separately. The first two lines of the dynamic equation of the multi-degree-of-freedom wheeled inverted pendulum are the dynamics-related terms of the equivalent wheeled inverted pendulum. The partial feedback linearization method is used to process the first two lines of the dynamic equation to obtain a new system with the angular acceleration of the driving wheel as the control input and a feedforward equation with the control torque of the driving wheel as the output. The controller of the new system is designed using a linearization or nonlinear method to calculate the angular acceleration of the driving wheel. Substituting it into the feedforward equation obtains the desired torque of the final driving wheel. The desired center of mass position or velocity is calculated by solving inverse kinematics to calculate the motion angle or angular velocity of the joint. The control method proposed by the present invention still has strong adaptability to large acceleration and deceleration motions and uneven road movement conditions.

Description

Technical Field

[0001] The present invention belongs to the field of robotics technology, and in particular relates to a two-dimensional stable movement control method for a multi-degree-of-freedom wheeled inverted pendulum type robot. Background Art

[0002] The multi-freewheel inverted pendulum is based on the traditional wheeled inverted pendulum, and adds multiple degrees of freedom or driving joints to achieve more diverse functions. Common multi-freewheel inverted pendulum types include adding a single robotic arm to the wheeled inverted pendulum base (as shown in the figure). Figure 2a as shown) or a human-like torso and arms (as Figure 2b The other is represented by Boston Dynamics' Handle robot (as shown in Figure 2c (as shown), the overall structure is similar to that of a traditional humanoid biped robot, with a drive wheel at the end of each leg replacing the traditional foot structure. Due to the separate and independent leg structure, it has the ability to move and pass on uneven roads. Compared with biped robots, the multi-free wheel inverted pendulum system has the advantages of fast movement speed and high energy efficiency. In addition, the multiple degrees of freedom enable it to have more diverse functions than the traditional wheeled inverted pendulum, such as dynamic operation, carrying people and objects, high dynamic movement, and the ability to move and pass on complex terrain. However, the added multiple degrees of freedom make the system dynamics more complex and exhibit strong nonlinearity. In addition, the system also inherits the under-driven characteristics of the wheeled inverted pendulum. Controlling this type of robot system to achieve stable movement will be a huge challenge.

[0003] In the prior art, a method for omnidirectional motion control of a two-legged wheeled composite robot controls the wheel-ground force by constructing a virtual generalized force in the task space and solving the torque in the joint space, so that the trunk posture is not affected by the change of the ground slope. This patent focuses on the force distribution and wheel-ground force control of the multi-link part, and is oriented towards three-dimensional motion. It does not provide the control of the trunk posture on uneven roads, but does not provide the driving torque control of the driving wheel. Traditional control algorithms generally calculate the equivalent center of mass and inertia of the multi-rigid body link part, and the dynamic model still uses the traditional wheeled inverted pendulum, but the equivalent center of mass and inertia parameters are updated according to the multi-degree-of-freedom wheeled inverted pendulum. The controller is designed using time-varying LQR or gain scheduling. Summary of the Invention

[0004] In order to solve the above technical problems existing in the prior art, the present invention provides a two-dimensional stable movement control method for a multi-degree-of-freedom wheeled inverted pendulum type robot, which includes two parts: the design of a drive wheel torque controller and the design of a center of mass motion controller. The specific steps are as follows:

[0005] The dynamic equation of the multi-degree-of-freedom wheeled inverted pendulum is established in the following form

[0006]

[0007] in, is a symmetric positive definite inertia matrix, are the Coriolis force and centrifugal force terms, is the gravity term; Select the matrix for the control input;

[0008] Extract the first two lines of the dynamic equation; process the first two lines using the partial feedback linearization method to obtain an equivalent new system with the driving wheel angular acceleration as the control input and a feedforward equation with the driving wheel desired control torque as the output; design a controller for the new system using a linearization or nonlinear method to obtain a control input with the driving wheel angular acceleration is the input control law; Substituting into the feedforward equation, we can get the desired control torque τ of the driving wheel: w,d ;

[0009] A center of mass motion controller is designed for the multi-rigid-body linkage. The forward kinematics and velocity Jacobian matrices of the center of mass motion of the multi-link are established along the horizontal X and vertical Z directions, respectively. A control law is designed for the center of mass of the multi-rigid-body linkage along the horizontal X direction, which includes two feedforward compensation terms and two feedback control terms. The feedforward compensation terms include compensation for center of mass displacement on uneven roads and compensation for center of mass acceleration. The feedback control terms include feedback control of the robot system's movement speed and the first link's posture. The Z direction is used to control the center of mass height. The desired center of mass position or velocity is calculated by solving inverse kinematics to calculate the joint motion angle or angular velocity.

[0010] Furthermore, the kinetic equation has the following form after expansion:

[0011]

[0012] Among them, M ij represents the i-th row and j-th column element of the matrix M(q); C i ,G i Represents vectors and the i-th row element of G(q); the first two rows of the dynamic equation are equations about the driving wheel and the first link; the controller is designed based on the first two rows of the dynamic equation for the desired input torque to the driving wheel.

[0013] Furthermore, the partial feedback linearization method is used to process the first two rows as follows:

[0014] Eliminate τ by adding the first two lines of the kinetic equations v The following equivalent system is obtained

[0015]

[0016] In the formula, is the control input; the relevant variable or vector is defined as

[0017]

[0018]

[0019] This new system can be written in standard form as follows

[0020]

[0021] The control input is The four state variables are θ w , θ1,

[0022] Furthermore, the linear controller includes LQR, pole placement or robust control; the nonlinear controller includes sliding mode variable structure or backstepping method; the controller design form is as follows:

[0023]

[0024] Where, i=w,1,2,3,…,n-1 is the state variable θ i About q, Δu1 is the driving wheel angular acceleration term related to the robot system's moving speed control; Δu2 is the driving wheel angular acceleration term associated with the inertial force generated by the movement of the multi-rigid body link part, which is Δu3 is the driving wheel angular acceleration term related to the Coriolis force, centrifugal force and gravity generated by the motion of the multi-rigid body link part, which is Δu4 is the driving wheel angular acceleration term related to the mobile position control of the robot system;

[0025] For linear controllers i=w, 1,2,3,…,n-1 are constants, and Δu3 is zero; for the joint angular acceleration in this formula Design the following PD control law

[0026]

[0027] Among them, θ i,ref (t) is the reference motion trajectory of the i-th joint; k p,i ,k d,i are the PD control gains of the i-th joint respectively. The reference motion trajectory θ of each joint in the multi-link part i,ref (t) is given by the center of mass controller; the formula is written in the form of matrix or vector operation as

[0028]

[0029] Wherein, the subscript “ref” indicates the reference value of the corresponding vector; the PD feedback gain matrix has a diagonal form, The expected motion trajectory of each active drive joint of the multi-link part is

[0030] Furthermore, partial feedback linearization is used to eliminate the first two rows of the dynamic equations. The feedforward equation is obtained as follows

[0031]

[0032] The relevant variables or vectors in the formula are defined as

[0033]

[0034]

[0035] The above obtained and Substituting them into the formula respectively, we can get the final result of the desired control torque of the driving wheel.

[0036] Furthermore, the center of mass positive kinematics and velocity Jacobian matrix of the multi-link part are as follows:

[0037] Calculate the positive kinematic relationship of the center of mass of the multi-link part of the robot system, that is, the coordinates of the center of mass of the multi-link part are

[0038] Among them, the mass of the multi-link part is m i is the mass of the i-th member; (x ci ,z ci ) is the center of mass coordinate of member i; the center of mass positive kinematics of the multi-link part can be further written as a nonlinear function of the joint angle of the multi-link part

[0039] ΔX=f ΔX (q h ),Z=f Z (q h )

[0040] Taking the derivative, we get the Jacobian relation of the velocity of the center of mass

[0041]

[0042] Furthermore, the center of mass motion control of the multi-rigid body link part can be written as follows:

[0043]

[0044] Among them, the compensation amount of center of mass displacement caused by surface undulation is δX gr =K gr rsinα; feedforward coefficient K gr is an adjustable coefficient, α is the ground slope at the current wheel-ground contact point; the ground slope α is obtained by directly estimating the surface undulation through three-dimensional radar or visual equipment; if radar or visual equipment is not equipped, it can be achieved by combining the KALMAN filter algorithm with the dynamic equation; r is the radius of the driving wheel; the center of mass displacement compensation term caused by the acceleration of the system is Feedforward coefficient K acc (Z) is a linear function of the center of mass height Z; K acc (Z) also contains the derivative of gravitational acceleration; They are the feedback control items about the moving speed of the robot system and the posture of the first link, which can be designed as PD control.

[0045] Furthermore, the position or velocity of the desired center of mass is calculated by solving inverse kinematics to calculate the angle or angular velocity of the joint, as follows:

[0046] The angular velocity of the joints of the multi-link can be obtained from the velocity of the center of mass by the following inverse kinematics form:

[0047]

[0048] The desired joint angular velocity vector is obtained as

[0049]

[0050] This invention primarily designs a controller for the two-dimensional movement of a multi-degree-of-freedom wheeled inverted pendulum. It presents a general form of a drive wheel torque controller based on dynamics. The controller's application is not limited to such two-wheeled legged robotic systems. The advantages of the control method proposed in this invention are reflected in the following aspects:

[0051] 1. The present invention derives the general form of the driving wheel torque controller based on the dynamics of a multi-degree-of-freedom wheeled inverted pendulum. Since the dynamics of the multi-rigid body connecting rod part are fully considered, it has better control performance, stability and dynamic performance than the traditional wheeled inverted pendulum algorithm.

[0052] 2. This type of multi-joint wheeled inverted pendulum system with multiple degrees of freedom is universal.

[0053] 3. It still has strong adaptability to complex motion conditions and environments (such as large acceleration and deceleration movements and movement on uneven roads).

[0054] Fourth, it has high operability in practical applications. For systems with a large number of multi-links and degrees of freedom, determining the desired joint control torque input based on the entire dynamic model is computationally intensive, and real-time performance may be difficult to ensure in some systems. The control method proposed in this invention only uses torque control for the control input of the drive wheels, while position and velocity control is used for other multi-link joints to achieve the desired control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 It is a schematic diagram of the plane model of a multi-degree-of-freedom wheeled inverted pendulum;

[0056] Figure 2a-2c There are three types of robot systems for multi-degree-of-freedom wheeled inverted pendulum applications;

[0057] Figure 3 It is an abstract model and symbolic representation of the center of mass of a multi-degree-of-freedom wheeled inverted pendulum;

[0058] Figure 4 This is a schematic diagram of the motion of an abstract model of the center of mass of a multi-degree-of-freedom wheeled inverted pendulum on a slope. DETAILED DESCRIPTION

[0059] The present invention will be further described below with reference to the accompanying drawings.

[0060] like Figure 1 As shown, the sagittal plane has Figure 1 A controller is designed for a multi-DOF wheeled inverted pendulum system of the configuration shown. The multi-DOF wheeled inverted pendulum system consists of a driving wheel and n-1 rigid links, with a total of n degrees of freedom. The generalized state vector and joint driving torque vector of the system are defined as follows:

[0061] q=[θ w ,θ1,θ2,…,θ n-1 ] T ,τ=[τ w ,τ2,τ3,…,τ n-1 ] T

[0062] The symbol definitions of each variable in the vector are as follows: Figure 1 As shown. Now define the generalized state vector of the multi-link part as

[0063]

[0064] Where T represents the transpose of a matrix or vector. hv is the configuration vector of the active drive joint of the multi-link part. The dynamic equations of the multi-freedom wheeled inverted pendulum system can be established using Lagrange or Newton-Euler

[0065]

[0066] in, is a symmetric positive definite inertia matrix, are the Coriolis force and centrifugal force terms, is the gravity term. Select the matrix for the control input. The dynamic equation of the multi-degree-of-freedom wheeled inverted pendulum can be expanded into the following form

[0067]

[0068] Among them, M ij Represents the i-th row and j-th column element of the matrix M(q). C i ,G i Represents vectors and the i-th row element of G(q). The first two rows of the dynamic equations represent the equations for the drive wheel and the first member. The controller design for the desired input torque at the drive wheel is based on the first two rows of the dynamic equations. The controller design and calculation steps for the desired drive torque at the drive wheel are now given as follows:

[0069] In the first step, partial feedback linearization is used to process the first two lines of the dynamic equations. By adding the first two lines of the dynamic equations, τ is eliminated. v get

[0070]

[0071] The relevant variables or vectors in the formula are defined as

[0072]

[0073]

[0074] This formula is the equivalent control system obtained by applying partial feedback linearization to the first two lines of the dynamic equation, which can be written in the following standard form

[0075]

[0076] The control input is The generalized state variables in the formula do not change, and the control input becomes the angular velocity of the driving wheel.

[0077] The second step is to design a controller for the new system. There are many methods that can be used, including linear controllers such as LQR, pole placement, and robust control. Nonlinear controllers include sliding mode variable structure, backstepping, etc. The general form of the controller designed by this invention using these methods is as follows

[0078]

[0079] Where, i=w,1,2,3,…,n-1 is the state variable θ i About q, Δu1 is the driving wheel angular acceleration term related to the robot system movement speed control. Δu2 is the driving wheel angular acceleration term associated with the inertial force generated by the movement of the multi-rigid body link part, which is Δu3 is the driving wheel angular acceleration term related to the Coriolis force, centrifugal force and gravity generated by the motion of the multi-rigid body link part, which is Δu4 is the angular acceleration of the driving wheel related to the mobile position control of the robot system. In most cases, we only focus on the mobile speed of the control system, and the Δu4 term can be set to zero. For linear controllers such as LQR, pole configuration, etc., i=w,1,2,3,…,n-1 are usually constants, and Δu3 is zero. Design the following PD control law

[0080]

[0081] Among them, θ i,ref (t) is the reference motion trajectory of the i-th joint. k p,i ,k d,i are the PD control gains of the i-th joint respectively. The reference motion trajectory θ of each joint in the multi-link part i,ref (t) is given by the center of mass controller below. This formula can be written in the form of matrix or vector operation as

[0082]

[0083] Wherein, the subscript “ref” indicates the reference value of the corresponding vector. The PD feedback gain matrix has a diagonal form, The expected motion trajectory of each active drive joint of the multi-link part is Step 3: Eliminate the first two lines of the dynamic equation The following calculation formula for the desired control torque of the driving wheel is obtained:

[0084]

[0085] The relevant variables or vectors are defined as

[0086]

[0087]

[0088] The above obtained and Substituting them into the formula respectively, we can get the final result of the desired control torque of the driving wheel.

[0089] In order to further enhance the stability of the system, a controller is designed for the center of mass motion of the multi-rigid body link part, including the horizontal direction X and the vertical direction Z. The relevant symbols are defined as follows Figure 3 As shown. Among them, the position of the center of mass in the X direction is most closely related to the acceleration and deceleration performance and stability of the system. The Z direction is mainly used to control the height of the center of mass. Calculate the positive kinematic relationship of the center of mass of the multi-link part of the robot system, that is, the coordinates of the center of mass of the multi-link part are Among them, the mass of the multi-link part is m i is the mass of the i-th member. (x ci ,z ci ) is the center of mass coordinate of member i. The forward kinematics of the center of mass of the multi-link part can be further written as a nonlinear function of the joint angle of the multi-link part

[0090] ΔX=f ΔX (q h ),Z=f Z (q h )

[0091] Furthermore, the Jacobian relation of the center of mass velocity is obtained by derivation:

[0092]

[0093] Now design a controller for the center of mass position of the multi-rigid body link part. The control law of the center of mass in the X direction relative to the equilibrium position is given, which includes two feedforward compensation terms and two feedback control terms.

[0094]

[0095] Among them, such as Figure 4 As shown, the compensation amount of the center of mass displacement caused by the surface undulation is δX gr =K gr rsinα. Feedforward coefficient K gr is an adjustable coefficient with a theoretical value of 1. It is adjusted according to the feedback effect in actual application. α is the ground slope at the current wheel-ground contact point. The ground slope α can be obtained by directly estimating the surface undulation using a 3D radar or visual device. In the absence of radar or visual equipment, it can be achieved by combining the KALMAN filter algorithm with the dynamic equation, but the estimation accuracy is greatly affected by sensor noise and dynamic modeling accuracy. r is the radius of the driving wheel. The compensation term for the center of mass displacement caused by the acceleration of the system is Feedforward coefficient K acc (Z) is a linear function of the center of mass height Z. In addition, K acc(Z) also contains the derivative of gravity acceleration. In actual system application, K acc The adjustment of (Z) can be based on the ZMP principle, that is, let the extension line of the resultant force of inertia and gravity pass through the wheel-ground contact point. They are the feedback control items for the robot system's moving speed and the link 1's posture, and the simplest design is PD control. ref A differential calculation can be performed to obtain the X-direction velocity of the center of mass

[0096] Furthermore, the controller with the center of mass in the Z direction can be written as

[0097]

[0098] Among them, Z d is the feedforward term calculated for the planned motion trajectory. ref is the desired center of mass height including PD feedback control. The desired motion speed of the center of mass is Calculate the angular velocity of the joints of the multi-link It can be written in the following inverse kinematic form

[0099]

[0100] Theoretically, the expected joint angular velocity obtained by inverse kinematics is There are infinite solutions. The unique solution or optimal solution can be obtained by imposing necessary constraints on the joint angles or calculating the generalized inverse of the Jacobian. The desired joint angular velocity vector can be written as

[0101]

Claims

1. A two-dimensional stable motion control method for a multi-degree-of-freedom wheeled inverted pendulum type robot, comprising two parts: the design of a drive wheel torque controller and the design of a center of mass motion controller, characterized in that The steps include: The dynamic equation of the multi-degree-of-freedom wheeled inverted pendulum is established in the following form in, is a symmetric positive definite inertia matrix, are the Coriolis force and centrifugal force terms, is the gravity term; Select the matrix for the control input; Extract the first two lines of the dynamic equation; use the partial feedback linearization method to process the first two lines of the dynamic equation to obtain an equivalent new system with the driving wheel angular acceleration as the control input and a feedforward equation with the driving wheel desired control torque as the final output; use the linearization or nonlinear method to design a controller for the new system to obtain the driving wheel angular acceleration is the input control law; Substituting into the feedforward equation, we can get the desired control torque τ of the driving wheel: w,d ; A center of mass motion controller is designed for the multi-body linkage. The forward kinematics and velocity Jacobian matrices of the center of mass motion are established along the horizontal X and vertical Z directions, respectively. A control law is designed for the center of mass of the multi-body linkage along the horizontal X direction, which includes two feedforward compensation terms and two feedback control terms. The feedforward compensation term includes compensation for center of mass displacement on uneven roads and compensation for center of mass acceleration. The feedback control term includes feedback control of the system movement speed and the posture of the first link. The Z direction is used to control the height of the center of mass. The desired center of mass position or velocity is calculated by solving inverse kinematics to calculate the joint angle or angular velocity.

2. The two-dimensional stable movement control method of a multi-degree-of-freedom wheeled inverted pendulum type robot according to claim 1, characterized in that: The kinetic equation has the following form after expansion: Among them, M ij represents the i-th row and j-th column element of the matrix M(q); C i ,G i Represent the i-th row elements of vectors C(q,q) and G(q) respectively; the first two rows of the dynamic equation are equations about the drive wheel and the first connecting rod; based on the first two rows of the dynamic equation, the controller of the drive wheel is designed to obtain the expected input torque.

3. The two-dimensional stable movement control method of a multi-degree-of-freedom wheeled inverted pendulum type robot according to claim 2, characterized in that: The partial feedback linearization method is used to process the first two rows as follows: Eliminate τ by adding the first two lines of the kinetic equations v The following equivalent system is obtained Where, For control inputs, the relevant variables or vectors are defined as This new system can be written in standard form as follows The control input is The four state variables are θ w , θ1, 4. The two-dimensional stable movement control method of a multi-degree-of-freedom wheeled inverted pendulum type robot according to claim 3, characterized in that: Linear controllers include LQR, pole placement, or robust control; nonlinear controllers include sliding mode variable structure or backstepping. The controller design is as follows: Where, is the state variable θ i About q, Δu1 is the driving wheel angular acceleration term related to the robot system's moving speed control; Δu2 is the driving wheel angular acceleration term associated with the inertial force generated by the movement of the multi-rigid body link part, which is Δu3 is the driving wheel angular acceleration term related to the Coriolis force, centrifugal force and gravity generated by the motion of the multi-rigid body link part, which is Δu4 is the driving wheel angular acceleration term related to the mobile position control of the robot system; For linear controllers The term is a constant, and the term Δu3 is zero; for the joint angular acceleration in this formula Design the following PD control law Among them, θ i,ref (t) is the reference motion trajectory of the i-th joint; k p,i ,k d,i are the PD control gains of the i-th joint respectively. The reference motion trajectory θ of each joint in the multi-link part i,ref (t) is given by the center of mass controller, which can be expressed in the form of matrix or vector operation as follows: Wherein, the subscript "ref" indicates the reference value of the corresponding vector; the PD feedback gain matrix has a diagonal form, The expected motion trajectory of each active drive joint of the multi-link part is 5. The two-dimensional stable movement control method of a multi-degree-of-freedom wheeled inverted pendulum type robot according to claim 4, characterized in that: Partial feedback linearization is used to eliminate the first two rows of the dynamic equations The following feedforward equation is obtained The relevant variables or vectors are defined as The above obtained and Substituting them into the formula respectively, we can get the final result of the desired control torque of the driving wheel.

6. The two-dimensional stable movement control method of a multi-degree-of-freedom wheeled inverted pendulum type robot according to claim 5, characterized in that: The center of mass positive kinematics and velocity Jacobian matrix of the multi-link part are as follows: Calculate the center of mass positive kinematics relationship of the multi-link part of the robot system, that is, the center of mass coordinates of the multi-link part are Among them, the mass of the multi-link part is m i is the mass of the i-th member; (x ci ,z ci ) is the center of mass coordinate of member i; the center of mass positive kinematics of the multi-link part can be further written as a nonlinear function of the joint angle of the multi-link part ΔX=f ΔX (q h ),Z=f Z (q h ) Taking the derivative, we get the Jacobian relation of the velocity of the center of mass:

7. The two-dimensional stable movement control method of a multi-degree-of-freedom wheeled inverted pendulum type robot according to claim 6, characterized in that: The center of mass motion control of the multi-rigid body link part can be written as follows Among them, the compensation amount of center of mass displacement caused by surface undulation is δX gr =K gr rsinα;K gr is the adjustable feedforward coefficient, α is the ground slope at the current wheel-ground contact point; the ground slope α is directly estimated and measured by three-dimensional radar or visual equipment based on the surface undulation; if radar or visual equipment is not equipped, it can be achieved by combining the KALMAN filter algorithm with the dynamic equation; r is the radius of the driving wheel; the center of mass displacement compensation term caused by the system acceleration motion is Feedforward coefficient K acc (Z) is a linear function of the center of mass height Z; K acc (Z) also contains the derivative of gravitational acceleration; They are the feedback control items about the moving speed of the robot system and the posture of the first link, which can be designed as PD control.

8. The two-dimensional stable movement control method of a multi-degree-of-freedom wheeled inverted pendulum type robot according to claim 7, characterized in that: The position or velocity of the desired center of mass is calculated by solving inverse kinematics to calculate the joint angle or angular velocity, as follows: The angular velocity of the joints of the multi-link can be obtained from the velocity of the center of mass by the following inverse kinematics form: Where, the expected joint angular velocity vector is

Images