Self-adaptive vibration suppression control method of multi-degree-of-freedom robot and robot

Abstract

The invention discloses a self-adaptive vibration suppression control method of a multi-degree-of-freedom robot and the robot, and relates to the technical field of robot control, and the self-adaptive vibration suppression control method comprises the following steps: acquiring initial state data of the robot, and setting an expected position; according to the initial state data and the expected position, a kinematic model of the robot is established, and resultant force borne by the mass center of the robot is obtained through calculation; and calculating an external resultant force based on the resultant force and the virtual traction force, constructing a self-adaptive vibration suppression controller, and controlling the robot to move under the external resultant force and the virtual traction force by adjusting a stiffness matrix in the self-adaptive vibration suppression controller. Based on the multi-degree-of-freedom spring machine body vibration reduction control theory, the stress condition of the machine body is improved, the vibration resistance efficiency of the machine body is improved, vibration is resisted in a flexible mode, and the movement reliability is enhanced.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and more particularly to an adaptive vibration suppression control method for a multi-degree-of-freedom robot and the robot. Background Art

[0002] The gecko-like robot has a wide range of application prospects. It can enter a small space to search and rescue trapped people. In construction, it can replace manual work at high altitudes, greatly improving work efficiency and safety. In industry, it can be used for the maintenance and inspection of industrial equipment, achieving regular inspections without manual intervention. These application scenarios demonstrate the help and important value of the gecko-like robot to humans.

[0003] However, most of the research on gecko-like robots focuses on the development and application of actuators to adapt to different surfaces and achieve reliable adhesion and detachment. During the movement of the gecko-like robot, its body will inevitably be affected by vibration. These vibrations may come from the interaction between the robot and the moving surface, such as bumps when crawling on a rough surface, or internal stress changes when performing some complex movements. If the vibration problem is not effectively controlled, it may cause the robot's movement to be unstable, affect its adsorption effect, and may even damage the precision components inside the robot, reducing the robot's service life and working reliability.

[0004] Therefore, how to provide an adaptive vibration suppression control method and robot for a multi-degree-of-freedom robot, improve the stress condition of the body, improve the body's anti-vibration efficiency, use a flexible method to resist vibration, and enhance the reliability of movement is an urgent problem that technical personnel in this field need to solve. Summary of the invention

[0005] In view of this, the present invention provides an adaptive vibration suppression control method for a multi-degree-of-freedom robot and a robot, which is based on the fuselage vibration reduction control theory of a multi-degree-of-freedom spring model, and aims to improve the force condition of the fuselage, improve the anti-vibration efficiency of the fuselage, use a flexible method to resist vibration, and enhance the reliability of movement.

[0006] In order to achieve the above object, the present invention adopts the following technical solution: an adaptive vibration suppression control method for a multi-degree-of-freedom robot, comprising:

[0007] Get the robot's initial state data and set the desired position;

[0008] According to the initial state data and the desired position, a kinematic model of the robot is established, and the resultant force acting on the center of mass of the robot is calculated;

[0009] The external resultant force is calculated based on the resultant force and the virtual traction force, an adaptive vibration suppression controller is constructed, and the movement of the robot under the external resultant force and the virtual traction force is controlled by adjusting the stiffness matrix in the adaptive vibration suppression controller.

[0010] Preferably, the robot's body structure adopts a symmetrical design, and the joint configuration of each leg is the same;

[0011] Select one of the legs to establish a single-leg kinematic model, perform symmetric transformation based on the body coordinate system to obtain the kinematic models of the other legs, and finally obtain the kinematic model of the robot.

[0012] Preferably, calculating the resultant force on the center of mass of the robot includes:

[0013] According to the DH method, the coordinate systems of each joint of any single leg of the robot are analyzed to obtain the DH parameter table;

[0014] According to the expected position, the transformation matrix between the end effector coordinate system and the body coordinate system is calculated to obtain the robot kinematics positive solution;

[0015] The inverse transformation method is used to calculate the joint angles of the robot movement;

