An underwater thruster control method based on multi-motor cooperation

Abstract

The present invention belongs to the technical field of underwater robot electric propulsion control. Specifically, it is a control method for an underwater thruster based on multi-motor cooperation. This solution proposes a thrust normalization and dimensionality reduction distribution method to reduce the matrix dimension, simplify the complexity of multi-motor control, and achieve multi-motor differential speed control. For this multi-motor cooperative control system, the present invention proposes to use an exponentially decaying torque observer to feedback the rotational speed of the propulsion motor on the driven shaft, so as to solve the contradiction that torque sensors cannot be installed on underwater robots, but the main shaft method has high requirements for torque detection accuracy, ensure the proportional coordination and synchronization process of multi-motors, and have strong anti-disturbance ability.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control, and in particular, is an underwater thruster control method based on multi-motor coordination. Background Art

[0002] Developing the ocean has become a way to resolve the contradiction between limited land resources and excessive manpower needs. Exploring and developing the ocean cannot be separated from the support of advanced technology. Underwater robots are important tools for difficult underwater exploration and important carriers for dangerous deep-sea missions. When underwater robots work in shallow water or complex environments, they are often disturbed by turbulence. Severe turbulence will cause yaw / rollover and other serious problems. The design of efficient thrust instructions and multi-propulsion motor coordinated control can eliminate the influence of turbulence, stabilize the robot's posture, and lay a solid foundation for subsequent inertial navigation, trajectory control, and formation control.

[0003] At present, there are mainly the following schemes for underwater propulsion control:

[0004] (1) Vector propulsion control system

[0005] The principle of vector propulsion control is to obtain additional control torque by deflecting the direction of the propeller of the propeller. Its characteristic is that the control torque is closely related to the propeller. However, it is very cumbersome to calculate the torque value and angle of multiple propellers, and it is not suitable for multiple underwater propellers.

[0006] (2) Bionic propulsion control system

[0007] Bionic propulsion control simulates the unique swimming style of fish, using the fluctuation of the fish body or tail fin to generate translational waves behind the tail, thereby generating forward thrust. The pectoral fins play a role in assisting propulsion or changing direction, and the propulsion is generated by mixed flapping or undulation. This solution is difficult to control and is not suitable for occasions where the main propulsion of the robot is required to be high. The reliability is average.

[0008] (3) Multi-motor propulsion control system

[0009] The principle of multi-motor propulsion control is to coordinate the control of multiple motors arranged on the main body of the underwater robot, and calculate the deviation through the feedback speed or torque between the motors. By obtaining the actual speed of each motor, unified control is performed, and after the controller is calculated, the feedback relationship of each motor parameter is obtained, and the reference speed is sent to each thruster, and synchronous control is achieved through feedback deviation. However, the current multi-motor coordination technology is only applicable to the situation where the speed of multiple slave motors is consistent.

[0010] Table 1 Comparison of the advantages and disadvantages of current domestic thruster control methods

[0011]

[0012] In actual navigation, the underwater environment is complex and the viscosity coefficient of water is relatively large. When the underwater robot makes a large path change or the surrounding water flow causes continuous interference, the hull will generate continuous oscillation, making it impossible to achieve precise trajectory tracking. Even worse, it may deviate from the expected trajectory, resulting in unstable tracking control. Aiming at the actual operating conditions of the underwater robot: the differential speed coordination of multiple propulsion motors during large path changes. Summary of the Invention

[0013] The purpose of the present invention is to solve the above technical problems. The present invention proposes a thrust normalization dimensionality reduction distribution method for multi-motor collaborative control to reduce the complexity of multi-motor control and achieve differential speed collaborative control. In addition, an exponential decay torque observer is designed to solve the contradiction that torque sensors cannot be installed on underwater robots, but high-precision torque detection is required for multi-motor collaboration, and the estimated torque is fed back to the main shaft and driven shaft controllers in real time. Thus, the underwater robot adopting the multi-motor master-slave shaft collaborative control method of the present invention can still maintain strong stability during large path changes.

[0014] The specific technical solution adopted by the present invention is as follows:

