An underwater robot trajectory planning method based on motion constraints

Abstract

The present application relates to underwater robot motion control field, especially to variable structure underwater robot online trajectory planning method. The function relationship of motion velocity error, position error and motion velocity increment is described by using augmented state matrix form, the target function is reduced to the standard quadratic programming function composed of motion velocity increment, and the calculation efficiency of the target function is improved. At the same time, aiming at the problem of low search efficiency of multi-constraint space such as motion velocity constraint and motion increment constraint, the multi-constraint space such as motion velocity constraint and motion increment constraint is normalized to linear constraint based on motion increment, the search efficiency in the constraint space is improved, the online trajectory planning of variable structure underwater robot under the motion constraint conditions such as limited motion velocity and limited velocity increment is realized, the online real-time trajectory guiding signal is provided for underwater robot, the control amount of thruster and mechanical arm joint is distributed according to the target guiding signal of trajectory planning, and the motion control of underwater robot is realized.

Description

Technical Field

[0001] This invention relates to the field of underwater robot motion control, and more particularly to an online trajectory planning method for variable structure underwater robots. Background Technology

[0002] Underwater robots are key equipment in marine engineering projects such as deep-sea resource exploration, observation, deep-sea sampling, and marine oil and gas exploration. Traditional underwater robots mainly include remotely operated underwater vehicles (ROVs) and autonomous underwater vehicles (AUVs). ROVs receive power and control commands via an umbilical cable, carry a robotic arm, and undertake complex underwater tasks. ROVs do not need to consider energy constraints, so they have the endurance to work continuously for several days with the support of a mother ship. AUVs carry batteries, rely on their own power supply, are not constrained by umbilical cables, have a larger operating radius, and are often used for large-scale underwater resource exploration, regional patrol and monitoring, and long-distance target delivery. Compared to ROVs, AUVs have a low-drag streamlined shape and better hydrodynamic maneuverability, but their energy and space are limited, preventing them from carrying a robotic arm and limiting their operational capabilities.

[0003] Compared to traditional underwater robots, variable-structure underwater robots are a new concept robot that combines the advantages of autonomous underwater vehicles (AUVs) and remotely operated underwater vehicles (ROVs). Equipped with its own power source and a robotic arm, the variable-structure underwater robot can simultaneously perform area reconnaissance and precise target operations in small areas, achieving fully autonomous integrated underwater exploration and operation. Unlike AUVs, which often use pre-programmed or offline trajectory planning, variable-structure underwater robots need to plan their motion trajectory online based on the current task state and the robot's own state. The optimization objective is to minimize the cost of the motion trajectory, and the optimization constraints are the capability constraints of the underwater robot's motion actuators, such as the boundaries of motion speed and the boundaries of motion speed increments. The main technical challenge of variable-structure underwater robots is how to achieve online motion trajectory planning within the robot's capability constraints, providing the robot controller with trajectory control target guidance signals. Summary of the Invention

[0004] To address the online trajectory planning problem of variable-structure underwater robots under finite motion capability constraints, this invention proposes a motion-constrained online trajectory planning method for underwater robots. It innovatively employs an augmented state matrix to describe the functional relationship between motion velocity error, position error, and motion velocity increment. To address the high computational cost of the high-order trajectory planning objective function composed of position and velocity errors, the objective function is reduced to a standard quadratic programming function composed of motion velocity increments, improving computational efficiency. Simultaneously, to address the low search efficiency of solving multi-dimensional constraint spaces such as motion velocity constraints and motion increment constraints, the multi-dimensional constraint space is normalized to linear constraints based on motion increments, improving the search efficiency within the constraint space. This enables online trajectory planning for variable-structure underwater robots under motion constraints such as finite motion velocity and finite velocity increments, providing real-time online trajectory guidance signals for the underwater robot. The underwater robot then allocates control quantities to the thrusters and robotic arm joints based on the trajectory planning objective guidance signals for motion control.

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

[0006] A motion-constrained trajectory planning method for underwater robots includes the following steps:

[0007] 1) Obtain the motion state variables of the underwater robot, and calculate the state transition matrix and control matrix from the current moment to the next moment;

[0008] 2) Based on the state transition matrix and control matrix, construct the discretized state feedback error equation from the current time step to the next time step;

[0009] 3) Based on the discretized state feedback error equation, an augmented matrix method combining state quantity error and input quantity error is used to construct the motion prediction equation from the current time to the next time.

[0010] 4) Based on the motion prediction equation for the next time step, the observation equation is used to reduce the order of the augmented error state and construct a time segment motion prediction equation based on the motion input increment.

[0011] 5) Based on the motion prediction equation for time segments, construct the trajectory planning objective evaluation function and constraint set for the time segments;

[0012] 6) The objective evaluation function of time segment trajectory planning is optimized by combining augmented matrix with time segment motion prediction equation, and then standardized by secondary planning.