[0016] The end force of any single leg of the robot is calculated according to the Jacobian matrix, and the resultant force acting on the center of mass of the robot is obtained.

[0017] Preferably, an adaptive vibration suppression controller is constructed based on the Lagrange equation, which is expressed as:

[0018]

[0019] In the formula, represents the robot acceleration vector, M represents the robot's inherent inertia matrix, K E represents the spring stiffness matrix, A is the partial derivative matrix of the virtual spring extension, and f ext For external joint forces;

[0020] in,

[0021]

[0022]

[0023] Where k1, k2, k3, k4 represent the virtual stiffness of the left front leg, right front leg, left hind leg, and right hind leg respectively; δl i represents the extension of the virtual spring; x, y, z are the three-dimensional position coordinates of the robot in the world coordinate system, and α, β, γ are the deflection angles of the robot compared to the initial posture in the world coordinate system. var is the resultant force of external generalized forces acting on the center of mass, fq represents the virtual traction force to maintain a constant speed, m represents the robot's own mass, and I xx , I yy ,I zz Represents the body inertia.

[0024] Preferably, the legs of the robot all include springs, and the length of the springs when the robot is stationary is the original length of the springs of the robot legs.

[0025] Preferably, the virtual traction force f is maintained at a constant speed. q It is expressed as:

[0026]

[0027] Among them, x err =x cur -x, x err =x cur -x,

[0028] In the formula, x err Represents the position difference at the current moment, Indicates the speed difference at the current moment; x cur Indicates the actual position obtained by the sensor, Indicates the actual speed obtained by the sensor, represents the desired velocity of the leg end; x represents the desired position of the leg end, K represents the stiffness parameter and D represents the damping parameter.

[0029] Preferably, the three-dimensional vector δl of the spring elongation is calculated by the following formula: δl = [x f ,y f ,z f ] T -(R z (γ-γ0)R y (β-β0)R x (α-α0)[L / 2,-W / 2,0] T +[x-x0,y-y0,z-z0] T );

[0030] Among them, L and W are the length and width of the robot body, R x (θ),R y (θ),R z (θ) is the rotation matrix of θ degrees around a coordinate axis, [x f ,y f ,z f ] T Represents the coordinates of the point at the end of any single leg on the motion plane; [x0, y0, z0, α0, β0, γ0]T represents the initial state of the robot in the world coordinate system, x, y, z are the three-dimensional position coordinates of the robot in the world coordinate system, α, β, γ are the deflection angles of the robot in the world coordinate system compared with the initial posture.

[0031] Preferably, according to the DH parameter table, the kinematic positive solution is calculated as:

[0032]

[0033] Among them, L1, L2, L3 represent the lengths of the other three legs, s i = sinθ i , c i = cosθ i ,s ij = sin(θ i +θ j ), c ij =cos(θ i +θ j ), and so on; θ i represents the rotation angle of joint i;

[0034] Represents the desired position of the end of any single leg in the world coordinate system;

[0035] The inverse transformation method is used to calculate the joint angles of the robot motion, including:

[0036]

[0037] in,

[0038]

[0039] Preferably, the end force of any single leg is obtained according to the torque and end force conversion formula:

[0040] f=(J(θ) T ) + τ;

[0041] Among them, τ∈R n×1 is the joint torque matrix, n is the number of joints, f∈R 3×1 is the three-dimensional force at the end, J(θ) is the Jacobian matrix;

[0042] The resultant force on the robot's center of mass is expressed as: f b =∑f i ,i∈1~4.

[0043] Preferably, a robot comprises: a body and legs, wherein the legs are symmetrically mounted on both sides of the body.

