CN110609556A - A collaborative control method for multiple unmanned boats based on LOS navigation method - Google Patents

Abstract

A method for collaborative control of multiple unmanned boats based on the LOS navigation method, comprising the steps of: establishing a basic framework for closed-loop control of multiple unmanned boats according to the control task requirements of the multi-unmanned boats and the information transfer process of the control process; The desired goal or desired route is transformed into the desired speed u _d and desired heading angle ψ _d ; by designing the disturbance observer and sliding mode control algorithm, the control command is converted into the actual control input, so that the unmanned vehicle maintains the desired heading ;Select one of the unmanned boats as the path tracking reference boat to plan its route path, and the other unmanned boats maintain the same navigation path as the path tracking reference boat; real-time detection of the position, heading angle, hull The speed is compared with the expected speed u _d and the expected heading ψ _d , so as to adjust the current sailing speed and sailing heading, forming a complete closed-loop control. The invention can make the unmanned boat travel according to the expected route, and maintain the coordination and consistency of multiple unmanned boats, and has the advantages of safety, stability and reliability.

Description

A collaborative control method for multiple unmanned boats based on LOS navigation method

technical field

The invention belongs to the technical field of unmanned boat control, in particular to a multi-unmanned boat cooperative control method based on the LOS navigation method.

Background technique

For a complete UAV system, the core required mainly includes independent units such as radar, wireless communication, optical perception module, motion control system, and power actuator. At present, there are two trends in the development of unmanned boats. The first trend is the transition from manual remote control to unmanned autonomous driving, and the other trend is to expand from the control of a single unmanned boat to the control of an unmanned boat formation. Under the general trend of automatic driving gradually replacing manual remote control driving, motion control is an important part of it, mainly including the following six research directions:

1) Speed and heading control; 2) Dynamic positioning control; 3) Path tracking; 4) Trajectory tracking; 5) Intelligent planning, obstacle avoidance and navigation; 6) Multi-agent cooperative formation control, etc.

In terms of speed and course control of unmanned boats, there have been a lot of research and literature results at home and abroad, and the main research methods include PID control method, backstepping method, sliding mode control theory, fuzzy control method and adaptive control method. Wait. In future wars, it is difficult to adapt to the complex battlefield environment only by relying on a single unmanned boat to operate autonomously, but a group formation of unmanned boats with effective coordination strategies can better complete the task. Because the ship formation formed by multi-ship cooperation has multiple advantages such as strong fault tolerance, good adaptability and high execution efficiency in maritime search and rescue, marine resource detection and environmental monitoring, and fleet coordinated operations. Therefore, through the necessary research on the control methods of ship formations, the effective control of multi-ship formations has a broad application scope and broad development prospects in the fields of military defense, production and transportation.

Combining the leader-follower formation method with the sliding mode control theory, Fahimi proposed two sliding mode control laws, l_ψ and l_l, to control the relative positions of the ships in the formation. In the design process of its control law, the adverse effects such as changes in motion model parameters and external environmental disturbances have been considered, but its controller can only guarantee the stability of motion under smooth curves, and the following ship is prone to course oscillation at the inflection point. This reduces the reliability of the controller in practical applications.

Contents of the invention

In order to solve the above technical problems, the present invention provides a cooperative control method for multiple unmanned boats based on the LOS navigation method.

In order to solve the above technical problems, the present invention takes the following technical solutions:

A method for cooperative control of multiple unmanned boats based on the LOS navigation method, comprising the following steps:

S1, according to the control task requirements of the unmanned vessel and the information transmission process of the control process, establish the basic framework of the closed-loop control of the unmanned vessel;

S2, converting the expected target or expected route of the UAV into expected speed u _d and expected heading angle ψ _d ;

S3, by designing the disturbance observer and the sliding mode control algorithm, the control instruction is converted into the actual control input, that is, the thrust T of the water jet and the size of the nozzle angle δ are designed respectively, so that the unmanned vehicle maintains the desired course;

