Adaptive motion planning method for unmanned ships based on model predictive control

Abstract

The present invention relates to an unmanned ship adaptive motion planning method based on model predictive control, which can adaptively adjust the speed of the unmanned ship according to the environmental obstacle information and obtain an optimized motion trajectory while satisfying the constraints of the unmanned ship system, including the following steps: generating a collision-free state trajectory that satisfies the unmanned ship speed dynamics; constructing an adaptive safety state trajectory within the field of view and detection range of the unmanned ship; designing an adaptive motion planner based on model predictive control; sending the planned motion trajectory to the underlying tracking controller; acquiring new system state and environmental information at the new planning moment, and rolling planning until the unmanned ship enters the target area. The method of the present invention can not only improve the obstacle avoidance capability and planning performance of the unmanned ship, but also adaptively adjust the speed of the unmanned ship according to the obstacle environment.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned ship motion planning, and specifically relates to an unmanned ship adaptive motion planning method based on model predictive control. Background Art

[0002] Unmanned ships are highly autonomous and flexible surface transport platforms. They are the main tool for improving the exploration, development and application of marine resources, and have played an important role in both civil and military applications. Among them, the civil applications are mainly the monitoring and research of the marine environment, the construction and inspection of underwater facilities, seabed mapping and scientific surveys, marine transportation and aquaculture; the military applications are mainly the reconnaissance and collection of information, the detection and removal of enemy equipment, and maritime patrols and escorts.

[0003] The motion of unmanned ships is subject to the constraints of the system's own kinematics and dynamics as well as the external obstacles and environment. How to enable unmanned ships to complete their tasks safely and efficiently has long been a concern of scholars at home and abroad. Motion planning is a method that can obtain the motion trajectory from the starting point to the target area in the best way and can take into account the system's own limitations and environmental constraints. Its essence is to obtain a feasible discrete state-input set with optimized performance. The motion planning technology of unmanned ships is the key to ensuring that unmanned ships can complete various tasks safely, autonomously and efficiently, and has become an important factor in measuring the degree of autonomy of unmanned ships. At present, the research on unmanned ship motion planning is mainly divided into: graph search-based methods, such as Dijkstra algorithm, A* algorithm, D* algorithm, which mainly use graph search methods to search the constructed geometric environment model and obtain the optimal path; sampling-based methods, such as probabilistic landmark method, rapid expansion random tree method, which mainly use random sampling and collision detection to explore the state space and obtain the collision-free optimal path; curve interpolation-based methods, such as polynomial curve method, Bezier curve method, B-spline curve method, which can use the properties of geometric curves and obtain optimized curve paths by adjusting the control parameters of the curve; numerical optimization-based methods, such as artificial potential field method, genetic algorithm, particle swarm optimization algorithm, and model predictive control-based methods, which mainly describe the planning problem as a constrained optimization problem and obtain the optimal trajectory by solving the optimization problem. Among them, the model predictive control-based method has attracted widespread attention due to its significant advantages in dealing with complex constraints and obtaining optimization performance. For example, D. Kufoalor et al. used the model predictive control method to realize the motion planning of surface ships. The designed objective function included obstacle avoidance function and penalty function for violation of rules; V. Huynh et al. used the model predictive control method to determine the optimal motion in an unknown ocean current environment. The designed objective function was to minimize the energy consumed by the surface ship. The optimized variable was a predetermined finite control set, and the model used only contained the kinematic model of the surface ship.

[0004] Although the model predictive control method has made some progress in the motion planning of unmanned ships, the existing methods still have the following shortcomings: 1) The kinematic and dynamic constraints of the unmanned ship system and the actual state constraints and input constraints of the unmanned ship system are not considered at the same time, which makes it difficult to ensure the actual feasibility and optimality of the planning results; 2) The planning ability only depends on the prediction time domain, which limits the obstacle avoidance ability of the unmanned ship and the performance of the motion planner; 3) It is impossible to adaptively control the motion speed of the unmanned ship according to the situation of external obstacles. Therefore, there is an urgent need for a motion planning method that can not only meet the actual physical constraints of the unmanned ship system and the obstacle avoidance constraints of the external environment, but also improve the obstacle avoidance ability and planning performance of the unmanned ship and can adaptively control the speed of the unmanned ship according to the obstacle environment. Summary of the invention

[0005] Technical issues to be solved

[0006] In order to avoid the shortcomings of the prior art, the present invention proposes an unmanned ship adaptive motion planning method based on model predictive control.

