Unmanned ship dynamic obstacle avoidance control system and method based on stochastic nonlinear model predictive control

Abstract

The invention discloses an unmanned ship dynamic obstacle avoidance control system and method based on stochastic nonlinear model predictive control, and adopts a control structure of a planning layer and a control layer to solve the problem of dynamic obstacle avoidance of an unmanned ship in a complex obstacle environment. The planning layer is based on a random nonlinear model predictive control technology, and firstly, random modeling is carried out on the unmanned ship and dynamic obstacles encountered by the unmanned ship in the sailing process; designing obstacle avoidance switching conditions according to the whole actual obstacle avoidance process; in an obstacle avoidance random nonlinear model prediction planning control algorithm, uncertainty propagation of a dynamic obstacle and an unmanned ship in a whole prediction time domain is evaluated through unscented transformation, then obstacle avoidance constraint is described in a chance constraint mode, a chance constraint expectation operator transformation algorithm and a unit jump function approximation algorithm are designed, and a chance constraint expectation operator transformation algorithm and a unit jump function approximation algorithm are designed. Opportunity constraints are converted into deterministic constraints, and then a nonlinear optimization problem is constructed for planning. And the control layer adopts a PI D algorithm to track a planning value so as to realize dynamic obstacle avoidance of the unmanned ship.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned ship dynamic obstacle avoidance control, and relates to an unmanned ship dynamic obstacle avoidance control system and method based on stochastic nonlinear model predictive control. Background Art

[0002] Unmanned ships can autonomously perform various tasks in complex environments, and thus have received increasing attention. The research on unmanned ships involves various technologies such as communication technology, navigation and guidance technology, sensor technology, and control technology. Among them, motion control technologies such as target tracking, path tracking, and trajectory tracking have become popular research technologies in the field of unmanned ships. In reality, in ship accident investigations, accidents caused by collisions account for a high proportion. In order to ensure the normal navigation of unmanned ships in complex dynamic environments, it is very necessary to conduct research on unmanned ship obstacle avoidance.

[0003] Unmanned ships may encounter various obstacles during normal navigation. Compared with static obstacles, it is more difficult to perform real-time obstacle avoidance for dynamic obstacles. Existing research on unmanned ship obstacle avoidance has increasingly focused on dynamic obstacle avoidance. Currently, methods used for unmanned ship dynamic obstacle avoidance include artificial potential field method, velocity obstacle method, dynamic window method, line-of-sight guidance algorithm, and intelligent algorithms, etc. Although these methods can achieve the effect of obstacle avoidance, because they do not have the ability to predict the unmanned ship and dynamic obstacles for a long time, and because generally the attitude change of the unmanned ship is relatively slow, algorithms with predictive ability have more advantages in dynamic obstacle avoidance. Model predictive control has received increasing attention from more and more researchers due to its inherent predictability and the convenience of dealing with constraints.

[0004] When researchers use model predictive control to design an unmanned ship dynamic obstacle avoidance scheme, they generally assume that the dynamic obstacle is in uniform linear motion with a constant speed magnitude and direction within the prediction time domain. However, in fact, the dynamic obstacle may accelerate, decelerate, and turn, and the uniform linear motion assumption is not accurate. In other words, there is uncertainty in the position of the dynamic obstacle. On the other hand, due to factors such as uncertain state estimation caused by limited sensor accuracy, unmanned ship modeling errors, and random disturbances, the state of the unmanned ship also has uncertainty. Therefore, the stochastic model predictive control technology based on probability is a very promising technology to solve such problems.

[0005] Considering that usually before performing a task, an unmanned ship will plan a global path connecting the starting point and the target point based on known information such as obstacles, and then control the unmanned ship to track this global path. However, this global path does not consider dynamic obstacles and sudden static obstacles. Therefore, when the unmanned ship is tracking the global path, it is necessary to perform local obstacle avoidance adjustment based on the real-time information of the obstacle perception sensor. The present invention designs a series of methods such as a local obstacle avoidance planning controller to implement a dynamic obstacle avoidance control system for an unmanned ship based on stochastic nonlinear model predictive control. Summary of the Invention

[0006] Considering the above problems, the present invention designs a dynamic obstacle avoidance control system and method for an unmanned ship based on stochastic nonlinear model predictive control to ensure that the unmanned ship can avoid dynamic obstacles in real time and navigate normally in a dynamic obstacle environment. First, obtain the real-time obstacle state information through a series of obstacle perception algorithms; determine whether the unmanned ship starts to avoid obstacles through the obstacle avoidance switching condition; then based on the stochastic nonlinear model predictive control technology, design the dynamic obstacle avoidance chance constraint for the unmanned ship, and through the chance constraint expectation operator transformation algorithm and the jump function approximation algorithm, transform the obstacle avoidance chance constraint into a deterministic constraint, and design a cost function to plan the optimal obstacle avoidance speed and angular velocity in real time. Then use a PID controller to track the optimal obstacle avoidance speed and angular velocity, so as to finally realize the dynamic obstacle avoidance control of the unmanned ship.

[0007] To achieve the above object, the present invention is implemented by adopting the following technical solutions:

[0008] A dynamic obstacle avoidance control system for an unmanned ship based on stochastic nonlinear model predictive control, characterized in that: the dynamic obstacle avoidance control system for the unmanned ship includes an obstacle perception module, a planning layer, a control layer and the unmanned ship; the planning layer is composed of an obstacle avoidance switching module, an obstacle avoidance stochastic nonlinear model predictive planning controller and a path tracking planning controller; the control layer is composed of a speed tracking controller; wherein:

[0009] The planning layer transmits the real-time obstacle state information and the real-time state information of the unmanned ship to the obstacle avoidance switching module;

