A Robust Adaptive Control Method for an Underwater Dual-Arm Robot

Abstract

The present invention relates to the technical field of motion control of underwater robots, and particularly to the motion control of a dual-arm underwater robot when the difference between the inertia of the robotic arm and the inertia of the carrier is not obvious. On the basis of using a physical model and system state to predict the disturbance between the carrier and the robotic arm, the present invention additionally considers using an extended Kalman filter to estimate the remaining disturbance, making the state estimation more accurate and capable of adapting to the load at the end of the robotic arm. Based on the computed torque control and disturbance estimation, the main control law of the system is obtained, and the sliding mode control technology is used to construct an auxiliary control law. In the presence of external disturbances, the control accuracy is ensured, the control stability of the system is achieved, and furthermore, the robustness of the controller is enhanced, so that during the movement of the robotic arm, the end of the robotic arm can still accurately track the reference trajectory, enhancing the operation ability of the system.

Description

Technical Field

[0001] The present invention relates to the technical field of motion control of underwater robot-manipulator systems, and particularly to a motion control method for an underwater flying manipulator, which realizes the feedforward compensation of the carrier under the condition of manipulator motion, and further realizes the stable tracking of the manipulator end. Background Art

[0002] The development and progress of human society are inseparable from the development and utilization of resources. The construction, maintenance, and repair of equipment and structures have brought a large number of underwater operation requirements, so underwater robots have been widely used. An underwater flying manipulator is a two-arm underwater robot for narrow spaces. The system takes the manipulator as the core and can realize fine underwater operations with a certain load. In order to reduce the influence of manipulator motion on the carrier attitude, traditional underwater robots all adopt the scheme of matching a large carrier with a small manipulator, and the inertia of the carrier is much larger than that of the manipulator. Compared with traditional underwater robots, the manipulator of the underwater flying manipulator robot accounts for 30% of the total system mass, and the difference in inertia between the carrier and the manipulator is not obvious. The manipulator motion will bring huge disturbances to the carrier, and this non-linear large disturbance is a key technical problem in system motion control.

[0003] For traditional underwater robots, the disturbance between the manipulator and the carrier is not obvious. Therefore, the manipulator motion can be directly regarded as an external disturbance and solved by using the proportional-integral-derivative control method. However, for the case where the inertia of the carrier and the manipulator is not much different, generally, the force acting on the carrier is estimated by using the dynamic and kinematic parameters of the manipulator, or a force sensor is installed at the connection between the manipulator and the carrier for direct observation. However, in practical applications, an accurate dynamic model of the manipulator cannot be obtained, and the technology of underwater force sensors is not yet mature. Summary of the Invention

[0004] The purpose of the present invention is to provide a robust and disturbance-adaptive control method for underwater robots, which overcomes the disturbance problem caused by the motion of a large-inertia manipulator to a small-inertia carrier and improves the operation ability of underwater robots.

[0005] The technical solution adopted by the present invention to achieve the above purpose is as follows:

[0006] A robust adaptive control method for an underwater two-arm robot, comprising the following steps:

[0007] 1) Based on the position, velocity, and acceleration of the manipulator of the underwater robot, predict the force exerted by the manipulator on the underwater robot;

[0008] 2) Based on the dynamic model, according to the force exerted by the manipulator on the underwater robot, predict the external disturbance of the underwater robot system to obtain a disturbance estimation;

[0009] 3) Obtain the main control rate of the system according to the disturbance estimator;

[0010] 4) Obtain the auxiliary control rate of the system through sliding mode control;

[0011] 5) Calculate the total control rate of the system based on the main control rate and the auxiliary control rate of the system, and control the movement of the underwater dual-arm robot according to the total control rate.

[0012] The said step 1) includes the following steps:

[0013] 1.1) Using the standard DH method, establish the robot coordinate system at each joint of the robotic arm respectively, and then obtain the velocities i ω i , i ν i , i v i,c and accelerations

[0014] 1.2) Use Newton's second law to obtain the inertial forces and the force conditions at each joint of each arm rod;

[0015] 1.3) Represent the forces and torques acting on the origin of the coordinate system of the robotic arm 0 in the robot body coordinate system, and then obtain the coupling forces between the robotic arm and the underwater robot