[0013] 7) Perform order reduction optimization on the set of constraints;

[0014] 8) Establish a system of equations consisting of the objective evaluation function and the set of constraints, and solve for the motion increment input during the time interval;

[0015] 9) Repeat steps 1) to 8) to perform trajectory planning for the underwater robot based on the obtained motion increment input.

[0016] The state transition matrix A in step 1) dis (k) and control matrix B dis (k), specifically:

[0017]

[0018] Where I is the identity matrix, and the state variables η(k) = [x(k), y(k), z(k), θ(k), ψ(k)] T Let x(k), y(k), z(k), θ(k), and ψ(k) represent the northward, eastward, vertical, pitch, and heading angles of the underwater robot at time k, respectively, and refer to the target state variable η. r (k)=[x r (k),y r (k),z r (k),θ r (k),ψ r (k)] T x r (k),y r (k),z r (k),θ r (k),ψ r (k) represent the northward, eastward, vertical, pitch, and heading angles of the underwater robot's reference target state at time k, respectively, and the motion input ν(k) = [u(k), q(k), r(k)]. T , where u(k), q(k), and r(k) represent the forward velocity, rightward velocity, vertical velocity, pitch angular velocity, and bow angular velocity of the underwater robot at time k, respectively, and Δt is the time interval between time k and time k+1.

[0019] The motion prediction equation ξ(k+1|k) in step 3) is:

[0020]

[0021] in, This represents the augmented error state at time k+1 predicted from the current time k, i.e., the motion state at the next time step. This represents the augmented error state of the estimate at time k from the current time k. This represents the estimated increment of motion input at time k from time k. This represents the error in estimating the motion input at time k-1. Let A(k) represent the error in the predicted motion input at time k, A(k) represent the state transition matrix of the prediction equation from time k to time k+1, B(k) represent the control matrix from time k to time k+1, and C is the observation matrix.

[0022] The motion prediction equation Y(k) for the time segment in step 4) is:

[0023]

[0024] in, and Let represent the prediction state error at time k for times k+1, k+2, and k+N, respectively, and let ΔU(k) ​​represent the prediction increment of motion input from time k to time k+N-1 at time k. and Let F(k) represent the predicted motion input increments for time k, k+1, and k+N-1, respectively. Let F(k) represent the state transition matrix for time k from time k to time k+N, G(k) represent the control matrix for time k from time k to time k+N, A(k) represent the state transition matrix of the prediction equation from time k to time k+1, and P(i,h,k,A,0) represent the descending consecutive multiplication of any matrix A. i is the minimum value of the index, h is the maximum value of the index, k is the radix of the index, A represents the calculation matrix of continuous multiplication, and 0 indicates that the continuous multiplication is performed in descending order, that is, continuous multiplication from the maximum value of the index to the minimum value of the index.

[0025] In step 5), the trajectory planning objective evaluation function J(k) and the set of constraints... Specifically:

[0026]

[0027] in, Let ΔU(k) ​​represent the motion input error at time k, k+2, and k+N-1, where ΔU(k) ​​represents the increment of motion input over the time interval, Q represents the weight matrix of the state error over the time interval, and R represents the weight matrix of the increment of motion input over the time interval. and These represent the minimum and maximum values ​​of the motion input error, respectively. and Let ε represent the minimum and maximum values ​​of the motion input increment, respectively, where ε = [ε1, ε2]. T Let ε represent the relaxed state variable. max =[ε1max ,ε 2max ] T It is the upper bound vector of the relaxation factor, ε 1max and ε 2max These are the upper bounds of the relaxation factor ε1 and the upper bound of the relaxation factor ε2, respectively.

[0028] Step 6) specifically involves:

[0029]

[0030] Where H(k) is the Hessian coefficient matrix of the time segment trajectory planning objective evaluation function, L(k) is the linearized coefficient matrix of the time segment trajectory planning objective evaluation function, and D(k) is the uncorrelation matrix of the time segment trajectory planning objective evaluation function.

[0031] Step 7) specifically involves:

[0032]

[0033] Where Ω1 and Ω2 represent the sub-block matrix elements of Ω(ΔU(k),ε), Ω1 represents the motion input constraint and the motion increment input constraint, and Ω2 represents the motion input constraint and the motion increment input constraint, respectively. Δu L represents the correlation coefficient matrix of the motion input boundary. u It is a constant matrix, W Δu V represents the correlation coefficient matrix at the boundary of the motion increment input. max and V min Let ΔV and ΔV represent the correlation coefficients of the motion input boundary relaxation factors, respectively. max and ΔV min Let α and β represent the correlation coefficient matrices of the motion increment input boundary relaxation factors, respectively. min and α max Let Δα represent the lower and upper bounds of the relevant variables for the relaxation factor ε1, respectively. min and Δα max These represent the lower and upper bounds of the variables related to the relaxation factor ε2, respectively. and These represent the upper and lower bound matrices of the motion velocity error, respectively. and These represent the upper and lower bound matrices of the motion increment, respectively.

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