[0044] It can be seen from the above technical solutions that compared with the prior art, the present invention discloses an adaptive vibration suppression control method and robot for a multi-degree-of-freedom robot, including: obtaining the initial state data of the robot and setting the desired position; establishing a kinematic model of the robot according to the initial state data and the desired position, and calculating the resultant force on the center of mass of the robot; calculating the external resultant force based on the resultant force and the virtual traction force, constructing an adaptive vibration suppression controller, and controlling the movement of the robot under the external resultant force and the virtual traction force by adjusting the stiffness matrix in the adaptive vibration suppression controller. The present invention has the advantages of overcoming the difficulty of complex plane vibration and realizing high-precision control of a multi-degree-of-freedom wall-climbing robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0046] Figure 1 The present invention provides a flow chart of an adaptive vibration suppression control method for a multi-degree-of-freedom robot.

[0047] FIG. 2( a ) is a schematic diagram showing the relationship between the robot coordinate system and the inertial coordinate system provided in an embodiment of the present invention.

[0048] FIG2( b ) is a schematic diagram of the coordinate transformation of a single leg of a robot provided in an embodiment of the present invention.

[0049] Figure 3 A robot gait planning diagram provided by an embodiment of the present invention.

[0050] Figure 4 A block diagram of a vibration suppression algorithm provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0051] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0052] The embodiment of the present invention discloses an adaptive vibration suppression control method for a multi-degree-of-freedom robot and the robot, aiming to establish an accurate control method for a multi-degree-of-freedom quadruped wall-climbing robot based on a vibration environment, including given the initial state and expected trajectory of the robot, establishing a kinematic model and a dynamic model of the wall-climbing robot based on basic parameters; using the Jacobian matrix to calculate the force on the robot, designing an adaptive vibration suppression controller, and adjusting the stiffness parameters. The embodiment of the present invention has the advantages of overcoming the difficulty of complex plane vibration and achieving high-precision control of a multi-degree-of-freedom wall-climbing robot.

[0053] In a specific embodiment of the present invention, an adaptive vibration suppression control method for a multi-degree-of-freedom robot is provided. Figure 1 and Figure 4 As shown, including:

[0054] Get the robot's initial state data and set the desired position;

[0055] According to the initial state data and the desired position, a kinematic model of the robot is established, and the resultant force acting on the center of mass of the robot is calculated;

[0056] The external resultant force is calculated based on the resultant force and the virtual traction force, an adaptive vibration suppression controller is constructed, and the movement of the robot under the external resultant force and the virtual traction force is controlled by adjusting the stiffness matrix in the adaptive vibration suppression controller.

[0057] Figure 4 This is an algorithm block diagram of an embodiment of the present invention, specifically, a gait generator generates a support trajectory plan and a swing trajectory plan; a multi-degree-of-freedom spring model is established according to the support trajectory plan, the stiffness and damping parameters of the spring model are determined, the current position data and force data are obtained through sensors, the expected acceleration is obtained, the required trajectory control position is calculated by numerical method, the kinematic inverse solution is obtained and sent to the joint actuator to realize closed-loop control.

[0058] Specifically, the robot's body structure adopts a symmetrical design, and each leg has the same joint configuration;

[0059] Select one of the legs to establish a single-leg kinematic model, perform symmetric transformation based on the body coordinate system to obtain the kinematic models of the other legs, and finally obtain the kinematic model of the robot.

[0060] Specifically, the DOF robot is a quadruped wall-climbing robot with four legs, as shown in FIG2(a), and the robot includes a left front leg, a right front leg, a left hind leg, and a right hind leg; the mass m of the robot is set, the length, width, and height of the body are L, W, and H respectively, and the body inertia I is calculated. xx , I yy ,I zz, set the robot leg lengths L1, L2, L3. Based on the basic parameters, a kinematic model of the quadruped wall-climbing robot is established to provide an analytical solution that can be calculated in real time for trajectory planning. In the embodiment of the present invention, the right front leg is selected to calculate the kinematic model, and the kinematic models of the other three legs are obtained by symmetric transformation based on the body coordinate system, as shown in Figure 2(b).

