A posture control method for underwater multi-legged robot based on sliding mode control

Abstract

The present invention relates to underwater robot posture control technology. The purpose is to provide a posture control method for an underwater multi-legged robot based on sliding mode control, which should be able to improve the posture control accuracy and anti-interference ability of the underwater multi-legged robot. The technical scheme is: a posture control method for an underwater multi-legged robot based on sliding mode control, comprising the following steps: step 1, establishing a kinematic model of the robot; ① first establishing a D-H model of the robot; ② establishing the relationship between the joint angular velocity and the posture angular velocity; step 2, selecting a sliding mode variable; step 3, designing a sliding mode control rate by a double power reaching law with a negative exponent; step 4, posture control.

Description

Technical Field

[0001] The invention relates to an underwater robot posture control technology, and in particular to a sliding mode posture control method for an underwater multi-legged robot. Background Art

[0002] China is a maritime country with abundant water resources on land and underwater resources. The country is paying more and more attention to the development of underwater resources. Due to the high training requirements and long training cycle of divers, the harsh underwater environment, and the limited physical strength and oxygen carried by divers, the exploration efficiency of underwater resources is not high, and deep waters cannot be explored. Therefore, underwater robots, as an important tool for underwater resource survey, are also gaining more and more attention.

[0003] In the research of underwater robots, many of them are used in the mode of underwater vehicles to detect underwater resources, which cannot detect deep underwater, resulting in inaccurate and incomplete detection. The existing robots are generally divided into three types: foot-type, wheel-type and crawler-type. However, due to the soft underwater soil, uneven terrain and complex terrain, wheel-type and crawler-type are not suitable. Only foot-type can adapt to the complex underwater environment.

[0004] When the robot is conducting underwater surveys, it needs to ensure stable posture control, for example, it needs to bypass obstacles and cross protruding rocks in a certain posture and direction; at the same time, due to the complex underwater environment, the water flow has a great impact on the robot's posture, and the robot's different postures will be affected differently by the water flow. Therefore, stable, reliable, and anti-interference posture control is extremely critical. Summary of the invention

[0005] The purpose of the present invention is to overcome the deficiencies in the above-mentioned background technology and to provide a posture control method for an underwater multi-legged robot based on sliding mode control, which method should be able to improve the posture control accuracy and anti-interference ability of the underwater multi-legged robot.

[0006] A method for controlling an underwater multi-legged robot based on sliding mode control comprises the following steps:

[0007] Step 1: Establish the robot's kinematic model

[0008] ①First, establish the DH model of the robot:

[0009] The robot comprises a body and a plurality of mechanical feet, each of which comprises a first joint, a second joint, a thigh, a third joint, and a calf connected in sequence; a world coordinate system is established, a robot coordinate system is established, and a mechanical foot coordinate system is established;

[0010] ② Establish the relationship between joint angular velocity and posture angular velocity:

[0011]

[0012] In the formula, is the rotation matrix of the robot coordinate system relative to the world coordinate system,

[0013]

[0014]

[0015]

[0016]

[0017]

[0018] B P F = B P o + o P F

[0019] is an antisymmetric matrix; B P F represents the position of the end of the mechanical foot in the robot coordinate system, B P o represents the position of the first joint in the robot coordinate system, o P F Indicates the position of the end of the robot foot in the robot foot coordinate system;

[0020] l2 and l3 are the lengths of the thigh and calf respectively;

[0021] γ, β, and α are the pitch angle, roll angle, and heading angle of the robot in its current posture, respectively; They are respectively the pitch angular velocity, roll angular velocity, and heading angular velocity of the robot in its current posture;

[0022] θ1, θ2, and θ3 are the first joint angle, the second joint angle, and the third joint angle of the robot’s mechanical foot in the current posture, respectively; They are the first joint angular velocity, the second joint angular velocity, and the third joint angular velocity of the robot's mechanical foot in the current posture;

[0023] Step 2: Select sliding mode variables

[0024] Taking time t as the control period, the robot's posture error is defined as: e = η - η d , the sliding mode is defined as: s=e+a|e| b / c ;

[0025] Where η = [γβα] T , η d =[γ d β d α d ] T , a, b, c are sliding mode parameters, γ d , β d , α d They are the pitch angle, roll angle, and heading angle of the robot when it reaches the target posture after time t;

[0026] Step 3: Design the sliding mode control rate by using the double power reaching law with negative exponent

[0027] Design a double-power sliding mode reaching law:

[0028] In the formula, is the differential of the sliding surface s, sgn() is the sign function, k1, k2, k3, m, n, p, q are the parameters of the reaching law;

