An improved null-space formation maneuver control method for unmanned surface vehicle escort task

Abstract

The application discloses an improved zero-space formation maneuvering control method for an unmanned ship escort task, which comprises the following steps: according to the motion intention of an unmanned ship formation, the unmanned ship maneuvering process is decomposed, then a motion model is established, a new obstacle avoidance behavior function is designed while the state of an escort target is corrected, a global fusion strategy is adopted to fuse the motion model, and finally the speed and direction are obtained.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of multi-robot formation control, and particularly relates to an improved zero-space formation maneuvering control method for an unmanned surface vehicle escort task. BACKGROUND

[0002] An unmanned surface vehicle is a water surface vehicle without manual operation, which is used to replace personnel to perform water surface tasks, has high intelligent degree and strong maneuverability, and is widely used in marine monitoring, intelligence reconnaissance and the like. The multi-unmanned vehicle formation cooperation technology applied to the unmanned vehicle combines the advantages of single vehicle flexibility and autonomy and group intelligence, can improve the operation efficiency of the unmanned vehicle, greatly expand the application breadth and range, and complete more complex tasks. When the escort target travels in a large area outside the safe water area, it is likely to encounter obstacles. If the formation shape is maintained all the time, a collision with the obstacle may occur, resulting in failure of the escort task. If only the obstacle is considered to be avoided, the formation shape may be destroyed, and the escort task cannot be performed. If both the formation escort task and the obstacle avoidance are performed, the two behaviors need to be performed at the same time, and conflicts may occur when the unmanned vehicle group performs multiple behaviors, resulting in failure of the formation escort task. Therefore, a zero-space behavior fusion algorithm can be used to solve the multi-behavior conflict problem.

[0003] In the paper "Multi-robot Formation Control Based on NSB Method" by Wu Linbo et al., a behavior-based formation control method, NSB (Null Space) method, is used to study the problem of multi-robot formation control. The method is applied to the intelligent soccer robot instantiation platform, and finally the application of the method in the intelligent soccer robot instantiation platform is discussed through simulation experiments.

[0004] In the invention patent "Self-Adaptive Null Space Behavior Fusion Method for Multi-Robot Formation" by Fan Jiajia et al., a self-adaptive null space behavior fusion method for multi-robot formation is disclosed. A gain coefficient is added to the null space algorithm, and the speed obtained by solving is not changed by the change of the working condition environment, has good adaptability, and can effectively control the speed while considering efficiency and performance. The method has significant progress in motion planning.

[0005] In the paper "Autonomous Underwater Vehicle Obstacle Avoidance Strategy Based on Null Space Behavior Method" by Pang Shikun, Liang Xiaofeng et al., the autonomous underwater vehicle obstacle avoidance strategy based on the null space behavior method is studied, and the obstacle avoidance problem of AUV for dynamic and static obstacles is analyzed. Finally, the feasibility of the null space algorithm in the dynamic obstacle avoidance task of underwater robots is verified through simulation experiments.

[0006] However, the traditional null-space behavior fusion method does not consider dynamic escort targets, and after the formation is assembled, the formation lag phenomenon occurs; and because the traditional null-space method adopts a local fusion strategy, that is, when the distance between the robot and the obstacle is less than a certain threshold, the formation escort behavior and the formation obstacle avoidance behavior are executed at the same time, and the formation obstacle avoidance behavior is upgraded to the highest priority, and when the distance between the unmanned ship and the obstacle is greater than the threshold, the formation obstacle avoidance behavior is not executed, and only the formation escort behavior is executed, which can cause the trajectory of the formation to oscillate. SUMMARY

[0007] The purpose of the present application is to provide an improved null-space formation maneuver control method for unmanned ship escort tasks.

[0008] The purpose of the present application is achieved by the following technical solutions:

[0009] An improved null-space formation maneuver control method for unmanned ship escort tasks, the specific steps are as follows:

[0010] Step 1: Decompose the motion process and define the priority of the sub-behavior;

[0011] Step 2: Use the null-space algorithm to establish the motion model; correct the state of the escort target; design the obstacle avoidance behavior function under the strategy of global fusion, so that the unmanned ship executes part of the formation obstacle avoidance behavior before reaching the safe distance, and successfully completes the task of the formation keeping phase;