[0007] Technical Solution

[0008] An unmanned ship adaptive motion planning method based on model predictive control, characterized by comprising:

[0009] S1: Generate a collision-free trajectory that satisfies the speed dynamics of the unmanned ship;

[0010] S2: construct an adaptive safe state trajectory within the field of view and detection range of the unmanned ship based on the generated trajectory;

[0011] S3: Using the constructed adaptive safe state trajectory as a reference trajectory, an adaptive motion planner based on model predictive control is designed.

[0012] A further technical solution of the present invention: Step S1 is as follows:

[0013] Step 1.1: Initialize the parameters related to the generated state trajectory: the radius of the unmanned ship r v , obstacle radius r o , the interval distance r c , the radius of the detection range r d , the end point accepts the radius r of the circle a , the expected velocity of the state trajectory Sampling period δ, planned prediction time domain N p , the planned control domain N c , historical state trajectory X r (0), the initial state x(0) and initial position x of the unmanned ship at the current initial moment p(0), the initial state x of the state trajectory at the current initial moment r (0), initial position End point

[0014] Step 1.2: Use the onboard sensors to detect the surrounding environment and obtain the current t k Obstacle information at the moment b (t k );

[0015] Step 1.3: Build from To the end The path from the obstacle O b (t k ) to filter out the first obstacle that collides with the path Structure and The circles have the same center and radius (r o +r v +r c ) expansion obstacle

[0016] Step 1.4: Using Straight and Dilation Obstacles The arc segments are constructed from To the end The collision-free optimal path;

[0017] Step 1.5: Sample along the collision-free optimal path obtained in step 1.4 to obtain the current t k The state trajectory of no collision at all times and meeting the speed dynamics of the unmanned ship The velocity U of the trajectory r (t) Satisfaction Where T is the time constant of the unmanned ship's speed dynamics, t∈[t k , t k +(N p -1)δ].

[0018] A further technical solution of the present invention: Step S2 is as follows:

[0019] Step 2.1: Set the current t k The state of the unmanned ship at the moment is x(t k ), position is x p (t k ), construct from x p (t k ) to the end The path, from the obstacle information O in step 1.2 b (t k ) to filter out the first obstacle o that collides with the path b , structure and ob The circles have the same center and radius (r o +r v ) expansion obstacle

[0020] Step 2.2: Construct the p (t k ) as the starting point and with the expansion obstacle Tangent ray, state trajectory obtained from step 1.5 Filter out the objects on one side of the ray and the expansion obstacles State trajectory on the opposite side of the center This trajectory is the current t of the unmanned ship. k The safety status trajectory within the field of view at all times, where is the number of states contained in the trajectory;

[0021] Step 2.3: Construct x p (t k ) is the center of the circle and its radius is r d The detection circle, the state trajectory obtained from step 2.2 Filter out the state trajectories within the detection circle This trajectory is a safe state trajectory that is both within the field of view of the unmanned ship and within the area detected by the unmanned ship. is the number of states contained in the trajectory;

[0022] Step 2.4: Construct the current t k The number of states at a time is N p Safety status trajectory in The quantity is Status track Taken from t k-1 Historical security status trajectory at time X r (t k-1 ), N all is the number of states contained in the historical state trajectory;

[0023] Step 2.5: Update t k Historical security status track at each moment in Taken from t k Always keep in the safe state track within the field of vision of the unmanned ship and the explored area.

[0024] A further technical solution of the present invention: Step S3 is as follows:

[0025] Step 3.1: Establish the kinematic and dynamic model of the unmanned ship x(k+1, x0)=f(x(k, x0), u(k)):

[0026]

[0027]

[0028] where [x, y, ψ] T is the position and heading angle of the unmanned ship in the ground coordinate system, [u, v, r] T is the linear velocity and angular velocity of the unmanned ship in the body coordinate system, M i (i=1, 2, 3) is the system inertia term, D i (i=1, 2, 3) is the system damping term, x=[x,y,ψ,u,v,r] T is the system state of the unmanned ship, x0 is the current t k The state of the unmanned ship at the moment, u=[X,N] T Provide control input for unmanned vessels;

[0029] Step 3.2: Build t k Objective function of the moment model predictive control constrained optimization problem Where N p is the forecast time domain of the plan, x0 is t k The actual state of the unmanned ship at the moment, Q and R are weight matrices, x r (i) is the safe state trajectory X obtained in step 2.4 r The i-th state of is the error of the predicted state (the predicted state With reference state x r (·)), is the control input for prediction;