[0029] Step 4: Posture Control

[0030] Calculate attitude angular velocity

[0031] Calculate joint angular velocity

[0032] Get the joint angles of the robot's target posture: Rotate the joint of the robot foot to the specified angle q t ; In the formula, q t-1 is the joint angle of the robot's mechanical foot in the current posture.

[0033] In step 2, a∈R + , b and c are both positive odd numbers and b / c>1;

[0034] In step 3, k1>0, k2>0, k3>0, 0<m<1, n>1, p and q are both positive odd numbers, and p / q>1.

[0035] The technical solution of the present invention is:

[0036] The beneficial effects of the present invention are:

[0037] 1. Due to the complexity of the underwater environment, the water flow speed may change at any time, and at the same water flow speed, the resistance of the robot is different, which makes the dynamic model of the robot more complicated. The present invention uses kinematics to establish the model of the underwater multi-legged robot, which simplifies the modeling;

[0038] 2. The present invention applies the non-singular fast terminal sliding surface (the selected sliding variable in step 2) to the control system of the underwater multi-legged robot, which can converge to zero in a limited time and has good dynamic quality;

[0039] 3. The present invention adopts a posture control rate combining a double power reaching law and an exponential reaching law, so that the posture tracking error can converge quickly at different stages, which improves the response speed of the system while reducing the chattering, and has good robustness to the uncertainty and interference of the system;

[0040] 4. The present invention adopts a multi-power sliding mode approach law with negative exponents to design the underwater robot attitude control law, which can avoid singular problems in the convergence process of terminal sliding mode control. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 It is a schematic diagram of an underwater multi-legged robot.

[0042] Figure 2 It is a schematic diagram of the mechanical feet of the underwater multi-legged robot.

[0043] Figure 3 This is a schematic diagram comparing the approach rates of different approach laws.

[0044] Figure 4 It is a schematic diagram of the convergence rate of different combinations of convergence laws.

[0045] Figure 5 It is a schematic diagram of the heading angle error, the expected heading angle and the actual heading angle.

[0046] Figure 6 It is a schematic diagram of the roll angle error, the expected heading angle, and the actual angle.

[0047] Figure 7 It is a schematic diagram of the pitch angle error, the expected heading angle and the actual angle. DETAILED DESCRIPTION

[0048] The present invention takes a mechanical foot of an underwater multi-legged robot as an example to establish a kinematic model, obtains the robot's posture through an onboard posture sensor, including pitch angle, roll angle, and heading angle, and further uses a convergence law combining multi-power and exponential convergence laws to design the attitude control rate, and performs stability analysis to achieve the purpose of convergence of attitude tracking error to zero. The following is a detailed and complete description of the underwater multi-legged robot attitude control method in conjunction with the following drawings.

[0049] A method for controlling an underwater multi-legged robot based on sliding mode control comprises the following steps:

[0050] Step 1: Establish the kinematic model of the underwater multi-legged robot

[0051] ①First, establish the DH model of the underwater robot

[0052] Robot DH model such as Figure 1 As shown; the robot includes a body and a plurality of mechanical feet, each of which includes a first joint J1, a second joint J2, a thigh L2, a third joint J3, and a calf L3 connected in sequence; each joint is driven by a motor, and a sensor for obtaining the posture of the robot is provided on the body;

[0053] Establish a world coordinate system {W}, establish a robot coordinate system {B}, and establish a mechanical foot coordinate system {O}; the origin of the robot coordinate system is the body center of the robot, the origin of the mechanical foot coordinate system is the first joint, and in the present invention, the origin of the world coordinate system is the body center of the robot at the initial position;

[0054] ②Establish the relationship between joint angular velocity and posture angular velocity

[0055] According to the control cycle t, the pitch angle γ, roll angle β, and heading angle α of the robot in the current posture (the start time of this control cycle t) are obtained, and the pitch angle γ of the robot in the target posture (the end time of this control cycle t) is set. d , roll angle β d , heading angle α d ; The present invention uses Euler angles to describe the posture of the underwater robot. The roll angle is obtained by rotating around the x-axis, the pitch angle is obtained by rotating around the y-axis, and the heading angle is obtained by rotating around the z-axis. The above Euler angles are all Euler angles of the body relative to the world coordinate system.

[0056] Based on the DH model of the robot, the position of the end of the mechanical foot in the robot coordinate system is obtained:

[0057] B P F = B P o + o P F (1)

[0058] In formula (1), B P F Indicates the position of the end of the mechanical foot L4 in the robot coordinate system, B P o represents the position of the first joint J1 in the robot coordinate system, o P F Indicates the position of the end L4 of the robot foot in the robot foot coordinate system;