[0061] Specifically, the gait of the robot is designed according to the desired position: the gait cycle planning diagram of the embodiment of the present invention is as follows: Figure 3 As shown, Figure 3 The gait cycle of the robot's left front leg, right front leg, left hind leg, and right hind leg is described; the black block is the foot support phase, and the white block is the foot swing phase; BMP (body move phase) represents the body movement period, and FSP (foot swing phase) represents the foot movement period. In a gait cycle, the robot first supports the body to the left front, then swings the right hind leg and the right front leg in sequence, then supports the body to the right front, and finally swings the left hind leg and the left front leg in sequence.

[0062] According to the robot's posture information, the robot's fourth degree of freedom joint always keeps the robot in positive contact with the adhesion surface to provide maximum preload. The posture information includes the robot's center of mass coordinate system {B} and the world coordinate system {W}; in the initial state, the robot's center of mass coordinate system {B} and the world coordinate system {W} coincide, and the robot's posture in the world coordinate system is [x0, y0, z0, α0, β0, γ0] T .

[0063] Specifically, the resultant force acting on the robot's center of mass is calculated, including:

[0064] According to the DH method, each joint coordinate system of any single leg of the robot is analyzed to obtain a DH parameter table; in the embodiment of the present invention, the right front leg is taken as an example, and the DH parameter table is shown in Table 1;

[0065] According to the expected position, the transformation matrix between the end effector coordinate system and the body coordinate system is calculated to obtain the robot kinematics positive solution;

[0066] The inverse transformation method is used to calculate the joint angles of the robot movement;

[0067] The end force of any single leg of the robot is calculated according to the Jacobian matrix, and the resultant force acting on the center of mass of the robot is obtained.

[0068] Since the goal of robot forward kinematics is to establish the transformation relationship from joint space to Cartesian space, the position and posture of the robot end is calculated based on the known joint angles. Robot forward kinematics needs to calculate the transformation matrix of the end coordinate system {N} relative to the base coordinate system {0} First, the transformation matrix of coordinate system {i} relative to coordinate system {i-1} should be given It is equal to the product of the following four sub-transformations:

[0069] (1) Around X i-1 Axis rotation α i-1 horn;

[0070] (2) Along X i-1 Axis movement a i-1 ;

[0071] (3) Around Z i Axis rotation θ i horn;

[0072] (4) Along Z i Axis movement d i .

[0073] Because these transformations are described relative to a moving coordinate system, multiplying the transformations "from left to right" yields:

[0074]

[0075] According to the DH method, the embodiment of the present invention analyzes the joint coordinate systems of the right front leg of the quadruped wall-climbing robot to obtain a DH parameter table, as shown in Table 1. Calculate the transformation matrix of the intermediate joint by formula, multiply it successively to get the transformation matrix of the end effector and the fuselage coordinate system, and get the corresponding Calculate the transformation matrix between the end effector coordinate system and the fuselage coordinate system

[0076]

[0077] Simplify to get

[0078] in,

[0079]

[0080] Where, L i is the length of each joint, θ i is the angle of each joint, s i = sinθ i ,c i = cosθ i ,c ij =cos(θ i +θ j ),s ij = sin(θ i +θ j );

[0081] Specifically, according to the DH parameter table, the kinematic solution is calculated as:

[0082]

[0083] Among them, L1, L2, L3 represent the lengths of the other three legs, s i = sinθ i , c i = cosθ i ,s ij = sin(θ i +θ j ), c ij =cos(θ i +θ j ), and so on; θ i represents the rotation angle of joint i; x, y, z represent the position of the end execution point in the shoulder joint coordinate system;

[0084] Represents the desired position of the end of any single leg in the world coordinate system;

[0085] After obtaining the robot's kinematic solution, the robot's end position and posture are solved based on the robot's joint angles. However, in actual planning, the robot's motion trajectory is often planned first, and then the end position of each leg is planned. Therefore, for a known end position, the inverse transformation method is used to obtain the inverse kinematics solution of the gecko-like robot when the joint angle is calculated.

[0086] As shown in Figure 2(a), since adhesion contact requires the sole of the foot to be perpendicular to the plane, the end posture is always downward. In order to facilitate the analytical solution, let θ4 = θ1.

[0087] Specifically, the kinematic equations in the previous step are