[0016] The said step 1.1) is specifically:

[0017]

[0018] where q i are respectively the position, velocity and acceleration of the i-th joint of the robotic arm, is the rotation matrix from coordinate system i to coordinate system i + 1, is the unit rotation vector of coordinate system i, i ω i , i v i are respectively the angular velocity and linear velocity of the origin of coordinate system i represented in coordinate system i, i v i,c is the linear velocity of the centroid of arm rod i represented in coordinate system i, r i+1 represents the position vector from the origin of coordinate system i to the origin of coordinate system i + 1 in coordinate system i + 1, r i+1,c represents the position vector from the origin of coordinate system i to the centroid origin of arm rod i + 1 represented in coordinate system i + 1.

[0019] The said step 1.2) is specifically:

[0020]

[0021] Among them, M i , I i are the mass and inertia of the arm i, r i , r i,c are the displacement vector from the origin of coordinate system i-1 to i and the position vector from the origin of coordinate system i-1 to the center of gravity of the arm i in coordinate system i respectively, ρ is the density of water, are the volume of the arm i and the gravitational acceleration vector in coordinate system i respectively, i F i , i T i are the inertial force and moment that cause the movement of the arm i respectively, i f i , i n i are the force and moment received by the joint i respectively.

[0022] The specific content of step 1.3) is as follows:

[0023]

[0024] Among them, r B,m is the position vector from the origin of the body coordinate system to the origin of the manipulator 0 coordinate system, are the coupling force and moment of the manipulator estimated in the robot body coordinate system on the robot carrier respectively, are the coupling force and moment acting on the left and right arms of the underwater robot respectively, 08 is an 8-row zero vector, is the coupling force between the manipulator and the underwater robot.

[0025] The specific content of step 1.2) is as follows:

[0026]

[0027] Among them, X is the system state selected by the extended Kalman filter and serves as the disturbance estimation quantity of the underwater robot system, q is the current position of the underwater robot system, is the velocity of τ d is the force and moment generated by the modeling error and external disturbance, is the derivative of the system state selected by the Kalman filter, f(X,τ,w) is the system equation, w and v are the system noise and measurement noise respectively, τ is the total control rate of the system, Z is the measurement equation of the system, q m =[q l1 q l2 q l3 ql4 q r1 q r2 q r3 q r4 T is the position of the left and right robotic arm joints in joint space, q v = [xyzφθψ] T The position and Euler angles of the underwater robot carrier, Q and R are the covariance matrices of the system noise and measurement noise respectively, and A, W, H, and V are all Jacobian matrices P k are the prior covariance matrix and posterior at the k-th moment respectively, K k is the Kalman gain at the k-th moment are the prior estimate and posterior estimate of the state selected by the extended Kalman filter at the k-th moment respectively, h is the prior estimate of the measured quantity, and I is the identity matrix

[0028] The main control rate of the system in step 3) is:[[]]

[0029]

[0030] where τ CTC is the main control rate of the system, M(q) is the mass and inertia matrix, q d is the current system reference position is the current system reference speed is the current system reference acceleration are the errors in the system position and speed respectively is the disturbance estimate obtained by the extended Kalman filter, K P , K D are diagonal coefficient matrices respectively

[0031] The auxiliary control rate of the system in step 4) is:[[]]

[0032]

[0033] where τ SMC is the system auxiliary control rate, s is the defined sliding mode surface, β, γ, K1, and K2 are diagonal coefficient matrices respectively, sign(·) is the sign function, and sig(·) is the intermediate variable

[0034] The total control rate of the system in step 5) is:[[]]

[0035] τ = τ CTC + τ SMC .

[0036] The present invention has the following beneficial effects and advantages:[[]]

[0037] ​1. Compared with the control method of traditional underwater robots that uses a dynamic model to predict manipulator disturbances, the present invention adds an external disturbance estimation method based on an extended Kalman filter, which has better adaptability. A sliding mode control is used to construct a system auxiliary control rate to enhance the robustness of the system, realizing the stable control of the system state during the movement of the underwater flying manipulator and improving the operation ability of the underwater flying manipulator.

[0038] 2. The present invention is not only applicable to the motion control of underwater flying manipulator robots, but the control method can be applicable to the motion control of any underwater large-arm small-carrier system, and helps to improve the operation ability of underwater small-volume systems.

