[go: up one dir, main page]

CN116520860B - Method for tracking and controlling movement path of side movement of autonomous underwater robot - Google Patents

Method for tracking and controlling movement path of side movement of autonomous underwater robot

Info

Publication number
CN116520860B
CN116520860B CN202310459496.8A CN202310459496A CN116520860B CN 116520860 B CN116520860 B CN 116520860B CN 202310459496 A CN202310459496 A CN 202310459496A CN 116520860 B CN116520860 B CN 116520860B
Authority
CN
China
Prior art keywords
control
waypoint
lateral
current
heading angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310459496.8A
Other languages
Chinese (zh)
Other versions
CN116520860A (en
Inventor
冀大雄
汪新伟
梅德庆
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Research Institute Of Zhejiang University
Zhejiang University ZJU
Original Assignee
Shenzhen Research Institute Of Zhejiang University
Zhejiang University ZJU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Research Institute Of Zhejiang University, Zhejiang University ZJU filed Critical Shenzhen Research Institute Of Zhejiang University
Priority to CN202310459496.8A priority Critical patent/CN116520860B/en
Publication of CN116520860A publication Critical patent/CN116520860A/en
Application granted granted Critical
Publication of CN116520860B publication Critical patent/CN116520860B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/04—Control of altitude or depth
    • G05D1/06—Rate of change of altitude or depth
    • G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及水下机器人控制领域，旨在提供一种自主水下机器人的计入侧移运动路径跟踪控制方法。本发明基于视线法的思想使用了模型预测控制方法；采用计入侧向运动误差的控制策略，将原本高维的模型拆分成两个更低维的模型；提出基于加速度计和多普勒计程仪的传感器数据进行融合。本发明能够克服复杂水下环境，在存在控制约束的条件下得到最优的控制量，从而实现自主水下机器人快速平稳的控制效果；在理论上能降低计算复杂度，提高算法计算效率，保证误差的快速收敛；能够提高水下位置的估计精度，进一步提升控制性能。

The present invention relates to the field of underwater robot control, and aims to provide a method for tracking and controlling the lateral motion path of an autonomous underwater robot. The present invention uses a model predictive control method based on the idea of the line of sight method; adopts a control strategy that takes lateral motion errors into account to split the original high-dimensional model into two lower-dimensional models; and proposes the fusion of sensor data based on accelerometers and Doppler logs. The present invention can overcome complex underwater environments and obtain the optimal control quantity under the condition of control constraints, thereby achieving a fast and stable control effect for the autonomous underwater robot; in theory, it can reduce computational complexity, improve algorithm computational efficiency, and ensure rapid convergence of errors; it can improve the estimation accuracy of underwater position and further enhance control performance.

Description

Method for tracking and controlling movement path of side movement of autonomous underwater robot
Technical Field
The invention relates to the field of underwater robot control, in particular to a method for tracking and controlling a side-moving motion path of an autonomous underwater robot.
Background
The underwater robots can be classified into a cabled remote underwater robot and a cableless autonomous underwater robot. Compared with a remote control underwater robot, the autonomous underwater robot has the advantages of being capable of being controlled autonomously, wide in operation range and the like, and can be suitable for more underwater operation scenes.
In autonomous underwater robot operation, path tracking control is a primary problem, and only if the autonomous underwater robot can accurately track a preset operation path, an operation task can be effectively completed. The Line of Sight method (Line of Sight) is a common method for solving the problem of path tracking of an autonomous underwater robot, and the method converts a target path point of the autonomous underwater robot into a target course of the autonomous underwater robot, and finally realizes path tracking by controlling the course. Although the line-of-sight method is simple and feasible and has more application, the method has the defects that when a larger corner exists in a path, the change amount of a course angle is large, so that larger deviation can occur during path tracking, and meanwhile, the situation that a target path point is lost due to constraint of a controller is superposed.
In order to obtain better tracking effect, some intelligent control algorithms including model predictive control are increasingly paid attention to and applied. The model predictive control algorithm is also called a rolling optimization algorithm, and is an intelligent control algorithm widely used in industry. The control principle is that at the current sampling moment, an optimal control sequence of a limited time domain is solved according to an optimization function of a control system, only a first control quantity of the optimal control sequence is acted on a controlled object, and the steps are repeated at each sampling moment until the task is finished.
The model predictive control algorithm has the advantages that 1) the control error can be converged rapidly because the optimal control quantity is adopted at each moment. 2) Because the optimization function of the control system can be freely designed, the control problem of the complex system with constraint can be effectively solved. However, the model predictive control algorithm has the disadvantage that the efficiency of the solution is not high enough and the calculation amount increases in a square relation with the dimension of the model used.
Therefore, the invention designs a model prediction control algorithm for accounting for side movement, introduces the judgment of the side movement on the basis of the thought of a sight line method, splits the original high-dimensional model in the model prediction control algorithm into two low-dimensional models, reduces the total calculation complexity theoretically, and ensures the convergence rate on the other hand.
Disclosure of Invention
The invention aims to solve the technical problem of overcoming the defects in the prior art and providing a method for tracking and controlling a side-movement-counting movement path of an autonomous underwater robot.
In order to solve the technical problems, the invention adopts the following solutions:
The method for tracking and controlling the movement path of the side-entering movement of the autonomous underwater robot comprises the following steps:
(1) Designing two independent model prediction controllers, namely a forward-steering motion controller C1 and a transverse motion controller C2, according to a dynamics model of the autonomous underwater robot;
(2) According to a pre-planned autonomous underwater robot path, setting a plurality of target waypoints and final waypoints which are passed through in the course, and a transverse error threshold delta l and a navigation angle error threshold delta Ψ which are traveled by the autonomous underwater robot in the course of course movement;
(3) Acquiring the current position (x, y) and the current target waypoint (x k,yk) of the autonomous underwater robot, calculating the distance L between the current position (x, y) and the current target waypoint (x k,yk), and judging whether the current target waypoint is reached or not;
(4) Comparing the current target waypoint (x k,yk) with the final waypoint set in the step (2), ending all tasks if the current target waypoint is the final waypoint, updating the position information of the current target waypoint according to the target waypoint sequence set in the step (2) if the current target waypoint is not the final waypoint, and continuously executing the step (5);
(5) Acquiring specific data of a current course angle ψ, a current target course point (x k,yk) and a next target course point (x k+1,yk+1), and simultaneously calculating a current lateral error dl and a course angle error dψ, if the lateral error and the course angle error are smaller than a set threshold value, calculating an expected course angle ψ d and a lateral displacement y d, starting an advancing-steering motion controller C1 and a lateral motion controller C2, otherwise, calculating an expected course angle ψ d, and starting the advancing-steering motion controller C1;
(6) The forward-steering motion controller C1 and the transverse motion controller C2 respectively calculate control amounts according to the judgment result of the previous step, and then send corresponding control instructions to each propeller so as to control the autonomous underwater robot to approach the target waypoint;
(7) Utilizing accelerometer sampling data a x,ay, then carrying out fusion filtering with sampling data v x,vy of the Doppler log, estimating forward displacement x and transverse displacement y on the basis, and then returning to the execution step (3);
(8) Repeating the steps (3) - (7), so that the autonomous underwater robot can realize motion control and target path tracking in the process of travelling.
Compared with the prior art, the invention has the beneficial effects that:
(1) The vision-based thought of the invention uses a model predictive control method, which can overcome the complex underwater environment and obtain the optimal control quantity under the condition of control constraint, thereby realizing the rapid and stable control effect of the autonomous underwater robot.
(2) According to the invention, the control strategy of accounting for the lateral motion error is adopted, the original high-dimensional model is split into two lower-dimensional models, so that on one hand, the calculation complexity can be reduced theoretically, the algorithm calculation efficiency is improved, and on the other hand, the rapid convergence of the error can be ensured.
(3) The invention provides a method for fusing sensor data based on an accelerometer and a Doppler log, which can improve the estimation accuracy of the underwater position and further improve the control performance.
Drawings
FIG. 1 is a flow chart of a control algorithm of the present invention;
FIG. 2 is a schematic view of an underwater vehicle coordinate system;
FIG. 3 is a diagram of a stress analysis of an autonomous underwater rotor robot;
FIG. 4 is a flow chart of a Kalman fusion filtering method;
FIG. 5 is a comparative graph of the results of the method of the present invention and a normal gaze method waypoint tracking at a large right turn angle;
FIG. 6 is a comparison of the results of the method of the present invention and a normal gaze method waypoint tracking at a large left turn angle;
Fig. 7 is a graph comparing tracking errors of the method of the present invention and a common line-of-sight waypoint.
Detailed Description
The invention will now be described in detail with reference to the accompanying drawings and specific examples.
For convenience of subsequent description, the coordinate system and the contracted symbols of the underwater vehicle are introduced.
As shown in fig. 2, the attitude angle and position of the underwater vehicle are defined under a geographic coordinate system. Wherein x represents displacement in the x-axis, y represents displacement in the y-axis, and z represents displacement in the z-axis, wherein phi represents the angle of rotation about the x-axis, theta represents the angle of rotation about the y-axis, and ψ represents the angle of rotation about the z-axis.
The compact matrix is written as a pose matrix eta= [ x, y, z, phi, theta, phi ] T.
Under the body coordinate system, the linear velocity and the angular velocity of the underwater vehicle are defined. Where μ represents displacement in the x-axis, v represents displacement in the y-axis, w represents displacement in the z-axis, where p represents angular velocity of rotation about the x-axis, q represents angular velocity of rotation about the y-axis, and r represents angular velocity of rotation about the z-axis.
The compact matrix is written as a velocity matrix v= [ μ, v, w, p, q, r ] T. The underwater vehicle has a mass m and an inertial tensor mThe diagonal elements I x,Iy,Iz are the moment of inertia on the x, y, z axes, respectively, and the remaining non-diagonal elements are the corresponding products of inertia. (x G,yG,zG) is the centroid coordinates of the underwater vehicle.
In addition, the external force and the external moment applied to the underwater vehicle in the X, Y and Z axes are (X, Y, Z) and (K, M, N), respectively.
According to the related deduction content of the first chapter in Marine Control System written by Fossen, a kinetic model in the form of an underwater vehicle matrix can be obtained as follows:
(1) Where m=m RB+MA is the quality matrix
M RB and M A represent a rigid body inertial matrix and an additional mass matrix, respectively. Wherein the rigid body inertial matrix is used for generating a rigid body inertial matrix,
And for the underwater autonomous robot which is approximately triaxial-symmetrical in whole and has low navigational speed, the additional mass matrix is a diagonal matrix.
The diagonal elements correspond to additional masses or additional moments of inertia in six degrees of freedom, respectively.
(2) Wherein C (v) =c RB+CA is coriolis Li Juzhen
(3) Wherein D (v) =d L+DNL is a fluid resistance matrix
DL＝diag(Xu,Yv,Zw,Kp,Mq,Nr)
DNL＝diag(Xu|u||u|,Yv|v||v|,Zw|w||w|,Kp|p||p|,Mq|q||q|,Nr|r||r|)
D L is a diagonal matrix called linear drag matrix, the diagonal elements are linear drag coefficients in six degrees of freedom, D NL is also a diagonal matrix called nonlinear drag matrix, the diagonal elements are the product of the nonlinear drag coefficients in six degrees of freedom and the absolute value of velocity (or angular velocity).
(4) Wherein g (eta) is a restoring force matrix
W is the gravity of the underwater vehicle, and B is the buoyancy of the underwater vehicle.
(5) Wherein τ= [ X, Y, Z, K, M, N ] T is an external force matrix to which the underwater vehicle is subjected.
The invention relates to a method for tracking and controlling a motion path of a side-moving of an autonomous underwater robot, which comprises the following steps:
First, selecting proper state vectors and input vectors according to a dynamics model of the autonomous underwater robot, and designing two model prediction controllers, namely a forward-steering motion controller C1 and a transverse motion controller C2.
The basic design method of the model predictive controller can refer to the contents of chapter five and chapter six of model predictive control written by Chen Hong, and the dynamics model of the autonomous underwater robot is described above. The forward-steering motion controller and the transverse motion controller are based on the same dynamic model, and only different state vectors and corresponding input vectors are selected in the dynamic model according to specific control targets in the design process, so that a model used in the specific model prediction controller can be formed. The method specifically comprises the following steps:
(a) From the dynamics model and the state vector, the input vector gets the model needed by the model predictive controller:
wherein the state vector is x (t) ∈r n, the input vector is u (t) ∈r n,x0 is the initial state, and the input and state constraints are:
u is a finite set formed by input vector constraints, and X is a finite set formed by state vector constraints;
(b) The optimization problem is determined as follows:
The constraint conditions are satisfied:
Wherein, the
T p is the prediction time domain, Q epsilon R n×n and R epsilon R m×m are positive symmetry weighting matrixes, J is the objective cost function of the control problem, and U and x are input constraint and state constraint sets respectively; The model predictive controller predicts the system state variables and input variables in the process, Ω being the set of system states including the balance point.
(C) After the state vector and the input vector are selected, the optimal control input is obtained through the model predictive controller:
Positive definite symmetric matrix P calculation
Linearizing the model at the equilibrium point can result in:
Wherein, the
If the linearization system is controllable, a linear feedback u=kx can be found such that the system is closed loop
A K =a+bk is asymptotically stable, there must be a unique symmetric positive definite matrix P that satisfies:
(d) To this end, by solving the above-described optimization problem at each sampling timing, the optimal control sequence U * can be obtained, and the optimal control input at that sampling timing is the first element of U * (t), that is, U *＝U* (t).
Examples:
(1) In the state of the autonomous underwater robot dynamics model, the state vector x 1 is formed by selecting the quantity related to forward-steering motion from a velocity matrix and a pose matrix, such as forward speed u, z-axis rotation angular speed r and the like, the input vector u 1 is formed by selecting the corresponding component in an external force matrix, and the model required by the forward-steering motion controller C1 can be obtained according to the dynamics model assuming that the target cost function of the forward-steering motion control problem is J 1:
Where x 1 is the initial state of the device, Representing the derivative of x 1 (t), f 1 represents a second order continuous differentiable function associated with x 1,u1.
By solving the constraint optimization problem:
Constraint:
can obtain the optimal input vector
In the above formula:
T p is the prediction horizon, U represents the input constraint set, X represents the state constraint set, Representing the variables of the intra-controller prediction system,Is a positively-defined symmetric weighting matrix,Is a positive definite symmetric matrix, n 1 is the length of the state vector x 1, and m 1 is the length of the input vector u 1.For the model predictive controller to predict system state variables and input variables in the process, Ω 1 is the set of system states including the balance point.
(2) In the state of the dynamics model of the autonomous underwater robot, selecting the quantities related to transverse motion, such as transverse velocity v, transverse displacement y and the like, from a velocity matrix and a pose matrix to form a state vector x 2, selecting the corresponding components in an external force matrix to form an input vector u 2, setting the objective cost function of the transverse motion control problem to be J 2, and obtaining the model required by the transverse motion controller C2 according to the dynamics model:
where x 2 is the initial state of the device, Representing the derivative of x 2 (t), f 2 represents a second order continuous differentiable function associated with x 2,u2.
By solving the constraint optimization problem:
Constraint:
can obtain the optimal input vector
In the above formula:
T p is the prediction horizon, U represents the input constraint set, X represents the state constraint set, Representing the variables of the intra-controller prediction system,Is a positively-defined symmetric weighting matrix,Is a positive definite symmetric matrix, n 2 is the length of the state vector x 2, and m 2 is the length of the input vector u 2.For the model predictive controller to predict system state variables and input variables in the process, Ω 2 is the set of system states including the balance point.
And secondly, setting a plurality of target waypoints and final waypoints which pass through in the course according to a pre-planned autonomous underwater robot path, and setting a transverse error threshold delta l and a navigation angle error threshold delta Ψ of the autonomous underwater robot in the course of movement along the course. The method specifically comprises the following steps:
(a) Determining a navigation route of the autonomous underwater vehicle according to an actual task, setting a discrete navigation point obtained by sampling in advance as a target navigation point in a tracking process, wherein the set of all target navigation points is as follows:
{(x1,y1),(x2.y2),(x3,y3),...,(xn,yn)}
Wherein, (x n,yn) is the final waypoint;
(b) And setting a transverse error threshold delta l and an angle error threshold delta Ψ according to the size and control requirements of the autonomous underwater robot.
Thirdly, the current position (x, y) and the current target waypoint (x k,yk) of the autonomous underwater robot are obtained, the distance L between the current position (x, y) and the current target waypoint (x k,yk) is calculated, and the critical radius R of the waypoint switching is calculated to judge whether the current target waypoint is reached. And if the current target waypoint is not reached, the fifth step is executed. The method specifically comprises the following steps:
(a) Calculating the distance L between the current position (x, y) and the current target waypoint (x k,yk):
(b) The calculation formula of the critical radius is as follows:
Wherein R min,Rmax is the minimum and maximum value of critical radius, u is the current speed, theta p is the included angle between two continuous paths, l is the radial length of the underwater robot, and k u,kθ is the speed influence factor and the angle influence factor, respectively.
(C) When the distance L is less than or equal to the critical radius R, the current target waypoint is considered to be reached, and the following fourth step is performed.
(D) When the distance L is greater than the critical radius R, the current target waypoint has not been reached yet, and the following fifth step is performed.
Fourth, the current target waypoint (x k,yk) is compared with the final waypoint set in the second step. And if the target waypoint is not the final waypoint, updating the position information of the current target waypoint according to the target waypoint sequence set in the second step, and continuously executing the fifth step. The method specifically comprises the following steps:
(a) Acquiring the values of a current target navigation point (x k,yk) and a final navigation point (x n,yn);
(b) If x k＝xn and y k＝yn, it is stated that the final waypoint is reached, all tasks are ended.
(C) If x k≠xn or y k≠yn, indicating that the final waypoint has not been reached, updating the current target waypoint to be the next target waypoint in the target waypoint sequence.
Fifthly, acquiring data of specific position information of a current course angle ψ, a target course point (x k,yk) and a next target course point (x k+1,yk+1), and simultaneously calculating a current lateral error dl and a course angle error dψ, if the lateral error and the course angle error are smaller than a set threshold value, calculating an expected course angle ψ d and a lateral displacement y d, and starting an advancing-steering motion controller C1 and a lateral motion controller C2, otherwise, only calculating an expected course angle ψ d, and starting the advancing-steering motion controller C1, wherein the method specifically comprises the following steps:
(a) Calculating the current lateral error dl:
firstly, calculating a linear equation of a current path point (x k,yk) and a next path point (x k+1,yk+1):
Ax+By=C
Wherein:
A=yk+1-yk
B=xk-xk+1
C=xk(yk-yk+1)-yk(xk-xk+1)
According to the current position (x, y), there is a lateral error:
(b) Calculating a heading angle error dψ:
First, the direction of the target path is calculated according to the current path point (x k,yk) and the next path point (x k+1,yk+1):
Then, based on the current heading angle ψ, the heading angle error can be calculated as:
dΨ=|Ψ-θroad|
(c) Determine whether to activate the lateral motion controller C2
When the lateral error dl is smaller than the lateral error threshold delta l and the heading angle error dψ is smaller than the heading angle error threshold delta Ψ, the forward-steering motion controller C1 and the lateral motion controller C2 are required to be started, otherwise, only the forward-steering motion controller C1 is required to be started, and the lateral motion controller C2 is not required to be started;
(d) When the lateral motion controller C2 needs to be activated, the expected heading angle and the expected lateral motion displacement are calculated as follows:
the expected heading angle calculation formula is:
the expected lateral motion displacement is:
(e) When the lateral motion controller C2 does not need to be activated, the expected heading angle is calculated as follows:
Firstly, setting a look-ahead distance d, and solving an intermediate waypoint (x temp,ytemp) which is close to the next target waypoint on the current path, wherein the intermediate waypoint (x temp,ytemp) meets the following requirements:
Axtemp+Bytemp=C
(xtemp-x)2+(ytemp-y)2＝dl2+d2
The expected heading angle is calculated as follows:
and the desired lateral displacement need not be designed.
And sixthly, respectively starting the forward-steering motion controller C1 and the transverse motion controller C2 to calculate the control quantity according to the judging result of the previous step, and then respectively sending control instructions to the forward propeller or the lateral propeller to control the autonomous underwater robot to approach the target waypoint. The method specifically comprises the following steps:
(a) If in the fifth step it is determined that the lateral motion controller C2 needs to be activated, the expected heading angle ψ d is input first, and the control amount is calculated by the forward-steering controller C1 The forward propeller is sent out a control command to control the current course angle to approach the expected course angle, and simultaneously, the expected lateral displacement is input, and the control quantity is calculated by using a lateral motion controller C2And a control instruction is sent to the lateral propeller to control the current lateral displacement to approach the expected lateral displacement.
(B) If in the fifth step, it is determined that the lateral motion controller C2 is not required to be activated, only the expected heading angle ψ d is input, and the control amount is calculated by the forward-steering controller C1A control instruction is sent to the forward propeller to control the current course angle to approach the expected course angle;
Seventh, according to the acquired sampling data a x,ay of the accelerometer and the acquired sampling data v x,vy of the doppler log, performing kalman fusion filtering according to the flow shown in fig. 4, and estimating the current position (x, y), specifically including:
(a) After executing the control instruction of the controller in the sixth step, acquiring the sampling data a x,ay of the accelerometer and the sampling data v x,vy of the Doppler log;
(b) Giving a measurement error E m of the Doppler log;
(c) Judging whether the fusion filtering is the first fusion filtering or not, and respectively calculating to obtain a filtering gain k x,ky;
if the first fusion filtering is performed, k x＝ky =0.5;
if not, the first fusion filtering is calculated by the following two formulas:
kx＝Ex/(Ex+Em)
ky＝Ey/(Ey+Em)
Wherein E x,Ey is the estimated error obtained by the last filtering.
(D) Calculating a filtered velocity valueAndThe calculation formula is as follows:
(e) The estimation errors E x and E y are calculated as follows:
(f) The final position (x, y) is output and the calculation formula is as follows:
(g) Returning to the operation content of the third step.
And eighth step, repeating the operation contents of the third step to the seventh step, so that the autonomous underwater robot can realize efficient and stable motion control in the running process and accurately track the target path.
A more specific example of application:
One specific implementation of the invention will now be described in connection with an autonomous underwater vehicle of the rotor type. The rotor type autonomous underwater vehicle has four vertical thrusters and two horizontal thrusters, and has six degrees of freedom control capability. The force analysis schematic diagram is shown in fig. 3, and F 1,F2,...,F6 is the thrust of 6 propellers respectively. The method for tracking and controlling the calculated side-moving motion path of the autonomous underwater robot comprises the following steps:
1. The forward-steering controller and the lateral motion controller are designed.
Combining the above-mentioned water power motion equation of the aircraft, selecting the state vector x 1＝[x,y,ψ,u,v,r]T and the input vector u 1＝[F5,F6]T to derive the model of the forward-steering model predictive controller of the rotor-type autonomous underwater robot, namely, self-adaptiveThe deployment is as follows:
Wherein l is the axial length of the rotor type autonomous underwater robot.
The forward-steering controller C1 can be obtained in combination with the model predictive control design method described above.
Similarly, the state variable x 2＝[y,φ,v,p]T is selected, and the input variable u 2＝[F1,F2,F3,F4]T can be deduced to obtain the model of the transverse motion model predictive controller of the rotor type autonomous underwater robot, namely The deployment is as follows:
the lateral motion controller C2 can be obtained by combining the model predictive control design method described above.
It should be noted that the input vectors of the two model predictive controllers C1 and C2 are composed of the thrust of the propeller, so that the final controller obtains the optimal thrust of each propeller, and the propeller thrust has a fixed functional relationship with the propeller rotation speed, so that the rotation speed of the propeller can be reversely pushed out through the optimal thrust, and further the required propeller instruction is obtained.
2. And determining a target waypoint. In this example, the waypoints are as follows:
{(3,0),(3,15),(13,15),(13,5),(23,5),(23,15),(33,15)}
Determining a lateral error threshold Δl=0.5m and an angular error threshold
3. The calculation contents of the third to eighth steps are completed by means of a computer simulation program according to the method described above.
In order to embody the effectiveness of the method, the applicant also makes a comparison simulation experiment under the same condition by using the method and a common sight line method respectively. Wherein, fig. 5 shows the comparison of the results of the method of the present invention and the normal sight line method waypoint tracking when turning right a large angle, and fig. 6 shows the comparison of the results of the method of the present invention and the normal sight line method waypoint tracking when turning left a large angle, it can be seen that the method of the present invention can track the path more quickly when turning right a large angle.
Fig. 7 further shows the comparison result of the tracking error of the method and the normal line-of-sight method, and it is obvious that the tracking error of the method is smaller than that of the normal line-of-sight method on the whole path by comparing the two curves. Therefore, the method of the present invention has significant advantages in reducing tracking errors.
Finally, it should be noted that the above examples merely illustrate the method of applying the present invention to a specific rotor type autonomous underwater robot, and in practice, the present invention is not only applicable to the rotor type autonomous underwater robot illustrated in the above examples, but also can implement a practical control system for any autonomous underwater robot with six degrees of freedom being fully controllable according to the present invention.