[0088] Using inverse transform

[0089] Calculate the matrices on both sides respectively and make the last columns of the matrices equal, and we can get:

[0090]

[0091] Since the equation has 4 unknowns and the number of equations is 3, there are countless solutions. In order to enable the system to calculate in real time and avoid the numerical method process, the task classification method is adopted. Since the rotation axis of θ4 is consistent with θ1 and is responsible for the posture correction of the robot end, θ4 can be set to θ1+θ t , where θ t is the angle required by the task. In the plane crawling task, θt It is set to the angle between the body and the plane, which is generally 0 degrees, and the final kinematic inverse solution of the robot is obtained, that is, the joint angle of the robot's motion.

[0092] Specifically, the inverse transformation method is used to calculate the joint angles of the robot movement, including:

[0093]

[0094] in,

[0095]

[0096] Table 1 DH parameters of the robot's right front leg

[0097]

[0098] Specifically, the end force of any single leg is obtained according to the torque and end force conversion formula:

[0099] f=(J(θ) T ) + τ;

[0100] Among them, τ∈R n×1 is the joint torque matrix, n is the number of joints, f∈R 3×1 is the three-dimensional force at the end, J(θ) is the Jacobian matrix;

[0101] The resultant force on the robot's center of mass is expressed as: f b =∑f i ,i∈1~4.

[0102] In a specific embodiment of the present invention, a method for obtaining a Jacobian matrix includes:

[0103] According to the kinematics formula, x = f(q); taking the derivative of both sides with respect to time t, we get

[0104] in is the speed of the leg end movement, is the robot joint motion speed, and the calculation formula of the Jacobian matrix is:

[0105]

[0106] in:

[0107]

[0108] In the embodiment of the present invention, the torque τ obtained by the robot joint torque sensor is used to obtain the Moore-Penrse inverse for the Jacobian matrix. The steps for solving the Moore-Penrse inverse are as follows: First, the Jacobian matrix J is subjected to singular value decomposition to construct J=UΣV T , where U is an m×m orthogonal matrix, Σ is an m×m diagonal matrix containing non-negative singular values, and V is an n×n orthogonal matrix. Take out the singular values ​​σi in the diagonal matrix Σ and take the inverse of them to construct Σ + For each non-zero singular σi, the corresponding inverse is 1 / σi; for zero singular values, they are not included in the calculation. Σ + The dimension is n×m, where 1 / σi is filled in the corresponding position. Finally, we get J + =VΣ + U T , forming a pseudo-inverse.

[0109] According to the conversion formula of robot arm torque and end force: τ=J(θ) T f;

[0110] Get the force at the end of the single leg: f = (J (θ) T ) + τ;

[0111] In order to simplify the calculation of the force on the fuselage, the resultant force at the center of mass is used to replace the generalized force on the four legs. Since the gecko-like robot moves slowly and there is an end adhesion force that can balance the end torque, the torque effect is ignored and the resultant force on the center of mass of the robot is obtained: f b =∑f i ,i∈1~4;

[0112] The embodiment of the present invention calculates the Jacobian equation of the robot, and obtains the conversion formula between the motor torque and the end effector force through the Jacobian equation, which provides a necessary basis for the subsequent calculation of the robot's dynamic equations.

[0113] Specifically, an adaptive vibration suppression controller is constructed based on the Lagrange equation, which is expressed as:

[0114]

[0115] In the formula, represents the robot acceleration vector, M represents the robot's inherent inertia matrix, K E represents the spring stiffness matrix, A is the partial derivative matrix of the virtual spring extension, and f ext For external joint forces;

[0116] In a specific embodiment of the present invention, the Lagrange equation provides a systematic method to describe the robot and its dynamic behavior, especially in the case of multiple degrees of freedom and complex motion. It can effectively handle the motion and interaction of the robot joints. Compared with the traditional Newton-Euler method, it is easier to handle constrained motion and coordinate transformation, and can simplify the dynamic analysis of complex mechanical systems. Therefore, establishing the robot Lagrange equation can effectively help understand and analyze the dynamic characteristics of the robot, thereby providing theoretical support for practical applications.