[0010] The obstacle avoidance switching module judges and selects an obstacle avoidance stochastic nonlinear model predictive planning controller or a path tracking planning controller according to the obstacle avoidance switching condition;

[0011] The planning layer obtains the real-time optimal speed planning value and angular velocity planning value of the unmanned ship according to the obstacle avoidance stochastic nonlinear model predictive planning controller or the path tracking planning controller and transmits them to the control layer;

[0012] The speed tracking controller in the control layer adopts the PID algorithm, and calculates the forward driving force and steering driving torque of the unmanned ship based on the speed planning value of the unmanned ship, the planning value of the angular velocity, and the actual state of the unmanned ship;

[0013] The unmanned ship will perform real-time control according to the forward driving force and steering driving torque.

[0014] Furthermore, the obstacle perception module obtains the dynamic obstacle state process according to the following steps, including:

[0015] Obtain the point cloud information of a single dynamic obstacle through clustering algorithms such as K-means, and then obtain its size and center position information through an elliptical envelope;

[0016] Calculate the real-time center position information of a single dynamic obstacle through the extended Kalman filter algorithm to obtain the information of the position, speed, and motion direction of the obstacle.

[0017] Furthermore, the obstacle avoidance switching module determines and selects the random nonlinear model predictive planning controller or the path tracking planning controller according to the obstacle avoidance switching conditions, including:

[0018] Establish the obstacle avoidance reaction safety distance D oas , and its expression is:

[0019] D oas =max{N p ·Δt·(v o +u d ),D m}

[0020] where max{·} represents the maximum value function, Np is the prediction horizon, Δt is the sampling time, v o is the motion speed of the dynamic obstacle, u d is the desired forward speed of the unmanned ship, and D m represents the default safety distance;

[0021] Establish the obstacle avoidance switching conditions for determining and selecting the random nonlinear model predictive planning controller or the path tracking planning controller:

[0022]

[0023] where D is the closest distance between the unmanned ship and the obstacle, Δθ is the difference in the included angle between the vector angle from the unmanned ship to the obstacle and the vector angle from the unmanned ship to the target point, θ m is the switching minimum included angle parameter, and ∪ represents the logical "or", and either of the two conditions is satisfied.

[0024] Furthermore, the process by which the obstacle avoidance stochastic nonlinear model predictive programming controller obtains the real-time optimal speed programming value and angular velocity programming value of the unmanned ship includes:

[0025] 401. Stochastic Nonlinear Model of Dynamic Obstacles and Stochastic Nonlinear Prediction Model of Unmanned Ship

[0026] According to the position, speed, and motion direction of the obstacle, assuming that the acceleration α and angular velocity β of the dynamic obstacle in the prediction time domain follow a normal distribution with a mean of 0, the following stochastic nonlinear model of the dynamic obstacle is constructed:

[0027]

[0028] where x o and y o are the positions of the obstacle, v o and θ o are the speed and orientation of the obstacle, jointly constituting the dynamic obstacle state;

[0029] The stochastic nonlinear prediction model of the unmanned ship is as follows:

[0030]

[0031] where x p represents the predicted state vector, x p = [x y ψ v] T are the coordinates, heading angle, and lateral speed of the unmanned ship respectively, u p represents the control vector, u p = [u r] T are the forward speed and yaw angular velocity respectively, w p represents the prediction time domain state uncertainty vector;

[0032] 402. Uncertainty Propagation of Dynamic Obstacles and Unmanned Ship in the Prediction Time Domain:

[0033] Predict the states of dynamic obstacles and unmanned ships in multiple steps and evaluate their covariance through unscented transformation, that is, the uncertainty propagation of the states, namely: The selection methods of 2N Sigma points and weights are as follows:

[0034]

[0035] where, s x,h is S the h-th column of x, h ∈ {1, 2, …, N - 1, N}, g ∈ {N + 1, N + 2, …, 2N - 1, 2N};

[0036] Calculate the mean \(m(k)\) and covariance \(P(k)\) of the predicted state from the Sigma points mapped by the non - linear model;

[0037] Assume that the obstacle state at the \(k\) - th step follows a normal distribution with mean \(m(k)\) and variance \(P(k)\) to evaluate the uncertainty of the dynamic obstacle and the unmanned ship in the entire prediction time domain;

[0038] 403. Obstacle Avoidance Opportunity Constraint of Unmanned Ship and Its Transformation

[0039] From \(m(k)\) and \(P(k)\), the two - dimensional random variable \(p o,i (k)\) of the position of the \(i\) - th dynamic obstacle and the two - dimensional random variable \(p(k)\) of the position of the unmanned ship within the prediction time domain can be obtained; the obstacle avoidance opportunity constraint of the unmanned ship at the \(k\) - th step within the prediction time domain is:

[0040]