[0012] Step 3: Use the global fusion strategy to determine the task priority, and use the null-space method to fuse the formation keeping behavior function and the formation obstacle avoidance behavior function to obtain the speed and direction of each unmanned ship;

[0013] Step 4: Output the speed and direction obtained to the motion control center of the unmanned ship to drive the unmanned ship to move forward; determine whether the unmanned ship has reached the target point; if it has reached the target point, the process is ended, if it has not reached the target point, return to step 3.

[0014] Further, the step 1 unmanned ship group divides the formation task into two sub-behaviors: formation keeping and formation obstacle avoidance when executing the formation task.

[0015] Further, the step 2 makes the unmanned ship execute part of the formation obstacle avoidance behavior before reaching the safe distance, and successfully completes the task of the formation keeping phase;

[0016] P is defined as ob = [x ob ,y ob ] T The position coordinates of the obstacle, the following formation collision avoidance behavior function is designed:

[0017]

[0018] wherein, l ob and c ob are adjustable parameters. The Jacobian matrix of σ ob is:

[0019]

[0020] Then:

[0021]

[0022] Let σ d = r ob , then the output of the formation obstacle avoidance behavior function is:

[0023]

[0024] wherein, l ob , c ob and Λ ob are adjustable parameters.

[0025] Further, the state of the escorted target is corrected in the step two:

[0026] The formation of the escorted formation is centered on the escorted target, and the following behavior function is designed:

[0027]

[0028] wherein, P i = [x i , y i ] T is the position coordinate of the ith unmanned ship, and P i,d is the expected position of the ith unmanned ship.

[0029]

[0030] wherein, P t (t) is the correction amount of the position coordinate of the escorted target, P t (t k ) is the current position coordinate of the escorted target, P t (t k-1 ) is the position coordinate of the escorted target at the last time, and λ is the correction gain, and r is the escorting radius.

[0031] The Jacobian matrix is:

[0032] J f = (P i -P i,d ) T

[0033] Then:

[0034]

[0035] Let σ d = 0, the output function of the formation keeping behavior function is:

[0036]

[0037] Wherein, Λ f is the formation keeping behavior gain.

[0038] Further, the step three uses a global fusion strategy to judge the task priority, and uses the null space method to fuse the formation keeping behavior function and the formation obstacle avoidance behavior function, so as to obtain the speed size and direction of each unmanned ship;

[0039] When the distance between the unmanned ship and the obstacle is greater than the threshold value, the formation obstacle avoidance behavior can be partially completed, that is, the formation keeping behavior is taken as the highest priority, the formation obstacle avoidance behavior is taken as the secondary priority, the outputs of the two behavior functions are fused, and the fused behavior function output is obtained:

[0040]

[0041] Wherein, v i,nsb is the expected linear velocity vector defined in the geodetic coordinate system;

[0042] When the distance between the unmanned ship and the obstacle is less than the threshold value, the formation obstacle avoidance behavior is taken as the highest priority, the formation keeping behavior is taken as the secondary priority, the outputs of the two behavior functions are fused, and the fused behavior function output is obtained:

[0043]

[0044] Wherein, ν i,nsb is the expected linear velocity vector defined in the geodetic coordinate system.

[0045] The beneficial effects of the present application are:

[0046] Compared with the traditional null space formation method, in the formation keeping behavior of the present application, the correction gain λ is added, the state of the convoy target is corrected, and the phenomenon of formation lag after the formation is gathered is avoided; under the strategy of global fusion, a new obstacle avoidance behavior function is designed, so that the unmanned ship can perform part of the formation obstacle avoidance behavior before reaching the safe distance, successfully complete the task of the formation keeping phase, and avoid the problem of trajectory shock existing in the traditional null space behavior fusion algorithm. BRIEF DESCRIPTION OF DRAWINGS

[0047] Figure 1This is a flow chart of an improved zero-space formation maneuvering control method for unmanned boat escort missions. DETAILED DESCRIPTION

[0048] The present invention will be further described below with reference to the accompanying drawings.

[0049] Figure 1 This is a flow chart of an improved zero-space formation maneuvering control method for unmanned boat escort missions.

[0050] The technical solution of the present invention is achieved as follows:

[0051] (1) Decomposition of motion process: When the UAV group performs the formation task, the formation task can be decomposed into two simple sub-behaviors: formation maintenance and formation obstacle avoidance.