[0117] Defined by the Lagrange equation:

[0118]

[0119] It can be seen that the Lagrangian equation of the robot requires the robot's potential energy T, kinetic energy V, and the generalized force f acting on the robot.

[0120] All supporting legs of the robot are spring systems, and the kinetic energy and potential energy of the robot are obtained as follows:

[0121]

[0122] Since only the leg actuators of the robot touch the external plane, the external force of the robot only acts on the end actuator. According to the above steps, the force applied to the end actuator is normalized to the center of gravity to obtain the external resultant force.

[0123] Substituting into the Lagrange formula we get

[0124]

[0125] represents the robot acceleration vector, M∈R 6×6 represents the robot's inherent inertia matrix, K E ∈R 4×1 represents the spring stiffness matrix, A∈R 6×4 is the partial derivative matrix of virtual spring extension; f ext ∈R 6×1 For external joint forces;

[0126] in,

[0127]

[0128] Where k1, k2, k3, k4 represent the virtual stiffness of the left front leg, right front leg, left hind leg, and right hind leg respectively; δl i represents the extension of the virtual spring; x, y, z are the three-dimensional position coordinates of the robot in the world coordinate system, and α, β, γ are the deflection angles of the robot compared to the initial posture in the world coordinate system. varis the resultant force of external generalized forces acting on the center of mass, f q represents the virtual traction force to maintain a constant speed, m represents the robot's own mass, and I xx , I yy ,I zz Represents the body inertia.

[0129] Among them, f var ∈R 6×1 It is the resultant force of the external generalized force acting on the center of mass. By adjusting the stiffness matrix, the movement of the robot under external force and virtual traction is controlled to maintain the stability and movement reliability of the robot in a vibration environment.

[0130] The embodiment of the present invention calculates the robot Lagrange equation to obtain the mechanical equation between the robot's potential energy, kinetic energy and external generalized force, calculates the robot's acceleration, obtains a predictable robot state, and realizes the vibration suppression control of the gecko-like robot. The independently adjustable parameter matrix provides help for subsequent motion optimization.

[0131] Specifically, the legs of the robot include springs, and the length of the springs when the robot is stationary is the original length of the springs of the robot legs.

[0132] In a specific embodiment of the present invention, δl i It is about the center of mass position and posture [x, y, z, α, β, γ] T The function represents the elongation of the virtual spring. For a given virtual spring, the length of the spring when the robot is stationary is set to be the original length of the virtual spring of the robot leg. The initial state robot's center of mass coordinate system {B} and the world coordinate system {W} are set to coincide. At this time, the robot's posture in the world coordinate system is [x0, y0, z0, α0, β0, γ0] T , taking the right front leg as an example, let the coordinates of the point at the end of the leg on the motion plane be [x f ,y f ,z f ] T , in the world coordinate system, the three-dimensional vector δl of the spring extension can be calculated by the following formula:

[0133] δl=[x f ,y f ,z f ] T -(R z (γ-γ0)R y (β-β0)R x (α-α0)[L / 2,-W / 2,0] T +[x-x0,y-y0,z-z0] T );

[0134] Among them, L and W are the length and width of the robot body, R x (θ),R y (θ),R z (θ) is the rotation matrix of θ degrees around a coordinate axis, [x f ,y f ,z f ] T Represents the coordinates of the point at the end of any single leg on the motion plane; [x0, y0, z0, α0, β0, γ0] T represents the initial state of the robot in the world coordinate system, x, y, z are the three-dimensional position coordinates of the robot in the world coordinate system, α, β, γ are the deflection angles of the robot in the world coordinate system compared with the initial posture.