[0041] where \(E i is the semi - major axis length of the \(i\) - th dynamic obstacle, \(L\) is the length of the unmanned ship, \(D a is the obstacle avoidance distance margin parameter, \(\delta\) is the collision probability threshold, and \(N o is the number of dynamic obstacles that need to be avoided;

[0042] The chance constraint needs to be transformed into a deterministic constraint. Let \(x p and \(u p be abbreviated as \(x\) and \(u\). The transformation algorithm is as follows:

[0043] Define the function Then the chance constraint becomes:

[0044]

[0045] where represents the expectation operator, and \(H(\cdot)\) is the unit step function, defined as follows:

[0046]

[0047] For the calculation of the expectation, the following formula is used:

[0048]

[0049] The \(H(\cdot)\) function is a step function and is replaced by the following approximation function:

[0050]

[0051] where \(atan(\cdot)\) is the arctangent function, or it can also be replaced by the following approximation function:

[0052] U 2(a, b, G k,i (x, u)) = 0.5tanh(a·G k,i (x, u) + b) - 0.5tanh(b) + 1

[0053] where tanh(·) is the hyperbolic tangent function. The functions U 1 and U 2 are both continuous, monotonically increasing, bounded above, always greater than the unit step function H(·) and always greater than 0, and the approximation degree can be adjusted according to the parameters a and b;

[0054] Finally, the chance constraint is transformed into the following deterministic constraint that can be directly calculated by the optimization solver:

[0055]

[0056] 404. Right-turning tendency obstacle avoidance cost function

[0057] The right-turning tendency obstacle avoidance cost function included in the overall cost function is:

[0058]

[0059] where ropt(u) is the right-turning tendency function, u t+k is the decision variable at the k-th step within the prediction time domain at time t, u d is the desired speed, R 2 is the planned value weight matrix, ropt(u) = [u(1) fal(u(2))] T , and fal(x) is a non-linear function, and its expression is:

[0060]

[0061] k c is the right-turning cost gain, 0 < k c < 1, and k c takes a relatively small value;

[0062] 405. Formulation of the obstacle avoidance stochastic non-linear model predictive planning control problem

[0063] Construct the cost function J(u) by minimizing the control energy consumption, the deviation from the target point, and the right-turning tendency obstacle avoidance;

[0064] Construct the constraints of the model predictive control problem according to the internal constraints and external obstacle avoidance constraints caused by the finiteness of the power that the actuator of the unmanned ship can provide;

[0065] The obstacle avoidance stochastic non-linear model predictive planning control problem is formulated as the following non-linear optimization problem:

[0066] min J(u)

[0067] such that x p,t+k+1|t = f p (x p,t+k|t , u p,t+k|t )

[0068] x p,t|t = x p,t

[0069] u p,t+k|t = u p,t+k

[0070] u min ≤ u p,t+k ≤ u max

[0071]

[0072] where u min and u max are the minimum and maximum values of the planned values. Solving the above non - linear optimization problem yields the optimal forward speed and angular velocity for obstacle avoidance.

[0073] Furthermore, the speed tracking controller adopts the PID algorithm and calculates the forward driving force and steering driving torque of the unmanned boat based on the planned value of the unmanned boat speed, the planned value of the angular velocity, and the actual motion value of the unmanned boat, including:

[0074] The speed tracking controller receives the optimal speed and angular velocity information obtained from the planning controller, as well as the real - time speed and angular velocity information of the unmanned boat, and uses the PID controller to obtain the driving force τ u and the steering torque τ u ;

[0075] Furthermore, the unmanned boat will perform real - time control according to the forward driving force and the steering driving torque, including:

[0076] The unmanned boat will receive the forward driving force and the steering driving torque obtained from the speed tracking controller, and then perform real - time control according to the forward driving force and the steering driving torque.

[0077] A dynamic obstacle avoidance control method for an unmanned boat based on stochastic non - linear model predictive control, which uses the system mentioned in the above technical solution to control the dynamic obstacle avoidance of the unmanned boat, including:

[0078] The planning layer transmits the real - time obstacle state information and the real - time state information of the unmanned boat to the obstacle avoidance switching module;

[0079] The obstacle avoidance switching module judges and selects a stochastic non - linear model predictive planning controller or a path tracking planning controller according to the obstacle avoidance switching conditions;

[0080] The planning layer transmits the real-time optimal speed planning value and angular velocity planning value of the unmanned ship to the control layer according to the stochastic nonlinear model predictive planning controller or the path tracking planning controller; where: the stochastic nonlinear model predictive planning controller is:

[0081] minJ(u)

[0082] s.t.x p,t+k+1|t =f p (x p,t+k|t ,u p,t+k|t )

[0083] x p,t|t =x p,t

[0084] u p,t+k|t =u p,t+k

[0085] u min ≤u p,t+k ≤u max

[0086]

[0087] The speed tracking controller uses the PID algorithm to calculate the forward driving force and steering driving torque of the unmanned ship based on the speed planning value, angular velocity planning value of the unmanned ship and the actual motion value of the unmanned ship;

[0088] The unmanned ship will perform real-time control according to the forward driving force and steering driving torque, so as to realize the whole process of dynamic obstacle avoidance of the unmanned ship.

[0089] Beneficial effects

[0090] (1) The obstacle avoidance switching condition is proposed, which conforms to the whole process of unmanned ship obstacle avoidance.

[0091] (2) The uncertainties of the dynamic obstacle state and the unmanned ship's own state in the unmanned ship obstacle avoidance scenario are considered, and the uncertainties of the propagated dynamic obstacle state and the predicted state of the unmanned ship are effectively evaluated.

[0092] (3) The obstacle avoidance probability constraint is transformed into an inequality constraint described by a deterministic expectation operator, and it is transformed into an inequality constraint through the unscented transformation and the jump function approximation method, which is convenient for direct calculation by a nonlinear optimization solver.

[0093] (4) The design of the obstacle avoidance cost function conforms to the International Regulations for Preventing Collisions at Sea, and the right-turning obstacle avoidance strategy is closer to the actual obstacle avoidance situation. Description of the drawings

[0094] Figure 1It is a framework diagram of the dynamic obstacle avoidance algorithm for an unmanned ship.

[0095] Figure 2 It is a schematic diagram of the state of the unmanned ship and the relative position of the elliptical obstacle in the coordinate system.

[0096] Figure 3 It is a schematic diagram of the obstacle avoidance switching of the unmanned ship.

[0097] Figure 4 It is the approximation effect diagram of the jump function by the approximate function. Specific implementation manners

[0098] In view of the need for the unmanned ship to avoid dynamic obstacles in real time during navigation, the present invention designs a dynamic obstacle avoidance control system and method for an unmanned ship based on stochastic nonlinear model predictive control. The system is based on a twin-propeller underactuated unmanned ship platform. The following further elaborates on the specific implementation manners in conjunction with the accompanying drawings.

[0099] The present invention provides a dynamic obstacle avoidance control system and method for an unmanned ship based on stochastic nonlinear model predictive control. Aiming at the dynamic obstacle avoidance problem of the unmanned ship in a complex obstacle environment, the present invention adopts the control structure of the general planning layer and control layer of the unmanned ship. The planning layer uses stochastic nonlinear model predictive control technology, and the control layer adopts the PID algorithm. First, a stochastic model is established for the unmanned ship and the dynamic obstacles it encounters during navigation. Then, the obstacle avoidance switching conditions are designed according to the actual whole process of the unmanned ship avoiding obstacles. In the obstacle avoidance stochastic nonlinear model predictive planning control algorithm, the uncertainty propagation of the dynamic obstacles and the unmanned ship in the entire prediction time domain is evaluated through unscented transformation. Then, the obstacle avoidance constraints are described in the form of chance constraints, and the chance constraint expectation operator transformation algorithm and the unit jump function approximation algorithm are designed to transform the chance constraints into deterministic constraints, which is convenient for the nonlinear optimization solver to calculate. As Figure 1 shown, the dynamic obstacle avoidance control system of the unmanned ship includes an obstacle perception module, a planning layer, a control layer and the unmanned ship. The planning layer consists of an obstacle avoidance switching module, an obstacle avoidance stochastic nonlinear model predictive planning controller and a path tracking planning controller. The control layer consists of a speed tracking controller. Among them:

[0100] The planning layer transmits the real-time obstacle state information and the real-time state information of the unmanned ship to the obstacle avoidance switching module.

[0101] The obstacle avoidance switching module selects the obstacle avoidance stochastic nonlinear model predictive planning controller or the path tracking planning controller based on the judgment of the obstacle avoidance reaction safety distance.

[0102] The planning layer obtains the real-time optimal speed planning value and angular velocity planning value of the unmanned ship according to the obstacle avoidance stochastic nonlinear model predictive planning controller or the path tracking planning controller and transmits them to the control layer.

[0103] The speed tracking controller of the control layer adopts the PID algorithm, and calculates the forward driving force and steering driving torque of the unmanned ship based on the speed planning value of the unmanned ship, the planning value of the angular velocity, and the actual state of the unmanned ship;

[0104] The unmanned ship will perform real-time control according to the forward driving force and steering driving torque. The specific content is as follows:

[0105] 1. Algorithm framework for dynamic obstacle avoidance control of unmanned ship

[0106] The algorithm framework of the dynamic obstacle avoidance control system of the unmanned ship based on stochastic nonlinear model predictive control is as Figure 1 shown.

[0107] The unmanned ship tracks a given reference path in an obstacle-free environment. When obstacle perception sensors such as lidar detect obstacles in the surrounding environment, the size, position, motion direction, speed and other state information of the obstacles are obtained in real time through a series of obstacle perception algorithms including clustering algorithms, envelope algorithms, extended Kalman filters or cubature Kalman filters, and the covariance of the state information is obtained. Considering randomness, a dynamic obstacle model is built, and this uncertainty is described by a Gaussian distribution, with the mean being the state information and the variance being the covariance of the state information. The information of the obstacle state is sent to the obstacle avoidance switching condition module and the obstacle avoidance stochastic nonlinear model predictive planning controller. In the obstacle avoidance switching condition module, it is comprehensively judged whether to enter the obstacle avoidance stochastic nonlinear model predictive planning controller according to the reference path, the dynamic obstacle state, and the unmanned ship state. If not, the path tracking planning controller is continued to be executed, otherwise the obstacle avoidance stochastic nonlinear model predictive planning controller is executed according to the dynamic obstacle state and the unmanned ship state information. The real-time optimal speed and angular velocity information are obtained by using the obstacle avoidance stochastic nonlinear model predictive planning controller or the path tracking planning controller, and then sent to the speed tracking controller for tracking control. This controller uses the PID algorithm to obtain the driving force and torque for the received speed and angular velocity planning values and the real ship motion information, and the unmanned ship performs real-time control according to the forward driving force and steering driving torque, so as to realize the whole process of dynamic obstacle avoidance of the unmanned ship.

[0108] 2. Algorithm design

[0109] 2.1 Obstacle perception algorithm and dynamic obstacle randomness modeling

[0110] During the navigation of an unmanned ship, the dynamic obstacles encountered are generally other ships that are also in navigation. The dynamic obstacle avoidance algorithm needs to obtain the size and shape of the obstacles, as well as real-time information such as position and speed. In the present invention, a two-dimensional lidar is used as the obstacle perception sensor. For the real-time point cloud data obtained by the lidar, first, single dynamic obstacle point cloud information can be obtained through clustering algorithms such as K-means. The single dynamic obstacle point cloud information can obtain its size, shape, and central position information through envelope fitting. Without loss of generality, the obstacle is represented by an ellipse. The schematic diagram of the relative position between the obstacle and the unmanned ship is as shown in Figure 2 shown. Then, the real-time central position information of a single dynamic obstacle obtained by clustering and envelope fitting is used to obtain the state information such as the position, speed, and motion direction of the obstacle in real time through the extended Kalman filter or cubature Kalman filter algorithm, and further obtain the covariance of the state information. Through the above series of obstacle perception algorithms, the size, state, and covariance of the state of the dynamic obstacle can be obtained. This part belongs to the uncertainty of the initial state in the prediction algorithm.

[0111] The covariance of the above dynamic obstacle state is part of the measurement noise. In addition, considering that the dynamic obstacle may accelerate, decelerate, and turn in the prediction time domain, that is, there is process noise, so a model considering randomness is established. Assuming that the acceleration α and angular velocity β of the dynamic obstacle follow a Gaussian distribution with a mean of 0, the following dynamic obstacle random nonlinear model is established:

[0112]

[0113] where x o and y o are the positions of the obstacle, v o and θ o are the speed and orientation of the obstacle, which together constitute the state of the dynamic obstacle. Let the sampling time be Δt, and the forward Euler method is used to discretize Equation (1). The discretized model is:

[0114]

[0115] It should be noted that static obstacles can be regarded as dynamic obstacles with a speed of 0. Therefore, the method of the present invention is also applicable to static obstacles.

[0116] 2.2 Randomness Model of Unmanned Ship

[0117] Considering factors such as uncertain state estimation caused by limited sensor accuracy, modeling errors of unmanned ships, and random disturbances, the state of the unmanned ship also has uncertainty. The following form of the unmanned ship random nonlinear model is established:

[0118]

[0119] Among them, \(x\) represents the state vector of the unmanned ship, \(x = [x\ y\ \psi\ u\ v\ r]\) T They are respectively the coordinates, heading angle, forward velocity, lateral velocity, and yaw angular velocity of the unmanned ship in the XOY coordinate system. The coordinate system and the state of the unmanned ship are as Figure 2 shown. \(u\) represents the control input vector of the unmanned ship. The research object of this invention is an underactuated unmanned ship, and there is no lateral thrust. \(u = [\tau\) u \(\tau\) r T They are respectively the forward thrust and the turning moment. \(w\) represents the state uncertainty vector of the unmanned ship, \(w = [w\) x \(w\) y \(w\) ψ \(w\) u \(w\) v \(w\) r T They are respectively the uncertainties corresponding to each state variable. This invention assumes that the uncertainty is Gaussian noise with a mean of 0. The specific expression of Equation (3) used in this invention is:

[0120]

[0121] Among them, \(m\) 1 , \(m\) 2 , \(m\) 3 are the added mass in surge, added mass in sway, and added moment of inertia. \(m\) 12 = \(m\) 1 - \(m\) 2 , \(d\) 1 , \(d\) 2 , \(d\) 3 are the hydrodynamic damping coefficient in the longitudinal direction, hydrodynamic damping coefficient in the lateral direction, and hydrodynamic damping coefficient in the turning direction.

[0122] In addition, in the stochastic nonlinear model predictive control algorithm, with the forward velocity \(u\) and the yaw angular velocity \(r\) as the control variables, the stochastic nonlinear prediction model of the unmanned ship is:

[0123]

[0124] Among them, \(x\) p = \([w\ y\ \psi\ v]\) T represents the state vector, \(u\) p = \([u\ r]\) T represents the control vector, \(w\) p = \([w\) x \(w\) y \(w\) ψ \(w\) v T represents the prediction horizon state uncertainty vector. The specific form of Equation (5) is: ​​​

[0125]

[0126] Considering the low accuracy of the forward Euler discretization method, to improve the discretization accuracy of the nonlinear model (5), the fourth-order Runge-Kutta method is used to obtain the discrete stochastic nonlinear prediction model of the unmanned ship as follows:

[0127]

[0128] 2.3 Obstacle avoidance switching conditions of the unmanned ship

[0129] Considering the characteristics of environmental obstacle perception sensors such as lidar and the entire obstacle avoidance process, the obstacle avoidance reaction safety distance D is proposed oas , and its expression is:

[0130] D oas = max{Np·Δt·(v o + u d ), D m} (8)

[0131] Among them, max{·} represents the maximum value function, Np is the prediction time domain, u d is the expected forward speed of the unmanned ship, and D m represents the default safety distance.

[0132] The unmanned ship starts to avoid obstacles only when the distance between the unmanned ship and the obstacle decreases to the obstacle avoidance reaction safety distance. The schematic diagram of obstacle avoidance switching is as shown in Figure 3 . The unmanned ship sails towards the target point p g along the blue dotted line. When the distance between the unmanned ship and the obstacle is detected to be less than or equal to the obstacle avoidance reaction safety distance at the p1 position, the path tracking planning controller is switched to the stochastic nonlinear model predictive planning obstacle avoidance controller. When the unmanned ship sails to the p2 point (the green dotted line is the obstacle avoidance trajectory of the unmanned ship), although the distance between the unmanned ship and the obstacle is still less than the obstacle avoidance reaction safety distance, it can be regarded as having successfully avoided the obstacle at this time, and the path tracking planning controller should be switched. Noticing the included angle between the vector from the unmanned ship to the obstacle and the vector from the unmanned ship to the target point at the p2 position, the following switching conditions are designed:

[0133]

[0134] Among them, D is the closest distance between the unmanned ship and the obstacle, Δθ is the included angle between the vector from the unmanned ship to the obstacle and the vector from the unmanned ship to the target point, θ m is the switching minimum included angle parameter, and ∪ represents the logical "or", and either of the two conditions is satisfied.

[0135] 2.4 Stochastic nonlinear model predictive control planning obstacle avoidance of the unmanned ship

[0136] According to the mean value p of the current position of the unmanned ship and the target position p g and the desired speed u d =[u d 0] T and the known obstacle distribution, a discretized path X connecting the current position and the desired position can be obtained through the global path planning algorithm ref . During the process of the unmanned ship tracking the discretized path, when there are no obstacles nearby, the path tracking planning controller is used to normally track the path. When there are obstacles, it switches to the stochastic nonlinear model predictive obstacle avoidance planning controller for local planning to avoid obstacles. After the obstacle avoidance is completed, it switches back to the path tracking planning controller to continue tracking the reference path. The path tracking planning controller can be designed by methods such as model predictive control and line-of-sight guidance method. The focus of this patent is on local obstacle avoidance planning, so it will not be elaborated here. The design of the obstacle avoidance stochastic nonlinear model predictive planning controller is as follows:

[0137] According to the "International Regulations for Preventing Collisions at Sea", the situations of two ships meeting include meeting, overtaking, crossing, etc. Considering the situations of two ships meeting comprehensively, the right-turn obstacle avoidance strategy is more in line with the actual obstacle avoidance situation. Let ‖x‖ Q =x T Qx represent the weighted quadratic norm of the vector x, and Q is the weight matrix. Then, within the prediction horizon Np, the cost function is:

[0138]

[0139] where u = u p =[u r] T is the decision variable, u t+k is the decision variable at the k-th step within the prediction horizon at time t, p t is the mean value of the position of the unmanned ship at time t, p t+k is the predicted mean value of the position of the unmanned ship at the k-th step within the prediction horizon at time t, ropt(u) is the right-turn tendency function, and fal(x) is a nonlinear function. R 1 , R 2 , R 3 are the obstacle avoidance weight matrix, the planned value weight matrix, and the terminal weight matrix respectively. k c is the right-turn cost gain, 0 < k c < 1, and k c takes a relatively small value.

[0140] Within the prediction horizon, the constraints are divided into external constraints and internal constraints of the unmanned ship. The internal constraints are due to the limited power that the actuators of the unmanned ship can provide. For the sake of reasonable planning, a planned value constraint is added:

[0141] u min ≤ u t+k ≤ umax (12)

[0142] The external constraint specifically refers to the obstacle avoidance constraint in the present invention. The specific implementations of the obstacle avoidance constraint are described in Sections 2.5 and 2.6 below.

[0143] 2.5 Uncertainty Propagation of Dynamic Obstacles and Unmanned Vessels within the Prediction Horizon

[0144] In the stochastic nonlinear model predictive control algorithm, it is necessary to predict the states of dynamic obstacles and unmanned vessels in multiple steps and evaluate their covariance based on Equations (2) and (7), that is, the uncertainty propagation of the states. For the nonlinear dynamics model, the unscented transformation is used to evaluate the uncertainty propagation of the states, which is specifically as follows:

[0145] (1) Obtain the mean m(0) and covariance matrix P(0) of the initial state;

[0146] (2) Generate 2N Sigma points and weights (N is the state dimension):

[0147]

[0148] Where, s x,h is the h-th column of S x , h ∈ {1, 2, …, N - 1, N}, g ∈ {N + 1, N + 2, …, 2N - 1, 2N};

[0149] (3) Propagate all x n (0) through the nonlinear mathematical model, n ∈ {1, 2, …, 2N - 1, 2N}, such as Equation (2) or Equation (7), to obtain x n (k);

[0150] (4) Calculate the mean and variance of the state at the k-th step through the following formula:

[0151]

[0152] Although even if the initial state follows a Gaussian distribution, the distribution after the state uncertainty propagation no longer strictly follows a Gaussian distribution, it can be approximated and described by a Gaussian distribution. The state of the obstacle at the k-th step follows a Gaussian distribution with a mean of m(k) and a variance of P(k).

[0153] 2.6 Opportunity Constraint for Unmanned Vessel Obstacle Avoidance and Its Transformation

[0154] According to Section 2.5, the two-dimensional random variable p of the position of the dynamic obstacle i at the k-th step within the prediction horizon can be obtained from m(k) and P(k). o,i(k) and the two - dimensional random variable p(k) of the unmanned ship's position. Whether the unmanned ship collides with the moving obstacle depends on the distance between p o,i (k) and p(k), as well as the sizes, shapes, and orientations of the dynamic obstacle and the unmanned ship. Without loss of generality and for the sake of simplifying the constraint expression, the obstacle - avoidance constraint for the unmanned ship at the k - th step within the prediction time domain is:

[0155]

[0156] The collision - avoidance constraint is formulated in a probabilistic way, which is the so - called chance constraint. Among them, E i is the semi - major axis length of the i - th dynamic obstacle, L is the length of the unmanned ship, D a is the obstacle - avoidance distance margin parameter, δ is the collision probability threshold, and N o is the number of dynamic obstacles that need to avoid obstacles.

[0157] In the actual algorithm, formula (15) cannot be directly calculated in the optimization solver and needs to be transformed into a deterministic inequality constraint for easy solution. The transformation algorithm is as follows:

[0158] Define the function Then formula (15) can be changed to:

[0159] P k,i =Pr(G k,i (x,u)≥0)≤δ (16) Formula (16) can be further transformed into the following form:

[0160]

[0161] Among them, represents the expectation operator, and H(·) is the unit step function, which is defined as follows:

[0162]

[0163] For the calculation of the expectation of formula (17), the stochastic quadrature formula is generally used, but the result is only accurate when the number of sampling points in the summation part is very large. Given the characteristics of the unscented transform, for the calculation of the expectation of formula (17), we use the following formula:

[0164]

[0165] The selection methods of the sigma points and weights are the same as those in formula (13).

[0166] Although formula (19) can already be calculated, in fact, since the H(·) function is a step function, considering the wide range of gradient - based optimization strategy solvers, it is a feasible strategy to approximate it with a smooth function. Therefore, the following approximation function is designed:

[0167]

[0168] Among them, atan(·) is the arctangent function, or the following approximation function can also be used:

[0169] U 2 (a, b, G k,i (x, u)) = 0.5tanh(a·G k,i (x, u) + b) - 0.5tanh(b) + 1 (21)

[0170] Among them, tanh(·) is the hyperbolic tangent function. The function U 1 and U 2 are both continuous, monotonically increasing, bounded above, always greater than the unit step function H(·) and always greater than 0. The approximation degree can be adjusted according to the parameters a and b. The approximate function approximation effect diagram of the unit step function can be seen in Figure 4 .

[0171] Finally, the chance constraint of formula (15) is transformed into the following deterministic constraint that can be directly calculated by the optimization solver:

[0172]

[0173] Combining Sections 2.4, 2.5, and 2.6, the stochastic nonlinear model predictive programming control problem can be transformed into the following nonlinear optimal control problem:

[0174]

[0175] By solving equation (23) through a nonlinear optimization solver, the real-time optimal forward speed and angular speed can be obtained.

[0176] 2.7 Speed Tracking PID Controller

[0177] The speed tracking controller receives the optimal speed and angular speed information obtained by the planning controller, as well as the real-time speed and angular speed information of the unmanned ship, and uses the PID controller to obtain the driving force τ u and the steering torque τ u . The general situation of the PID controller will not be elaborated here.

[0178] 2.8 Unmanned Ship Control

[0179] The unmanned ship receives the driving force τ u and the steering torque τ u, if a simulation experiment is carried out, the state of the unmanned ship at the next moment is obtained according to the unmanned ship random model in formula (4); if a real ship experiment is carried out, the forward driving force and the steering driving torque are resolved into the motor voltages corresponding to the left and right paddles of the unmanned ship according to the actual unmanned ship:

[0180]

[0181] where F L and F R are the motor voltages corresponding to the left and right paddles of the unmanned ship respectively, k is the actual proportionality coefficient between the driving force and the motor voltage, and d m is the motor shaft distance corresponding to the left and right paddles. After converting F L and F R into PWM signals and then applying them to the unmanned ship for real-time control, the state of the unmanned ship is obtained by sensors such as GPS and IMU, so as to realize the whole process of dynamic obstacle avoidance of the unmanned ship.

[0182] The present invention is not limited to the embodiments described above. The above description of the specific embodiments is intended to describe and illustrate the technical solutions of the present invention. The above specific embodiments are merely illustrative and not restrictive. Without departing from the spirit of the present invention and the scope protected by the claims, those of ordinary skill in the art can make many specific transformations in form under the inspiration of the present invention, and these all fall within the protection scope of the present invention.

Claims

1. A dynamic obstacle avoidance control system for an unmanned ship based on random nonlinear model predictive control, characterized in that: The unmanned boat dynamic obstacle avoidance control system includes an obstacle perception module, a planning layer, a control layer and an unmanned boat; the planning layer is composed of an obstacle avoidance switching module, an obstacle avoidance random nonlinear model prediction planning controller and a path tracking planning controller; the control layer is composed of a speed tracking controller; wherein: The planning layer transmits the real-time obstacle status information and the real-time status information of the unmanned ship to the obstacle avoidance switching module; The obstacle avoidance switching module determines and selects an obstacle avoidance random nonlinear model prediction planning controller or a path tracking planning controller according to the obstacle avoidance switching condition; The planning layer obtains the real-time optimal speed planning value and angular velocity planning value of the unmanned boat according to the obstacle avoidance random nonlinear model prediction planning controller or the path tracking planning controller and transmits them to the control layer; The speed tracking controller of the control layer adopts PID algorithm to calculate the forward driving force and steering driving torque of the unmanned ship based on the planned value of the unmanned ship speed, the planned value of the angular velocity and the actual state of the unmanned ship; The unmanned ship will be controlled in real time according to the forward driving force and the steering driving torque.

2. The unmanned ship dynamic obstacle avoidance control system based on random nonlinear model predictive control according to claim 1, characterized in that: The obstacle perception module obtains the dynamic obstacle state process according to the following steps, including: The point cloud information of a single dynamic obstacle is obtained through clustering algorithms such as K-means, and then its size and center position information are obtained through the elliptical envelope; The extended Kalman filter algorithm is used to calculate the real-time center position information of a single dynamic obstacle to obtain the position, speed, and movement direction of the obstacle.

3. The unmanned ship dynamic obstacle avoidance control system based on random nonlinear model predictive control according to claim 1, characterized in that: The obstacle avoidance switching module determines and selects a random nonlinear model prediction planning controller or a path tracking planning controller according to the obstacle avoidance switching condition, including: Establish obstacle avoidance reaction safety distance D oas , whose expression is: D oas =max{Np·Δt·(v o +u d ),D m } Where max{·} represents the maximum value function, Np is the prediction time domain, Δt is the sampling time, and v o is the speed of the dynamic obstacle, u d is the expected forward speed of the unmanned ship, D m Indicates the default safety distance; Establish the obstacle avoidance switching conditions for judging whether to select the random nonlinear model predictive planning controller or the path tracking planning controller: Where D is the closest distance between the unmanned ship and the obstacle, Δθ is the angle difference between the vector angle from the unmanned ship to the obstacle and the vector angle from the unmanned ship to the target point, and θ m It is the parameter for switching the minimum angle. ∪ represents the logical "or", and either of the two conditions can be met.

4. The unmanned ship dynamic obstacle avoidance control system based on random nonlinear model predictive control according to claim 1, characterized in that: The obstacle avoidance random nonlinear model prediction planning controller obtains the real-time optimal speed planning value and angular velocity planning value of the unmanned ship, including:

401. Random nonlinear model of dynamic obstacles and random nonlinear prediction model of unmanned ships According to the position, speed and direction of movement of the obstacle, assuming that the acceleration α and angular velocity β of the dynamic obstacle in the prediction time domain obey a normal distribution with a mean of 0, the following dynamic obstacle random nonlinear model is constructed: Among them, x o and o is the position of the obstacle, v o and θ o are the speed and orientation of the obstacle, which together constitute the dynamic obstacle state; The random nonlinear prediction model of the unmanned ship is as follows: Among them, x p represents the predicted state vector, x p =[xy ψ v] T They are the coordinates, heading angle, and lateral speed of the unmanned ship, u p represents the control vector, u p =[ur] T are the forward speed and the yaw angular velocity, w p Represents the uncertainty vector of the predicted time domain state; 402. Uncertainty propagation of dynamic obstacles and unmanned ships in the prediction time domain: The states of multi-step dynamic obstacles and unmanned ships are predicted through unscented transformation and their covariance is evaluated, that is, the uncertainty propagation of the state, that is, the selection of 2N Sigma points and weights is as follows: in, YesS x The h-th column of , h∈{1,2,…,N-1,N}, g∈{N+1,N+2,…,2N-1,2N}; The mean m(k) and covariance P(k) of the predicted state are calculated from the Sigma points after nonlinear model mapping; The obstacle state at step k follows a normal distribution with a mean of m(k) and a variance of P(k) to evaluate the uncertainty of the dynamic obstacles and the unmanned ship in the entire prediction time domain. 403、Unmanned boat obstacle avoidance opportunity constraints and their transformation From m(k) and P(k), we can get the two-dimensional random variable p of the position of the dynamic obstacle i in the kth step in the prediction time domain: o,i (k) and the position of the unmanned ship are two-dimensional random variables p(k); the obstacle avoidance opportunity constraint of the unmanned ship in the kth step in the prediction time domain is: Among them, E i is the length of the semi-major axis of the ith dynamic obstacle, L is the length of the unmanned ship, and D a is the obstacle avoidance distance margin parameter, δ is the collision probability threshold, N o is the number of dynamic obstacles that need to be avoided; Chance constraints need to be transformed into deterministic constraints, and x p and u p Abbreviated as x and u, the conversion algorithm is as follows: Defining functions The opportunity constraint then becomes: in, represents the expectation operator, H(·) is the unit jump function, and is defined as follows: To obtain the expected value, the following formula is used: The H(·) function is a jump function, which can be replaced by the following approximation function: Among them, atan(·) is the inverse tangent function, or it can be replaced by the following approximation function: U2(a,b,G k,i (x,u))=0.5tanh(a·G k,i (x,u)+b)-0.5tanh(b)+1 Among them, tanh(·) is the hyperbolic tangent function. Functions U1 and U2 are both continuous, monotonically increasing, have upper bounds, are always greater than the unit jump function H(·) and are always greater than 0. The degree of approximation can be adjusted according to parameters a and b; Finally, the chance constraints are transformed into deterministic constraints that can be directly calculated by the optimization solver as follows:

404. Right Turn Obstacle Avoidance Cost Function The right turn tendency obstacle avoidance cost function included in the overall cost function is: Among them, ropt(u) is the right turn tendency function, u t+k is the decision variable of the kth step in the prediction domain at time t, u d is the expected speed, R2 is the planning value weight matrix, ropt(u)=[u(1)fal(u(2))] T , fal(x) is a nonlinear function, and its expression is: k c is the right turn cost gain, 0 <k c <1, k c Take the smaller value; 405、Statement of the Obstacle Avoidance Stochastic Nonlinear Model Predictive Planning Control Problem The cost function J(u) is constructed based on minimizing the control energy consumption, the deviation from the target point and the right turn tendency to avoid obstacles; According to the internal constraints and external obstacle avoidance constraints caused by the limited power that the actuator of the unmanned ship can provide, the constraints of the model predictive control problem are constructed; The obstacle avoidance stochastic nonlinear model predictive planning control problem can be expressed as the following nonlinear optimization problem: minJ(u) s.t.x p,t+k+1|t =f p (x p,t+k|t ,u p,t+k|t ) x p,t|t =x p,t in p,t+k|t =in p,t+k in min in p,t+k in mx Among them, u min and u max are the minimum and maximum planning values. Solving the above nonlinear optimization problem can obtain the optimal obstacle avoidance forward velocity and angular velocity.

5. The unmanned ship dynamic obstacle avoidance control system based on random nonlinear model predictive control according to claim 1, characterized in that: The speed tracking controller adopts PID algorithm to calculate the forward driving force and steering driving torque of the unmanned ship according to the planned value of the unmanned ship speed, the planned value of the angular velocity and the actual motion value of the unmanned ship, including: The speed tracking controller receives the optimal speed and angular velocity information obtained by the planning controller, as well as the real-time speed and angular velocity information of the unmanned ship, and uses the PID controller to obtain the driving force τ u and steering torque τ u .

6. The unmanned ship dynamic obstacle avoidance control system based on random nonlinear model predictive control according to claim 1, characterized in that: The unmanned ship will be controlled in real time according to the forward driving force and the steering driving torque, including: The unmanned ship will receive the forward driving force and the steering driving torque obtained by the speed tracking controller, and then perform real-time control according to the forward driving force and the steering driving torque.

7. A method for dynamic obstacle avoidance control of an unmanned ship based on random nonlinear model predictive control, characterized in that The method adopts the system of claims 1-6 to control the unmanned boat to dynamically avoid obstacles, comprising: The planning layer transmits the real-time obstacle status information and the real-time status information of the unmanned ship to the obstacle avoidance switching module; The obstacle avoidance switching module determines whether to select a random nonlinear model prediction planning controller or a path tracking planning controller according to the obstacle avoidance switching condition; The planning layer obtains the real-time optimal speed planning value and angular velocity planning value of the unmanned ship according to the random nonlinear model prediction planning controller or the path tracking planning controller and transmits them to the control layer; wherein: the random nonlinear model prediction planning controller is: minJ(u) s.t.x p,t+k+1|t =f p (x p,t+k|t ,u p,t+k|t ) x p,t|t =x p,t in p,t+k|t =in p,t+k in min in p,t+k in max The speed tracking controller uses a PID algorithm to calculate the unmanned ship's speed planning value, angular velocity planning value and unmanned ship's actual motion value to obtain the unmanned ship's forward driving force and steering driving torque; The unmanned boat will be controlled in real time according to the forward driving force and steering driving torque, thereby realizing the whole process of dynamic obstacle avoidance of the unmanned boat.

Images