[0030] Step 3.3: Initialize the model parameters M of the unmanned ship i (i=1, 2, 3) and D i (i=1, 2, 3), setting the state error weight matrix Q and control input weight matrix R of the model predictive control constrained optimization problem;

[0031] Step 3.4: Construct the system state constraints of the model predictive control constrained optimization problem based on the actual physical constraints of the unmanned ship system and control input constraints in is the system state constraint set, Enter a set of constraints for control;

[0032] Step 3.5: Expand the obstacle obtained in step 2.1 Constructing the obstacle avoidance constraints for the model predictive control constrained optimization problem in The predicted position of the unmanned ship, For expansion obstacles The center of the circle;

[0033] Step 3.6: Based on the unmanned ship dynamic model obtained in step 3.1, the objective function obtained in step 3.2, the state set and input set obtained in step 3.4, and the obstacle avoidance constraint obtained in step 3.5, construct t k Time-to-moment model predictive control constrained optimization problem:

[0034]

[0035]

[0036]

[0037]

[0038]

[0039] Step 3.7: Solve the constrained optimization problem constructed in step 3.6 and obtain the optimal motion trajectory Sent to the underlying tracking controller, where

[0040] Step 3.8: Low-level tracking controller executes N c After time δ, repeat the above steps and design t k+1 The adaptive motion planner based on model predictive control is used to plan the movement until the unmanned ship reaches the end point. is the center of the circle and its radius is r a within the acceptance circle.

[0041] Beneficial Effects

[0042] The present invention proposes an unmanned ship adaptive motion planning method based on model predictive control. The generated collision-free state trajectory meets the speed dynamics of the unmanned ship and is generated on the shortest path composed of straight lines and circular arcs; the constructed adaptive safety state trajectory not only takes into account the detection range of the ship-borne sensor, but also takes into account the field of view of the unmanned ship; the designed model predictive control-based adaptive motion planner utilizes an adaptive safety reference trajectory, takes into account the kinematic and dynamic constraints of the unmanned ship system and the state limitations and input limitations of the actual system, ensures the safety and feasibility of the planning results, improves the obstacle avoidance capability of the unmanned ship and the performance of the motion planner, and enables the unmanned ship to adaptively control the movement speed of the unmanned ship according to the external obstacle environment.

[0043] 1. In step 1, a collision-free state trajectory that meets the unmanned ship's speed dynamics is generated based on the environmental information detected by the shipboard sensor. The designed state trajectory is not only collision-free, but also meets the unmanned ship's speed dynamics, and the starting point of the collision-free state trajectory can be flexibly changed according to the dynamic situation of the obstacle environment.

[0044] 2. In step 2, an adaptive safe state trajectory within the field of view and detection range of the unmanned ship is constructed based on the collision-free state trajectory generated in step 1. The designed safe state trajectory not only takes into account the limited detection range of the shipborne sensor, but also takes into account the obstruction of the field of view of the unmanned ship by obstacles.

[0045] 3. In step 3, the adaptive safety state trajectory constructed in step 2 is used as a reference trajectory, and then an adaptive motion planner based on model predictive control is designed. The designed motion planner not only considers the kinematic and dynamic models of the unmanned ship, but also considers the actual physical constraints of the unmanned ship system and the obstacle information of the current position of the unmanned ship. At the same time, the rolling planning strategy is used to improve the ability of the designed motion planner to cope with dynamic environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] The drawings are only for the purpose of illustrating particular embodiments and are not to be considered limiting of the present invention. Like reference symbols denote like components throughout the drawings.

[0047] Figure 1 Algorithm flow chart of adaptive motion planner based on model predictive control;

[0048] Figure 2 Explanatory diagram of the relevant parameters of the adaptive motion planning controller;

[0049] Figure 3 Generate a schematic diagram of the collision-free state trajectory;

[0050] Figure 4 Schematic diagram of building a safe state trajectory;

[0051] Figure 5 Unmanned ship platform;

[0052] Figure 6 Position and velocity trajectories planned by the adaptive motion planner;

[0053] Figure 7 Obstacle distribution and velocity graph controlled by the adaptive planner. DETAILED DESCRIPTION