Claims (7)

1.一种自主水下机器人的计入侧移运动路径跟踪控制方法，其特征在于，包括以下步骤：1. A method for tracking and controlling a motion path of an autonomous underwater robot, including lateral movement, comprising the following steps: (1)根据自主水下机器人的动力学模型设计两个独立的模型预测控制器，分别是前向-转向运动控制器C1和横向运动控制器C2；(1) Design two independent model predictive controllers based on the dynamic model of the autonomous underwater vehicle: the forward-steering motion controller C1 and the lateral motion controller C2. (2)根据预先规划的自主水下机器人路径，设定航程中途经的若干个目标航点和最终航点，以及沿航程运动过程中自主水下机器人行进的横向误差阈值Δl与航角误差阈值ΔΨ；(2) According to the pre-planned path of the autonomous underwater robot, several target waypoints and the final waypoint are set during the voyage, as well as the lateral error threshold Δl and the heading angle error threshold ΔΨ of the autonomous underwater robot during the movement along the voyage; (3)获取自主水下机器人的当前位置(x,y)和当前目标航点(xk,yk)，计算两者的距离L与航点切换临界半径R，判断是否已经到达当前目标航点；若到达当前目标航点，继续执行第(4)步；若未到达当前目标航点，则转为执行第(5)步；(3) Obtain the current position (x, y) and the current target waypoint ( xk , yk ) of the autonomous underwater robot, calculate the distance L between the two and the critical radius R of the waypoint switching, and determine whether the current target waypoint has been reached; if the current target waypoint has been reached, continue to execute step (4); if the current target waypoint has not been reached, switch to execute step (5); (4)将当前的目标航点(xk,yk)与第(2)步中设定的最终航点进行对比，若为最终航点就结束所有任务；若不是最终航点，则按第(2)步设定的目标航点序列对当前目标航点的位置信息进行更新，并继续执行第(5)步；(4) Compare the current target waypoint ( xk , yk ) with the final waypoint set in step (2). If it is the final waypoint, end all tasks; if it is not the final waypoint, update the location information of the current target waypoint according to the target waypoint sequence set in step (2) and continue to execute step (5); (5)获取当前航向角Ψ、当前目标航点(xk,yk)和下一目标航点(xk+1,yk+1)的具体位置信息的数据，同时计算当前的横向误差dl和航向角误差dΨ；若横向误差和航向角误差均小于设定的阈值，则需要计算预期航向角Ψd和横向位移yd，并启用前进-转向运动控制器C1和横向运动控制器C2；否则，只需计算预期航向角Ψd，并启用前进-转向运动控制器C1；(5) Obtain the specific location information of the current heading angle Ψ, the current target waypoint ( xk , yk ), and the next target waypoint (xk +1 , yk +1 ), and calculate the current lateral error dl and heading angle error dΨ. If both the lateral error and the heading angle error are less than the set threshold, calculate the expected heading angle Ψd and lateral displacement yd , and enable the forward-steering motion controller C1 and the lateral motion controller C2. Otherwise, only calculate the expected heading angle Ψd and enable the forward-steering motion controller C1. (6)前进-转向运动控制器C1和横向运动控制器C2根据前一步骤的判断结果，分别计算控制量；然后对各推进器发出相应控制指令，以控制自主水下机器人逼近目标航点；该步骤具体包括：(6) The forward-steering motion controller C1 and the lateral motion controller C2 calculate the control variables based on the judgment results of the previous step. Then, corresponding control instructions are issued to each thruster to control the autonomous underwater robot to approach the target waypoint. This step specifically includes: (a)若在步骤(5)中判断需要启用横向运动控制器C2，则先输入预期航向角Ψd，(a) If it is determined in step (5) that the lateral motion controller C2 needs to be activated, first input the expected heading angle Ψ d , 利用前向-转向控制器C1计算出控制量对前向推进器发出控制指令，控制当前的航向角逼近预期航向角；同时，输入预期横向位移，利用横向运动控制器C2计算出控制量对侧向推进器发出控制指令，控制当前横向位移逼近预期的横向位移；The control quantity is calculated using the forward-steering controller C1 Send a control command to the forward thruster to control the current heading angle to approach the expected heading angle; at the same time, input the expected lateral displacement and use the lateral motion controller C2 to calculate the control amount Sending control instructions to the lateral thrusters to control the current lateral displacement to approach the expected lateral displacement; (b)若在步骤(5)中判断无需启用横向运动控制器C2，则仅输入预期航向角Ψd，(b) If it is determined in step (5) that the lateral motion controller C2 does not need to be activated, only the expected heading angle Ψ d is input, 利用前向-转向控制器C1计算出控制量对前向推进器发出控制指令，控制当前的航向角逼近预期航向角；The control quantity is calculated using the forward-steering controller C1 Send control commands to the forward thruster to control the current heading angle to approach the expected heading angle; (7)利用加速度计采样数据ax,ay，然后再与多普勒计程仪的采样数据vx,vy进行融合滤波，并在此基础上估计出前向位移x和横向位移y；然后返回执行步骤(3)；(7) Using the accelerometer sampling data a x , a y , and then fusing and filtering it with the Doppler odometer sampling data v x , v y , and on this basis estimating the forward displacement x and lateral displacement y; then returning to step (3); (8)重复以上步骤(3)～(7)，使自主水下机器人能够在行进过程中实现运动控制和目标路径跟踪。(8) Repeat the above steps (3) to (7) to enable the autonomous underwater robot to achieve motion control and target path tracking during the movement. 2.根据权利要求1所述的方法，其特征在于，所述步骤(1)中，模型预测控制器的设计及控制输出的实现包括以下步骤：2. The method according to claim 1, wherein in step (1), the design of the model predictive controller and the implementation of the control output comprise the following steps: (a)由动力学模型和状态向量，输入向量得到模型预测控制器所需模型：(a) The model required for the model predictive controller is obtained from the dynamic model, state vector, and input vector: 其中，状态向量为x(t)∈Rn,输入向量为u(t)∈Rn，x0为初始状态，输入和状态约束为Among them, the state vector is x(t) ∈Rn , the input vector is u(t) ∈Rn , x0 is the initial state, and the input and state constraints are U为输入向量约束形成的有限集合，X为状态向量约束形成的有限集合；U is a finite set of input vector constraints, and X is a finite set of state vector constraints; (b)确定优化问题如下：(b) The optimization problem is determined as follows: 满足约束条件：Satisfy the constraints: 其中，in, Tp为预测时域，Q∈Rn×n和R∈Rm×m是正定对称加权矩阵；J为控制问题的目标代价函数，U和X分别是输入约束和状态约束集合；为模型预测控制器预测过程中的系统状态变量和输入变量，Ω为包含平衡点在内的系统状态集合； Tp is the prediction time domain, Q∈Rn ×n and R∈Rm ×m are positive definite symmetric weight matrices; J is the target cost function of the control problem, U and X are the input constraint and state constraint sets respectively; are the system state variables and input variables in the prediction process of the model predictive controller, and Ω is the system state set including the equilibrium point; (c)正定对称矩阵P计算：(c) Calculation of positive definite symmetric matrix P: 在平衡点处对模型进行线性化得到：Linearizing the model at the equilibrium point yields: 其中， in, 若该线性化系统是可控的，则求得线性反馈u＝Kx，使得闭环系统AK＝A+BK是渐近稳定的；If the linearized system is controllable, then the linear feedback u = Kx is obtained so that the closed-loop system A K = A + BK is asymptotically stable; 则一定有唯一对称正定矩阵P满足：Then there must be a unique symmetric positive definite matrix P that satisfies: (d)在每一个采样时刻求解上述优化问题，求得最优化控制序列U*；(d) solving the above optimization problem at each sampling moment to obtain the optimal control sequence U * ; 则该采样时刻的最优控制输入为U*(t)的第一个元素，即有u*＝U*(t)。Then the optimal control input at the sampling moment is the first element of U * (t), that is, u * =U * (t). 3.根据权利要求1所述的方法，其特征在于，所述步骤(2)具体包括：3. The method according to claim 1, wherein step (2) specifically comprises: (a)根据实际的任务确定自主式水下机器的航行路线，将事先采样获得的路线中的离散航点设定为跟踪过程中的目标航点，所有目标航点的集合为：(a) Determine the navigation route of the autonomous underwater vehicle based on the actual mission, and set the discrete waypoints in the route obtained by pre-sampling as the target waypoints in the tracking process. The set of all target waypoints is: {(x1,y1),(x2.y2),(x3,y3),…,(xn,yn)}{(x 1 ,y 1 ),(x 2 .y 2 ),(x 3 ,y 3 ),…,(x n ,y n )} 式中，(xn,yn)为最终航点；Where (x n ,y n ) is the final waypoint; (b)根据自主水下机器人的尺寸和控制要求，设定横向误差阈值Δl和航角误差阈值ΔΨ。(b) According to the size and control requirements of the autonomous underwater vehicle, set the lateral error threshold Δl and the heading angle error threshold ΔΨ . 4.根据权利要求1所述的方法，其特征在于，所述步骤(3)具体包括：4. The method according to claim 1, wherein step (3) specifically comprises: (a)计算当前位置(x,y)和当前目标航点(xk,yk)的距离L：(a) Calculate the distance L between the current position (x, y) and the current target waypoint ( xk , yk ): (b)临界半径的计算公式如下：(b) The critical radius is calculated as follows: 其中Rmin，Rmax分别为临界半径的最小最大值，u为当前速度，θp为两个连续路径之间的夹角，l为水下机器人的径向长度，ku，kθ分别为速度影响因子和角度影响因子；Where R min and R max are the minimum and maximum values of the critical radius, u is the current speed, θ p is the angle between two consecutive paths, l is the radial length of the underwater robot, k u and k θ are the speed influence factor and angle influence factor, respectively; (c)如距离L小于或等于临界半径R，认为已到达当前目标航点，执行第(4)步；(c) If the distance L is less than or equal to the critical radius R, it is considered that the current target waypoint has been reached and step (4) is executed; 如距离L大于临界半径R，认为未到达当前目标航点，转为执行第(5)步。If the distance L is greater than the critical radius R, it is considered that the current target waypoint has not been reached and the process goes to step (5). 5.根据权利要求1所述的方法，其特征在于，所述步骤(4)具体包括：5. The method according to claim 1, wherein step (4) specifically comprises: (a)获取当前目标航点(xk,yk)和最终航点(xn,yn)的数值；(a) Obtain the values of the current target waypoint (x k , y k ) and the final waypoint (x n , yn ); (b)若xk＝xn且yk＝yn，说明到达最终航点，结束所有任务；(b) If x k = x n and y k = yn , it means that the final waypoint has been reached and all tasks are completed; (c)若xk≠xn或yk≠yn，说明未到达最终航点，更新当前目标航点为目标航点(c) If xk ≠ xn or yk ≠ yn , it means that the final waypoint has not been reached. Update the current target waypoint to the target waypoint. 序列中的下一目标航点。The next destination waypoint in the sequence. 6.根据权利要求1所述的方法，其特征在于，所述步骤(5)具体包括：6. The method according to claim 1, wherein step (5) specifically comprises: (a)计算当前的横向误差dl：(a) Calculate the current lateral error dl: 先计算当前路径点(xk,yk)和下一路径点(xk+1,yk+1)的直线方程：First calculate the equation of the line between the current path point (x k , y k ) and the next path point (x k+1 , y k+1 ): Ax+By＝CAx+By＝C 其中：in: A＝yk+1-yk A＝y k+1 −y k B＝xk-xk+1 B＝x k −x k+1 C＝xk(yk-yk+1)-yk(xk-xk+1)C＝x k (y k -y k+1 )-y k (x k -x k+1 ) 根据当前的位置(x,y)，则有横向误差为：According to the current position (x, y), the lateral error is: (b)计算航向角误差dΨ：(b) Calculate the heading angle error dΨ: 先根据当前路径点(xk,yk)和下一路径点(xk+1,yk+1)计算目标路径的方向：First, calculate the direction of the target path based on the current path point (x k , y k ) and the next path point (x k+1 , y k+1 ): 则根据当前航向角Ψ，计算航向角误差为：According to the current heading angle Ψ, the heading angle error is calculated as: dΨ＝|Ψ-θroad|dΨ＝|Ψ-θ road | (c)判断是否启用横向运动控制器C2：(c) Determine whether to enable the lateral motion controller C2: 当横向误差dl小于横向误差阈值Δl，且航向角误差dΨ小于航向角误差阈值ΔΨ时，需要启用前进-转向运动控制器C1和横向运动控制器C2；否则，只需启When the lateral error dl is less than the lateral error threshold Δl , and the heading angle error dΨ is less than the heading angle error threshold ΔΨ , the forward-steering motion controller C1 and the lateral motion controller C2 need to be enabled; otherwise, only the forward-steering motion controller C1 and the lateral motion controller C2 need to be enabled. 用前进-转向运动控制器C1，不需要启用横向运动控制器C2；When using the forward-steering motion controller C1, there is no need to enable the lateral motion controller C2; (d)当需要启用横向运动控制器C2时，计算预期航向角和预期横向运动位移如下：预期航向角计算公式为：(d) When the lateral motion controller C2 is activated, the expected heading angle and expected lateral motion displacement are calculated as follows: The expected heading angle is calculated as follows: 预期横向运动位移为：The expected lateral motion displacement is: (e)当不需要启用横向运动控制器C2时，计算预期航向角如下：(e) When the lateral motion controller C2 does not need to be activated, the expected heading angle is calculated as follows: 先设定一个前瞻距离d，求解出在当前路径上靠近下一个目标航点的一个中间航点(xtemp,ytemp)，满足：First, set a look-ahead distance d and find an intermediate waypoint (x temp , y temp ) on the current path close to the next target waypoint, satisfying: Axtemp+Bytemp＝CAx temp +By temp = C (xtemp-x)2+(ytemp-y)2＝dl2+d2 (x temp -x) 2 +(y temp -y) 2 =dl 2 +d 2 则预期航向角计算如下：The expected heading angle is then calculated as follows: 并且无需设计预期横向位移。And there is no need to design for expected lateral displacement. 7.根据权利要求1所述的方法，其特征在于，所述步骤(7)具体包括：7. The method according to claim 1, wherein step (7) specifically comprises: (a)获取加速度计的采样数据ax,ay和多普勒计程仪的采样数据vx,vy；(a) Obtain the sampling data a x , a y of the accelerometer and the sampling data v x , v y of the Doppler log; (b)给定多普勒计程仪的测量误差Em；(b) Given the measurement error E m of the Doppler log; (c)判断是否为第一次融合滤波，并分别计算得到滤波增益kx,ky；(c) Determine whether it is the first fusion filtering and calculate the filter gains k x , ky respectively; 若为第一次融合滤波，则kx＝ky＝0.5；If it is the first fusion filtering, then k x = ky = 0.5; 若不是第一次融合滤波，则通过下面两个公式计算：If it is not the first fusion filter, it is calculated using the following two formulas: kx＝Ex/(Ex+Em)k x = Ex /( Ex + Em ) ky＝Ey/(Ey+Em) ky ＝ Ey /( Ey + Em ) 其中，Ex，Ey为上一次滤波得到的估计误差；Among them, Ex , Ey is the estimated error obtained by the last filtering; (d)计算滤波后的速度值和计算公式如下：(d) Calculate the filtered velocity value and The calculation formula is as follows: (e)计算估计误差Ex和Ey，计算公式如下：(e) Calculate the estimated errors Ex and Ey using the following formula: (f)输出最终的位置(x,y)，计算公式如下：(f) Output the final position (x, y), calculated as follows: (g)返回执行步骤(3)。(g) Return to step (3).
CN202310459496.8A 2023-04-23 2023-04-23 Method for tracking and controlling movement path of side movement of autonomous underwater robot Active CN116520860B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310459496.8A CN116520860B (en) 2023-04-23 2023-04-23 Method for tracking and controlling movement path of side movement of autonomous underwater robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310459496.8A CN116520860B (en) 2023-04-23 2023-04-23 Method for tracking and controlling movement path of side movement of autonomous underwater robot