[0052] (2) Establish the motion model for each sub-behavior:

[0053] Define the position of the i-th unmanned boat

[0054] p i =[x i ,y i ] T

[0055] Then the speed of the i-th unmanned boat is for:

[0056]

[0057] Defining a vector

[0058]

[0059] Define the following behavior function

[0060] σ=f(P)

[0061] P i The derivative is:

[0062]

[0063] in, is the Jacobian matrix associated with the action function.

[0064] According to the closed-loop inverse kinematics principle of singular robustness, in order to complete the task function σ d , the least squares solution of the expected speed of the unmanned boat is:

[0065]

[0066] In order to solve the problem of numerical drift in the process of calculation, the algorithm is improved by using the principle of closed-loop inverse kinematics:

[0067]

[0068] Where Λ is a positive definite gain diagonal matrix.

[0069] The motion model of each behavior is solved according to the motion information of the unmanned ship, and the state of the escorted target is corrected to avoid the phenomenon of formation lag. Since the traditional zero space behavior fusion method does not consider the dynamic escorted target, the phenomenon of formation lag will appear after the formation is gathered, so the state of the escorted target needs to be corrected, in addition, considering that the formation of the escorted formation is a circle with the escorted target as the center, the following behavior function is designed:

[0070]

[0071] Where P i = [x i , y i ] T is the position coordinates of the i-th unmanned ship, and P i,d is the expected position of the i-th unmanned ship.

[0072]

[0073] Where P t (t) is the correction of the position coordinates of the escorted target, P t (t k ) is the current position coordinates of the escorted target, P t (t k-1 ) is the position coordinates of the escorted target at the last time, and λ is the correction gain, and r is the escort radius.

[0074] The Jacobian matrix is:

[0075] J f = (P i -P i,d ) T

[0076] Then:

[0077]

[0078] Let σ d = 0, the output function of the formation keeping behavior function is:

[0079]

[0080] Where Λ f is the formation keeping behavior gain.

[0081] Since the traditional NSB method adopts local fusion strategy, that is, when the distance between the robot and the obstacle is less than a certain threshold, the formation escort behavior and the formation obstacle avoidance behavior are executed at the same time, and the formation obstacle avoidance behavior is upgraded to the highest priority, when the distance between the unmanned ship and the obstacle is greater than the threshold, the formation obstacle avoidance behavior is not executed, only the formation escort behavior is executed. In this section, a kind of obstacle avoidance behavior function is designed under the global fusion strategy, so that the unmanned ship can execute part of the formation obstacle avoidance behavior before reaching the safe distance, and successfully complete the task of formation maneuvering phase.

[0082] Define P ob = [x ob , y ob ] T The position coordinates of the obstacle, the following collision avoidance behavior function is designed:

[0083]

[0084] Where, l ob and c ob are adjustable parameters. The Jacobian matrix of σ ob is:

[0085]

[0086] Then:

[0087]

[0088] Let σ d = r ob , then the output of the formation obstacle avoidance behavior function is:

[0089]

[0090] Where, l ob , c ob and Λ ob are adjustable parameters.

[0091] (3) The zero space method is used to fuse the formation keeping behavior function and the formation obstacle avoidance behavior function:

[0092] Using global fusion strategy, the priority of the task is judged, the zero space method is used to fuse the formation keeping behavior function and the formation obstacle avoidance behavior function, and the speed and direction of each unmanned ship are obtained:

[0093] The traditional null-space method adopts a local fusion strategy, that is, when the distance between the unmanned ship and the obstacle is less than a threshold, the formation escort behavior and the formation obstacle avoidance behavior are executed simultaneously, and the formation obstacle avoidance behavior is upgraded to the highest priority, and when the distance between the unmanned ship and the obstacle is greater than the threshold, the formation obstacle avoidance behavior is not executed, and only the formation escort behavior is executed.

[0094] Unlike the traditional fusion strategy, the application adopts a global fusion strategy, when the distance between the unmanned ship and the obstacle is greater than the threshold, the formation obstacle avoidance behavior can be partially completed, that is, the formation keeping behavior is taken as the highest priority, the formation obstacle avoidance behavior is taken as the secondary priority, the outputs of the two behavior functions are fused to obtain the fused behavior function output:

[0095]