[0015] An underwater thruster control method based on multi-motor collaboration, which consists of a thrust normalization dimensionality reduction distribution method and an exponential decay torque observer. The thrust normalization dimensionality reduction distribution method is designed through a contribution coefficient vector to improve the input end of the multi-motor collaboration method. The exponential decay torque observer is based on the torque expansion equation, uses the exponential decay rate to accelerate the convergence speed, and adds a disturbance rejection gain to enhance the robustness of the control system.

[0016] In the above technical solution, for the thrust normalization dimensionality reduction distribution method, according to the motion control architecture of the underwater robot, the degree-of-freedom demand command vector is calculated based on the contribution coefficient vector of the propulsion motor, and the differential input signal for multi-motor master-slave shaft collaborative control is designed, so that the propulsion motor closed-loop system has a fast trajectory tracking response speed and realizes multi-motor differential control.

[0017] The above technical solution specifically includes the following steps:

[0018] Step 1: The multi-motor master-slave shaft collaborative method uses the real-time estimated torque of the driven shaft propulsion motor as feedback, and the main shaft controls the differential speed collaborative movement of the driven shaft propulsion motor through feedback regulation and speed distribution;

[0019] Step 2: Propose a thrust normalization and dimension reduction distribution method. By decomposing the thrust command of the host computer and designing the thrust normalization and dimension reduction distribution method based on the contribution coefficient vector, it can change the thrust setting value in real time when multiple propulsion motors perform trajectory tracking, reduce the matrix dimension, simplify the complexity of multi-motor control, and thus complete the coordinated control of multiple driven shafts. Based on the maximum set speed, all speeds are set to the driven shaft motors in a proportional synchronization manner to achieve differential cooperative control of multiple propulsion motors;

[0020] Step 3: Design an exponentially decaying torque observer. Construct a torque expansion equation based on the current speed measurement error. Considering the speed and accuracy requirements of the system, design a torque observer with disturbance rejection gain and exponential decay rate to estimate the real-time torque at high speed and directly feedback it to the main shaft and driven shaft controllers to solve the contradiction that underwater robots cannot install torque sensors, but multi-motor cooperation has high requirements for torque detection accuracy;

[0021] Step 4: By integrating Steps 1, 2, and 3, propose an underwater thruster control method based on multi-motor cooperation, design the structure of the underwater robot power propulsion control system, and finally give a complete multi-motor cooperation control system for underwater robots; the structure of the underwater robot power propulsion control system consists of a main shaft controller and multiple driven shaft controllers. The main shaft controller is connected to a virtual motor, and each driven shaft controller is connected with an exponentially decaying torque observer and a motor control system.

[0022] Advantages of the present invention:

[0023] (1) Compared with the existing technical solutions, the multi-motor master-slave shaft cooperation method uses the thrust normalization and dimension reduction distribution method to achieve differential cooperation of multiple motors on the driven shaft, reduces the complexity of the multi-motor cooperation control system, and meets the working conditions when the underwater robot changes its path significantly;

[0024] (2) The exponentially decaying torque observer solves the contradiction that underwater robots cannot install torque sensors, but multi-motor cooperation has high requirements for torque detection accuracy, and ensures the stability of the multi-motor cooperation method;

[0025] (3) The multi-motor master-slave shaft cooperation method combines the above two methods, meets the high requirements for differential cooperation of multiple motors when the underwater robot changes its path significantly, and is beneficial to the application in the power propulsion during the trajectory tracking of underwater robots. Brief description of the drawings

[0026] Figure 1 It is the structure block diagram of the multi-motor master-slave shaft cooperation control system in the embodiment of the present invention.

[0027] Figure 2This is the block diagram of the multi-motor master-slave axis collaborative structure in the embodiment of the present invention.

[0028] Figure 3 This is the schematic diagram of the motion control system of the underwater robot in the embodiment of the present invention.

[0029] Figure 4 This is the schematic diagram of the thrust normalization and dimensionality reduction allocation method in the embodiment of the present invention. Detailed implementation manners

[0030] To deepen the understanding of the present invention, the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments. The embodiments are only used to explain the present invention and do not limit the protection scope of the present invention.

