Robot under-actuated motion control method under condition of failure of multiple joints

Abstract

The invention relates to a robot under-actuated motion control method under the condition of failure of multiple joints, which comprises the following steps: acquiring multi-source sensor data of an underwater robot, fusing, calculating the actual angle and angular velocity of each joint of the robot, and judging the failure joints; establishing an angular velocity-angle prediction model of the failed joint, and calculating a reference angular velocity of the failed joint on a pitching and yaw control plane; and based on the actual angular velocity and the reference angular velocity of the failed joint, estimating an angular velocity control error of the failed joint, and adjusting parameters of a PID controller by using a fuzzy system and a radial basis function neural network so as to calculate compensation output of a propeller where the failed joint is located, thereby completing an under-actuated motion control process of the failed joint. Compared with the prior art, the method has good real-time performance, robustness and adaptive capacity, can improve the fault-tolerant capability of the underwater super-redundant dexterous robot, and is suitable for autonomous operation in a complex underwater environment.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and particularly to a method for under-actuated motion control of a robot in the case of multiple joint failures. Background Art

[0002] With the development of fields such as ocean resource development, scientific research, and underwater engineering, underwater hyper-redundant dexterous robots, as important tools for performing complex underwater tasks, have an increasing demand for applications. Underwater hyper-redundant dexterous robots usually have multiple joints and degrees of freedom, and can achieve flexible motion and operation to adapt to complex underwater environments and diverse task requirements. However, during actual underwater operations, due to the influence of various factors such as harsh ocean environments, wear of mechanical components, and failures of electrical systems, one or more joints of underwater hyper-redundant dexterous robots may fail. Joint failures will limit the robot's motion ability and prevent it from moving along the predetermined trajectory and posture, seriously affecting the execution effect of tasks and the safety of the robot. Therefore, there is an urgent need for a motion control method that can adapt to joint failures to improve the reliability of underwater robot task execution.

[0003] Patent CN104589349B discloses a method for autonomous control of a combined body with a single-joint robotic arm in a hybrid suspension microgravity environment, and patent CN114643582B discloses a model-free joint fault tolerance control method and device for a redundant robotic arm, etc. In the existing underwater robot motion control technology, although there are already some methods for diagnosing and dealing with joint faults, when the existing fault tolerance control methods face simultaneous failures of multiple joints, it is often difficult to ensure the stability and motion performance of the robot in a complex underwater environment, and it is impossible to effectively achieve the autonomous motion control of the robot; at the same time, the auxiliary control effect of the thruster on the failed joint is ignored, and the driving ability of the thruster cannot be effectively utilized. Summary of the Invention

[0004] The purpose of the present invention is to provide a method for under-actuated motion control of a robot in the case of multiple joint failures, which can improve the stability and operability of an underwater robot.

[0005] The purpose of the present invention can be achieved by the following technical solutions:

[0006] A method for under-actuated motion control of a robot in the case of multiple joint failures includes the following steps:

[0007] Obtain multi-source sensor data of the underwater robot, perform fusion, calculate the actual angles and angular velocities of each joint of the robot, and determine the failed joints;

[0008] Establish an angular velocity-angle prediction model for the failed joint and calculate the reference angular velocity of the failed joint in the pitch and yaw control planes;

[0009] Based on the actual angular velocity and the reference angular velocity of the failed joint, estimate the angular velocity control error of the failed joint, and use a fuzzy system and a radial basis neural network to adjust the parameters of the PID controller to calculate the compensation output of the thruster where the failed joint is located, completing the underactuated motion control process of the failed joint.

[0010] Further, the multi-source sensors include nine-axis inertial navigation distributed on each section of the robot, binocular vision sensors at the head and tail, and a DVL (Doppler Velocity Log) acoustic Doppler velocimeter in the middle.

[0011] Further, the determination steps of the failed joint include:

[0012] According to the initial motion moment of the robot underwater, calibrate the initial attitude data of the robot and calculate the reference Roll0, Pitch0, Yaw0, where Roll0 is the initial forward and backward rotational motion, Pitch0 is the initial pitch angle, and Yaw0 is the initial yaw angle;

[0013] According to the multi-source sensor data, perform filtering processing using the Kalman filtering method and establish a rotation matrix based on the current attitude where the rotation matrix is expressed as:

[0014]

[0015] R i =R yi R pi R ri

[0016] In the formula, R ri is the roll matrix, roll i is the forward and backward rotational motion of the robot, R pi is the pitch matrix of the i-th joint, pitch i is the pitch angle of the robot, R yi is the yaw matrix, yaw i is the yaw angle, R i is the combined rotation matrix of the i-th joint;