[0096] Wherein v i,nsb is the desired linear velocity vector defined in the geodetic coordinate system.

[0097] When the distance between the unmanned ship and the obstacle is less than the threshold, the formation obstacle avoidance behavior is taken as the highest priority, the formation keeping behavior is taken as the secondary priority, the outputs of the two behavior functions are fused to obtain the fused behavior function output:

[0098]

[0099] Wherein v i,nsb is the desired linear velocity vector defined in the geodetic coordinate system.

[0100] (4) The calculated speed size and direction are output to the motion control center of the unmanned ship to drive the unmanned ship to move forward.

[0101] (5) It is judged whether the unmanned ship reaches the target point. If it reaches, the process ends, and if it does not reach, it returns to step (3).

[0102] In summary, according to the improved null-space formation maneuver control method for unmanned ship escort tasks provided by the application, the problems of formation lag and trajectory oscillation in the formation maneuver process can be solved.

[0103] The above only describes the preferred embodiments of the application and is not used to limit the application. For those skilled in the art, the application can have various modifications and changes. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the application shall be included in the protection scope of the application.

Claims

1. An improved null-space formation maneuver control method for unmanned surface vehicle escort mission, characterized in that: Step one: decompose the motion process and define the priority of sub-behavior; Step two: use null-space algorithm to establish motion model; correct the state of the escort target; design obstacle avoidance behavior function under the strategy of global fusion, so that the unmanned surface vehicle performs part of the formation obstacle avoidance behavior before reaching the safe distance, and successfully completes the task of formation keeping phase; The formation of the escort formation is centered on the escort target, and the following behavior function is designed: where P i = [x i , y i ] T is the position coordinate of the ith unmanned surface vehicle, and P i,d is the desired position of the ith unmanned surface vehicle. where P t (t) is the correction of the escort target position coordinate, P t (t k ) is the current position coordinate of the escort target, P t (t k-1 ) is the position coordinate of the escort target at the previous time, λ is the correction gain, and r is the escort radius. The Jacobian matrix is: J f = (P i -P i,d ) Τ Then: Let σ d = 0, the output function of the platoon keeping behavior function is: wherein, Λ f is a platoon keeping behavior gain; Definition P ob = [x ob ,y ob ] T is the position coordinate of the obstacle, and the formation collision avoidance behavior function is designed as follows: where l ob and c ob are adjustable parameters; the Jacobian matrix of σ ob is: Then: Let σ d = r ob The output of the platoon obstacle avoidance behavior function is: wherein, l ob , c ob and Λ ob are adjustable parameters; Step three: use the global fusion strategy to judge the priority of the task, and use the null-space method to fuse the formation keeping behavior function and the formation obstacle avoidance behavior function to obtain the speed and direction of each unmanned surface vehicle; Step four: output the speed and direction obtained to the motion control center of the unmanned surface vehicle to drive the unmanned surface vehicle forward; judge whether the unmanned surface vehicle has reached the target point; if it has reached the target point, the process ends, if it has not reached the target point, return to step three.

2. The improved null-space formation maneuvering control method for unmanned surface vehicle escorting task according to claim 1, characterized in that: In step one, the unmanned surface vehicle group decomposes the formation task into two sub-behaviors: formation keeping and formation obstacle avoidance when performing the formation task.

3. The improved null-space formation maneuvering control method for unmanned surface vehicle escorting task according to claim 1, characterized in that: In step three, the global fusion strategy is used to judge the priority of the task, and the null-space method is used to fuse the formation keeping behavior function and the formation obstacle avoidance behavior function to obtain the speed and direction of each unmanned surface vehicle; When the distance between the unmanned surface vehicle and the obstacle is greater than this threshold, the formation obstacle avoidance behavior can be partially completed, that is, the formation keeping behavior is given the highest priority, the formation obstacle avoidance behavior is given the second priority, and the outputs of the two behavior functions are fused to obtain the fused behavior function output: wherein v i,nsb is the desired linear velocity vector defined in the earth coordinate system; When the distance between the unmanned surface vehicle and the obstacle is less than this threshold, the formation obstacle avoidance behavior is given the highest priority, and the formation keeping behavior is given the second priority, and the outputs of the two behavior functions are fused to obtain the fused behavior function output: where v i,nsb is the desired linear velocity vector defined in the earth coordinate frame.

Images