[0054] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0055] The present invention proposes an adaptive motion planning method for an unmanned ship based on model predictive control. The implementation method thereof is mainly divided into three parts. The first part is to generate a collision-free state trajectory that satisfies the speed dynamics of the unmanned ship; the second part is to construct an adaptive safety state trajectory within the field of view and detection range of the unmanned ship according to the trajectory generated by the first part; the third part is to use the adaptive safety state trajectory constructed in the second part as a reference trajectory, and then design an adaptive motion planner based on model predictive control. The algorithm flow is as follows Figure 1 As shown, the specific steps are:

[0056] Step 1: Generation of collision-free trajectory

[0057] Step 1.1: Initialize the parameters related to the generated state trajectory: the radius of the unmanned ship r v =0.7725m, obstacle radius r o =2.5m, spacing distance r c =0.8m, the radius of the detection range r d =15m, the radius of the end point acceptance circle is r a =2m, the expected speed of the state trajectory Sampling period δ = 0.2s, planned prediction time domain N p =20, planned control time domain N c =10, historical status trajectory The initial state of the unmanned ship at the current initial moment is x(0) = [0,0,π / 2,0,0,0] T , initial position x p (0) = [0,0] T , the initial state x of the state trajectory at the current initial moment r (0) = x(0), initial position End point Reference for explanation of relevant parameters of adaptive motion planner based on model predictive control Figure 2 ;

[0058] Step 1.2: Use the onboard sensors to detect the surrounding environment and obtain the current t k Obstacle information at the moment b (t k ),like Figure 3 The obstacle information shown is O b(t k )=[1.5,8.75] T ;

[0059] Step 1.3: Build from To the end The path from the obstacle O b (t k ) to filter out the first obstacle that collides with the path Structure and The circles have the same center and radius (r o +r v +r c ) expansion obstacle like Figure 3 Obstacles shown and expansion obstacles

[0060] Step 1.4: Using Straight and Dilation Obstacles The arc segments are constructed from To the end The collision-free optimal path is Figure 3 The collision-free shortest path shown;

[0061] Step 1.5: Sample along the collision-free optimal path obtained in step 1.4 to obtain the current t k The state trajectory of no collision at all times and meeting the speed dynamics of the unmanned ship The velocity U of the trajectory r (t) Satisfaction Where T c is the time constant of the unmanned ship speed dynamics, t∈[t k ,t k +(N p -1)δ], such as Figure 3 The collision-free state trajectory shown in Figure 1 is shown in Figure 2, where T c =0.2;

[0062] Step 2: Construction of adaptive security state trajectory

[0063] Step 2.1: Set the current t k The state of the unmanned ship at the moment is x(t k ), position is x p (t k ), construct from x p (t k ) to the end The path, from the obstacle information O in step 1.2 b (t k ) to filter out the first obstacle o that collides with the path b , structure and o bThe circles have the same center and radius (r o +r v ) expansion obstacle like Figure 4 Obstacles shown o b and expansion obstacles

[0064] Step 2.2: Construct the p (t k ) as the starting point and with the expansion obstacle Tangent ray, state trajectory obtained from step 1.5 Filter out the objects on one side of the ray and the expansion obstacles State trajectory on the opposite side of the center This trajectory is the current t of the unmanned ship. k The state trajectory within the field of view at any moment, where is the number of states contained in the trajectory, such as Figure 4 The state trajectory within the field of view shown;

[0065] Step 2.3: Construct x p (0) is the center of the circle and its radius is r d The detection circle, the state trajectory obtained from step 2.2 Filter out the state trajectory within the detection circle This trajectory is a safe state trajectory that is both within the field of view of the unmanned ship and within the area detected by the unmanned ship. is the number of states contained in the trajectory, such as Figure 4 The safety status trajectory within the field of view and detection range is shown;

[0066] Step 2.4: Construct the current t k The number of states at a time is N p Safety status trajectory in The quantity is Status track Taken from t k-1 Historical security status trajectory at time X r (t k-1 ), N all is the number of states contained in the historical state trajectory, such as Figure 4 The number of states shown is N p Safety status trajectory X r ;

[0067] Step 2.5: Update t k Historical security status track at each moment in Taken from t kAlways be in the safe state track within the field of vision of the unmanned ship and the explored area;

[0068] Step 3: Design of adaptive motion planner based on model predictive control

[0069] Step 3.1: Establish the kinematic and dynamic model of the unmanned ship x(k+1,x0)=f(x(k,x0),u(k)):

[0070]

[0071]