[0059] The position of the end of the robot foot in the world coordinate system is:

[0060]

[0061] in, The rotation matrix of the robot coordinate system relative to the world coordinate system

[0062]

[0063] Derivative the left and right sides of formula (2) with respect to time:

[0064]

[0065] When the mechanical foot is in the supporting state, the end position of the mechanical foot remains unchanged, that is, And the position of the first joint in the robot is fixed, so According to formula (3), we can get:

[0066]

[0067] Substituting formula (1) into formula (4) yields:

[0068]

[0069] Get the relationship between the joint angular velocity of the mechanical foot and the posture angular velocity of the robot:

[0070]

[0071] in: is the joint angular velocity of the robot foot; θ1, θ2, and θ3 are the first joint angle, the second joint angle, and the third joint angle of the robot foot in the current posture, respectively; They are the first joint angular velocity, the second joint angular velocity, and the third joint angular velocity of the robot's mechanical foot in the current posture; is the robot's body posture, Expressed as K is the transformation matrix, which transforms the attitude angular velocity in the world coordinate system to the body coordinate system. is the robot's attitude angular velocity in the world coordinate system, are the pitch angular velocity, roll angular velocity, and heading angular velocity of the robot in the current posture; J is the Jacobian matrix;

[0072]

[0073]

[0074] l2 and l3 are the thigh length and shank length of the mechanical foot, respectively;

[0075] According to formula (6), we can get:

[0076]

[0077] It can be expressed as The antisymmetric matrix and rotation matrix of Multiply, use Expressed as Antisymmetric matrix;

[0078]

[0079] Step 2: Select sliding mode variables

[0080] Taking time t as the control period, the attitude error of the underwater robot is defined as:

[0081] e=η-η d (8)

[0082] In formula (8), η = [γβα] T is the actual posture of the robot, η d =[γ d β d α d ] T is the target posture of the underwater robot;

[0083] The sliding mode is defined as:

[0084] s=e+a|e| b / c (9)

[0085] In formula (9), a, b, c are sliding mode parameters, a∈R + , b and c are both positive odd numbers and b / c>1;

[0086] Step 3: Design the sliding mode control rate by using the double power reaching law with negative exponent

[0087] In order to make the underwater robot posture tracking error converge to zero quickly, the present invention adopts the double power reaching law to design the sliding mode control rate:

[0088]

[0089] In formula (10), is the differential of the sliding surface s, sgn() represents the sign function, k1, k2, k3, m, n, p, q are the parameters of the reaching law, and k1>0, k2>0, k3>0, 0<m<1, n>1, p and q are both positive odd numbers and p / q>1;

[0090] The reaching law of the present invention is divided into three parts, such as Figure 3As shown, the convergence speeds of the three reaching laws are obviously different at different stages. When s>1, β>1, |s| β sgns converges quickly, when α<s<1, ks converges quickly, when s<α, 0<α<1, |s| α SGNS converges quickly and can guarantee convergence to 0. Therefore, appropriate parameters are selected at different stages so that the system can converge quickly at different stages and the attitude tracking error can converge to zero in a limited time. Figure 4 As shown, under the same initial conditions, the combination of the double power reaching law and the exponential reaching law (|s| α sgns+|s| β sgns+ks), which converges faster than the double power reaching law (|s| α sgns+|s| β sgns), which meets the requirements of underwater robot attitude control for rapid control rate. The existence of negative exponential terms avoids singular points generated during the calculation process;

[0091] Step 4: Posture Control

[0092] Taking the derivative of both sides of the sliding surface (9) with respect to time, we can obtain:

[0093]

[0094] Substituting formula (10) and formula (11) into formula (8) we can get the attitude angular velocity:

[0095]

[0096] The joint angular velocity is obtained by formula (7) and formula (12):

[0097]

[0098] Joint angular velocity The joint angle can be obtained by integrating Calculate the joint angle q to reach the target posture t :

[0099]

[0100] In formula (14), q t-1 is the joint angle of the current posture, t is the control period;

[0101] Finally, start the motor of the robot foot to rotate the joint to the specified angle q t , the robot reaches the target posture; return to step 1 and perform posture control in the next control cycle t.

[0102] Lyapunov stability analysis

[0103] For a control system, stability is the most important issue to be solved first. The present invention adopts Lyapunov stability theory to determine the stability of the system. Define Lyapunov function:

[0104]

[0105] After taking the derivative of V, we can convert the formula (10) into Substituting in:

[0106]