[0035] 1. Compared with traditional offline trajectory planning methods for underwater robots, this invention has the ability to plan trajectory in real time under effective motion capability constraints, and has better algorithm robustness and environmental adaptability. It realizes online trajectory planning for underwater robots under motion constraints such as finite motion speed and finite speed increment. Using the target guidance signal of trajectory planning as input, the control quantities of the thrusters and robotic arm joints are allocated to achieve high-precision motion control of the underwater robot.

[0036] 2. Wide range of applications. This invention can be applied not only to trajectory planning for area exploration and underwater operation of variable structure underwater robots, but also to trajectory planning for application scenarios such as area cruising and target search of autonomous underwater robots.

[0037] 3. Low economic cost. This invention does not require additional sensors but relies solely on the robot's own sensors to achieve trajectory planning for the underwater robot, thus reducing the design cost of the underwater robot. Attached Figure Description

[0038] Figure 1 This is a top view of the variable structure underwater robot of the present invention.

[0039] Figure 2 This is a flowchart of the online trajectory planning method for the variable structure underwater robot of the present invention. Detailed Implementation

[0040] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments.

[0041] The entire system consists of a variable structure underwater robot, and the equipment carried by the variable structure underwater robot includes a Doppler log, a fiber optic compass, a variable structure device, a robotic arm, and a thruster. The variable structure underwater robot is the underwater vehicle carrying the equipment; Doppler and fiber optic compasses are the navigation sensors of the underwater robot. The Doppler log is responsible for measuring the underwater robot's speed relative to the seabed, including forward speed, rightward speed, and vertical speed; the fiber optic compass measures the underwater robot's attitude angles relative to the Earth's coordinate system and its angular velocity relative to the underwater robot's coordinate system. The attitude angles it measures include roll angle, pitch angle, and heading angle, and the angular velocities it measures include roll angular velocity, pitch angular velocity, and bow angular velocity; the robotic arm is the actuator that performs underwater tasks; the variable structure device is the actuator that lifts the top of the robot, and its function is to quickly switch between agile navigation mode and efficient operation mode; the thrusters provide forward, rightward, and vertical control forces for the variable structure underwater robot, and provide roll, pitch, and bow control torques.

[0042] System composition such as Figure 1 As shown.

[0043] like Figure 2 As shown, the entire system operates according to the following process:

[0044] First, the state transition matrix and control matrix of the underwater robot at the current moment, as well as the discretized state feedback error equation, are calculated. Then, the functional relationship between motion velocity error, position error, and motion velocity increment is described using an augmented state matrix form. A time-segment motion prediction equation and a trajectory planning objective evaluation function are constructed, and the objective evaluation function is optimized to improve its computational efficiency. Finally, the set of multivariate constraints, such as motion velocity and motion increment, is normalized into a set of constraints based on motion increment, improving the efficiency of constraint space search and enabling online trajectory planning for the variable-structure underwater robot. The output values ​​of velocity input increment, velocity input quantity, and position error quantity within the time segment are used as real-time trajectory guidance signals to control the underwater robot's navigation and operations.

[0045] For ease of explanation, the symbolic variables in this paper are defined as follows:

[0046] Define φ, θ, and ψ to represent the roll angle, pitch angle, and heading angle of the underwater robot, respectively; define x, y, and z to represent the northward, eastward, and vertical positions of the underwater robot, respectively; define u, v, w, q, and r to represent the forward velocity, rightward velocity, vertical velocity, pitch rate, and yaw rate of the underwater robot, respectively; define x r y r z r These represent the northward, eastward, and vertical positions of the underwater robot's reference target, respectively; define u r v r w r q r r r These represent the forward velocity, rightward velocity, vertical velocity, pitch angular velocity, and bow angular velocity of the underwater robot, respectively.

[0047] Define the state variables η = [x, y, z, θ, ψ] T Define the reference target state variable η r =[x r ,y r ,z r ,θ r ,ψ r ] T Define the motion input ν = [u, q, r] T Define the reference motion input ν r =[u r ,q r ,r r ] TLet n represent the dimension of the state variable, with a value of n=5. Since the roll angle is a small quantity during underwater robot navigation, this paper does not consider it as a state variable. Let m represent the dimension of the motion input variable, with a value of m=3. In the motion control of underwater robots, the rightward velocity, vertical velocity, and roll angular velocity have relatively small proportions, so this paper does not consider them as motion input variables.

[0048] Define t as a continuous-time variable, t k This represents the k-th time point, where k represents the time index, and t k-1 ≤t k For ease of explanation, k is used in the formula to represent time t. k .

