Autonomous navigation-tracking-obstacle avoidance integrated method for underwater vehicle
An integrated method for autonomous navigation and obstacle avoidance in underwater vehicles uses formation control, model predictive control, and improved potential fields with an extended Kalman filter to optimize trajectories, addressing collision risks and enhancing safety in complex environments.
Patent Information
- Application Number
- CN202411184375.8
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-27
- Publication Date
- 2024-12-06
AI Technical Summary
Underwater vehicles face a high risk of collision in complex obstacle environments due to the limitations of traditional control methods, which often ignore system and environmental constraints, leading to local minimum states and inefficient obstacle avoidance.
An integrated method for autonomous navigation, tracking, and obstacle avoidance using formation control strategies, kinematic and dynamic models, model predictive control, and improved artificial potential fields, combined with an extended Kalman filter to optimize obstacle avoidance trajectories.
The method enhances the safety and efficiency of underwater vehicles by reducing collision risks and enabling successful completion of complex underwater tasks through optimized obstacle avoidance and dynamic control.
Smart Images

Figure CN119088065A_ABST
Abstract
Description
Technical Field
[0001] The invention belongs to the field of navigation, guidance and control, and in particular relates to an integrated method of autonomous navigation, tracking and obstacle avoidance for an underwater vehicle. Background Art
[0002] With the continuous development of resource exploration and marine development, the demand for underwater unmanned vehicles in related research is increasing. The technical core of autonomous underwater vehicles includes formation control technology, autonomous navigation technology, sensor technology, Internet of Things technology, and artificial intelligence technology. These advanced technologies enable underwater unmanned vehicles to work efficiently, flexibly and safely in various environments in the ocean and rivers, and have more advantages than manned vehicles and ships. With the continuous advancement and optimization of related technologies, and driven by growing demand and the market, autonomous underwater vehicles will continue to develop rapidly. However, at this stage, underwater vehicles still have a large risk of damage during missions. With the development of modern intelligent control technology and communication technology, underwater missions have gradually become more complicated, making underwater vehicles urgently need the ability to cope with complex navigation environments.
[0003] Model predictive control (MPC) is currently the only control algorithm that can explicitly handle constraints. Compared with other control strategies, MPC has the following outstanding advantages: (1) The iterative optimization process of MPC enables its control performance to reach the optimal level; (2) MPC can effectively handle control constraints and state constraints in specific engineering implementations, and improve system performance through feedback control. At the same time, MPC is currently the most effective method for dealing with nonlinear control problems. Introducing MPC into the formation control of underwater vehicles can provide ideal solutions for many practical problems. However, the way in which control inputs are generated between the MPC-based formation control method and the traditional formation control strategy is essentially different, so relevant research is still needed. In specific engineering applications, the state values and input values of the control system often have an upper limit range, that is, there are system constraints, but traditional control theory often ignores the influence of constraints. In the control research of multiple underwater vehicle formations, it is necessary to consider a variety of system constraints and environmental constraints, and model predictive control has become an effective method to solve this problem.
[0004] The artificial potential field method is widely used in the field of underwater vehicle formations because of its simple structure, strong flexibility, and easy combination with other control methods. The artificial potential field method guides underwater vehicles to avoid obstacles by establishing virtual gravitational fields and virtual repulsive fields at the target point and the obstacle influence area. However, the current artificial potential field method often has zero potential energy points due to the problem of offsetting the gravitational force of the target point and the repulsive force of the obstacle, causing the underwater vehicle to enter a local minimum state. Therefore, research on autonomous navigation and obstacle avoidance of vehicles in complex obstacle environments has broad practical significance and engineering value. Summary of the invention
[0005] The purpose of the present invention is to provide an integrated autonomous navigation-tracking-obstacle avoidance method, thereby reducing the risk of collision of underwater vehicles in complex obstacle environments and enabling the vehicles to successfully complete various complex underwater tasks.
[0006] To achieve the above object, the present invention provides an integrated method for autonomous navigation, tracking and obstacle avoidance of an underwater vehicle, comprising:
[0007] Establish formation control strategy and formation information flow topology;
[0008] Establish kinematic and dynamic models of fully-driven underwater vehicles;
[0009] Establish dynamic obstacle motion model;
[0010] Establish the system control model of model predictive control method;
[0011] Design improved artificial potential field functions;
[0012] Based on the extended Kalman filter, the trajectory of the vehicle is predicted at the obstacle avoidance trajectory, the obstacle avoidance trajectory is optimized, and the navigation, tracking and obstacle avoidance integration of the underwater vehicle is realized.
[0013] Optionally, establishing a formation control strategy and a formation information flow topology structure includes: constructing an adjacency matrix based on a distributed control strategy and directed graph point set theory.
[0014] Optionally, the process of establishing the kinematic and dynamic models of the fully-driven underwater vehicle includes: based on the horizontal plane motion assumption, simplifying its motion model into two-dimensional motion again, and establishing the kinematic equations and dynamic equations of the vehicle.
[0015] Optionally, establishing the dynamic obstacle motion model includes: simplifying the dynamic obstacle into a particle model, and simplifying the common underwater dynamic obstacle motion mode to a certain extent.
[0016] Optionally, establishing a system control model of the model predictive control method includes: establishing a discretized system state space model and a prediction model, and designing rolling optimization and feedback control based on them.
[0017] Optionally, designing an improved artificial potential field function includes: establishing an artificial potential field and taking partial derivatives thereof to obtain an artificial potential function.
[0018] Optionally, predicting the trajectory of the aircraft based on the extended Kalman filter at the obstacle avoidance trajectory includes: establishing a discretized Kalman filter equation, and optimizing the trajectory based on the obstacle avoidance trajectory measurement value.
[0019] Beneficial effects:
[0020] Based on the research and analysis of underwater dynamic obstacles, the present invention establishes an extended Kalman filter prediction mechanism for the underwater vehicle obstacle avoidance trajectory, which solves the problem of obstacle avoidance trajectory oscillation at the potential field boundary to a certain extent, optimizes the obstacle avoidance trajectory, and improves the safety of formation obstacle avoidance. At the same time, the algorithm can accurately control the formation to perform dynamic obstacle avoidance, which has certain engineering application value. BRIEF DESCRIPTION OF THE DRAWINGS
[0021] Figure 1 The present invention is a flow chart of the underwater vehicle autonomous navigation-tracking-obstacle avoidance integrated method. DETAILED DESCRIPTION
[0022] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0023] In the description of the present invention, it should be understood that the terms "upper", "middle", "outer", "inner" and the like indicating directions or positional relationships are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the components or elements referred to must have a specific direction, be constructed and operated in a specific direction, and therefore should not be understood as limiting the present invention.
[0024] As shown in Table 1-5, this embodiment provides a dynamic environment SLAM method based on deep learning target detection, including the following steps:
[0025] Step 1:
[0026] Establish a communication topology structure, and the communication between adjacent AUVs is denoted as G{ν,ε}, where ν={1,2,K,N v} is a directed graph point set, consisting of node numbers, is the edge set of a directed graph, which indicates that there is communication between the spacecraft, and the direction of the edge represents the flow of information. The above graph theory can be used to express the adjacency matrix H matrix express, a ij The values are as follows:
[0027]
[0028] Among them, i,j∈v. a ij =1 means that the directed edge {j,i} is in the edge set, which means that AUVi can receive the information of AUVj. Definition of reachable set II i is the set of all aircraft that aircraft i can receive information from. For example, the adjacency matrix of a certain formation is:
[0029]
[0030] Step 2:
[0031] The dynamic model is established. The present invention adopts the leader-follower formation method. The kinematic model of the i-th aircraft can be expressed as:
[0032]
[0033] In the formula is the position and heading vector of the i-th aircraft; x i is the longitudinal position of the i-th spacecraft; y i is the lateral position of the i-th spacecraft; is the heading angle of the i-th aircraft; v i =[u i v i r i ] T is the velocity vector of the i-th spacecraft; u i is the longitudinal velocity of the i-th spacecraft; v i is the lateral velocity of the i-th spacecraft; r i is the angular velocity of the heading of the i-th vehicle; M is the inertia coefficient matrix including the additional mass coefficient; C(v i ) is the Coriolis force-centripetal force coefficient matrix including the additional mass coefficient of the i-th spacecraft; D is the damping force matrix; g(η i ) is the restoring force vector of the i-th spacecraft; for a fully driven spacecraft, is the control force of the i-th spacecraft; for an underactuated spacecraft without lateral thrusters, is the standard rotation matrix of the i-th spacecraft.
[0034] Step 3:
[0035] The formation and movement of the control formation are generated according to the model predictive control. The following is an example of a typical objective function to solve the optimization:
[0036] J = ∑(x(k+i|k) T Qx(k+i|k)+u(k+i|k) T Ru(k+i|k)) (1-4)
[0037] Among them, Q and R are the state error and the weight matrix of the control input. Find an optimal control input for the above objective function so that the output of the above equation is optimal.
[0038] For the MPC controller of the underwater vehicle formation, a series of constraints need to be added so that the controller can meet the actual navigation conditions of the vehicle, which are specifically expressed as follows:
[0039]
[0040] Step 4:
[0041] Establish a repulsive potential field:
[0042]
[0043] Where U rep (q) virtual repulsive potential field; η is the gain coefficient; p(q,q abx ) is the distance between the spacecraft and the obstacle; p0 is the range of the repulsive field. abx )>p0, the spacecraft is no longer affected by the virtual repulsive field.
[0044] Solving the negative gradient of the repulsive potential field gives the potential field repulsion:
[0045]
[0046] Generally speaking, an underwater vehicle will be affected by multiple obstacle repulsive potential fields at the same time in an obstacle environment. In this case, it is only necessary to geometrically add the virtual repulsive forces:
[0047] F(q)=F alt (q)+∑F repi (q) (1-8)
[0048] Step 5:
[0049] Establish an extended Kalman filter. The state equation of the system is:
[0050] x1=Fx t-1 +Bu t-1 +w t-1 (1-9)
[0051] The observation equation is:
[0052] z t =Hx t +v t (1-10)
[0053] Among them, x t is the state at time t, F is the state transfer matrix, x t-1 is the state at time t-1, B is the control matrix, u t-1 is the control input at time t-1, w k-1 is the process noise; z t is the observed value at time t, H is the measurement matrix, v t is the measurement noise. And w k-1 and v t All obey Gaussian distribution, that is, w k-1 :N(0,Q),v t :N(0,R).
[0054] First, make information prediction:
[0055]
[0056] Then update the status:
[0057]
[0058] in, is the prior estimate of the state at time t; is the posterior estimate of the state at time t-1; is the posterior estimate of the state at time t; P t - is the prior estimated covariance matrix of the state at time t; P t and P t-1 are the posterior estimated covariance matrices of the states at time t and time t-1 respectively; K t is the Kalman gain matrix.
[0059] In the description of this specification, the description with reference to the terms "one embodiment", "example", "specific example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described can be combined in any one or more embodiments or examples in a suitable manner.
[0060] The preferred embodiments of the present invention disclosed above are only used to help illustrate the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to the specific implementation methods described. Obviously, many modifications and changes can be made according to the content of this specification. This specification selects and specifically describes these embodiments in order to better explain the principles and practical applications of the present invention, so that those skilled in the art can understand and use the present invention well. The present invention is limited only by the claims and their full scope and equivalents.
Claims
1. An integrated method for autonomous navigation, tracking and obstacle avoidance of an underwater vehicle, characterized in that: Based on dynamics theory and reference coordinate system, the kinematic and dynamic models of the fully-driven underwater vehicle are established; The communication model of the aircraft formation is established through the information flow topology; Establish dynamic obstacle motion model; Establish the system control model of model predictive control method; Design improved artificial potential field functions; The vehicle trajectory is predicted based on the extended Kalman filter at the obstacle avoidance trajectory, the obstacle avoidance trajectory is optimized, and the integration of underwater vehicle navigation, tracking and obstacle avoidance is realized.
2. The underwater vehicle autonomous navigation-tracking-obstacle avoidance integrated method according to claim 1, characterized in that: The information flow topology structure expresses whether communication is established between two underwater vehicles in the form of a directed graph. The established communication can be unidirectional or bidirectional.
3. The underwater vehicle autonomous navigation-tracking-obstacle avoidance integrated method according to claim 1, characterized in that: The dynamic obstacle motion model is expressed as a kinematic equation containing position, velocity, and acceleration.
4. The underwater vehicle autonomous navigation-tracking-obstacle avoidance integrated method according to claim 1, characterized in that: Model predictive control includes prediction model, rolling optimization and feedback control. The objective function and control constraints are designed through quadratic programming method to achieve collaborative navigation control of underwater vehicle formations.
5. The underwater vehicle autonomous navigation-tracking-obstacle avoidance integrated method according to claim 1, characterized in that: The improved artificial potential field method uses the real-time position of the virtual navigator instead of the target point as the center of the gravitational potential field, and introduces virtual coordination force under specific circumstances to avoid local minima and forced collisions.
6. The underwater vehicle autonomous navigation-tracking-obstacle avoidance integrated method according to claim 1, characterized in that: The Kalman filter model is used to estimate the measurement values of the aircraft and reduce the measurement error.
7. The underwater vehicle autonomous navigation-tracking-obstacle avoidance integrated method according to claim 1, characterized in that: According to the prediction model of the underwater vehicle based on the MPC prediction model and the Kalman filter, the obstacle avoidance trajectory of the vehicle is optimized to make the obstacle avoidance curve smoother and reduce the formation distortion.