[0107] From formula (16), we can get Therefore, the parameter design is reasonable and the system is stable. If the designed parameters such as k1, k2, k3, m, n, a, b, c, p, q cannot meet the stability requirements, corresponding modifications should be made. The parameters designed in this example are only for this example, and are intended to illustrate the universality of the attitude control method proposed in the present invention. The specific parameters should be determined according to the actual model and actual debugging conditions.

[0108] simulation

[0109] The sliding mode attitude controller of the underwater multi-legged robot proposed in this invention is jointly simulated in MATLAB and ADAMS. The reaching law parameters in the attitude control law are k1=3, k2=1, k3=5, α=1 / 3, β=3, p / q=17 / 11, and the initial attitude of the robot is [0, 0, 0] T , the desired attitude curve is set to a sine curve:

[0110] [αβγ] T =[5sin(πt / 5),5sin(πt / 5),5sin(πt / 5)] T (17)

[0111] In order to verify the interference ability and robustness of the non-singular fast terminal sliding mode, Gaussian noise with a mean of 0 and a variance of 5 is added. The final simulation is as follows Figure 5 , Figure 6 , Figure 7 shown.

[0112] Figure 5 It represents the heading angle tracking error of the underwater robot. From the figure, we can see that the heading angle error is kept in a very small range, and the maximum error is 0.14deg. Figure 6 It represents the rolling angle tracking error of the underwater robot. The error converges quickly, and the maximum error is 0.04392deg after stabilization. Figure 7It represents the pitch angle tracking error. The error converges to zero quickly. After stabilization, the maximum error is 0.08682deg. The initial simulation error is 0.8037deg, which is caused by the instability of the aircraft at the beginning.

[0113] In summary, the attitude control strategy of sliding mode control adopted in the present invention has good robustness when facing interference, has no obvious chattering, can maintain the stability of the underwater robot, and realizes precise attitude control.

[0114] The above is only a specific implementation of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art who is familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A posture control method for an underwater multi-legged robot based on sliding mode control, comprising the following steps: Step 1: Establish the robot's kinematic model ①First, establish the DH model of the robot: The robot comprises a body and a plurality of mechanical feet, each of which comprises a first joint, a second joint, a thigh, a third joint, and a calf connected in sequence; a world coordinate system is established, a robot coordinate system is established, and a mechanical foot coordinate system is established; ② Establish the relationship between joint angular velocity and posture angular velocity: In the formula, is the rotation matrix of the robot coordinate system relative to the world coordinate system, B P F = B P o + o P F is an antisymmetric matrix; B P F represents the position of the end of the mechanical foot in the robot coordinate system, B P o represents the position of the first joint in the robot coordinate system, o P F Indicates the position of the end of the robot foot in the robot foot coordinate system; l2 and l3 are the lengths of the thigh and calf respectively; γ, β, and α are the pitch angle, roll angle, and heading angle of the robot in its current posture, respectively; They are respectively the pitch angular velocity, roll angular velocity, and heading angular velocity of the robot in its current posture; θ1, θ2, and θ3 are the first joint angle, the second joint angle, and the third joint angle of the robot’s mechanical foot in the current posture, respectively; They are the first joint angular velocity, the second joint angular velocity, and the third joint angular velocity of the robot's mechanical foot in the current posture; Step 2: Select sliding mode variables Taking time t as the control period, the robot's posture error is defined as: e = η - η d , the sliding mode is defined as: s=e+a|e| b / c ; Where η = [γ β α] T , η d =[γ d β d α d ] T , a, b, c are sliding mode parameters, γ d , β d , α d They are the pitch angle, roll angle, and heading angle of the robot when it reaches the target posture after time t; Step 3: Design the sliding mode control rate by using the double power reaching law with negative exponent Design a double-power sliding mode reaching law: In the formula, is the differential of the sliding surface s, sgn() is the sign function, k1, k2, k3, m, n, p, q are the parameters of the reaching law; Step 4: Posture Control Calculate attitude angular velocity Calculate joint angular velocity Get the joint angles of the robot's target posture: Rotate the joint of the robot foot to the specified angle q t ; In the formula, q t-1 is the joint angle of the robot's mechanical foot in the current posture.

2. The method for controlling the posture of an underwater multi-legged robot based on sliding mode control according to claim 1, characterized in that: In step 2, a∈R + , b and c are both positive odd numbers and b / c>1.

3. The method for controlling the posture of an underwater multi-legged robot based on sliding mode control according to claim 2, characterized in that: In step 3, k1>0, k2>0, k3>0, 0<m<1, n>1, p and q are both positive odd numbers, and p / q>1.

Images