[0049] Regarding the notation for combining time t with variables, we define a(t) to represent the value of any variable a at time t. For example, we define u(t), v(t), w(t), q(t), and r(t) to represent u, v, w, q, and r of an underwater robot at time t, respectively; similarly, u... r (t), v r (t), w r (t), q r (t), r r (t) represent the underwater robot's u at time t. r v r w r q r r r η(t) represents the underwater robot's η at time t. r (t) represents the underwater robot's η at time t. r .

[0050] Regarding the notation for combining time k with variables, we define a(k) as representing any variable a at discrete time k, i.e., time t. k The values ​​of u, v, w, q, and r are given. For example, u(k), v(k), w(k), q(k), and r(k) are defined to represent u, v, w, q, and r of the underwater robot at time k, respectively; similarly, u... r (k), v r (k), w r (k), q r (k), r r (k) represent the underwater robot's u at time t. r v r w r q r r r .

[0051] η(k) represents the underwater robot at time k, i.e., t. k η, η r(t) represents the underwater robot at time k, i.e., t. k η r .

[0052] To facilitate the description of matrix and vector operations, we define T as the transpose operator of a matrix or vector, dlg(·) as the diagonal matrix operator, c(·) as the cosine operator, and s(·) as the sine operator; we define [A; B] as the row-wise merged matrix of any matrix A and any matrix B; and we define... I represents a function that calculates the derivative of any variable u; m Let m×m be the identity matrix, which is a known constant matrix; define 0. m×n Let A be an m×n zero matrix, which is a known constant matrix; define any matrix A. a×b In this matrix, a×b represents matrix A with a rows and b columns. Any matrix A... a×b In this context, a×b represents matrix a rows and b columns; R is defined as follows: a×b R represents an a×b dimensional space matrix; a The symbol represents an a-dimensional vector space; ∈ represents a membership relation; for example, ∈R a×b Describes a matrix belonging to an a×b dimensional space, ∈R a This indicates that it belongs to an a-dimensional vector space. The operator ≡ defines equality, meaning the expression on the right side of the equals sign is a definition expression of the variable on the left side. The `where` keyword defines that the expression following the keyword is an analytical expression of the expression preceding the keyword. The `st` keyword defines that the expression following the keyword is a constraint expression.

[0053] For ease of describing continuous multiplication of matrix A, we define P(i,h,k,A,0) as denoteing continuous multiplication of any matrix A in descending order, where i is the minimum value of the index, h is the maximum bound of the index, k is the cardinality of the index, A represents the calculation matrix of continuous multiplication, and 0 indicates that the continuous multiplication is performed in descending order, that is, continuous multiplication from the maximum value of the index to the minimum value of the index.

[0054] The Doppler log is set to measure the forward, rightward, and vertical velocities of the variable structure robot in real time; these are known measurement data. The fiber optic compass is set to measure the roll angle, pitch angle, heading angle, roll speed, pitch speed, and yaw rate of the variable structure robot in real time; these are also known measurement data.

[0055] The first step is to calculate the state transition matrix and control matrix from the current time step to the next time step.

[0056] In practical engineering applications, the actuators of underwater robots are all discrete inputs. For example, the thruster speed and the speed of the robotic arm drive joints are discrete control inputs. Therefore, establishing discrete state error equations has certain engineering application value. To establish discrete state error equations from the current time k to the next time k+1, it is necessary to calculate the state transition matrix A of the discrete state error equations. dis (k) and control matrix B dis (k), which is calculated as follows:

[0057]

[0058] Where A dis (k) represents the state transition matrix at time k, which is the variable to be solved; B dis (k) represents the control matrix at time k, which is the variable to be solved; I is the identity matrix, a constant matrix. The state variables η(k) = [x(k), y(k), z(k), θ(k), ψ(k)] T Where x(k), y(k), z(k), θ(k), and ψ(k) represent the northward, eastward, vertical, pitch, and heading angles of the underwater robot at time k, respectively, and are known quantities; the reference target state quantity η r (k)=[x r (k),y r (k),z r (k),θ r (k),ψ r (k)] T , where x r ,y r ,z r ,θ r ,ψ r Let represent the northward position, eastward position, vertical position, pitch angle, and heading angle of the underwater robot at time k, respectively, which are known quantities; the motion input ν(k) = [u(k), q(k), r(k)] T Where u(k), q(k), and r(k) represent the forward velocity, rightward velocity, vertical velocity, pitch angular velocity, and bow angular velocity of the underwater robot at time k, respectively, and are known quantities. Δt is the time interval between time k and time k+1, and is also a known quantity.

[0059] The second step is to establish the discretized state feedback error equation from the current time to the next time step.