[0135] According to the Pythagorean theorem, the length of a line segment in space is the square root of the product of the three coordinates of its space vector: This gives us the matrix A, [x, y, z] T is the expected position of the robot's current center of mass. The expected position is obtained according to the robot's trajectory planning, and has [x, y, z] T =[x0,y0,z0] T +vt (t is the current running time, v is the robot's movement speed); according to the actual position x obtained by the sensor cur , actual speed Get running difference Select appropriate stiffness parameter K and damping parameter D;

[0136] Get virtual traction to maintain constant speed

[0137] In the embodiment of the present invention, K is D is selected as f var ∈R 6×1 It is the resultant force of the external generalized force acting on the center of mass. It is obtained by obtaining the current motor torque through the sensor and the Jacobi formula. Therefore, all the parameters on the right side of the above formula are available, and the acceleration of the robot controlled at the current moment can be calculated.

[0138] Specifically, the virtual traction force f to maintain a constant speed q It is expressed as:

[0139]

[0140] Among them, x err =x cur -x, x err =x cur -x,

[0141] In the formula, x err Represents the position difference at the current moment, Indicates the speed difference at the current moment; x cur Indicates the actual position obtained by the sensor, Indicates the actual speed obtained by the sensor, represents the desired velocity of the leg end; x represents the desired position of the leg end, K represents the stiffness parameter and D represents the damping parameter.

[0142] Specifically, the stiffness matrix is ​​adjusted and different stiffness parameters are used in different environments to achieve control optimization of the robot.

[0143] In the embodiment of the present invention, in the first swing phase, since the right front leg is swinging at the moment, k2 is set to 0, and the remaining spring stiffness parameters are given larger values; first, the stiffness of the supporting legs k1 and k4 (i.e., the stiffness of the left front and right rear legs) is adjusted so that the robot can show an up and down swinging trend during vibration operation; then the stiffness of the legs k3 (i.e., the stiffness of the left rear leg) is adjusted to reduce the lateral vibration of the robot; finally, k1, k4, and k3 are adjusted comprehensively so that the vibration frequency and amplitude of the robot during the swing phase are significantly reduced. In this cycle, in the nth swing phase, the stiffness of the corresponding swing leg is set to 0, and the other three legs are adjusted according to the above method to obtain the optimal control spring stiffness parameters.

[0144] In a specific embodiment of the present invention, a robot includes: a body and legs, wherein the legs are symmetrically installed on both sides of the body.

[0145] The embodiment of the present invention can adjust the robot spring stiffness parameters according to the above method according to different vibration crawling environments, so that the stability of the robot under vibration is significantly improved and the motion trajectory tends to be stable. By analyzing the changes in the kinetic energy and potential energy of the robot's movement, the force on the legs can be effectively adjusted to achieve optimal control of the generalized force, so that the overall work of the robot is reduced and the energy utilization rate is improved.

[0146] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0147] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. An adaptive vibration suppression control method for a multi-degree-of-freedom robot, characterized in that: include: Get the robot's initial state data and set the desired position; According to the initial state data and the desired position, a kinematic model of the robot is established, and the resultant force acting on the center of mass of the robot is calculated; The external resultant force is calculated based on the resultant force and the virtual traction force, an adaptive vibration suppression controller is constructed, and the movement of the robot under the external resultant force and the virtual traction force is controlled by adjusting the stiffness matrix in the adaptive vibration suppression controller.

2. The adaptive vibration suppression control method for a multi-degree-of-freedom robot according to claim 1, characterized in that: The robot's body structure is symmetrically designed, with the same joint configuration for each leg; Select one of the legs to establish a single-leg kinematic model, perform symmetric transformation based on the body coordinate system to obtain the kinematic models of the other legs, and finally obtain the kinematic model of the robot.

3. The adaptive vibration suppression control method for a multi-degree-of-freedom robot according to claim 1, characterized in that: The resultant force acting on the robot's center of mass is calculated, including: According to the DH method, the coordinate systems of each joint of any single leg of the robot are analyzed to obtain the DH parameter table; According to the expected position, the transformation matrix between the end effector coordinate system and the body coordinate system is calculated to obtain the robot kinematics positive solution; The inverse transformation method is used to calculate the joint angles of the robot movement; The end force of any single leg of the robot is calculated according to the Jacobian matrix, and the resultant force acting on the center of mass of the robot is obtained.