Publications (2)

Publication Number Publication Date
CN116520860A CN116520860A (en) 2023-08-01
CN116520860B true CN116520860B (en) 2025-08-19

Family

ID=87389678

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310459496.8A Active CN116520860B (en) 2023-04-23 2023-04-23 Method for tracking and controlling movement path of side movement of autonomous underwater robot

Country Status (1)

Country Link
CN (1) CN116520860B (en)

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8170739B2 (en) * 2008-06-20 2012-05-01 GM Global Technology Operations LLC Path generation algorithm for automated lane centering and lane changing control system
CN108248605A (en) * 2018-01-23 2018-07-06 重庆邮电大学 The transverse and longitudinal control method for coordinating that a kind of intelligent vehicle track follows
CN113602278B (en) * 2021-08-27 2022-12-23 吾驾之宝汽车技术(江苏)有限公司 Distributed model predictive path tracking control method for four-wheel independent drive electric vehicles
CN114715167B (en) * 2022-05-05 2024-10-08 吉林大学 An automatic driving trajectory tracking control method based on multi-driving modes of omnidirectional mobile vehicles

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Daxiong Ji,Minghui Xu.Adaptive Path Following Control Based on Nonlinear Disturbance Observer for a Six Thrusters AUV.《2023 WRC Symposium on Advanced Robotics and Automation》.2023,全文. *