[0031] An underwater thruster control method based on multi-motor collaboration consists of three parts: the multi-motor master-slave axis collaboration method, the thrust normalization and dimensionality reduction allocation method, and the exponential decay torque observer. The master-slave axis part consists of a main axis motor and a driven axis propulsion motor. The thrust normalization and dimensionality reduction allocation method redistributes the thrust command from the upper computer, and the exponential decay torque observer feeds back the real-time torque to solve the contradiction that the underwater robot cannot be equipped with a torque sensor, but the main axis method has high requirements for torque detection accuracy. A control system applied to underwater propulsion is completed. This method will assist in trajectory tracking control to complete the propulsion control of the underwater robot. The structure of the underwater robot's power propulsion control system consists of a main axis controller and multiple driven axis controllers. The main axis controller is connected to a virtual motor, and each driven axis controller is connected with an exponential decay torque observer and a motor control system, as Figure 1 shown.

[0032] Multi-motor master-slave axis collaboration

[0033] When the underwater robot makes a large path change, it has high requirements for the asynchronous collaboration of multiple motors. The multi-motor master-slave axis collaborative control simulates the actual mechanical axis by establishing a main axis with the same transmission characteristics as the actual mechanical axis, feeds back the load force on the actual driven axis to the main axis, and through the calculation of the main axis controller, achieves torque balance between the main axis and the actual driven axis. The formula for the driving torque of the main axis is as follows:

[0034] T ref =b(ω * -ω)+K m ∫(ω * -ω)dt (1)

[0035] In formula (1), T ref is the driving torque of the main axis, b is the attenuation coefficient of the main axis, K m is the elastic parameter at the input end of the main axis, ω * is the theoretically set angular velocity, and ω is the angular velocity of the main axis. Its dynamic equation is:

[0036]

[0037] In Equation (2), T refi is the feedback torque of each actual driven shaft motor, ω and θ respectively correspond to the angular velocity and rotation angle of the main shaft, and J is the moment of inertia of the main shaft.

[0038] The main shaft controller controls the synchronous motion of the multi-motor drive system through feedback regulation and speed distribution. Due to trajectory tracking and external turbulent interference of the underwater robot, each actual driven shaft motor needs to be dynamically adjusted, so its load torque T Li has time-varying and unmeasurable properties. As Figure 2 shown.

[0039] Thrust normalization and dimensionality reduction distribution method

[0040] The basic motion control architecture of the underwater robot is as Figure 3 shown. The planning system generates the corresponding desired pose according to the requirements. The controller compares the desired pose of the underwater robot with the current pose obtained based on sensor measurements to calculate the desired force and torque. The thrust distribution method is used to calculate the magnitude of the thrust that each thruster needs to provide when providing the required force and torque. The thrust provided by the thrusters is integrated into the force and torque in six degrees of freedom and used as the input value of the underwater robot model for calculation.

[0041] When the underwater robot realizes six-degree-of-freedom motion, it needs to change the speed and thrust of the propulsion motor in real time according to the working conditions. According to the relationship between the propeller speed and thrust, the desired thrust distribution formula is established:

[0042]

[0043] In Equation (3), V A is the advance speed, D is the propeller diameter, R·ω is the propeller speed, R is the gearbox reduction ratio, ω is the motor speed, K P is the torque coefficient, ρ is the liquid density of the water area where the hull is located,

[0044] F = BP d (4)