[0017] Based on the rotation matrix calculate the relative attitude between two joints, expressed as:

[0018] R ji =R j -1 R i

[0019] wherein, R ji is the relative attitude between two joints, and R j is the rotation matrix of the j-th joint;

[0020] Based on the relative attitude between the two joints, calculate the angles of the joint in the pitch and yaw control planes, expressed as:

[0021]

[0022] wherein, A jip is the pitch angle, R Ji is the rotation matrix, and A jiy is the yaw angle;

[0023] Based on the rotation matrix estimate the actual angles of each joint, and estimate the actual angular velocities of each joint according to the sampling frequency;

[0024] Compare the estimated actual angles of the joint with the angle control signal of the joint, and determine whether the error between the two is greater than a preset value. If so, it is considered that the joint fails; if not, it is considered that the joint does not fail.

[0025] Further, the steps of establishing the angular velocity-angle prediction model of the failed joint include:

[0026] Construct a kinematic discrete model of the angular velocity-angle of the failed joint, expressed as:

[0027] s k+1 = s k + Bu k + Cd k (1)

[0028] wherein, s k is the angular velocity state of the failed joint at time k, u k represents the planned angular velocity of the joint, d k represents the disturbance term, B is the control input matrix, indicating the influence weight of the planned angular velocity u k on the joint state s k , and C is the disturbance input matrix, indicating the influence weight of the external disturbance d k on the joint state;

[0029] Define the state quantity ε k = [s k , u k-1 T , and convert the kinematic discrete model of Equation (1) into:

[0030]

[0031] ​where \(I\) is the identity matrix, and \(\Delta u\) k is the change amount;

[0032] Define \(y\) k \(=[I, 0][s\) k , u\) k-1 \) T \(=D\varepsilon\) k , and construct a prediction model of angular velocity - angle within the prediction horizon \(N\), expressed as:

[0033]

[0034] where is the state transition matrix, is the control increment matrix;

[0035] Arrange Equation (3) to obtain the final prediction model of angular velocity - angle, expressed as:

[0036] \(Y = W\varepsilon\) k \(+ Z\Delta u+Md\ (4)\)

[0037] where:

[0038] \(Y=(y\) k+1 , y\) k+2 , \(\cdots\), y\) k+N ) T

[0039] \(W=(DB, DB\) 2 , \(\cdots\), DB\) N ) T

[0040] \(\Delta u = (\Delta u\) k , \(\Delta u\) k+1 , \(\cdots\), \(\Delta u\) k+M+1 ) T

[0041]

[0042] \(d=(d\) k , d\) k+1 , \(\cdots\), d\) k+N+1 ) T

[0043] where \(Y\) is the prediction model, is the state prediction matrix, \(Z\) is the lower triangular control increment matrix, and \(M\) is the block diagonal perturbation matrix.

[0044] Furthermore, the calculation steps of the reference angular velocity of the failed joint include:

[0045] Construct an optimization function of the planned angular velocity, expressed as:

[0046] minJ=(Y - S ref ) T R1(Y - S ref ) + Δu T R2Δu

[0047] s.t. u min ≤u≤u max

[0048] y min ≤y≤y max

[0049] Δu min ≤Δu≤Δu max

[0050] where J is the optimization function, Y is the prediction model of angular velocity - angle, S ref =(S ref1 , S ref2 , …, S refN ) is, Δu is, Δu min 、Δu max is, u is, u min 、u max is, y is, y min 、y max is, R1 and R2 are weight coefficients;

[0051] Substitute the prediction model of angular velocity - angle into the optimization function, and use the quadratic programming method for optimization to obtain the planned angular velocity as the reference angular velocity, where the optimization objective is expressed as:

[0052]

[0053] where J is the optimization objective, G = W T R1W is the state weight matrix, E = W T R1(ZΔu - S ref ) is the linear term coefficient, H = Z T R1Z + R2 is the control increment weight matrix, and C is the constant term.

[0054] Furthermore, the angular velocity control error includes the angular velocity control error e ωi of the failed joint body and the angular velocity coupling error e ωc between joints, and the expressions are respectively:

[0055] e ωi = ω i - ω ri

[0056] e ωc =(α1(ω1 - ω r1 ) + … + αj (ω j -ω rj )+…+α n (ω n -ω rn )) / (n - 1), j≠i