[0072] where [x, y, ψ] T is the position and heading angle of the unmanned ship in the ground coordinate system, [u,v,r] T is the linear velocity and angular velocity of the unmanned ship in the body coordinate system, M i (i=1,2,3) is the system inertia term, D i (i=1,2,3) is the system damping term, x=[x,y,ψ,u,v,r] T is the system state of the unmanned ship, x0 is the current t k The state of the unmanned ship at the moment, u=[X,N] T is the control input of the unmanned ship. The unmanned ship platform is as follows Figure 5 As shown;

[0073] Step 3.2: Build t k Objective function of the moment model predictive control constrained optimization problem Where N p is the forecast time domain of the plan, x0 is t k The actual state of the unmanned ship at the moment, Q and R are weight matrices, x r (i) is the safe state trajectory X obtained in step 2.4 r The i-th state of is the error of the predicted state (the predicted state With reference state x r (·)), is the control input for prediction;

[0074] Step 3.3: Initialize the model parameters M of the unmanned ship i (i=1, 2, 3) and D i(i=1, 2, 3), set the state error weight matrix Q and control input weight matrix R of the model predictive control constrained optimization problem, where M1=493.77, M2=455.81, M3=55.81, D1=29.23, D2=2173.70, D3=17.70, Q=diag([20, 20, 0.1, 0.1, 0.1]), R=diag([0.1, 0.001]);

[0075] Step 3.4: Construct the system state constraints of the model predictive control constrained optimization problem based on the actual physical constraints of the unmanned ship system and control input constraints in is the system state constraint set, is the control input constraint set, where

[0076] Step 3.5: Expand the obstacle obtained in step 2.1 Constructing the obstacle avoidance constraints for the model predictive control constrained optimization problem in The predicted position of the unmanned ship, For expansion obstacles The center of the circle;

[0077] Step 3.6: Based on the unmanned ship dynamic model obtained in step 3.1, the objective function obtained in step 3.2, the state set and input set obtained in step 3.4, and the obstacle avoidance constraint obtained in step 3.5, construct t k Time-to-moment model predictive control constrained optimization problem:

[0078]

[0079]

[0080]

[0081]

[0082]

[0083] Step 3.7: Solve the constrained optimization problem constructed in step 3.6 and obtain the optimal motion trajectory Sent to the underlying tracking controller, where

[0084] Step 3.8: Low-level tracking controller executes N c After δ=2s, repeat the above steps and design t k+1The adaptive motion planner based on model predictive control is used to plan the movement until the unmanned ship reaches the end point. is the center of the circle and its radius is r a =2m acceptance circle. Under the action of the model predictive control adaptive motion planner, the position trajectory and velocity trajectory of the unmanned ship are as follows Figure 6 As shown in the figure, it can be seen that the designed adaptive motion planner can plan the optimal motion trajectory from the starting point to the target area and meet the actual constraints of the unmanned ship system under complex conditions. The effect of the motion planner adaptively controlling the speed of the unmanned ship according to the obstacle environment is shown in the figure. Figure 7 As shown in the figure, it can be seen that the unmanned boat can gradually reduce its speed when approaching an obstacle, and gradually increase its speed after passing the obstacle; it can accelerate to pass through obstacles without collision, and can gradually reduce its speed when approaching the target area. It can be seen from the embodiment that the designed adaptive motion planner based on model predictive control can not only improve the obstacle avoidance ability and planning performance of the unmanned boat, but also can adaptively control the speed of the unmanned boat according to the situation of the obstacle environment.

[0085] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or substitutions within the technical scope disclosed in the present invention, and these modifications or substitutions should be included in the protection scope of the present invention.

Claims