[0039] 3. The present invention can estimate the operation load at the end of the manipulator, has good adaptability to operation tasks, and the underwater robot system still has good robustness in motion control under the influence of ocean currents. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 The structural diagram of the underwater flying manipulator involved in the present invention;

[0041] Figure 2 The flowchart of the control method of the underwater flying manipulator of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0042] The present invention will be further described in detail below with reference to the drawings and embodiments.

[0043] The entire system consists of a flying manipulator robot carrier, two five-degree-of-freedom manipulators, and a navigation system. The navigation system is equipped with necessary navigation sensors, including an inertial measurement unit, an electronic compass, a Doppler velocimeter, and a depth gauge. Among them, the role of the inertial measurement unit is to measure the three-axis acceleration of the robot carrier's movement; the role of the electronic compass is to measure the attitude angles such as the heading angle, pitch angle, and roll angle of the robot carrier's movement; the role of the Doppler velocimeter is to measure the translational speed of the robot carrier; the role of the depth gauge is to measure the depth of the robot carrier; the role of the manipulator is to perform underwater operation tasks, and each manipulator joint is equipped with an encoder. The role of the encoder is to indicate the actual position of the joint in real time and can also output information such as the speed and acceleration of the joint.

[0044] The structure of the flying manipulator robot is as Figure 1 shown.

[0045] As Figure 2 shown, the entire system works according to the following process:

[0046] The dynamic model of the underwater flying manipulator can be expressed as

[0047]