S4, using the collaborative control method, select one of the unmanned boats as the path tracking reference boat to plan its route path, and the other unmanned boats maintain the same navigation path as the path tracking reference boat;

S5, detect the position, heading angle, and hull speed of each unmanned boat in real time, feed back to step S2, compare with the expected speed u _d and the expected heading ψ _d , thereby adjusting the current sailing speed and sailing heading, forming a Complete closed loop control.

The step S1 specifically includes:

Establish an undirected graph (v _p ,ε _p ), an undirected graph (v _p ,ε _p ) to represent the communication network between UAVs, where v _p ={1,…,p} is a finite non-empty node set , Represents the edge set, node i corresponds to unmanned boat i, node j corresponds to unmanned boat j, edge (i,j)∈ε _p means that unmanned boat i and unmanned boat j can get each other’s messages, and the number of nodes is equal to The number of unmanned boats, determine one of the nodes as the path-tracking reference node, and the other nodes are respectively connected with the path-tracking reference node, the path-tracking reference node corresponds to the path-tracking reference boat, and the other nodes correspond to other unmanned boats, so that other unmanned boats Both can communicate with the path tracking reference ship.

Described step S2 specifically comprises:

S201, determine the navigation path of the unmanned boat, that is, the desired route, and mark it on the two-dimensional plane;

S202, given the expected speed, using the LOS navigation method to calculate the expected heading angle ψ _d of the unmanned boat at the current position, using the following function to calculate:

Take out two adjacent waypoints from the navigation path, which are P _k (x _k ,y _k ) and P _k+1 (x _k+1 ,y _k+1 ), and the coordinates of the unmanned boat are ( x, y), where ψ _r is the relative angle between the current heading and the expected heading, e is the lateral tracking error, α _k is the direction angle of the navigation path, and the error e is positive when the unmanned boat is on the right side of the expected path. The boat is negative on the left side.

Described step S3 specifically comprises:

S301, select a three-degree-of-freedom unmanned vehicle motion model, and the disturbance functions on each degree of freedom are represented by w ₁ , w ₂ and w ₃ ;

S302, set the sailing speed of the unmanned boat to be a constant speed, and keep relatively constant the corresponding water jet thrust T and nozzle angle δ;

S303, using a sliding mode control algorithm to make the current heading angle of the unmanned boat the same as the expected heading angle or the error is within a set range.

The step S303 is specifically:

Define the true heading of the unmanned boat as ψ and the expected heading as ψ _d , then the corresponding expected heading error is Design the following first-order sliding mode surface S ₁ about the heading error, where the sliding mode variable is denoted by λ, the angular velocity of turning the bow is denoted by r, and the lateral speed and longitudinal speed of the UV are denoted by u and v respectively:

Define the Lyapunov function:

Differentiate both sides of the equation:

The mass coefficient m and damping coefficient d used in it are all known quantities, and k ₁ is the disturbance gain.

Define the desired nozzle angle as δ _d , and satisfy the following conditions:

δ _d = δ ₁ + δ ₂

In the formula, δ ₁ represents the nozzle angle at the desired heading angle, and δ ₂ represents the nozzle angle adjusted by control.

In the formula, k ₂ represents the control parameter; f(S ₁ ) represents the reaching law function of the control algorithm, which is the saturation function sat(S ₁ ) in the present invention, and the mathematical format of the saturation function is:

When S ₁ >κ or S ₁ <-κ:

Let δ=δ _d , and k ₂ >|S ₁ |/(4k ₁ ), then:

Therefore the value of |S ₁ | will gradually converge to be smaller than κ.

When -κ≤S ₁ ≤κ:

When heading angle error When it converges to zero, the control of the desired heading is completed.

Described step S4 specifically comprises:

A connection topology structure is established between each unmanned boat, and each unmanned boat can communicate with the path tracking reference boat, so that the parameter x _i of the target point can be exchanged between the unmanned boats in real time, and adjusted according to the value of the neighbor's target point own value, given the consensus algorithm:

Among them, a _ij is the adjacency matrix of the undirected graph.

Description of drawings

Accompanying drawing 1 is the schematic flow chart of the present invention;

Accompanying drawing 2 is a schematic diagram of the driving path of the unmanned boat;

Accompanying drawing 3 is an undirected graph of many unmanned boats.

Detailed ways

In order to further understand the features, technical means, and specific objectives and functions achieved by the present invention, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

As shown in accompanying drawings 1-3, the present invention discloses a method for cooperative control of multiple unmanned boats based on the LOS navigation method, comprising the following steps:

S1, according to the control task requirements of the unmanned vessel and the information transmission process of the control process, establish the basic framework of the closed-loop control of the unmanned vessel;

S2, converting the expected target or expected route of the UAV into expected speed u _d and expected heading angle ψ _d ;

S3, by designing the disturbance observer and the sliding mode control algorithm, the control instruction is converted into the actual control input, that is, the thrust T of the water jet and the size of the nozzle angle δ are designed respectively, so that the unmanned vehicle maintains the desired course;

S4, using the collaborative control method, select one of the unmanned boats as the path tracking reference boat to plan its route path, and the other unmanned boats maintain the same navigation path as the path tracking reference boat;

S5, detect the position, heading angle, and hull speed of each unmanned boat in real time, feed back to step S2, compare with the expected speed u _d and the expected heading ψ _d , thereby adjusting the current sailing speed and sailing heading, forming a Complete closed loop control.

The step S1 specifically includes:

Establish an undirected graph (v _p ,ε _p ), an undirected graph (v _p ,ε _p ) to represent the communication network between UAVs, where v _p ={1,…,p} is a finite non-empty node set , Represents the edge set, node i corresponds to unmanned boat i, node j corresponds to unmanned boat j, edge (i,j)∈ε _p means that unmanned boat i and unmanned boat j can get each other’s messages, and the number of nodes is equal to The number of unmanned boats, determine one of the nodes as the path-tracking reference node, and the other nodes are respectively connected with the path-tracking reference node, the path-tracking reference node corresponds to the path-tracking reference boat, and the other nodes correspond to other unmanned boats, so that other unmanned boats Both can communicate with the path tracking reference ship.

Described step S2 specifically comprises:

S201, determine the navigation path of the unmanned boat, that is, the desired route, and mark it on the two-dimensional plane;

S202, given the expected speed, using the LOS navigation method to calculate the expected heading angle ψ _d of the unmanned boat at the current position, using the following function to calculate:

Take out two adjacent waypoints from the sailing path, which are P _k (x _k ,y _k ) and P _k+1 (x _k+1 ,y _k+1 ), and the coordinates of the position of the unmanned boat are ( x, y), where ψ _r is the relative angle between the current heading and the expected heading, e is the lateral tracking error, α _k is the direction angle of the navigation path, and the error e is positive when the unmanned boat is on the right side of the expected path, and the unmanned The boat is negative on the left side.

Described step S3 specifically comprises:

S301, select a three-degree-of-freedom unmanned vehicle motion model, and the disturbance functions on each degree of freedom are represented by w ₁ , w ₂ and w ₃ ;

S302, set the sailing speed of the unmanned boat to be a constant speed, and keep relatively constant the corresponding water jet thrust T and nozzle angle δ;

S303, using a sliding mode control algorithm to make the current heading angle of the unmanned boat the same as the expected heading angle or the error is within a set range.

5. The multi-unmanned boat cooperative control method based on the LOS navigation method according to claim 4, wherein the step S303 is specifically:

Define the true heading of the unmanned boat as ψ and the expected heading as ψ _d , then the corresponding expected heading error is Design the following first-order sliding mode surface S ₁ about the heading error, where the sliding mode variable is denoted by λ, the angular velocity of turning the bow is denoted by r, and the lateral speed and longitudinal speed of the UV are denoted by u and v respectively:

Define the Lyapunov function:

Differentiate both sides of the equation:

The mass coefficient m and damping coefficient d used in it are all known quantities, and k ₁ is the disturbance gain.

Define the desired nozzle angle as δ _d , and satisfy the following conditions:

δ _d = δ ₁ + δ ₂

In the formula, δ ₁ represents the nozzle angle at the desired heading angle, and δ ₂ represents the nozzle angle adjusted by control.

In the formula, k ₂ represents the control parameter; f(S ₁ ) represents the reaching law function of the control algorithm, which is the saturation function sat(S ₁ ) in the present invention, and the mathematical format of the saturation function is:

When S ₁ >κ or S ₁ <-κ:

Let δ=δ _d , and k ₂ >|S ₁ |/(4k ₁ ), then:

Therefore the value of |S ₁ | will gradually converge to be smaller than κ.

When -κ≤S ₁ ≤κ:

When heading angle error When it converges to zero, the control of the desired heading is completed.

Described step S4 specifically comprises:

A connection topology structure is established between each unmanned boat, and each unmanned boat can communicate with the path tracking reference boat, so that the parameter x _i of the target point can be exchanged between the unmanned boats in real time, and adjusted according to the value of the neighbor's target point own value, given the consensus algorithm:

Among them, ai _j is the adjacency matrix of the undirected graph.

The position, heading angle, hull speed and other information of the unmanned boat are collected through ship-borne auxiliary equipment such as GPS, compass, speed sensor, etc., and the part of information feedback will not be repeated.

Through the navigation control simulation, it can be concluded that the LOS guidance algorithm invented in this paper has a good control effect on the curve path tracking. It should be emphasized that the path tracking control method of the present invention, whether it is for straight line path tracking or curved path tracking, separates the heading guidance law from the heading control, and under the joint control of two independent computing units, no one The boat can successfully complete the task of path tracking. The advantages of this method are:

1. By dividing the control tasks into levels, the control process is intuitive and easy to understand, and the performance of the two control units does not affect each other, which is simple and easy to implement during controller debugging;

2. The designed heading guidance law has a simple form and a small amount of calculation, which is easy to transform and apply to real ship path tracking.

It should be noted that the above are only preferred embodiments of the present invention, and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the embodiments, those skilled in the art can still implement the foregoing Modifications to the technical solutions described in the examples, or equivalent replacements for some of the technical features, but within the spirit and principles of the present invention, any modifications, equivalent replacements, improvements, etc., shall be included in the protection of the present invention. within range.

Claims (6)

1. A multi-unmanned-boat cooperative control method based on an LOS navigation method comprises the following steps:

s1, establishing a basic framework of closed-loop control of the unmanned ship according to the control task requirement of the unmanned ship and the information transmission process of the control process;

s2, converting the expected target or expected route of the unmanned ship into the expected speed u_dAnd desired heading angle psi_d；

S3, converting the control instruction into actual control input quantity by designing a disturbance observer and a sliding mode control algorithm, namely respectively designing the water spraying thrust T and the nozzle angle delta to enable the unmanned ship to keep an expected course;

s4, selecting one unmanned ship as a path tracking reference ship to plan the course path by adopting a cooperative control mode, and keeping the navigation path consistent with the path tracking reference ship by other unmanned ships;

s5, detecting the position, heading angle and hull speed of each unmanned ship in real time, feeding back to step S2, and calculating the expected speed u_dAnd desired heading psi_dAnd comparing the current sailing speed and the current sailing heading to form a complete closed-loop control.

2. The LOS navigation-based multi-unmanned-boat cooperative control method according to claim 1, wherein the step S1 specifically comprises:

set up undirected graph (v)_p,ε_p) Undirected graph (v)_p,ε_p) To represent the communication network between unmanned boats, where v_p1, …, p is a finite, non-empty set of nodes,representing an edge set, wherein a node i corresponds to an unmanned ship i, a node j corresponds to an unmanned ship j, and an edge (i, j) is epsilon_pThe unmanned ship i and the unmanned ship j can obtain messages of the other side mutually, the number of the nodes is equal to that of the unmanned ships, one node is determined to be a path tracking reference node, other nodes are communicated with the path tracking reference node respectively, the path tracking reference node corresponds to the path tracking reference ship, and other nodes correspond to other unmanned ships, so that the other unmanned ships can be communicated with the path tracking reference ship in information.

3. The LOS navigation-based multi-unmanned-boat cooperative control method according to claim 2, wherein the step S2 specifically comprises:

s201, determining a navigation path of the unmanned ship, namely a desired route, and marking on a two-dimensional plane;

s202, given an expected navigational speed, calculating an expected heading angle psi of the unmanned ship at the current position by using an LOS navigation method_dThe method is calculated by adopting the following function:

two adjacent path points are taken out from the navigation path, and are respectively P_k(x_k,y_k) And P_k+1(x_k+1,y_k+1) The coordinate of the position of the unmanned ship is (x, y), wherein psi_rIs the relative angle between the current course and the expected course, e is the transverse tracking error, alpha_kFor the sailing path azimuth, the error e is positive for the drone on the right side of the desired path and negative for the drone on the left side.

4. The LOS navigation-based multi-unmanned-boat cooperative control method according to claim 3, wherein the step S3 specifically comprises:

s301, selecting a three-degree-of-freedom unmanned ship motion model, wherein the disturbance function in each degree of freedom is w₁、w₂And w₃Represents;

s302, setting the sailing speed of the unmanned ship to be a constant speed, and keeping the corresponding water spraying thrust T and the corresponding nozzle angle delta to be relatively constant;

s303, the current heading angle of the unmanned ship is the same as the expected heading angle or the error is in a set range by adopting a sliding mode control algorithm.

5. The LOS navigation-based multi-unmanned-boat cooperative control method according to claim 4, wherein the step S303 specifically comprises:

defining the real heading of the unmanned boat as psi and the expected heading as psi_dThen the corresponding expected heading error isDesigning a first-order slip form surface S1 for the heading error, wherein the slip form variable is shown by lambda, the angular speed of turning the bow is shown by r, and the transverse speed and the longitudinal speed of the unmanned boat are respectively shown by u and v:

defining the Lyapunov function:

the equal two sides are respectively derived:

the mass coefficient m and the damping coefficient d used in the formula are known quantities,is the derivative of V, k₁Is the perturbation gain;

defining the desired nozzle angle as delta_dAnd the following conditions are satisfied:

δ_d＝δ₁+δ₂

in the formula of₁Indicating the nozzle angle, delta, at the desired heading angle₂Indicating the nozzle angle as adjusted by the control.

In the formula k₂Represents a control parameter; f (S)₁) An approximation law function representing a control algorithm; f (S)₁) Set as the saturation function sat (S)₁) The mathematical format of the saturation function is:

when S is₁>Kappa or S₁<- κ is:

let δ become δ ═ δ_dAnd k is₂>|S₁|/(4k₁) And then:

therefore | S₁The | value will gradually converge to less than κ;

when-kappa.ltoreq.S₁When kappa is less than or equal to kappa:

when the error of the heading angleAnd when the heading is converged to zero, the control of the expected heading is finished.

6. The LOS navigation-based multi-unmanned-boat cooperative control method according to claim 5, wherein the step S4 specifically comprises:

a communication topological structure is established among all unmanned boats, and all unmanned boats can be in information communication with a path tracking reference boat, so that the unmanned boats can exchange parameters x of target points in real time_iAnd adjusting the value of the target point according to the value of the neighbor target point to provide a consistency algorithm:

wherein,a_ijis a contiguous matrix of an undirected graph.