4. The adaptive vibration suppression control method for a multi-degree-of-freedom robot according to claim 1, characterized in that: An adaptive vibration suppression controller is constructed based on the Lagrange equation, which is expressed as: In the formula, represents the robot acceleration vector, M represents the robot's inherent inertia matrix, K E represents the spring stiffness matrix, A is the partial derivative matrix of the virtual spring extension, and f ext For external joint forces; in, Where k1, k2, k3, k4 represent the virtual stiffness of the left front leg, right front leg, left hind leg, and right hind leg respectively; δl i represents the extension of the virtual spring; x, y, z are the three-dimensional position coordinates of the robot in the world coordinate system, and α, β, γ are the deflection angles of the robot compared to the initial posture in the world coordinate system. var ∈R 6×1 is the resultant force of external generalized forces acting on the center of mass, f q represents the virtual traction force to maintain a constant speed, m represents the robot's own mass, and I xx , I yy ,I zz Represents the body inertia.

5. The adaptive vibration suppression control method for a multi-degree-of-freedom robot according to claim 4, characterized in that: The legs of the robot include springs, and the length of the springs when the robot is stationary is the original length of the springs of the legs of the robot.

6. The adaptive vibration suppression control method for a multi-degree-of-freedom robot according to claim 4, characterized in that: The virtual traction force f to maintain a constant speed q It is expressed as: Among them, x err =x cur -x, x err =x cur -x, In the formula, x err Represents the position difference at the current moment, Indicates the speed difference at the current moment; x cur Indicates the actual position obtained by the sensor, Indicates the actual speed obtained by the sensor, represents the desired velocity of the leg end; x represents the desired position of the leg end, K represents the stiffness parameter, and D represents the damping parameter.

7. The adaptive vibration suppression control method for a multi-degree-of-freedom robot according to claim 4, characterized in that: The three-dimensional vector δl of the spring extension is calculated by the following formula: δl = [x f ,y f ,z f ] T -(R z (γ-γ0)R y (β-β0)R x (α-α0)[L / 2,-W / 2,0] T +[x-x0,y-y0,z-z0] T ); Among them, L and W are the length and width of the robot body, R x (θ),R y (θ),R z (θ) is the rotation matrix of θ degrees around a coordinate axis, [x f ,y f ,z f ] T Represents the coordinates of the point at the end of any single leg on the motion plane; [x0, y0, z0, α0, β0, γ0] T represents the initial state of the robot in the world coordinate system, x, y, z are the three-dimensional position coordinates of the robot in the world coordinate system, α, β, γ are the deflection angles of the robot in the world coordinate system compared with the initial posture.

8. The adaptive vibration suppression control method for a multi-degree-of-freedom robot according to claim 3, characterized in that: According to the DH parameter table, the kinematic solution is calculated as: Among them, L1, L2, L3 represent the lengths of the other three legs, s i = sinθ i , c i = cosθ i ,s ij = sin(θ i +θ j ), c ij =cos(θ i +θ j ), and so on; θ i represents the rotation angle of joint i; Represents the desired position of the end of any single leg in the world coordinate system; The inverse transformation method is used to calculate the joint angles of the robot motion, including: in, 9. The adaptive vibration suppression control method for a multi-degree-of-freedom robot according to claim 4, characterized in that: According to the torque and end force conversion formula, the end force of any single leg is obtained: f=(J(θ) T ) + t; Among them, τ∈R n×1 is the joint torque matrix, n is the number of joints, f∈R 3×1 is the three-dimensional force at the end, J(θ) is the Jacobian matrix; The resultant force on the robot's center of mass is expressed as: f b =∑f i ,i∈1~4.

10. A robot, using the adaptive vibration suppression control method of a multi-degree-of-freedom robot according to any one of claims 1 to 9, characterized in that: include: A fuselage and legs, wherein the legs are symmetrically mounted on both sides of the fuselage.

Images