[0048] and q v =[xyzφθψ] T respectively represent the position and Euler angles of the underwater robot vehicle, and q m =[q l1 q l2 q l3 q l4 q r1 q r2 q r3 q r4 T respectively represent the positions in the joint spaces of the left and right robotic arms, and M(q) represents the mass and inertia matrix, including the added mass force, represents the Coriolis and centripetal matrix, including the added mass force, represents the hydrodynamic and damping matrix, and G(q) represents the force and moment vector generated by buoyancy and gravity, represents the coupling force between the robotic arm and the vehicle, τ represents the control input acting on the vehicle and the robotic arm, and τ d represents the force and moment generated by the modeling error and external disturbance.

[0049] First step, predict the force exerted by the robotic arm on the vehicle in the system

[0050] The current system position is q, the velocity is and the acceleration is These are the output quantities of the navigation system and are known quantities. Then, the force exerted by the robotic arm on the vehicle can be calculated based on the vehicle state and the output of the robotic arm encoder.

[0051] Using the standard DH method, establish coordinate systems at each joint of the robotic arm. Given that the position, velocity, and acceleration of the i-th joint of the robotic arm are q i , then the velocities and accelerations of each link in its coordinate system can be obtained, as shown in Equation (2). Where q i , are respectively the position, velocity, and acceleration of the i-th joint of the robotic arm, is the rotation matrix from coordinate system i to coordinate system i + 1, is the unit rotation vector of coordinate system i, i ω i , i v i are respectively the angular velocity and linear velocity of the origin of coordinate system i represented in coordinate system i, i v i,c is the linear velocity of the center of mass of link i represented in coordinate system i, and r i+1 ​represents the position vector from the origin of coordinate system \(i\) to the origin of coordinate system \(i + 1\) in coordinate system \(i+1\), \(r\) i+1,c represents the position vector from the origin of coordinate system \(i\) to the centroid origin of arm \(i + 1\) in coordinate system \(i+1\).

[0052]

[0053] Then, the inertial forces acting on each arm and the force conditions of each joint can be obtained using Newton's second law, and the calculation process is shown in Equation (3). Among them, \(M\) i , \(I\) i are the mass and inertia of arm \(i\), \(r\) i , \(r\) i,c are the displacement vector from the origin of coordinate system \(i - 1\) to \(i\) and the position vector from the origin of coordinate system \(i - 1\) to the center of gravity of arm \(i\) in coordinate system \(i\) respectively, \(\rho\) is the density of water, \(\nabla\) i , \(g\) i are the volume of arm \(i\) and the gravitational acceleration vector in coordinate system \(i\) respectively, i \(F\) i , i \(T\) i are the inertial force and torque causing the movement of arm \(i\) respectively, i \(f\) i , i \(n\) i are the force and torque acting on joint \(i\) respectively.

[0054]

[0055] Then, the force and torque acting on the origin of the manipulator coordinate system 0 can be obtained and represented in the robot body coordinate system B, and the calculation process is shown in Equation (4). Among them, \(r\) B,m is the position vector from the origin of the body coordinate system to the origin of the manipulator coordinate system 0, are the coupling force and torque of the manipulator on the robot carrier estimated in the robot body coordinate system respectively, are the coupling forces and torques acting on the left and right arms of the underwater robot respectively, 08 is an 8-row zero vector, is the coupling force between the manipulator and the underwater robot, B is represented in the body coordinate system:

[0056]

[0057] Second, predict the external disturbance according to the robot mathematical model.

[0058] Let Then, the mathematical model of the underwater flying manipulator can be simplified to

[0059]

[0060] The extended Kalman filtering technology can be used to predict the disturbance τ suffered by the underwater flying manipulator during the movement d , as shown in Equation (6). Where X is the system state selected by the extended Kalman filter, q is the current system position, is the speed, τ d is the force and moment generated by the modeling error and external interference, is the derivative of the system state selected by the Kalman filter, f(X,τ,w) is the system equation, w and v are the system noise and measurement noise respectively, τ is the total system control rate, Z is the measurement equation of the system, q m =[q l1 q l2 q l3 q l4 q r1 q r2 q r3 q r4 T is the position in the joint space of the left and right manipulators, q v =[xyzφθψ] T is the position and Euler angles of the underwater robot carrier, Q and R are the covariance matrices of the system noise and measurement noise respectively, A, W, H, and V are all Jacobian matrices, P k are the prior covariance matrix and posterior at the k-th moment respectively, K k is the Kalman gain at the k-th moment, are the prior estimate and posterior estimate of the state selected by the extended Kalman filter at the k-th moment respectively, h is the prior estimate of the measured quantity, obtained from , and I is the identity matrix.

[0061]

[0062] In the third step, the system control rate τ is given according to the computed torque control CTC . Let the current system reference position be q d , the reference speed be and the reference acceleration be Then is the error of the system position and speed, is the disturbance estimate obtained by the extended Kalman filter, K P , K D is the diagonal coefficient matrix. Then the main control rate of the system is as shown in Equation (7).

[0063]

[0064] In the fourth step, the system auxiliary control rate τ is given SMC ​The auxiliary control law is obtained by sliding mode control. Let s be the defined sliding mode surface, β, γ, K1, and K2 be diagonal coefficient matrices respectively, and sign(·) be the sign function. Then the sliding mode control law can be obtained as shown in Equation (8).

[0065]

[0066] Step 5: Give the total control law τ of the system and apply it to each joint of the robot carrier and the manipulator.

[0067] τ = τ CTC + τ SMC (9)

[0068] The present invention mainly includes three parts. First, the forces between the manipulator and the carrier are estimated by using the system states and physical parameters. Second, the disturbances outside the system are estimated by using the Kalman filter algorithm. Finally, the overall control law of the system is obtained by using the computed torque control framework and the sliding mode control method to achieve the robust adaptive control of the underwater flying manipulator system.

Claims

1. A robust adaptive control method for an underwater dual-arm robot, characterized in that, Including the following steps: 1) Predict the force exerted by the manipulator on the underwater robot based on the position, velocity, and acceleration of the manipulator of the underwater robot; 2) Based on the dynamic model, predict the external disturbance of the underwater robot system according to the force exerted by the manipulator on the underwater robot to obtain the disturbance estimation; 3) Obtain the main control law of the system according to the disturbance estimation; 4) Obtain the auxiliary control law of the system through sliding mode control; 5) Calculate the total control law of the system based on the main control law and the auxiliary control law of the system, and control the movement of the underwater dual-arm robot according to the total control law; The main control law of the system in step 3) is: where τ CTC is the main control rate of the system, M(q) is the mass and inertia matrix, is the current system reference acceleration, are the errors of the system position and velocity respectively, is the disturbance estimator obtained by the extended Kalman filter, K P , K D are diagonal coefficient matrices respectively, is the Jacobian matrix with respect to the position q and the velocity ; The auxiliary control law of the system in step 4) is: Among them, τ SMC is the system auxiliary control rate, s is the defined sliding mode surface, β, γ, K1, K2 are diagonal coefficient matrices respectively, sign(·) is the sign function, sig(·) is the intermediate variable, q r4 is the position of the r4th joint, and ρ is the density of water.