[0057] In the formula, ω i is the actual angular velocity of the i-th failed joint, ω ri is the reference angular velocity of the i-th failed joint, α1, α j , α n are, j is the j-th joint, and n is the number of joints.

[0058] Furthermore, the calculation process of the compensation output of the failed joint includes:

[0059] Based on the angular velocity control error of the failed joint, use the fuzzy system to obtain the initial control parameters P k , I k , D k of the PID controller on the coupling plane. Calculate the compensation output of each thruster group through the PID controller, and use the compensation output of each thruster group to obtain the compensation output of each failed joint. During the calculation process of the compensation output, a radial basis neural network is used to dynamically adjust the control parameters of the PID controller.

[0060] Furthermore, the steps for obtaining the initial control parameters P k , I k , D k include:

[0061] Initialize the fuzzy system for each failed joint;

[0062] Input the angular velocity control error of the failed joint into the fuzzy system, and output the initial control parameters P k , I k , D k of the PID controller, expressed as:

[0063] P k ={P c , P i}, I k ={I c , I i}, D k ={D c , D i}

[0064] In the formula, P c , P i are the proportional parameters, I c , I iis the integral parameter, D c , D i is the differential parameter.

[0065] Furthermore, the steps for obtaining the compensation output of each thruster group include:

[0066] Calculate the compensation output of each thruster according to the control parameters of the PID controller. The expression is:

[0067] T ci = P c e ωc + D c Δe ωc + β1I c ∫e ωc + … + P i e ωi + D i Δe ωi + β2I i ∫e ωi

[0068] In the formula, T ci is the compensation output of the thruster, P c , P i is the proportional parameter, I c , I i is the integral parameter, D c , D i is the differential parameter, Δe ωc is the angular velocity error of inter-joint coupling, Δe ωi is the angular velocity error of the i-th joint body. β1 and β2 are switch coefficients. When the error is within the threshold , the switch coefficient is 1 and the integral link is enabled; otherwise, the integral coefficient is 0.

[0069] Obtain the compensation output of each thruster group according to the compensation output of each thruster. The expression is:

[0070] T c = T c1 + T c2 + … + T cn

[0071] In the formula, T c is the compensation output of the thruster group, representing the sum of the outputs of the failed joint on different rotational freedom planes, and n is the number of thrusters.

[0072] Furthermore, the steps for dynamically adjusting the control parameters using a radial basis neural network include:

[0073] Construct a radial basis neural network, including an input layer, a hidden layer, and an output layer. The output layer vector is expressed as:

[0074] X i =[x a ,T ci ,e i T

[0075] x a =[ω1,…,ω n

[0076] In the formula, X i is the input vector, ω n is the actual angular velocity of the nth joint, x a is the joint angular velocity, T ci is the thruster output compensation, e i is the angular velocity control error and coupling error;

[0077] The activation function adopted by the hidden layer is the Gaussian function, expressed as:

[0078]

[0079] In the formula, h j (x i ) is the Gaussian function, x i is the current input vector, representing the real-time state of the system, c j is the Gaussian kernel center point vector, σ j is the Gaussian kernel width;

[0080] The output layer vector is expressed as:

[0081] Y=[ΔK p1 ,ΔK i1 ,ΔK d1 ,ΔK p2 ,ΔK i2 ,ΔK d2 ,d k1 ,…,d kn T

[0082] In the formula, Y is the output layer vector, ΔK p1 、ΔK p2 is the proportional parameter adjustment amount, ΔK i1 、ΔK i2 is the integral parameter adjustment amount, ΔK d1 、ΔK d2 , is the differential parameter adjustment amount, d kn is the coupling error correction term;

[0083] ​​​Based on the radial basis neural network, the gradient descent method is used to train and adjust the weights during the control process to obtain the dynamically adjusted control parameters. The operation expression of the gradient descent method is as follows:

[0084]

[0085] w ij = w ij + Δw ij + α(w ij - w ijlast )

[0086]

[0087] c j = c j + Δc j + α(c j - c jlast )

[0088]

[0089] σ j = σ j + Δσ j + α(σ j - σ jlast )

[0090] In the formula, D is the loss function, is the derivative of the actual joint angular velocity vector, is the derivative of the reference joint angular velocity vector, Δw ij is the weight increment at the previous moment, l r is the weight learning rate, w ij is the connection weight from the hidden layer to the output layer, h j is the output (Gaussian function value) of the j-th neuron in the hidden layer, α is the momentum factor, Δc j is the center point increment, Δσ j is the width increment, σ jlast is the σ j value at the previous moment.

[0091] Compared with the prior art, the present invention has the following beneficial effects:

[0092] (1) Based on multi-source sensor data fusion, this invention combines strategies such as failed joint state detection, angular velocity prediction model, failed joint dynamic kinematic model, online rolling optimization, and thruster compensation to achieve motion control and error compensation for failed joints. By using the techniques of fuzzy system parameter initialization and dynamic adjustment of radial basis neural network parameters, the adaptability of the system is improved. Through the construction of inter-joint coupling error estimation and optimizer, the angles of failed joints are compensated and adjusted, thereby maintaining the stability and maneuverability of the underwater hyper-redundant dexterous robot.

[0093] (2) The method of this invention improves the controllability of failed joints. By generating a thruster group that produces thrust in different directions, the joint angles of failed joints are controlled, taking into account both control accuracy and response speed. Compared with traditional failed joint control methods, the method proposed in this invention has higher control accuracy, effectively utilizes thrusters to compensate for failed joints, and can achieve motion control in the case of any number or even all joint failures. BRIEF DESCRIPTION OF THE DRAWINGS

[0094] Figure 1 is a schematic diagram of the method flow of this invention;

[0095] Figure 2 is a schematic diagram of a three-joint underwater hyper-redundant dexterous robot with two failed joints of this invention;

[0096] Figure 3 is a schematic diagram of the process of predicting the angular velocity of a failed joint of this invention;

[0097] Figure 4 is a schematic diagram of the process of thruster compensation calculation of this invention;

[0098] Figure 5 is a schematic diagram of the design of the fuzzy system of this invention;

[0099] Figure 6 is a schematic diagram of the output surface of the fuzzy system of this invention;

[0100] Figure 7 is the system structure diagram of this invention. DETAILED DESCRIPTION OF THE INVENTION

[0101] The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. This embodiment is implemented on the premise of the technical solution of the present invention, and gives detailed implementation manners and specific operation processes, but the protection scope of the present invention is not limited to the following embodiments.

[0102] This embodiment provides a method for under-actuated motion control of a robot in the case of multiple joint failures. Through an additional thruster group, the control quantity for controlling the angles of the failed joints is calculated. First, based on the current nine-axis inertial navigation, binocular camera, and DVL sensor, the current joint angles are calculated, and whether there are failed sensors is determined. For the failed sensors, based on the angular velocity-angle prediction model, the angular velocities on their different control planes are planned. Finally, according to the planned angular velocity and the sensed actual angular velocity, the angular velocity control error and the coupling error on the plane are calculated, and the thruster compensation output is calculated through an adaptive coupling plane PID controller. Next, taking a Figure 2 three-joint underwater hyper-redundant dexterous robot with two faulty joints shown in Figure 1 as an example, as shown in

[0103] the method includes the following steps:

[0104] The multi-source sensors are: nine-axis inertial navigation distributed on each joint of the underwater hyper-redundant dexterous robot, binocular vision sensors distributed at the head and tail of the underwater hyper-redundant dexterous robot, and DVL acoustic Doppler velocimeters distributed in the middle of the underwater hyper-redundant dexterous robot;

[0105] The method for multi-source sensor data fusion to sense joint angles / angular velocities includes the following sub-steps:

[0106] 1) At the initial movement moment of the underwater hyper-redundant dexterous robot, calibrate the initial attitude data of the underwater hyper-redundant dexterous robot as the attitude calculation reference Roll0, Pitch0, Yaw0.

[0107] 2) According to the different sensor data on each joint of the underwater hyper-redundant dexterous robot, use the Kalman filtering method to obtain a more accurate estimated value of the sensor data, and establish a solution rotation matrix according to the current attitude The specific process is as follows:

[0108] Write the rotation matrix at the current attitude according to the sensor data:

[0109]

[0110] R i =R yi R pi R ri

[0111] The relative attitude between two joints is expressed as:

[0112] R ji =R j -1 R i

[0113] The angles of the joints in the pitch and yaw control planes can be expressed as:

[0114]

[0115] 3) According to Calculate the angles of each joint, and estimate the average angular velocity of the joint according to the sampling frequency.

[0116] The method for judging joint failure is: compare the estimated joint angle with the actual joint angle control signal. If the error between the two is > 2%, it is considered that the joint is in a failure state.

[0117] Step S2: For the failed joint, sense the angle error A of each joint through multi-source sensor data fusion e , decompose the angle of the failed joint into the pitch and yaw planes, and calculate the reference angular velocity ω of the failed joint in each control plane based on the joint angular velocity-angle prediction model r , as Figure 3 shown, the specific process is:

[0118] Step S21: Establish an angular velocity-angle prediction model for the failed joint. The process of model establishment is specifically

[0119] 1) List the kinematic discrete model of joint angular velocity-angle:

[0120] s k+1 = s k + Bu k + Cd k

[0121] where s k is the joint angular velocity state at time k, u k represents the planned joint angular velocity, and d k represents the disturbance term.

[0122] 2) Define a new state variable ε k = [s k , u k-1 T , and convert the kinematic discrete model to:

[0123]

[0124] 3) Define y k = [I, 0][s k , u k-1 T = Dε k , and write the prediction model within the prediction horizon N as:

[0125] y​​k+1 = Dε k+1 = DB1ε k + DB2Δu k + DCd k

[0126]

[0127] 4) The final joint angular velocity - angle model can be written in the following form:

[0128] Y = Wε k + ZΔu + Md

[0129] Where:

[0130] Y = (y k+1 , y k+2 , …, y k+N ) T

[0131] W = (DB, DB 2 , …, DB N ) T

[0132] Δu = (Δu k , Δu k+1 , …, Δu k+N+1 ) T

[0133]

[0134] d = (d k , d k+1 , …, d k+N+1 ) T

[0135] Step S22, list the optimization function of the planned angular velocity:

[0136] min J = (Y - S ref ) T R1(Y - S ref ) + Δu T R2Δu

[0137] s.t. u min ≤ u ≤ u max

[0138] y min ≤ y ≤ y max

[0139] Δu min ≤ Δu ≤ Δu max

[0140] Where, Sref =(S ref1 , S ref2 , …, S refN ), where R1 and R2 are weight coefficients.

[0141] Step S23: Substitute the prediction model into the optimization function and use the quadratic programming method to optimize and obtain the planned angular velocity. The optimization objective is:

[0142]

[0143] Step S3: Estimate the actual angular velocity ω of each failed joint through multi-source sensor data fusion, and estimate the angular velocity control error e of the joint body and the angular velocity coupling error e ωi between joints according to the positional relationship between the failed joints; ωc

[0144] The calculation method of the joint body control error is:

[0145] e ωi = ω i - ω ri

[0146] The calculation method of the coupling error between joints on each plane is:

[0147] e ωc =(α1(ω1 - ω r1 ) + … + α j (ω j - ω rj ) + … + α n (ω n - ω rn )) / (n - 1), j ≠ i

[0148] Step S4: For each failed joint, based on its angular velocity control error e ωi and the angular velocity coupling error e ωc between joints, use the fuzzy system to obtain the initial control parameters P k , I k , D k of the coupling plane PID controller, and calculate the output T c compensated for each failed joint through the thruster group, as shown in Figure 4 ; the specific process is:

[0149] Step S41: For each failed joint, design and initialize a fuzzy system. The design of the fuzzy system is as shown in Figure 5 ; each fuzzy system contains 2 inputs, 6 outputs, and 121 rules. The fuzzy variables are set as shown in Table 1;

[0150] Table 1 Fuzzy Variable Settings

[0151]

[0152] The fuzzy output surface determined according to the fuzzy rules is as Figure 6 shown;

[0153] Step S42: Input the body control error and coupling error of each failed joint, and obtain the initial control parameters P k , I k , D k .

[0154] Among them, P k = {P c , P i}, I k = {I c , I i}, D k = {D c , D i}.

[0155] Step S43: Calculate the compensation output T c of the thruster according to the control parameters of the controller. The expression is:

[0156] T ci = P c e ωc + D c Δe ωc + β1I c ∫e ωc + … + P i e ωi + D i Δe ωi + β2I i ∫e ωi

[0157] Among them, β1 and β2 are switching coefficients. When the error is within the threshold , the switching coefficient is 1 and the integral link is enabled; otherwise, the integral coefficient is 0.

[0158] Step S44: The output of each thruster group finally is the sum of the outputs of the failed joint on different rotational degree-of-freedom planes. The expression is:

[0159] T c = T c1 + T c2

[0160] Step S5: For the coupling plane PID controller of each failed joint, use a radial basis neural network to dynamically adjust the controller parameters based on the gradient descent method. Specifically:

[0161] The radial basis neural network described in step S5 is specifically designed as follows:

[0162] The input layer vector is expressed as:

[0163] X i =[x a ,T ci ,e i T

[0164] x a =[ω1,…,ω n

[0165] where x a is the joint angular velocity, T ci is the thruster output, and e i is the angular velocity control error and coupling error.

[0166] The hidden layer contains 12 nodes, and the activation function uses the Gaussian function;

[0167]

[0168] The output layer vector is expressed as:

[0169] Y = [ΔK p1 ,ΔK i1 ,ΔK d1 ,ΔK p2 ,ΔK i2 ,ΔK d2 ,d k1 ,…,d kn T

[0170] where ΔK p1 etc. are the changes in the controller gains, and d ki is the disturbance estimate of the failed joint.

[0171] During the control process, the weights are trained and adjusted by the gradient descent method:

[0172]

[0173] w ij = w ij +Δw ij +α(w ij -w ijlast )

[0174]

[0175] σ j = σ j +Δσ j ​​​+α(σ j -σ jlast )

[0176] The simulation experiment results of this embodiment are shown in Table 2, where the reference angles in the pitch plane and the yaw plane are given respectively to verify the joint angle control effect described in the invention.

[0177] Table 2 Simulation experiment results of the embodiment

[0178]

[0179] As shown in Table 2, the fault joint controller described in the invention can achieve fast and stable control on the rotation planes of pitch and yaw, with extremely small control errors, and can achieve the control effect of controllable joints.

[0180] The above control method can be integrated into the under-actuated motion control system of the robot, as Figure 7 shown. The system includes: a multi-source sensor data fusion module 1, which is used to preprocess the data of each sensor obtained in real time, calculate the attitude, joint angle and angular velocity data of each section, and monitor the joint state;

[0181] A joint angular velocity planning module 2, which is used to plan the reference angular velocity of the failed joint according to the perception data of the multi-source sensor data fusion module 1 and in combination with the angular velocity-angle prediction model;

[0182] A joint angular velocity control module 3, which is used to calculate the compensation output of the thruster by combining the expected joint angular velocity planned by the joint angular velocity planning module 2, the actual joint angular velocity perceived by the multi-source sensor data fusion module 1, and the controller parameters calculated by the controller parameter adaptive adjustment module 4;

[0183] A controller parameter adaptive adjustment module 4, which is used to calculate the initial controller parameters through a fuzzy system according to the reference joint angular velocity planned by the joint angular velocity planning module 2 and the actual joint angular velocity perceived by the multi-source sensor data fusion module 1, and dynamically adjust the controller gain using a radial basis neural network.

[0184] When the above functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that makes a contribution to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs, etc., all of which can store program codes.

[0185] Those skilled in the art should understand that the embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memories, CD-ROMs, optical memories, etc.) containing computer-usable program codes. The solutions in the embodiments of the present invention can be implemented in various computer languages. For example, object-oriented programming languages such as Java and interpreted scripting languages such as JavaScript.

[0186] The present invention is described with reference to the flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to the embodiments of the present invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, and the combination of flows and / or blocks in the flowchart and / or block diagram, can be realized by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate a device for realizing the functions specified in Figure 1 one or more of the flows Figure 1 or multiple flows and / or blocks

[0187] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured product including an instruction device, and the instruction device realizes the functions specified in Figure 1 one or more of the flows Figure 1 or multiple flows and / or blocks

[0188] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus, so that a series of operation steps are performed on the computer or other programmable apparatus to produce a computer-implemented process, thereby providing instructions for implementing the functions specified in one process or a plurality of processes and / or blocks Figure 1 in one block or a plurality of blocks Figure 1 in the steps.

[0189] Although the preferred embodiments of the present invention have been described, additional changes and modifications can be made by those skilled in the art once they learn of the basic inventive concept. Therefore, the appended claims are intended to be construed to cover the preferred embodiments as well as all changes and modifications falling within the scope of the present invention.

[0190] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims

1. A robot underactuated motion control method in the case of multiple joint failures, characterized in that, It includes the following steps: Obtain multi-source sensor data of the underwater robot, perform fusion, calculate the actual angles and angular velocities of each joint of the robot, and determine the failed joints; Establish an angular velocity-angle prediction model for the failed joints, and calculate the reference angular velocities of the failed joints in the pitch and yaw control planes; Based on the actual angular velocity and reference angular velocity of the failed joints, estimate the angular velocity control error of the failed joints, and use a fuzzy system and a radial basis neural network to adjust the parameters of the PID controller to calculate the compensation output of the thruster where the failed joint is located, completing the underactuated motion control process of the failed joints.

2. A robot under-actuated motion control method in case of multiple joint failures, characterized in that The multi-source sensors include nine-axis inertial navigation distributed on each section of the robot, binocular vision sensors at the head and tail, and a DVL (Doppler Velocity Log) acoustic Doppler velocimeter in the middle.

3. A robot under-actuated motion control method in the case of multiple joint failures, characterized in that, The determination steps of the failed joints include: According to the initial motion moment of the robot underwater, calibrate the initial attitude data of the robot, and calculate the reference Roll0, Pitch0, and Yaw0, where Roll0 is the initial forward and backward rotational motion, Pitch0 is the initial pitch angle, and Yaw0 is the initial yaw angle; Perform filtering processing using the Kalman filtering method based on the multi-source sensor data, and establish a rotation matrix according to the current attitude wherein the rotation matrix is expressed as: R i = R yi R pi R ri where, R ri is the roll matrix, roll i is the forward and backward rotational motion of the robot, R pi is the pitch matrix of the i-th joint, pitch i is the pitch angle of the robot, R ti is the yaw matrix, yaw i is the yaw angle, R i is the combined rotation matrix of the i-th joint; Based on the rotation matrix Calculate the relative pose between two joints, expressed as: R ji = R j -1 R i where R ji is the relative pose between two joints, and R j is the rotation matrix of the j-th joint; Based on the relative attitude between the two joints, calculate the angles of the joints in the pitch and yaw control planes, expressed as: Where, A jip is the pitch angle, R Ji is the rotation matrix, and A jiy is the yaw angle; Based on the rotation matrix Estimate the actual angles of each joint, and estimate the actual angular velocities of each joint according to the sampling frequency; Compare the estimated actual angle of the joint with the angle control signal of the joint, and determine whether the error between the two is greater than a preset value. If so, the joint is considered to have failed; if not, the joint is considered not to have failed.

4. A robot underactuated motion control method in case of multiple joint failures, characterized in that, The steps of establishing the angular velocity-angle prediction model for the failed joints include: Construct a kinematic discrete model of the angular velocity-angle of the failed joints, expressed as: s k+1 = s k + Bu k + Cd k (1) where s k is the angular velocity state of the failed joint at time k, u k represents the planned joint angular velocity, d k represents the disturbance term, B is the control input matrix, indicating the influence weight of the planned angular velocity u k on the joint state s k , C is the disturbance input matrix, indicating the influence weight of the external disturbance d k on the joint state; Define the state variable ε k = [s k , u k-1 T , and convert the kinematic discrete model of Equation (1) into:​ where I is the identity matrix and Δu k is the change amount; Define y k = [I, 0][s k , u k-1 T = Dε k , construct a prediction model of angular velocity - angle within the prediction horizon N, expressed as:​ wherein, is the state transition matrix, is the control increment matrix; Sort out Equation (3) to obtain the final angular velocity-angle prediction model, expressed as: Y = Wε k + ZΔu + Md (4) Where: Y = (y k+1 , y k+2 , …, y k+N ) T W = (DB, DB 2 , …, DB N ) T Δu = (Δu k , Δu k+1 , …, Δu k+N+1 ) T d = (d k , d k+1 , …, d k+N+1 ) T where Y is the prediction model, is the state prediction matrix, Z is the lower triangular control increment matrix, and M is the block diagonal perturbation matrix.

5. A robot under-actuated motion control method in case of multiple joint failures, characterized in that, The calculation steps of the reference angular velocity of the failed joints include: Construct an optimization function for the planned angular velocity, expressed as: minJ=(Y - S ref ) T R1(Y - S ref ) + Δu T R2Δu s.t.u min ≤u≤u max y min ≤y≤y max Δu min ≤Δu≤Δu max Wherein, J is the optimization function, Y is the prediction model of angular velocity - angle, S ref =(S ref1 , S ref2 , …, S refN ) is, Δu is, Δu min , Δu max is, u is, u min , u max is, y is, y min , y max is, and R1 and R2 are weight coefficients; Substitute the angular velocity-angle prediction model into the optimization function, and use the quadratic programming method for optimization to obtain the planned angular velocity as the reference angular velocity, where the optimization objective is expressed as: Where J is the optimization objective, G = W T R1W is the state weight matrix, E = W T R1(ZΔu - S tef ) is the linear term coefficient, H = Z T R1Z + R2 is the control increment weight matrix, and C is the constant term.

6. The underactuated motion control method of a robot in the case of multiple joint failures according to claim 1, wherein The angular velocity control error includes the angular velocity control error e of the failed joint body ωi and the angular velocity coupling error e between joints ωc , and the expressions are respectively as follows: e ωi = ω i - ω ri e ωc =(α1(ω1 - ω r1 ) + … + α j (ω j - ω rj ) + … + α n (ω n - ω rn )) / (n - 1), j ≠ i where ω i is the actual angular velocity of the i-th failed joint, ω ri is the reference angular velocity of the i-th failed joint, α1, α j , α n are, j is the j-th joint, and n is the number of joints.

7. A robot under-actuated motion control method in the case of multiple joint failures, characterized in that, The calculation process of the compensation output of the failed joints includes: Based on the angular velocity control error of the failed joint, the initial control parameters P k , I k , D k of the PID controller on the coupling plane are obtained by using a fuzzy system. The compensation output of each thruster group is calculated by the PID controller, and the compensation output of each failed joint is obtained by using the compensation output of each thruster group. During the calculation process of the compensation output, a radial basis neural network is used to dynamically adjust the control parameters of the PID controller.

8. A method for under-actuated motion control of a robot in the case of multiple joint failures, characterized in that, The initial control parameters P k , I k , D k The acquisition steps include: Initialize the fuzzy system for each failed joint; Input the angular velocity control error of the failed joint into the fuzzy system, and output the initial control parameters P of the PID controller k , I k , D k , which is expressed as: P k = {P c , P i}, I k = {I c , I i}, D k = {D c , D i} where P c , P i is a proportional parameter, I c , I i is an integral parameter, D c , D i is a differential parameter.

9. A robot underactuated motion control method in the case of multiple joint failures, characterized in that The steps of obtaining the compensation output of each thruster group include: According to the control parameters of the PID controller, calculate the compensation output of each thruster, and the expression is: T ci = P c e ωc + D c Δe ωc + β1I c ∫e ωc + … + P i e ωi + D i Δe ωi + β2I i ∫e ωi where, T ci is the compensation output of the thruster, P c , P i is the proportional parameter, I c , I i is the integral parameter, D c , D i is the differential parameter, Δe ωc is the coupling angular velocity error between joints, Δe ωi is the angular velocity error of the i-th joint body, β1 and β2 are switching coefficients. When the error is within the threshold , the switching coefficient is 1 and the integral link is enabled; otherwise, the integral coefficient is 0. According to the compensation output of each thruster, obtain the compensation output of each thruster group, and the expression is: T c = T c1 + T c2 + … + T cn Where, T c is the compensation output of the thruster group, representing the sum of the outputs of the failed joints on different rotational degree-of-freedom planes, and n is the number of thrusters.

10. A robot under-actuated motion control method in the case of multiple joint failures, characterized in that, The steps of dynamically adjusting the control parameters using a radial basis neural network include: Construct a radial basis neural network, including an input layer, a hidden layer, and an output layer, where the output layer vector is expressed as: X i = [x a ,T ci ,e i T ​ x a = [ω1,…,ω n ​ where X i is the input vector, ω n is the actual angular velocity of the nth joint, x a is the joint angular velocity, T ci is the thruster output compensation, and e i is the angular velocity control error and coupling error; The activation function used in the hidden layer is the Gaussian function, expressed as: where h j (x i ) is a Gaussian function, x i is the current input vector, representing the real-time state of the system, c j is the center point vector of the Gaussian kernel, and σ j is the width of the Gaussian kernel; The output layer vector is expressed as: Y = [ΔK p1 , ΔK i1 , ΔK d1 , ΔK p2 , ΔK i2 , ΔK d2 , d k1 , …, d kn T ​ where Y is the output layer vector, ΔK p1 , ΔK p2 is the proportional parameter adjustment amount, ΔK i1 , ΔK i2 is the integral parameter adjustment amount, ΔK d1 , ΔK d2 , is the differential parameter adjustment amount, d kn is the coupling error correction term; Based on the radial basis neural network, use the gradient descent method to train and adjust the weights during the control process to obtain the dynamically adjusted control parameters, where the operation expression of the gradient descent method is: w ij = w ij + Δw ij + α(w ij - w ijlast ) c j = c j + Δc j + α(c j - c jlast ) σ j = σ j + Δσ j + α(σ j - σ jlast ) where D is the loss function, is the derivative of the actual joint angular velocity vector, is the derivative of the reference joint angular velocity vector, Δw ij is the weight increment at the previous moment, l r is the weight learning rate, w ij is the connection weight from the hidden layer to the output layer, h j is the output (Gaussian function value) of the j-th neuron in the hidden layer, α is the momentum factor, Δc j is the center point increment, Δσ j is the width increment, σ jlast is the σ at the previous moment j value.

Images