[0045] In Equation (4), F = [X Y Z K M N] T is the six-degree-of-freedom thrust and torque vector of the underwater robot composed of thrusters, B is the vector arrangement matrix of the thrusters, and P d = [P1 P2 P3 … P N T is the thrust of each thruster.

[0046] ​In order to reduce the matrix dimension and simplify the complexity of multi-motor control, multiple propulsion motors are converted into three propulsion motors along the x, y, and z axes. The execution result of the control instruction for each thruster is associated with its contribution to six degrees of freedom, and a certain contribution coefficient is assigned. For each degree-of-freedom control instruction of each thruster, a contribution coefficient vector related to the six degrees of freedom of the underwater robot can be formed.

[0047] When the controller issues control instructions for each degree of freedom of the underwater robot, the vector composed of the demand instructions for each degree of freedom of the underwater robot is dot-multiplied with the contribution coefficient vector corresponding to each degree of freedom of each thruster, and the result is the control instruction of the thruster in the direction of this degree of freedom.

[0048] The calculation rule of the demand instruction vector for degrees of freedom is as follows: Assume that the maximum thrust of the i-th thruster T i along its arrangement direction can be expressed as P imax (i = 1, 2, 3). The thrust vector P imax =(P xi , P yi , P zi ) can be obtained, and the coordinates of the thrust application point in the body coordinate system are (x i , y i , z i ). The contribution coefficients of the i-th thruster T i in the three translational degrees of freedom are represented by and , so the contribution coefficients of the thruster T i in the three degrees of freedom are

[0049]

[0050] The contribution coefficients of the three rotational degrees of freedom are represented by and ,

[0051]

[0052] The contribution coefficient matrix composed of the contribution coefficient vectors of each thruster is

[0053]

[0054] In Equation (7), P di is the thrust vector of the three simplified motors, and the reference value of the motor speed can be obtained from Equation (3). All the thrusters of the underwater robot are equivalently converted into three propulsion motors horizontally installed along the x, y, and z axes in the body coordinate, and the trajectory tracking and resistance to external turbulence interference can be simplified to the real-time speed control of the three motors.

[0055] According to the proportional synchronization requirement, define the initial proportionality coefficient: v1:v2:v3 = μ1:μ2:μ3. Compare the input speeds of the three motors, and define the highest speed axis as ω * , and define the motor with the largest proportionality coefficient (μ k = max(μ1, μ2, μ3)) as the reference value of the spindle speed.

[0056] ω * = ω max ω d = ω max (ω d1 , ω d2 , ω d3 ) (8)

[0057] In Equation (8), ω d1 , ω d2 and ω d3 are the reference speeds of the three motors, and v1, v2, and v3 are the hull propulsion speeds generated by these three equivalent propulsion motors in the x, y, and z directions.

[0058] Calculate the proportionality factor μ i , and set the maximum speed as the reference value

[0059]

[0060] Combine the transmission ratio coefficient with the spindle dynamics equation (2) to obtain the input speeds of each driven shaft. After normalization calculation, the reference speeds of each driven shaft are

[0061]

[0062] In Equation (10), is the reference rotational angular velocity of each driven shaft; ω corresponds to the angular velocity of the spindle. The feedback torque of the driven shaft is

[0063]

[0064] In Equation (11), b r is the damping gain; K r is the stiffness gain; K ir is the integral stiffness gain; is the reference rotational angular displacement of each driven shaft; θ i is the actual rotational angular displacement; θ corresponds to the spindle rotation angle.

[0065] Each driven shaft outputs the speed according to the actual working conditions. So far, the normalized design of the speed has been completed. The multi-motor propulsion under the proportional speed output can assist the underwater robot in the trajectory tracking and turning process, ensure the uniform transition of the robot during the turning process, and achieve faster and more efficient trajectory tracking, such as Figure 4As shown, the output of the normalization module is the control input of the main shaft.

[0066] Exponentially decaying torque observer

[0067] To suppress the multi-motor master-slave shaft collaborative out-of-step caused by the transient impact of the system when actively changing direction and when there are large changes in external disturbances. An exponentially decaying observer with disturbance suppression gain and exponential decay rate is designed to estimate the torque at high speed and directly feedback it to the slave shaft and the main shaft.

[0068] Considering the measurement errors caused by external water flow disturbances and dynamic response delays, a torque extended state equation is established

[0069]

[0070] Let C1 = [0 1], C2 = [1 0], D2 = [0 1], where, e wi is the current measurement error, is the measured value of i qi , e wω is the speed measurement error. The state observer is designed as

[0071]

[0072] In Equation (13), y is the measured rotational speed, y = C2x + D2w i , z is the load torque value T Li .

[0073] Let We can get

[0074]

[0075] Consider selecting the Lyapunov function V(t) = e ε T Pe ε , where P = P T > 0 is a positive definite matrix.

[0076] Under zero initial conditions, select the performance function as follows

[0077]

[0078] In Equation (15),

[0079]

[0080] When Q < 0, there is J < 0, ||Δz||2 < γ||w i ||2 is applicable to any w i≠ 0, which is equivalent to ||G Δzw || < γ. Solving Q < 0, the disturbance rejection gain K of the observer can be obtained.

[0081] To meet the requirements of real-time trajectory adjustment of the underwater robot, it is also required that the observer has a sufficiently fast dynamic response speed. When the measurement disturbance w i = 0, the Lyapunov function V(s) = e ε T Pe ε The time derivative of is

[0082] V(s) = e ε T (P(A - KC2)+(A - KC2) T P)e ε (17)

[0083] If there exists a scalar α > 0 such that

[0084]

[0085] Assume that the observation error e ε (t0) at time t0, it can be obtained that

[0086]

[0087] In Equation (19), λ(P) is the eigenvalue of the P matrix. When Equation (19) holds, the scalar α represents the exponential decay rate of the observation error.

[0088] To further improve the anti-disturbance performance of the system, the output of the observer is fed to the controller as an estimated lumped disturbance as a compensation part to enhance the robust performance.

[0089] It can be seen from the state observer Equation (13) that by combining the disturbance rejection rate K and the exponential decay rate α of the observation error and applying them to the multi-motor master-slave shaft cooperation, directly feeding back the observed torque, the main shaft will quickly respond to large load changes and can more accurately reflect the dynamic relationship of the slave shaft.

[0090] The above shows and describes the basic principles, main features and advantages of the present invention. Those skilled in the art should understand that the present invention is not limited by the above embodiments. What is described in the above embodiments and the specification is only to illustrate the principles of the present invention. Without departing from the spirit and scope of the present invention, the present invention will have various changes and improvements, and these changes and improvements all fall within the scope of the present invention claimed. The scope of protection claimed by the present invention is defined by the appended claims and their equivalents.

Claims

1. A control method for an underwater thruster based on multi-motor cooperation, characterized in that, Based on the multi-motor master-slave axis cooperative control method, it consists of the spindle thrust normalization and dimensionality reduction distribution method combined with the exponential decay torque observer of the driven axis propulsion motor, and includes the following steps: Step 1: The multi-motor master-slave axis cooperation method uses the real-time estimated torque of the driven axis propulsion motor as feedback, and the spindle controls the differential cooperative movement of the driven axis propulsion motor through feedback regulation and speed distribution; Step 2: Propose the thrust normalization and dimensionality reduction distribution method. By decomposing the thrust command of the upper computer, design the thrust normalization and dimensionality reduction distribution method based on the contribution coefficient vector; Step 3: Design an exponential decay torque observer. Construct a torque expansion equation according to the current speed measurement error. Considering the speed and accuracy requirements of the system, design a torque observer with disturbance suppression gain and exponential decay rate to estimate the real-time torque at high speed and directly feedback it to the spindle and driven axis controllers; Step 4: By integrating Steps 1, 2, and 3, propose an underwater thruster control method based on multi-motor cooperation, design the structure of the underwater robot power propulsion control system, and finally give a complete underwater robot multi-motor cooperation control system. The structure of the underwater robot power propulsion control system consists of a spindle controller and multiple driven axis controllers. The spindle controller is connected to a virtual motor, and each driven axis controller is connected with an exponential decay torque observer and a motor control system; In Step 2, in the thrust normalization and dimensionality reduction distribution method, according to the relationship between the propeller speed and thrust, establish the expected thrust distribution formula: In Equation (1), V A is the advance speed, D is the propeller diameter, R·ω is the propeller speed, R is the gearbox reduction ratio, ω is the motor speed, K P is the torque coefficient, and ρ is the liquid density of the water area where the hull is located; F = BP d (2) In Equation (2), F = [X Y Z K M N] T is the six-degree-of-freedom thrust and moment vector of an underwater robot composed of thrusters, and B is the vector arrangement matrix of the thrusters, P d = [P1 P2 P3 … P N T is the thrust of each thruster;​ In Step 2, when the controller issues control commands for each degree of freedom of the underwater robot, take the inner product of the vector composed of the demand commands for each degree of freedom of the underwater robot and the contribution coefficient vector corresponding to each degree of freedom of each thruster, and the result is the control command of the thruster in the direction of this degree of freedom; The calculation rule of the demand command vector for the degree of freedom is as follows: Set the i-th thruster T i The maximum thrust along its layout direction can be expressed as P imax (i = 1, 2, 3), Obtain the thrust vector p imax =(P xi , P yi , P zi ), and the coordinates of the thrust application point in the body coordinate system are (x i , y i , z i ). The contribution coefficients of the i-th thruster T i in the three translational degrees of freedom are represented by and , so the contribution coefficients of the thruster T i in the three degrees of freedom are The contribution coefficients of the three rotational degrees of freedom are represented by and . The contribution coefficient matrix composed of the contribution coefficient vectors of each thruster is In Equation (5), P di is the thrust vector of the three simplified motors. The reference value of the motor speed can be obtained from Equation (1). All the thrusters of the underwater robot are equivalent to three propulsion motors horizontally installed on the x, y, and z axes in the body coordinate system. Trajectory tracking and resistance to external turbulence interference can be simplified to the real-time speed control of the three motors; In the second step, according to the proportional synchronization requirement, define the initial proportionality coefficient: v1:v2:v3 = μ1:μ2:μ3, compare the input speeds of the three motors, and define the highest rotational speed axis as ω * , and define the motor with the largest proportionality coefficient (μ k = max(μ1, μ2, μ3)) as the reference value of the spindle rotational speed: ω * = ω max ω d = ω max (ω d1 , ω d2 , ω d3 ) (6) In Equation (6), ω d1 , ω d2 and ω d3 are the reference speeds of three motors, and v1, v2, and v3 are the hull propulsion speeds generated by the motors in the x, y, and z directions; Calculate the scale factor μ i , set the maximum speed to the reference value: After normalization calculation, the reference speeds of each driven axis are: In Equation (8), is the reference rotational angular velocity of each driven shaft; ω corresponds to the angular velocity of the main shaft, and the feedback torque of the driven shaft is In Equation (9), b r is the damping gain, K r is the stiffness gain, K ir is the integral stiffness gain, is the reference rotational angular displacement of each driven shaft, θ i is the actual rotational angular displacement, θ corresponding to the spindle rotation angle; When the underwater robot makes a large path change, there are high requirements for the asynchronous cooperation of multiple motors. The multi-motor master-slave axis cooperative control simulates the actual mechanical axis by establishing a spindle with the same transmission characteristics as the actual mechanical axis, feeds back the load force on the actual driven axis to the spindle, and through the calculation of the spindle controller, realizes torque balance between the spindle and the actual driven axis. The spindle driving torque formula is as follows: In Equation (10), T ref is the main shaft driving torque, b is the main shaft attenuation coefficient, K m is the elastic parameter at the input end of the main shaft, ω * is the theoretically set angular velocity, ω is the angular velocity of the main shaft, and its dynamic equation is: In formula (11), T refi is the feedback torque of each actual driven shaft motor, ω and θ respectively correspond to the angular velocity and rotation angle of the main shaft, J is the moment of inertia of the main shaft, and the main shaft controller controls the synchronous motion of the multi-motor drive system through feedback regulation and speed distribution; In Step 3, establish a torque expansion state equation based on the measurement error, select an optimization function to obtain the disturbance suppression rate and exponential decay rate of the observer, and feedback the torque value calculated by the observer to the spindle and driven axis controllers. The specific process includes the following: Considering the measurement error caused by external water flow interference and dynamic response delay, establish a torque expansion state equation: Let C1 = [01], C2 = [10], D2 = [01], Among them, e wi is the current measurement error, is i qi measurement value, and e wω is the speed measurement error. The state observer is designed as follows: In Equation (11), y is the rotational speed measurement value, and y = C2x + D2w i , z is the load torque value T Li ; Let It can be obtained that: Consider choosing the Lyapunov function \(V(t)=e\) ε T Pe ε , where \(P = P\) T > 0 is a positive definite matrix; Under zero initial conditions, select the performance function as follows: In Equation (13), When Q < 0, there is J < 0, ||Δz||2 < γ||w i ||2 holds for any w i ≠ 0, which is equivalent to ||G Δzw || < γ, solving for Q < 0, the disturbance rejection gain K of the observer can be obtained; To meet the requirements of real-time trajectory adjustment of the underwater robot, it is also required that the observer has a sufficiently fast dynamic response speed: when the measurement disturbance w i = 0, the time derivative of the Lyapunov function V(s) = e ε T Pe ε is: V(s) = e ε T (P(A - KC2)+(A - KC2) T P)e ε (15) Set a scalar α > 0 such that: Set the observation error e ε (t0) At time t0, we obtain: In Equation (17), λ(P) is the eigenvalue of the P matrix. When Equation (17) holds, the scalar α represents the exponential decay rate of the observation error.

Images