2. The robust adaptive control method for an underwater dual-arm robot according to claim 1, characterized in that, Step 1) includes the following steps: 1.1) Using the standard DH method, establish the robot coordinate system at each joint of the robotic arm respectively, and then obtain the velocities of each arm in its robot coordinate system i ω i , i ν i , i v i,c and accelerations 1.2) Use Newton's second law to obtain the inertial forces on each arm rod and the force conditions of each joint; 1.3) Represent the forces and torques acting on the origin of the coordinate system of the robotic arm 0 in the body coordinate system of the robot, and then obtain the coupling force between the robotic arm and the underwater robot 3. The robust adaptive control method for an underwater dual-arm robot according to claim 2, characterized in that, Specifically, step 1.1) is: where q i , are the position, velocity, and acceleration of the i-th joint of the robotic arm, respectively, is the rotation matrix from coordinate system i to coordinate system i + 1, is the unit rotation vector of coordinate system i, i ω i , i v i are the angular velocity and linear velocity of the origin of coordinate system i represented in coordinate system i, respectively, i v i,c is the linear velocity of the centroid of link i represented in coordinate system i, r i+1 represents the position vector from the origin of coordinate system i to the origin of coordinate system i + 1 represented in coordinate system i + 1, r i+1,c represents the position vector from the origin of coordinate system i to the centroid origin of link i + 1 represented in coordinate system i + 1.

4. The robust adaptive control method for an underwater dual-arm robot according to claim 2, characterized in that, Specifically, step 1.2) is: Among them, M i , I i are the mass and inertia of the arm i, r i , r i,c are respectively the displacement vector from the origin of coordinate system i - 1 to i and the position vector from the origin of coordinate system i - 1 to the center of gravity of the arm i in the representation of coordinate system i. ρ is the density of water, ▽ i , g i are respectively the volume of the arm i and the gravitational acceleration vector in coordinate system i, i F i , i T i are respectively the inertial force and moment that cause the movement of the arm i, i f i , i n i are respectively the force and moment received by the joint i.

5. The robust adaptive control method for an underwater dual-arm robot according to claim 2, characterized in that, Specifically, step 1.3) is: where r B,m is the position vector from the origin of the body coordinate system to the origin of the manipulator 0 coordinate system, are respectively the estimated coupling force and torque of the manipulator on the robot carrier in the robot body coordinate system, are respectively the coupling force and torque acting on the left and right arms of the underwater robot, and 08 is an 8-row zero vector, is the coupling force between the manipulator and the underwater robot.

6. The robust adaptive control method for an underwater dual-arm robot according to claim 1, characterized in that, Specifically, step 2) is: Wherein, X is the system state selected by the extended Kalman filter and serves as the disturbance estimator of the underwater robot system, q is the current position of the underwater robot system, is the velocity, τ d is the force and moment generated by the modeling error and external disturbance, is the derivative of the system state selected by the Kalman filter, f(X,τ,w) is the system equation, w and v are the system noise and measurement noise respectively, τ is the total system control rate, Z is the measurement equation of the system, q m =[q l1 q l2 q l3 q l4 q r1 q r2 q r3 q r4 T is the position in the joint space of the left and right robotic arms, q v =[xyzφθψ] T is the position and Euler angles of the underwater robot carrier, Q and R are the covariance matrices of the system noise and measurement noise respectively, A, W, H, and V are all Jacobian matrices, P k are the prior covariance matrix and posterior at the k-th moment respectively, K k is the Kalman gain at the k-th moment, are the prior estimate and posterior estimate of the state selected by the extended Kalman filter at the k-th moment respectively, h is the prior estimate of the measured quantity, and I is the identity matrix.​ 7. The robust adaptive control method for an underwater dual-arm robot according to claim 1, characterized in that, The total control law of the system in step 5) is: τ = τ CTC + τ SMC 。

Images