1. An adaptive motion planning method for an unmanned ship based on model predictive control, characterized in that: include: S1: Generate a collision-free trajectory that satisfies the speed dynamics of the unmanned ship; Step 1.1: Initialize the parameters related to the generated state trajectory: the radius of the unmanned ship r v , obstacle radius r o , the interval distance r c , the radius of the detection range r d , the end point accepts the circle with radius r a , the expected velocity of the state trajectory Sampling period δ, planned prediction time domain N p , the planned control domain N c , historical state trajectory X r (0), the initial state x(0) and initial position x of the unmanned ship at the current initial moment p (0), the initial state x of the state trajectory at the current initial moment r (0), initial position End point Step 1.2: Use the onboard sensors to detect the surrounding environment and obtain the current t k Obstacle information at the moment b (t k ); Step 1.3: Build from To the end The path from the obstacle O b (t k ) to filter out the first obstacle that collides with the path Structure and The circles have the same center and radius (r o +r v +r c ) expansion obstacle Step 1.4: Using Straight and Dilation Obstacles The arc segments are constructed from To the end The collision-free optimal path; Step 1.5: Sample along the collision-free optimal path obtained in step 1.4 to obtain the current t k The state trajectory of no collision at all times and meeting the speed dynamics of the unmanned ship The velocity U of the trajectory r (t) Satisfaction Where T is the time constant of the unmanned ship's speed dynamics, t∈[t k ,t k +(N p -1)δ]; S2: construct an adaptive safe state trajectory within the field of view and detection range of the unmanned ship based on the generated trajectory; Step 2.1: Set the current t k The state of the unmanned ship at the moment is x(t k ), position is x p (t k ), construct from x p (t k ) to the end The path, from the obstacle information O in step 1.2 b (t k ) to filter out the first obstacle o that collides with the path b , structure and o b The circles have the same center and radius (r o +r v ) expansion obstacle Step 2.2: Construct the p (t k ) as the starting point and with the expansion obstacle Tangent ray, state trajectory obtained from step 1.5 Filter out the objects on one side of the ray and the expansion obstacles State trajectory on the opposite side of the center This trajectory is the current t of the unmanned ship. k The safety status trajectory within the field of view at all times, where is the number of states contained in the trajectory; Step 2.3: Construct x p (t k ) is the center of the circle and its radius is r d The detection circle, the state trajectory obtained from step 2.2 Filter out the state trajectory within the detection circle This trajectory is a safe state trajectory that is both within the field of view of the unmanned ship and within the area detected by the unmanned ship. is the number of states contained in the trajectory; Step 2.4: Construct the current t k The number of states at a time is N p Safety status trajectory in The quantity is Status track Taken from t k-1 Historical security status trajectory at time X r (t k-1 ), N all is the number of states contained in the historical state trajectory; Step 2.5: Update t k Historical security status trajectory at each moment in Taken from t k Always be in the safe state track within the field of vision of the unmanned ship and the explored area; S3: Using the constructed adaptive safe state trajectory as a reference trajectory, an adaptive motion planner based on model predictive control is designed.

2. The method for adaptive motion planning of an unmanned ship based on model predictive control according to claim 1, characterized in that: The S3 steps are as follows: Step 3.1: Establish the kinematic and dynamic model of the unmanned ship x(k+1,x0)=f(x(k,x0),u(k)): where [x, y, ψ] T is the position and heading angle of the unmanned ship in the ground coordinate system, [u,v,r] T is the linear velocity and angular velocity of the unmanned ship in the body coordinate system, M i (i=1,2,3) is the system inertia term, D i (i=1,2,3) is the system damping term, x=[x,y,ψ,u,v,r] T is the system state of the unmanned ship, x0 is the current t k The state of the unmanned ship at the moment, u=[X,N] T Provide control input for unmanned vessels; Step 3.2: Build t k Objective function of the moment model predictive control constrained optimization problem Where N p is the forecast time domain of the plan, x0 is t k The actual state of the unmanned ship at the moment, Q and R are weight matrices, x r (i) is the safe state trajectory X obtained in step 2.4 r The i-th state of is the error of the predicted state (the predicted state With reference state x r (·)), is the control input for prediction; Step 3.3: Initialize the model parameters M of the unmanned ship i (i=1,2,3) and D i (i=1,2,3), set the state error weight matrix Q and control input weight matrix R of the model predictive control constrained optimization problem; Step 3.4: Construct the system state constraints of the model predictive control constrained optimization problem based on the actual physical constraints of the unmanned ship system and control input constraints in is the system state constraint set, Enter a set of constraints for control; Step 3.5: Expand the obstacle obtained in step 2.1 Constructing the obstacle avoidance constraints for the model predictive control constrained optimization problem in The predicted position of the unmanned ship, For expansion obstacles The center of the circle; Step 3.6: Based on the unmanned ship dynamic model obtained in step 3.1, the objective function obtained in step 3.2, the state set and input set obtained in step 3.4, and the obstacle avoidance constraint obtained in step 3.5, construct t k Time-to-moment model predictive control constrained optimization problem: Step 3.7: Solve the constrained optimization problem constructed in step 3.6 and obtain the optimal motion trajectory Sent to the underlying tracking controller, where Step 3.8: Low-level tracking controller executes N c After time δ, repeat the above steps and design t k+1 The adaptive motion planner based on model predictive control is used to plan the movement until the unmanned ship reaches the end point. is the center of the circle and its radius is r a within the acceptance circle.

Images