Also Published As

Publication number Publication date
CN116520860A (en) 2023-08-01

Similar Documents

Publication Publication Date Title
Borhaug et al. Cross-track control for underactuated autonomous vehicles
Gao et al. Hierarchical model predictive image-based visual servoing of underwater vehicles with adaptive neural network dynamic control
CN105955268B (en) A kind of UUV moving-target sliding mode tracking control methods considering Local obstacle avoidance
US6246929B1 (en) Enhanced stall and recovery control system
CN110308735A (en) A sliding mode control method for underactuated UUV trajectory tracking for input time delay
Xiang et al. Path tracking: Combined path following and trajectory tracking for autonomous underwater vehicles
CN111007854B (en) Under-actuated ship trajectory tracking control system
KR20160081906A (en) Filtering back-stepping ship motion control system based on adaptive fuzzy estimator
CN111857165B (en) A kind of trajectory tracking control method of underwater vehicle
Li et al. Improved active disturbance rejection heading control for unmanned wave glider
Viswanathan et al. Integrated guidance and nonlinear feedback control of underactuated unmanned aerial vehicles in SE (3)
Wang et al. Adaptive nonlinear model predictive control for autonomous surface vessels with largely varying payload
CN106647264A (en) An Extended Robust H∞ UAV Control Method Based on Control Constraints
Al Makdah et al. Modeling and optimal three-dimensional trajectory tracking for an autonomous underwater vehicle
Caharija et al. Path following control of underactuated surface vessels in the presence of multiple disturbances
CN116520860B (en) Method for tracking and controlling movement path of side movement of autonomous underwater robot
Liu et al. Large-angle and high-speed trajectory tracking control of a quadrotor UAV based on reachability
Reinhardt et al. Nonlinear model predictive control combined with geometric attitude and speed control for fixed-wing UAVs
Fukushima et al. An optimal control method based on the energy flow equation
Tian et al. Path following of autonomous surface vehicles with line-of-sight and nonlinear model predictive control
CN114661056B (en) Differential water surface unmanned ship trajectory tracking method considering propeller servo control
Soylu et al. Dynamics and control of tethered underwater-manipulator systems
CN112230550B (en) A trajectory tracking specified time convergence control system for underdriven ships
Bibuli et al. An advanced guidance & control system for an unmanned vessel with azimuthal thrusters
Nakano et al. Resolved acceleration control (RAC) of dual-arm 3-link underwater robot: comparison of control performance between RAC method and computed torque method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant