CN115857483A - Unmanned ship obstacle avoidance control method based on control obstacle function model predictive control algorithm - Google Patents
Unmanned ship obstacle avoidance control method based on control obstacle function model predictive control algorithm Download PDFInfo
- Publication number
- CN115857483A CN115857483A CN202210663601.5A CN202210663601A CN115857483A CN 115857483 A CN115857483 A CN 115857483A CN 202210663601 A CN202210663601 A CN 202210663601A CN 115857483 A CN115857483 A CN 115857483A
- Authority
- CN
- China
- Prior art keywords
- control
- unmanned ship
- obstacle
- function
- obstacle avoidance
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 26
- 238000005457 optimization Methods 0.000 claims abstract description 17
- 230000004888 barrier function Effects 0.000 claims description 41
- 230000008569 process Effects 0.000 claims description 6
- 230000009471 action Effects 0.000 claims description 3
- 230000007613 environmental effect Effects 0.000 claims description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 abstract description 8
- 238000010586 diagram Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- RZVAJINKPMORJF-UHFFFAOYSA-N Acetaminophen Chemical compound CC(=O)NC1=CC=C(O)C=C1 RZVAJINKPMORJF-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to an unmanned ship obstacle avoidance control method based on a control obstacle function model predictive control algorithm, and belongs to the technical field of unmanned ships. The method specifically comprises the following steps: s1: establishing a kinematics model of the unmanned ship and a collision avoidance obstacle target model; s2: designing an obstacle avoidance optimization target cost function under the constraint of an unmanned ship control obstacle function according to the pose information between the obstacle and the unmanned ship; s3: optimizing and solving a cost function in real time, and planning a reference track of the unmanned ship at a future moment; s4: and (3) taking the future moment track obtained by solving the planning algorithm as the reference track of the unmanned ship, and designing a layered closed-loop control algorithm to realize real-time tracking. The unmanned ship water surface real-time obstacle avoidance trajectory planning method can realize real-time obstacle avoidance trajectory planning of the unmanned ship by aiming at the characteristic of nonlinear multiple constraints in control in the motion of the unmanned ship.
Description
Technical Field
The invention belongs to the technical field of unmanned ships, and particularly relates to an unmanned ship obstacle avoidance control method based on a control obstacle function model predictive control algorithm.
Technical Field
The unmanned ship is a motion platform which can execute tasks in a specific water area only by depending on a self motion planning and control system, is limited by self range and communication distance in the face of a complex water environment, and is difficult to artificially control all the time in the operation process. Therefore, an excellent motion planning algorithm needs to be developed to improve the autonomous navigation capability of the unmanned ship.
Besides the interference of water surface fluctuation, the water surface obstacle is the biggest threat of unmanned ship safe operation, the current global obstacle avoidance track planning algorithm is mature, but the water surface environment changes with time, and a real-time obstacle avoidance track planning algorithm needs to be developed to improve the navigation safety of the unmanned ship. The traditional planning method has a high planning speed for fixed obstacles, but is difficult to adapt to the change of the environment, and does not consider the problems of various constraints existing in the operation of unmanned ships and the like.
Disclosure of Invention
Aiming at the technical problems, the unmanned ship obstacle avoidance control based on the control obstacle function model predictive control algorithm is designed, and the unmanned ship water surface real-time obstacle avoidance trajectory planning can be realized aiming at the characteristic of non-linearity and multiple constraints in the control in the unmanned ship motion.
In order to realize the functions, the invention adopts the following scheme:
an unmanned ship obstacle avoidance control method based on a control obstacle function model predictive control algorithm is characterized by comprising the following steps: the method specifically comprises the following steps: s1: establishing a unmanned ship kinematics model and a collision obstacle avoidance target model; s2: designing an obstacle avoidance optimization target cost function under the constraint of an unmanned ship control obstacle function according to the pose information between the obstacle and the unmanned ship; s3: optimizing and solving a cost function in real time, and planning a reference track of the unmanned ship at a future moment; s4: and (3) taking the future moment track obtained by solving the planning algorithm as the reference track of the unmanned ship, and designing a layered closed-loop control algorithm to realize real-time tracking.
Further, the S1 comprises the steps of:
s11: establishing a kinematics model of the unmanned ship, wherein the model is described as follows:
wherein, P x (k) And P y (k) Showing the position of the centroid in the horizontal plane projector, theta (k) showing the unmanned ship course angle, V x (k) And V y (k) Indicating travel speeds in both directionsAnd W (k) represents the steering angular velocity.
S12: establishing an obstacle target model in the environment, wherein the model is described as follows: the circumscribed circle of the unmanned ship is regarded as a mass center, and the circumscribed circle of the barrier is expanded, so that the expanded radius is the minimum safe distance between the circumscribed circle and the barrier.
Further, the S2 includes the steps of:
s21: designing a control barrier function, giving certain obstacle avoidance capacity to the unmanned ship by using distance constraint obtained by Euclidean norm, designing a model prediction control optimization calculation of Euclidean geometric distance constraint, and estimating and planning an objective function required to be optimized and system constraint conditions to be expressed as follows:
wherein x is t+k∣t Representing unmanned ship in input sequence u t:t+N-1∣t The state vector at the kth time period under the action of (1); q (x) t+k∣t ,u t+k∣t ) And p (x) t+N∣t ) Respectively representing process cost and terminal cost; g represents a distance constraint that complies with safe operation.
S22: applying the discrete control obstacle function and the model prediction control to the planning problem together, and designing the continuous differentiable function
As the safety guarantee conditions of the unmanned ship:
where h is the control barrier function, if for any
Are all present>
And there is an extension>
A generic function γ that enables the control barrier function to satisfy, for an arbitrary input u:
preferably, to accommodate processor operations, a discrete process is performed:
Δh(x k ,u k )≥-γh(x k ),0＜γ≤1 (5)
wherein, Δ h (x) k ,u k ):＝h(x k+1 )-h(x k ) Representing the difference between the control barrier function at the moment k +1 and the moment k, and adding a constraint condition to the control barrier function:
h(x k+1 )≥(1-γ)h(x k ) (6)
s23: designing a planning and control algorithm, and designing a model prediction controller with a control barrier function on the premise of ensuring the operation safety of an unmanned ship discrete time system:
where Δ h represents a control barrier function constraint, expressed as:
Δh(x t+k-1∣t )＝h(x t+k∣t )-h(x t+k-1∣t ) (8)
s24: adjusting the model predictive control parameters based on the control barrier function may be expressed as:
defining a set of state spaces that satisfy the control barrier function constraints and the initial state constraints as:
wherein,
a set of feasible states that satisfy the control barrier function constraints over each time period of the open-loop trajectory is described.
The boundary of the barrier function is defined as:
further, the step S3 includes:
s31: and converting the feasibility of the optimization problem into a problem of solving whether an intersection exists between the reachable domain and the feasible set of each prediction period. The adopted strategy is as follows: given an initial state x 0 Meanwhile, a fixed gamma value is selected, and the gamma parameter value of the barrier function is adjusted and controlled according to surrounding barrier information, so that the environmental adaptability of the unmanned ship can be improved, and finally, the optimization problem is determined to be solvable.
S32: the relation between the computer operation capability and the system control target is balanced, the appropriate prediction time domain length N =10 is selected, and the barrier function gamma parameter is controlled to be between 0.1 and 0.9, so that the purpose of excellent real-time control is achieved.
Further, the step S4 includes:
s41: establishing a layered control system according to the control task requirements of the unmanned ship, planning an obstacle avoidance track in real time according to the current pose state of the unmanned ship through an MPC track planning controller at the highest layer, and providing a reference track for a PID controller at an intermediate layer; the middle layer PID controller carries out high-frequency real-time control according to the current pose information of the unmanned ship and calculates expected thrust; and the bottom propeller controller realizes real-time thrust closed-loop control according to the rotating speed feedback so as to ensure the rapid action of the propeller.
S42: and inputting the reference track obtained after algorithm optimization into a control system of the unmanned ship.
The unmanned ship obstacle avoidance control system has the following beneficial effects:
(1) The unmanned ship obstacle avoidance control method based on the control obstacle function model predictive control algorithm is designed, and can realize real-time obstacle avoidance trajectory planning of the unmanned ship on the water surface according to the characteristic of nonlinear multi-constraint in control in the motion of the unmanned ship.
Drawings
FIG. 1 is a flow chart of the operation of a control system in an embodiment of the present invention;
FIG. 2 is a simplified illustration of an unmanned ship and obstacles in an embodiment of the present invention;
FIG. 3 is a simplified illustration of an embodiment of the present invention after inflation of the barrier;
FIG. 4 is a schematic diagram of model predictive control feasible region under a control barrier function in an embodiment of the present invention;
FIG. 5 is a schematic diagram of intersection sets of different parameters according to an embodiment of the present invention;
FIG. 6 is a schematic diagram of a layer module in an embodiment of the invention.
Detailed Description
The invention is further described with reference to the accompanying drawings:
fig. 1 to fig. 6 show an implementation method of an unmanned ship obstacle avoidance control method based on a fuzzy function predictive control method in the invention.
Firstly, according to the unmanned ship kinematics model, establishing a state space equation under the unmanned ship continuous time, which can be expressed as:
discretizing the system, and establishing a discrete state space equation of the unmanned ship at the time k, which can be expressed as:
x(k+1)＝A(k)x(k)+B(k)u(k) (2)
the discrete state space equation is expanded and can be expressed as:
as shown in fig. 2, the gray part is the unmanned ship body, the minimum circumscribed circle of the rectangle is selected as the operation space required by the unmanned ship at the current moment, the obstacle with complex characteristics is obtained, the minimum circumscribed circle is drawn according to the convex hull of the obstacle, and the distance between the center of the minimum circumscribed circle of the unmanned ship and the center of the minimum circumscribed circle of the convex hull of the obstacle is greater than the sum of two radiuses. For representing convenience, the circumscribed circle of the unmanned ship can be regarded as a mass center, at the moment, the circumscribed circle of the barrier can not guarantee operation safety any more, the circumscribed circle of the barrier needs to be expanded equally, and the radius after expansion is the minimum safe distance between the circumscribed circle of the barrier and the minimum safe distance.
And further, designing a model predictive control algorithm with a control barrier function required by optimization according to the environmental information of the unmanned ship and the set expected terminal pose state.
Firstly, a discrete optimization objective function required by optimization is established as follows:
then, according to the physical limit or other influence factors of the unmanned ship, the constraint conditions required to be met in the optimization process of the unmanned ship are designed:
and finally designing the safety region constraint allowed by the control barrier function:
Δh(x t+k∣t ,u t+k∣t )≥-γh(x t+k∣t ),k＝0,…,N-1 (6)
further, a suitable optimization function parameter is selected. As shown in fig. 3, the parameter varies from 0 to 1, and the intersection of the safe region under the control barrier function constraint and the reachable state set of the unmanned ship in the prediction time domain varies accordingly, so that a certain solvable optimization problem is determined.
And further, inputting the optimized reference track into a control system to realize the control and follow of the reference track by the control system. And judging whether the system is converged according to the state error between the s current pose and the target pose of the unmanned ship. If the error is smaller than a certain threshold value, the unmanned ship reaches the target pose, and the planning control is stopped; otherwise, repeating the steps.
The invention is described above with reference to the accompanying drawings, it is obvious that the invention is not limited to the above embodiments, and it is within the scope of the invention to use various modifications of the inventive method concept and solution, or to directly apply the inventive concept and solution to other applications without modification.
Claims (5)
1. An unmanned ship obstacle avoidance control method based on a control obstacle function model predictive control algorithm is characterized by comprising the following steps: the method specifically comprises the following steps: s1: establishing a unmanned ship kinematics model and a collision obstacle avoidance target model; s2: designing an obstacle avoidance optimization target cost function under the constraint of an unmanned ship control obstacle function according to the pose information between the obstacle and the unmanned ship; s3: optimizing and solving a cost function in real time, and planning a reference track of the unmanned ship at a future moment; s4: and (3) taking the future moment track obtained by the solution of the planning algorithm as the reference track of the unmanned ship, and designing a layered closed-loop control algorithm to realize real-time tracking.
2. The unmanned ship obstacle avoidance control method based on the control obstacle function model predictive control algorithm according to claim 1, characterized in that: the step S1 comprises the following steps:
s11: establishing a kinematics model of the unmanned ship, wherein the model is described as follows:
wherein, P x (k) And P y (k) Showing the position of the projector with the center of mass in the horizontal plane, theta (k) showing the course angle of the unmanned ship, V x (k) And V y (k) Represents the traveling speeds in both directions, and W (k) represents the steering angle speed;
s12: establishing an obstacle target model in the environment, wherein the model is described as follows: the circumscribed circle of the unmanned ship is regarded as a mass center, and the circumscribed circle of the barrier is expanded, wherein the expanded radius is the minimum safe distance between the circumscribed circle and the barrier.
3. The unmanned ship obstacle avoidance control method based on the control obstacle function model predictive control algorithm according to claim 1, characterized in that: the step S2 includes:
s21: designing a control barrier function, using the distance constraint of Euclidean norm to give certain obstacle avoidance capability to the unmanned ship, designing a model prediction control optimization algorithm of Euclidean geometric distance constraint, and estimating and planning the objective function and system constraint conditions required to be optimized, wherein the target function and the system constraint conditions can be expressed as follows:
wherein x is t+k∣t Representing unmanned ship in input sequence u t : t+N-1∣t The state vector at the kth time period under the action of (1); q (x) t+k∣t ,u t+k∣t ) And p (x) t+N∣t ) Respectively representing process cost and terminal cost; g represents a distance constraint that complies with safe operation;
s22: the discrete control barrier function and the model prediction control are applied to the planning problem together, and the continuous differentiable function is designed
As the safety guarantee conditions of the unmanned ship:
where h is the control barrier function, if for any
Are all present>
And an extension K exists ∞ A generic function γ that satisfies the control barrier function for any input u:
h&(x,u)≥-γ(h(x)),γ∈K ∞ (4)
preferably, to accommodate processor operations, a discrete process is performed:
Δh(x k ,u k )≥-γh(x k ),0＜γ≤1 (5)
wherein, Δ h (x) k ,u k ):＝h(x k+1 )-h(x k ) Representing the difference between the control barrier function at the moment k +1 and the moment k, and adding a constraint condition to the control barrier function:
h(x k+1 )≥(1-γ)h(x k ) (6)
s23: designing a planning and control algorithm, and designing a model prediction controller with a control barrier function on the premise of ensuring the operation safety of the unmanned ship discrete time system:
where Δ h represents a control barrier function constraint, expressed as:
Δh(x t+k-1∣t )＝h(x t+k∣t )-h(x t+k-1∣t ) (8)
s24: adjusting the model predictive control parameters based on the control barrier function may be expressed as:
defining a set of state spaces that satisfy the control barrier function constraints and the initial state constraints as:
S cbf,k ＝{x∈X:h(x t+k∣t )-h(x t+k-1∣t )≥-γh(x t+k-1∣t )} (10)
wherein S is cbf,k Describing a set of feasible states that satisfy the control barrier function constraint at each time period of the open-loop trajectory;
the boundary of the barrier function is defined as:
4. the unmanned ship obstacle avoidance control method based on the control obstacle function model predictive control algorithm according to claim 1, characterized in that: the step S3 includes:
s31: the feasibility of the optimization problem is converted into the problem of whether intersection exists between the reachable domain and the feasible set of each prediction period, and the adopted strategy is as follows: given an initial state x 0 Meanwhile, a fixed gamma value is selected, and the gamma parameter value of the barrier function is adjusted and controlled according to surrounding barrier information, so that the environmental adaptability of the unmanned ship can be improved, and finally, the optimization problem is determined to be solvable;
s32: the relation between the computer operation capability and the system control target is balanced, the appropriate prediction time domain length N =10 is selected, and the barrier function gamma parameter is controlled to be between 0.1 and 0.9, so that the purpose of excellent real-time control is achieved.
5. The unmanned ship obstacle avoidance control method based on the control obstacle function model predictive control algorithm according to claim 1, characterized in that: the step S4 includes:
s41: establishing a layered control system according to the control task requirements of the unmanned ship, planning an obstacle avoidance track in real time according to the current pose state of the unmanned ship through an MPC track planning controller at the highest layer, and providing a reference track for a PID controller at an intermediate layer; the middle layer PID controller carries out high-frequency real-time control according to the current pose information of the unmanned ship and calculates expected thrust; the bottom thruster controller realizes real-time thrust closed-loop control according to the rotating speed feedback so as to ensure the quick action of the thruster;
s42: and inputting the reference track obtained after algorithm optimization into a control system of the unmanned ship.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210663601.5A CN115857483A (en) | 2022-06-13 | 2022-06-13 | Unmanned ship obstacle avoidance control method based on control obstacle function model predictive control algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210663601.5A CN115857483A (en) | 2022-06-13 | 2022-06-13 | Unmanned ship obstacle avoidance control method based on control obstacle function model predictive control algorithm |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115857483A true CN115857483A (en) | 2023-03-28 |
Family
ID=85660152
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210663601.5A Pending CN115857483A (en) | 2022-06-13 | 2022-06-13 | Unmanned ship obstacle avoidance control method based on control obstacle function model predictive control algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115857483A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117572810A (en) * | 2024-01-16 | 2024-02-20 | 大连海事大学 | Mechanical arm safety cooperative control system based on control obstacle function |
-
2022
- 2022-06-13 CN CN202210663601.5A patent/CN115857483A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117572810A (en) * | 2024-01-16 | 2024-02-20 | 大连海事大学 | Mechanical arm safety cooperative control system based on control obstacle function |
| CN117572810B (en) * | 2024-01-16 | 2024-05-10 | 大连海事大学 | Mechanical arm safety cooperative control system based on control obstacle function |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111413966B (en) | Progressive model prediction unmanned planning tracking cooperative control method | |
| CN109283842B (en) | Unmanned ship track tracking intelligent learning control method | |
| Zhao et al. | Control method for path following and collision avoidance of autonomous ship based on deep reinforcement learning | |
| Liao et al. | Layered berthing method and experiment of unmanned surface vehicle based on multiple constraints analysis | |
| CN102768539B (en) | AUV (autonomous underwater vehicle) three-dimension curve path tracking control method based on iteration | |
| CN111487966B (en) | Self-adaptive path tracking control method for unmanned surface vehicle based on waypoints | |
| Wei et al. | MPC-based motion planning and control enables smarter and safer autonomous marine vehicles: Perspectives and a tutorial survey | |
| CN109050835B (en) | Full-drive autonomous underwater robot structure and recovery three-dimensional path tracking method | |
| Faramin et al. | Track-keeping observer-based robust adaptive control of an unmanned surface vessel by applying a 4-DOF maneuvering model | |
| An et al. | Proximate time optimal for the heading control of underactuated autonomous underwater vehicle with input nonlinearities | |
| Hu et al. | Trajectory tracking and re-planning with model predictive control of autonomous underwater vehicles | |
| Yin et al. | Predictive trajectory tracking control of autonomous underwater vehicles based on variable fuzzy predictor | |
| Fan et al. | Formation control of multiple unmanned surface vehicles using the adaptive null-space-based behavioral method | |
| CN114326702A (en) | Model prediction control-based inland river unmanned ship collision avoidance method | |
| Gao et al. | Online optimal control for dynamic positioning of vessels via time-based adaptive dynamic programming | |
| Chen et al. | Cooperative tracking of vessel trajectories based on curved dynamic coordinates | |
| CN115857483A (en) | Unmanned ship obstacle avoidance control method based on control obstacle function model predictive control algorithm | |
| Liang et al. | Economic MPC-based planning for marine vehicles: Tuning safety and energy efficiency | |
| CN114035567B (en) | Unmanned surface vehicle navigation control system | |
| Mu et al. | An Novel Model Switching Course Control for Unmanned Surface Vehicle With Modeling Error and External Disturbance | |
| Li et al. | [Retracted] A Systematic Pipelaying Control Method Based on the Sliding Matrix for Dynamically Positioned Surface Vessels | |
| Belleter et al. | Observer based path following for underactuated marine vessels in the presence of ocean currents: A global approach-with proofs | |
| Aung et al. | Investigation into control strategies for manoeuvring in adverse weather conditions | |
| Sinisterra et al. | Nonlinear and machine-learning-based station-keeping control of an unmanned surface vehicle | |
| Lian et al. | Trajectory optimization of unmanned surface vehicle based on improved minimum snap |
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 |