[0060] A discretized state feedback error equation is established to recursively calculate the state feedback error at time k+1 from the state feedback error at time k. Definition This represents the error in predicting the state feedback at time k+1 from time k. The calculation expressions are discretized state feedback error equations, and their calculation methods are as follows:

[0061]

[0062] in, This is the state feedback error at time k+1 predicted from time k, and it is the variable to be solved. The output of this step is... The calculation expression. η(k) is the state feedback error estimated at time k, which is an intermediate variable; η(k) is the state variable at time k, which is a known quantity; η r (k) is the reference target state quantity at time k, which is a known quantity; ν(k) represents the motion input error at time k, which is an intermediate variable; ν(k) represents the motion input at time k, which is the result of the previous calculation and is a known quantity; ν r (k) represents the reference value of the motion input at time k, which is a known quantity; A dis (k) represents the state transition matrix at time k, which is the result of the previous calculation and is a known quantity; B dis (k) represents the control matrix at time k, which is the result of the previous calculation and is a known quantity; I is the identity matrix, a constant matrix. State variables η(k) = [x(k), y(k), z(k), θ(k), ψ(k)] T Where x(k), y(k), z(k), θ(k), and ψ(k) represent the northward, eastward, vertical, pitch, and heading angles of the underwater robot at time k, respectively, and are known quantities; the reference target state quantity η r (k)=[x r (k),y r (k),z r (k),θ r (k),ψ r (k)] T , where x r ,y r ,z r ,θ r ,ψ r Let represent the northward position, eastward position, vertical position, pitch angle, and heading angle of the underwater robot at time k, respectively, which are known quantities; the motion input ν(k|k)=[u(k),q(k),r(k)] T , where u(k), q(k), and r(k) represent the forward velocity, rightward velocity, vertical velocity, pitch angular velocity, and bow angular velocity of the underwater robot at time k, respectively, and are known quantities.

[0063] The third step is to establish the motion prediction equations from the current moment to the next moment.

[0064] In practical engineering applications, the input to the actuators of underwater robots needs to satisfy the constraint of the input increment. Building upon the discretized state feedback error equation based on kinematics from the previous step, an augmented matrix method combining state and input errors is used to construct a motion prediction equation with the motion input increment as input, achieving motion prediction from the current time k to the next time step. Definition Let ξ(k+1|k) represent the augmented error state predicted at time k+1 from the current time k, i.e., the motion state at the next time. The expression for calculating ξ(k+1|k) is the motion prediction equation, and their calculation methods are as follows:

[0065]

[0066] in This represents the augmented error state predicted from the current time k to time k+1, i.e., the motion state at the next time step; it is the variable to be solved. The output of this step is the calculation expression for ξ(k+1|k). Definition This represents the augmented error state estimated from time k at the current time k. It is obtained through sensor measurements and is a known quantity. It represents the estimated increment of motion input at time k from time k, which is obtained indirectly through sensor measurement and is a known quantity; This represents the error in the state quantity estimated at time k. It can be obtained through sensor measurement and is a known quantity. Obviously, at time k... and Equal; definition This represents the error in estimating the motion input at time k-1 from the value at time k-1. It can be obtained through sensor measurement and is a known quantity. Obviously, at time k... and Equal; definition This represents the error in the predicted motion input at time k. It is the control command issued by the controller and is a known quantity. Obviously, at time k... and Equal; Define A(k) as the state transition matrix of the prediction equation from time k to time k+1, which is an intermediate variable; B(k) as the control matrix from time k to time k+1, which is an intermediate variable; A dis (k) represents the state transition matrix at time k, which is the result of the previous step and is a known quantity; B dis (k) represents the control matrix at time k, which is the result of the calculation in the previous step and is a known quantity. C is the observation matrix, which is a constant matrix and is also a known quantity.

[0067] The fourth step is to establish time-segment motion prediction equations based on the motion input increment.

[0068] Based on the prediction equation for the next time step, the observation equation is used to reduce the order and augment the error state, constructing a prediction equation for the increment of motion input over the time interval from time k to time k+N. Y(k) is defined as the state error predicted at time k for the time interval from time k to time k+N. The calculation expression for Y(k) is the motion prediction equation for the next time interval, and their calculation methods are as follows:

[0069]

[0070] in It is the variable to be solved; the output of this step is the calculation expression of Y(k). and Let ξ(k|k) represent the predicted state error at time k with respect to times k+1, k+2, and k+N, respectively; these are elements in the variables to be solved. ξ(k|k) represents the augmented error state estimated at the current time k with respect to time k; it is obtained through sensor measurement and is a known quantity in this step. N is the length of the time interval and is a known constant. ΔU(k) ​​is defined as the increment of motion input predicted at time k from time k to time k+N-1; for this step, it is an input variable and is a known quantity. and The values ​​represent the predicted motion input increments at time k, k+1, and k+N-1, respectively. For this step, these are input variables and are known quantities. F(k) represents the state transition matrix from time k to time k+N at time k, an intermediate variable that can be directly obtained through calculation. G(k) represents the control matrix from time k to time k+N at time k, an intermediate variable that can be directly obtained through calculation. A(k) represents the state transition matrix of the prediction equation from time k to time k+1, which is the result of the previous step and is a known quantity. B(k) represents the control matrix from time k to time k+1 and is a known quantity. B(k+1) and B(k+N-1) represent the control matrices at times k+1 and k+N-1, respectively, and are known quantities. A(k+1) and A(k+N-1) represent the state transition matrices at times k+1 and k+N-1, respectively, and are known quantities. For ease of describing continuous multiplication of matrix A, we define P(i,h,k,A,0) as denoteing continuous multiplication of any matrix A in descending order, where i is the minimum value of the index, h is the maximum value of the index, k is the radix of the index, A represents the calculation matrix of continuous multiplication, and 0 indicates that the continuous multiplication is performed in descending order, that is, continuous multiplication from the maximum value of the index to the minimum value of the index.

[0071] The fifth step is to establish the objective evaluation function and constraint equations for trajectory planning within the time interval.

[0072] Based on the optimization requirements of minimizing state error and minimum motion input increment in practical engineering, and considering both the boundary constraints of motion input and the boundary constraints of motion input increment, a target evaluation function composed of the state error and motion input increment for the time interval is constructed based on the motion prediction equation of the previous calculation result. This enables underwater robot trajectory planning based on motion constraints. To address the problem that the objective function optimization process is prone to getting trapped in local optima under the boundary constraints of motion input and motion input increment, a relaxed state variable is introduced into the objective function. This transforms the objective function optimization problem under hard constraints into one under soft constraints, improving the computational efficiency of the objective function optimization solution. Let J(k) represent the target evaluation function for trajectory planning in the next time interval at time k. The set of constraint expressions representing the motion input within a time interval is calculated as follows:

[0073]

[0074] Where J(k) represents the objective evaluation function of trajectory planning, which is the function to be optimized and also the output of this step; This represents the constraint expression for the motion input within the time interval. It is a composite function constraint condition of the motion input quantity, motion input increment, and relaxation factor. It is the set to be solved and also the output of this step. The output of this step is the calculation expression of the objective evaluation function J(k) and the set of motion input constraints for the time interval. The calculation expression. The value of represents the motion input error from time k to time k+N-1. In this step, it is an intermediate variable and its value does not need to be calculated. The symbol st indicates that the following expression is a constraint condition for the variable value, not a calculation expression. ΔU(k) ​​represents the increment of motion input in the time interval. For this step, it is an intermediate variable and its value does not need to be calculated. Q represents the weight matrix of the state quantity error in the time interval. It is a set parameter variable and is a known quantity. R represents the weight matrix of the increment of motion input in the time interval. It is a set parameter variable and is a known quantity. and These represent the minimum and maximum values ​​of the motion input error, respectively. They are the difference between the motion input and the reference motion input, and are known quantities. and Let ε represent the minimum and maximum values ​​of the motion input increment, respectively; these are known quantities. ε = [ε1, ε2] T This represents the relaxed state variable, which is an intermediate variable and does not need to be calculated in this step. and These represent the minimum and maximum values ​​of the motion input error, respectively. They are the difference between the motion input and the reference motion input, and are known quantities. and ε represents the minimum and maximum values ​​of the motion input increment, respectively, and are known quantities. max =[ε 1max ,ε 2max ] T It is the upper bound vector of the relaxation factor, and is a known constant; ε 1max and ε 2max These are the upper bounds of the relaxation factor ε1 and the upper bound of the relaxation factor ε2, respectively, and are constants that are known quantities.

[0075] The sixth step is to optimize the objective evaluation function for time segment trajectory planning and perform secondary planning standardization.

[0076] The objective evaluation function for time-segment trajectory planning is a multivariate composite function consisting of velocity error, position error, and velocity increment. This presents challenges due to high online computation time costs and complex coupling relationships between variables. To address these issues, an augmented matrix combined with the time-segment motion prediction equation is used to perform a quadratic programming standardization operation on the objective function, reducing it to a standard quadratic programming function containing only velocity increments. In this step, J(k) is defined as the objective evaluation function for the next time-segment trajectory planning at time k, and its calculation method is as follows:

[0077]

[0078] Here, J(k) represents the objective evaluation function of trajectory planning, which is the function to be optimized and the output of this step. The output of this step is the calculated expression of the objective evaluation function J(k). H(k) is the Hessian coefficient matrix of the objective evaluation function for time-segment trajectory planning, L(k) is the linearized coefficient matrix of the objective evaluation function for time-segment trajectory planning, and D(k) is the uncorrelation matrix of the objective evaluation function for time-segment trajectory planning. These are all intermediate variables that can be directly obtained through calculation and are known quantities for this step.

[0079] Step 7: Perform order reduction optimization on the set of constraints.

[0080] To address the issues of high dimensionality in the search space and low efficiency in finding the optimal solution of the objective evaluation function due to multivariate constraint functions such as motion velocity constraints and motion increment constraints, this paper normalizes the multivariate constraint space of motion velocity constraints and motion increment constraints into a linear constraint set based on motion increments, thereby improving the search efficiency of the objective evaluation function in the constraint space. Ω(ΔU(k),ε) is defined as the set of constraint expressions based on motion increments within a time interval, and its calculation method is as follows:

[0081]

[0082] Wherein, Ω(ΔU(k),ε) represents the set of constraint expressions based on motion increments within the time interval. It is a composite function constraint condition of motion input increment and relaxation factor. It is the set to be solved and also the output of this step. The motion input constraints are described using motion increment expressions in the Ω(ΔU(k),ε) set. The output of this step is the calculated expression of the time interval constraint set Ω(ΔU(k),ε) based on motion increment expressions. Ω1 and Ω2 represent the sub-block matrix elements of Ω(ΔU(k),ε), respectively. Ω1 represents the motion input constraint and the motion increment input constraint, and Ω2 represents the motion input constraint and the motion increment input constraint. They are intermediate variables and do not need to be directly calculated; only the calculated expression is needed. Δu The correlation coefficient matrix representing the motion input boundary is a known quantity; L u It is a constant matrix, and the quantities are known; W Δu The correlation coefficient matrix representing the input boundary of the motion increment is a known quantity; V max and V min These represent the correlation coefficients of the motion input boundary relaxation factors, which are user-defined variables and are known quantities; ΔV max and ΔV min These represent the correlation coefficient matrices of the motion increment input boundary relaxation factors, which are user-defined variables and are known quantities; α min and α max These represent the lower and upper bounds of the relevant variables for the relaxation factor ε1, respectively, and are known quantities; Δα min and Δα max These represent the lower and upper bounds of the relevant variables for the relaxation factor ε2, respectively, and are known quantities. and The upper and lower bound matrices represent the motion velocity error, respectively. They are determined by the driving capability of the underwater robot's actuators and are known quantities. and These represent the upper and lower bound vectors of the underwater robot's motion speed error, respectively. They are determined by the driving capability of the underwater robot's actuators and are known quantities. and The upper and lower bound matrices represent the motion increments, respectively. They are determined by the instantaneous acceleration capability of the underwater robot's actuators and are known quantities. and ε represents the upper and lower bound vectors of the motion increment of the underwater robot, respectively. These are determined by the instantaneous acceleration capability of the underwater robot's actuators and are known quantities. max It is the upper bound vector of the relaxation factor, and is a known constant; ε 1max and ε2max These are the upper bounds of the relaxation factor ε1 and the upper bound of the relaxation factor ε2, respectively, and are constants that are known quantities.

[0083] The eighth step is to establish a system of equations consisting of the objective evaluation function and constraints, and solve for the motion increment input during the time interval.

[0084] The objective evaluation function from step (6) and the set of constraints from step (7) are combined to form a set of quadratic programming equations. The general standard quadratic programming calculation method is used to solve the equations to obtain the increment of motion input ΔU(k) ​​representing the time interval. The set of equations formed by the quadratic programming and the constraints is as follows:

[0085]

[0086] The output of this step is the motion increment ΔU(k) ​​from time k to time k+N-1, which minimizes J(k) under the constraint Ω(ΔU(k),ε). The motion increment ΔU(k) ​​is the calculated output of online trajectory planning. Based on the motion increment ΔU(k), the motion input and target position error for the time interval can be calculated. These are then used as real-time trajectory guidance signals and input to the underwater robot controller. The underwater robot controller, based on the target guidance signal from trajectory planning, allocates control quantities to the thrusters and robotic arm joints to control the underwater robot's motion.

[0087] This invention iteratively calls steps one through eight in a cyclical manner. The input of the current cycle is the calculation result of the previous cycle, and the output of the current cycle is the input of the next cycle. This process obtains the motion input quantity and motion input increment for the underwater robot trajectory planning from time k to time k+N-1, and also obtains the target position error for the underwater robot trajectory planning from time k+1 to time k+N. Based on the trajectory planning results, the underwater robot controller allocates control quantities to the thrusters and robotic arm joints to perform motion operation control for the underwater robot.

Claims

1. A trajectory planning method for underwater robots based on motion constraints, characterized in that, Includes the following steps: 1) Obtain the motion state variables of the underwater robot and calculate the state transition matrix and control matrix from the current moment to the next moment; 2) Based on the state transition matrix and control matrix, construct the discretized state feedback error equation from the current time step to the next time step; 3) Based on the discretized state feedback error equation, an augmented matrix method combining state quantity error and input quantity error is used to construct the motion prediction equation from the current time to the next time. 4) Based on the motion prediction equation for the next time step, the observation equation is used to reduce the order of the augmented error state and construct a time segment motion prediction equation based on the motion input increment. 5) Based on the motion prediction equation of the time segment, construct the trajectory planning objective evaluation function and constraint set of the time segment; 6) The objective evaluation function of time segment trajectory planning is optimized by combining augmented matrix with time segment motion prediction equation, and then standardized by secondary planning. 7) Perform order reduction optimization on the constraint set; 8) Establish a system of equations consisting of the objective evaluation function and the set of constraints, and solve for the motion increment input during the time interval; 9) Repeat steps 1) to 8), and perform trajectory planning for the underwater robot based on the obtained motion increment input. The trajectory planning objective evaluation function in step 5) and set of constraints Specifically: in, Indicates at time k right k , k +2, k +N-1 time motion input error, This represents the increment of motion input over a time interval. The weight matrix represents the error of the state variables in the time interval. The weight matrix represents the increment of motion input over a time interval. and These represent the minimum and maximum values ​​of the motion input error, respectively. and These represent the minimum and maximum values ​​of the motion input increment, respectively. Represents the relaxed state variable. It is the upper bound vector of the relaxation factor. and These are relaxation factors upper bound, relaxation factor The upper boundary.

2. The underwater robot trajectory planning method based on motion constraints according to claim 1, characterized in that, The state transition matrix in step 1) and control matrix Specifically: in, I It is the identity matrix, the state variable. , The table represents the underwater robot's northward, eastward, vertical, pitch, and heading angles at time k, respectively, referencing the target state variables. , These represent the northward, eastward, vertical, pitch, and heading angles of the underwater robot's reference target state variables at time k, respectively, and the motion input. ,in Let k represent the forward velocity, rightward velocity, vertical velocity, pitch angular velocity, and bow angular velocity of the underwater robot at time k, respectively. It is the time interval between time k and time k+1.

3. The underwater robot trajectory planning method based on motion constraints according to claim 1, characterized in that, The motion prediction equation in step 3) for: in, This represents the augmented error state at time k+1 predicted from the current time k, i.e., the motion state at the next time step. Indicates the current moment k For time k The estimated augmentation error state, Indicates the current moment k For time k The estimated increment of motion input, Indicates in k Always k Error of motion input estimated at time -1 Indicates in k Always k Error in predicting motion input at any given time. Indicates time k At the time k +1 is the state transition matrix of the prediction equation. Indicates time k At the time k +1 control matrix, It is the observation matrix.

4. The underwater robot trajectory planning method based on motion constraints according to claim 1, characterized in that, The motion prediction equation for the time segment in step 4) for: in, , , and They represent the times respectively. k right k +1, k +2, k Predicted state error at time +N Indicates at time k Predict from time k At the time k +N-1 time interval motion input increment , and They represent the times respectively. k right k , k +1, k Predict the increment of motion input at time +N-1. Indicates time k For the state transition matrix from time k to time k+N, Indicates time k For the control matrix from time k to time k+N, Indicates time k At the time k +1 is the state transition matrix of the prediction equation. Represents an arbitrary matrix The descending consecutive multiplication, where , i It is the minimum value of the index. h It is the maximum value of the index. k It is the cardinality of the index. The computation matrix representing consecutive multiplication. This indicates that consecutive multiplication is performed in descending order, that is, consecutive multiplication from the maximum index value to the minimum index value.

5. The underwater robot trajectory planning method based on motion constraints according to claim 1, characterized in that, Step 6) specifically refers to: in, It is the Hessian coefficient matrix of the objective evaluation function for time segment trajectory planning. It is the linearized coefficient matrix of the objective evaluation function for time segment trajectory planning. It is the uncorrelated matrix of the objective evaluation function for trajectory planning in the inter-segment area.

6. The underwater robot trajectory planning method based on motion constraints according to claim 1, characterized in that, Step 7) specifically involves: in, and They represent Sub-block matrix elements, This represents motion input constraints and motion increment input constraints. This represents motion input constraints and motion increment input constraints. The correlation coefficient matrix represents the boundary of the motion input. It is a constant matrix. The correlation coefficient matrix represents the boundary of the motion increment input. and These represent the correlation coefficients of the motion input boundary relaxation factors, respectively. and These represent the correlation coefficient matrices of the motion increment input boundary relaxation factors, respectively. and They represent relaxation factors, respectively. The lower and upper bounds of the relevant variables, and They represent relaxation factors, respectively. The lower and upper bounds of the relevant variables, and These represent the upper and lower bound matrices of the motion velocity error, respectively. and These represent the upper and lower bound matrices of the motion increment, respectively.

Images