Stratosphere airship cluster regional coverage control method based on clustering artificial potential field method

Abstract

The application discloses a stratosphere airship cluster area covering control method based on a clustering artificial potential field method, relates to the automatic control technical field, and comprises the following steps: establishing a stratosphere airship ground covering channel model, and distributing covering targets; obtaining expected covering deployment center positions of each cluster unit based on a clustering method; establishing a dynamic artificial potential field to obtain expected positions of the stratosphere airship cluster; establishing a stratosphere airship dynamics model, and designing a stratosphere airship dynamic tracking controller to track the expected positions of the stratosphere airship cluster. Therefore, the above method can realize adaptive dynamic covering of the stratosphere airship cluster to ground targets, reduces the influence of environmental wind on the stratosphere airship cluster covering task, establishes a dynamic artificial potential field unified framework, greatly reduces the complexity of the task, and effectively realizes tracking of the expected positions of the stratosphere airship cluster by designing the stratosphere airship dynamic tracking controller.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of automatic control, in particular to a stratosphere airship cluster area coverage control method based on a clustering artificial potential field method. BACKGROUND

[0002] The stratosphere airship is a kind of aerostat running at the height of 20km to 22km at the bottom of the stratosphere. The stratosphere airship relies on the light gas (such as He, H2, etc.) carried by itself to generate net buoyancy to realize the take-off and hovering flight, uses the solar cell and the energy storage battery to establish a circulating energy system, and is equipped with a high-altitude propeller and an attitude adjusting device to realize the controllable maneuvering flight and the high-altitude area residence. It has the advantages of long hovering time, large payload and wide coverage, and has great use value in regional monitoring, communication relay and weather observation.

[0003] From the current application research results of the stratosphere airship, the multi-ship cluster will be the main form of the near space airship deployment application. The coverage control technology is the key to enable the stratosphere airship cluster to fully exert its application advantages. The coverage control technology can reasonably allocate the cluster resources, form a dense monitoring in the important area, and improve the utilization rate and the task execution efficiency of the cluster resources.

[0004] The stratosphere airship cluster area coverage control technology is mainly applied to the wide area warning patrol, disaster area emergency communication, target reconnaissance and other scenes, and the target is to complete the long-time effective coverage of the time-sensitive target at the minimum cost. In such scenes, the following technical difficulties need to be focused on: unknown area full exploration, limited communication resources and path planning under complex weather conditions. The traditional coverage control technology divides the target area by using the Vino division framework, such method is only suitable for the case that the target area is a two-dimensional compact convex plane, and needs to take the distribution of sensitive targets in the area as prior information. The actual scene of the stratosphere airship cluster coverage task is a non-convex area with irregular boundary and unknown target distribution, and the traditional coverage control method is difficult to apply. It is urgent to develop a coverage control method considering the special flight environment and task constraints of the stratosphere airship cluster. SUMMARY

[0005] The purpose of the present application is to provide a stratosphere airship cluster area coverage control method based on a clustering artificial potential field method, which realizes the coverage task of the stratosphere airship cluster to the ground.

[0006] In order to achieve the above purpose, the present application provides a stratosphere airship cluster area coverage control method based on a clustering artificial potential field method, comprising the following steps:

[0007] S1, a stratosphere airship coverage channel model to the ground is established, and a coverage target is distributed;

[0008] S2, obtaining the expected coverage deployment center position of each cluster unit based on a clustering method;

[0009] S3, establishing a dynamic artificial potential field to obtain the expected position of the stratosphere airship cluster;

[0010] S4, establishing a stratosphere airship dynamics model and designing a dynamic tracking controller for the stratosphere airship to track the expected position of the stratosphere airship cluster.

[0011] Preferably, in step S1, the signal-to-noise ratio is selected as a standard for measuring the communication quality between the stratosphere airship and the ground target based on small-scale fading and path attenuation; and the ground target is assigned to the stratosphere airship unit with the largest signal-to-noise ratio according to the signal-to-noise ratio of the ground target relative to the unit in the stratosphere airship cluster, to obtain a set of ground targets assigned to each stratosphere airship unit.

[0012] The formula of the signal-to-noise ratio is:

[0013]

[0014] In the formula, SINR is the signal-to-noise ratio, P r is the effective signal strength, I agg is the interference signal strength, and N0 is the random disturbance.

[0015] P r = P t (P Los h L +(1-P Los )h N )

[0016] In the formula, P t is the signal transmission gain, P Los is the line-of-sight transmission probability, h L is the line-of-sight transmission path attenuation term, and h N is the non-line-of-sight transmission path attenuation term.

[0017] Preferably, in step S2, the expected coverage deployment center point CC i of each cluster unit is as follows:

[0018]

[0019] In the formula, CC i is the expected coverage deployment center point of the airship cluster unit i, j is the jth ground target in the ground target set U i , Pos Ui,j is the current position of the jth ground target in the ground target set U i , and n Ui is the number of ground targets in the ground target set U iThe total number of ground targets contained.

[0020] Preferably, step S3 comprises:

[0021] S31, calculate the dynamic artificial potential field, including the regional boundary potential field, the cluster inter-cell repulsion potential field, the coverage center attraction potential field and the time-varying wind potential field, specifically as follows:

[0022] Pot sum,i (q) = Pot e,i (q) + Pot a,i (q) + Pot c,i (q) + Pot w,i (q)

[0023] In the formula, Pot sum,i (q) is the total dynamic artificial potential field energy of the i th stratospheric airship unit, Pot e,i (q), Pot a,i (q), Pot c,i (q), Pot w,i (q) is the regional boundary potential field energy, the cluster inter-cell repulsion potential field energy, the coverage center attraction potential field energy and the time-varying wind potential field energy of the i th stratospheric airship unit respectively;

[0024] S32, taking the current position of the stratospheric airship unit as the center, selecting a plurality of neighboring points at a fixed distance, and obtaining the total dynamic artificial potential field energy value of each neighboring point, the point with the largest total dynamic artificial potential field energy value is taken as the expected position of the stratospheric airship unit.

[0025] Preferably, step S31 comprises:

[0026] The regional boundary potential field calculation method is:

[0027]

[0028] In the formula, Pot e,i (q) is the regional boundary potential field energy of the i th stratospheric airship unit, q is any point in the region, k e is the regional boundary potential field gain, d e is the action range of the regional boundary potential field, is the distance from the i th stratospheric airship unit to the regional boundary;

[0029] The cluster inter-cell repulsion potential field calculation method is:

[0030]

[0031] In the formula, Pot a,i (q) is the cluster inter-cell repulsion potential field energy of the i th stratospheric airship unit, ka For the repulsion potential field gain between cluster units, d a For the action range of the area boundary potential field, For the distance between any point q in the area and the jth stratosphere airship unit, n is the total number of stratosphere airships included in the stratosphere airship cluster;

[0032] The calculation method of the central attraction potential field is:

[0033]

[0034] In the formula, Pot c,i (q) is the central attraction potential field energy of the ith stratosphere airship unit, k c is the central attraction potential field gain, is the distance between any point q in the area and the corresponding expected central deployment point CC i of the ith stratosphere airship unit;

[0035] The calculation method of the time-varying wind potential field is:

[0036] Pot w,i (q) = k w exp(wind(q))

[0037] In the formula, Pot w,i (q) is the time-varying wind potential field energy, k w is the time-varying wind potential field gain, and wind(q) is the wind speed value of the wind field at any point q in the area.

[0038] Preferably, step S4 comprises:

[0039] S41, according to Newton's second law, establish the stratosphere airship dynamics model;

[0040] S42, according to the backstepping control design method, design the stratosphere airship dynamic tracking controller.

[0041] Preferably, step S41 comprises establishing a stratosphere airship dynamics equation set, specifically as follows:

[0042]

[0043] Wherein,

[0044] X i = [x i , y i , ψ i ] T

[0045] Θ i = [u i , vi ,r i ] T

[0046] τ i =[τ u ,τ v ,τ r ] T

[0047] where X i is the derivative of the position and heading angle of the i th stratosphere airship, X i is the current position and heading angle of the i th stratosphere airship, Θ i is the velocity and angular velocity of the i th stratosphere airship, is the derivative of the velocity and angular velocity of the i th stratosphere airship, x i , y i , ψ i are the lateral position coordinate, longitudinal position coordinate and heading angle of the i th stratosphere airship respectively, R i is the coordinate rotation matrix of the i th stratosphere airship, u i , v i , r are the lateral velocity, longitudinal velocity and yaw rate of the i th stratosphere airship respectively, M i is the mass matrix of the i th stratosphere airship, N i is the model nonlinear term matrix of the i th stratosphere airship, F a,i is the aerodynamic force matrix of the i th stratosphere airship, τ i =[τ u ,τ v ,τ r ] T is the control force of the i th stratosphere airship.

[0048] Preferably, the step S42 comprises:

[0049] The airship position and attitude controller is designed as follows:

[0050]

[0051] where Θ i d is the desired velocity and angular velocity, is the derivative of the desired position and heading angle, K1 = diag{k 1,1 ,k 1,2 ,k 1,3} is a control parameter, is the position and heading angle error;

[0052] The airship velocity and angular velocity controller is designed as follows:

[0053]

[0054] In the formula, τ d It is the desired control. For the error between velocity and angular velocity, K2=diag{k 2,1 ,k 2,2 ,k 2,3} represents the control parameters.

[0055] Therefore, the present invention employs the above-mentioned method for controlling the regional coverage of stratospheric airship clusters based on the clustering artificial potential field method, which has the following technical effects:

[0056] (1) It can achieve adaptive dynamic coverage of ground targets by stratospheric airship clusters in time-varying wind field environments, reducing the impact of environmental wind on stratospheric airship cluster coverage missions.

[0057] (2) A unified framework for dynamic artificial potential field is established, which takes into account the constraints of flight area boundary, strong wind area avoidance, inter-cluster collision avoidance and coverage deployment center tracking in the ground coverage mission, greatly reducing the complexity of the mission.

[0058] (3) A dynamic tracking controller for stratospheric airships was designed, which effectively realized the tracking of the desired position of the stratospheric airship cluster.

[0059] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0060] Figure 1 This is a flowchart of a stratospheric airship cluster regional coverage control method based on clustering artificial potential field method;

[0061] Figure 2 This is a schematic diagram of a stratospheric airship in an embodiment of a stratospheric airship cluster regional coverage control method based on clustering artificial potential field method. Detailed Implementation

[0062] The present invention will be explained in more detail through the following embodiments. The purpose of disclosing the present invention is to protect all changes and modifications within the scope of the present invention. The present invention is not limited to the following embodiments.

[0063] like Figure 1 As shown, the present invention provides a method for controlling the coverage of stratospheric airship clusters based on the clustering artificial potential field method, comprising the following steps:

[0064] S1. Under the conditions of preset time-varying wind field and ground target distribution locations, establish a stratospheric airship-to-ground coverage channel model, and allocate coverage targets according to the maximum signal-to-noise ratio, as follows:

[0065] Considering the influence of small-scale fading and path attenuation on the wireless communication link, the signal-to-noise ratio is selected as the standard for measuring the communication quality between the stratospheric airship and the ground target, and the calculation formula is:

[0066]

[0067] In the formula, SINR is the signal-to-noise ratio, P r is the effective signal strength, I agg is the interference signal strength, and N0 is the random disturbance.

[0068] The effective signal strength expression is:

[0069] P r = P t (P Los h L +(1-P Los )h N )

[0070] In the formula, P t is the signal transmission gain, P Los is the line-of-sight transmission probability, h L is the line-of-sight transmission path attenuation term, and h N is the non-line-of-sight transmission path attenuation term.

[0071] According to the signal-to-noise ratio SINR of the ground target relative to all units of the stratospheric airship cluster, the stratospheric airship unit i corresponding to the maximum signal-to-noise ratio is calculated, and the ground target is assigned to the corresponding stratospheric airship unit i, to obtain the ground target set U i assigned to each stratospheric airship unit.

[0072] S2, based on the clustering method, the expected coverage deployment center position of each cluster unit, i.e. the center point CC i , is obtained, which is as follows:

[0073]

[0074] In the formula, CC i is the expected coverage deployment center point of the airship cluster unit i, j is the jth ground target in the ground target set U i , Pos Ui,j is the current position of the jth ground target in the ground target set U i , and n Ui is the total number of ground targets included in the ground target set U i .

[0075] S3. Considering flight area boundary constraints, strong wind zone avoidance constraints, inter-swarm collision avoidance constraints, and coverage deployment center tracking constraints, a unified framework of dynamic artificial potential field is established to obtain the desired position of the stratospheric airship swarm. Specifically as follows:

[0076] S31. Calculate the dynamic artificial potential field, including the regional boundary potential field, the repulsive potential field between cluster units, the attractive potential field of the coverage center, and the time-varying wind potential field.

[0077] (1) The method for calculating the potential field at the boundary of the region is as follows:

[0078]

[0079] In the formula, Pot e,i (q) represents the boundary potential energy of the i-th stratospheric airship unit, where q is any point within the region, and k is the boundary potential energy. e For the potential field gain at the region boundary, d e The range of influence of the potential field at the regional boundary. Let be the distance from the i-th stratospheric airship unit to the region boundary.

[0080] (2) The method for calculating the repulsive potential field between cluster units is as follows:

[0081]

[0082] In the formula, Pot a,i (q) represents the energy of the repulsive potential field between cluster units of the i-th stratospheric airship unit, k a For the repulsive potential gain between cluster units, d a The range of influence of the potential field at the regional boundary. Let q be the distance between any point q in the region and the j-th stratospheric airship unit, and n be the total number of stratospheric airships in the stratospheric airship cluster.

[0083] (3) The calculation method for the attractive potential field at the covering center is as follows:

[0084]

[0085] In the formula, Pot c,i (q) represents the energy of the attraction potential field at the coverage center of the i-th stratospheric airship unit, k c To cover the gain of the central attractive potential field, Let q be any point within the region and CC be the desired coverage deployment center point corresponding to the i-th stratospheric airship unit. i The distance.

[0086] (4) The calculation method for time-varying wind field is as follows:

[0087] Pot w,i (q)=kw exp(wind(q))

[0088] In the formula, Pot w,i (q) represents the energy of the time-varying wind field, k w The gain is the time-varying wind field, and wind(q) is the wind speed value at any point q in the region.

[0089] In summary, the method for calculating the total dynamic artificial potential field is as follows:

[0090] Pot sum,i (q)=Pot e,i (q)+Pot a,i (q)+Pot c,i (q)+Pot w,i (q)

[0091] In the formula, Pot sum,i (q) represents the total dynamic artificial potential energy of the i-th stratospheric airship unit.

[0092] S32, using the current position Pos of the stratospheric airship unit i =(x i ,y i Centered on s, neighboring points are taken in eight directions (north, northeast, east, southeast, south, southwest, west, and northwest) at a distance of s. The total dynamic artificial potential field value at each of the eight neighboring points is calculated. The neighboring point with the largest total dynamic artificial potential field value is taken as the expected position of the i-th stratospheric airship. Where x i y i These are the northward and eastward position coordinates of the i-th stratospheric airship unit, respectively.

[0093] S4, such as Figure 2 As shown, a dynamic model of a stratospheric airship is established, and a dynamic tracking controller for the stratospheric airship is designed to track the desired position of the stratospheric airship cluster, including:

[0094] S41. Establish a dynamic model of a stratospheric airship:

[0095] Based on Newton's second law, establish the following set of equations for the dynamics of stratospheric airships:

[0096]

[0097] in,

[0098] X i =[x i ,y i ,ψ i ] T

[0099] Θ i = [u i , v i , r i ] T

[0100] = [τ i , τ u , τ v ] r T

[0101] where X i is the position and heading angle of the i th stratosphere airship, is the derivative of the position and heading angle of the i th stratosphere airship, x i , y i , ψ i are the lateral position coordinate, longitudinal position coordinate and heading angle of the i th stratosphere airship respectively, R i is the coordinate rotation matrix of the i th stratosphere airship, Θ i is the velocity and angular velocity of the i th stratosphere airship, is the derivative of the velocity and angular velocity of the i th stratosphere airship, u i , v i , r are the lateral velocity, longitudinal velocity and yaw angular velocity of the i th stratosphere airship respectively, M i is the mass matrix of the i th stratosphere airship, N i is the model nonlinear term matrix of the i th stratosphere airship, F a,i is the aerodynamic force matrix of the i th stratosphere airship, τ i = [τ u , τ v , τ r ] T is the control force of the i th stratosphere airship.

[0102] S42, designing a stratosphere airship dynamic tracking controller:

[0103] Define the error between the current position, heading angle X i and the desired position, desired heading angle X i of the i th stratosphere airship as: d

[0104] ξ Xi = X i - X i d

[0105] where, is the position and heading angle error.

[0106] ​The derivative of the above equation is taken and substituted into the stratosphere airship dynamic equation set to obtain

[0107]

[0108] According to the backstepping control design method, the airship position and attitude controller can be designed as follows:

[0109]

[0110] In the formula, Θ i d is the desired speed, angular velocity, K1 = diag{k 1,1 ,k 1,2 ,k 1,3} is a control parameter.

[0111] The Lyapunov function is selected as follows:

[0112]

[0113] The first order derivative of the Lyapunov function with respect to time is

[0114]

[0115] Substituting the airship position and attitude controller into

[0116] , the following equation is obtained:

[0117]

[0118] Because V1 > 0 and is semi-negative definite, the Lyapunov stability criterion is satisfied, and it can be considered that under the action of the airship position and attitude controller, the airship position and heading angle error can uniformly and continuously converge to zero.

[0119] For the desired speed and angular velocity obtained by the airship position and attitude controller, the error between the current speed and angular velocity of the single airship and the desired speed and desired angular velocity is defined as:

[0120]

[0121] The derivative of the above equation is taken and substituted into the airship state space equation to obtain

[0122]

[0123] According to the backstepping method controller design framework, the airship speed and angular velocity controller can be designed as follows:

[0124] ​

[0125] where τ d is the desired control force, K2=diag{k 2,1 ,k 2,2 ,k 2,3} are control parameters.

[0126] A Lyapunov function is selected as follows:

[0127]

[0128] The first order derivative of V2 with respect to time is:

[0129]

[0130] Substitute the airship speed and angular velocity controller into , and the following can be obtained:

[0131]

[0132] Because V2>0, and is semi-negative definite, the Lyapunov stability criterion is satisfied, and it can be considered that under the action of the airship speed and angular velocity controller, the airship speed and angular velocity error can converge to zero uniformly and continuously.

[0133] Based on the above airship dynamic tracking controller design process, the stratospheric airship cluster can calculate the control force according to the position and the desired position, and transfer it to the actuator for motion control, so as to realize the tracking of the expected coverage deployment center point CC i .

[0134] In the application process, the control engineer can freely set the number of units of the airship cluster according to the actual situation, calculate the corresponding control amount by the method of the embodiment, and directly transfer it to the actuator to realize the automatic control of the stratospheric airship cluster for the ground coverage task.

[0135] Therefore, the stratospheric airship cluster regional coverage control method based on the clustering artificial potential field method is adopted, the unified framework of the dynamic artificial potential field is based on, the problems of full exploration of the target region, coverage range, communication link allocation under bandwidth limitation, adaptive deployment under time-varying wind field, etc. are comprehensively considered, and an effective solution is provided for the stratospheric airship cluster regional coverage control problem for the ground coverage task.

[0136] It should be pointed out finally that the above examples are only used to illustrate the technical solutions of the present application but not to limit it, and although the present application has been described in detail with reference to the preferred embodiments, it should be understood by those skilled in the art that the technical solutions of the present application can still be modified or replaced equivalently, and these modifications or equivalent replacements should not make the modified technical solutions deviate from the spirit and scope of the technical solutions of the present application.

Claims

1. A method for controlling regional coverage of stratospheric airship clusters based on clustering artificial potential field method, characterized in that, Includes the following steps: S1. Establish a stratospheric airship-to-ground coverage channel model and allocate coverage targets; S2. Obtain the expected coverage deployment center location for each cluster unit based on clustering methods; S3. Establish a dynamic artificial potential field to obtain the desired location of the stratospheric airship cluster, including the following steps: S31. Calculate the dynamic artificial potential field, including the region boundary potential field, the repulsive potential field between cluster units, the attractive potential field of the coverage center, and the time-varying wind potential field, as detailed below: In the formula, For the first Total dynamic artificial potential energy of a stratospheric airship unit , , , The first The potential field energy of the regional boundary of a stratospheric airship unit, the repulsive potential field energy between cluster units, the attractive potential field energy of the covering center, and the time-varying wind potential field energy. The method for calculating the potential field at the boundary of a region is as follows: In the formula, For the first Regional boundary potential energy of a stratospheric airship unit For any point within the region, For the potential field gain at the region boundary, The range of influence of the potential field at the regional boundary. For the first The distance from each stratospheric airship unit to the region boundary; The method for calculating the repulsive potential field between cluster units is as follows: In the formula, For the first Energy of the repulsive potential field between cluster units of a stratospheric airship For the repulsive potential field gain between cluster units, The range of influence of the potential field at the regional boundary. For any point within the region To the The distance between stratospheric airship units This represents the total number of stratospheric airships included in the stratospheric airship cluster. The method for calculating the attractive potential field at the center of coverage is as follows: In the formula, For the first The energy of the attractive potential field at the coverage center of each stratospheric airship unit To cover the gain of the central attractive potential field, For any point within the region To the The expected coverage deployment center point for each stratospheric airship unit The distance; The calculation method for time-varying wind fields is as follows: In the formula, Energy of the time-varying wind field. For time-varying wind field gain. For any point within the region Wind speed values ​​at the location; S32. Taking the current position of the stratospheric airship unit as the center, select several neighboring points at fixed distances, and obtain the total dynamic artificial potential energy value of each neighboring point. Take the point with the largest total dynamic artificial potential energy value as the desired position of the stratospheric airship unit. S4. Establish a dynamic model of the stratospheric airship and design a dynamic tracking controller for the stratospheric airship to track the desired position of the stratospheric airship cluster.

2. The method for controlling the regional coverage of stratospheric airship clusters based on clustering artificial potential field method according to claim 1, characterized in that, Step S1: Based on small-scale fading and path attenuation, the signal-to-noise ratio (SNR) is selected as the standard for measuring the communication quality between the stratospheric airship and the ground target; and according to the SNR of the ground target relative to the unit in the stratospheric airship cluster, the ground target is assigned to the stratospheric airship unit with the highest SNR, thereby obtaining the set of ground targets assigned to each stratospheric airship unit. The formula for signal-to-noise ratio is: In the formula, For signal-to-noise ratio, For effective signal strength, To interfere with signal strength, For random perturbations; In the formula, For signal transmission gain, For the probability of line-of-sight transmission, For line-of-sight transmission path attenuation, This is the attenuation term for non-line-of-sight transmission paths.

3. The method for controlling the regional coverage of stratospheric airship clusters based on clustering artificial potential field method according to claim 1, characterized in that, In step S2, each cluster unit aims to cover the deployment center point. The formula is as follows: In the formula, For airship cluster units The expected coverage of the deployment center point, Set of ground targets The Middle One ground target, Set of ground targets The Middle The current location of a ground target. Set of ground targets The total number of ground targets included.

4. The method for controlling the regional coverage of stratospheric airship clusters based on clustering artificial potential field method according to claim 1, characterized in that, Step S4 includes: S41. Based on Newton's second law, establish a dynamic model of a stratospheric airship; S42. Based on the backstepping control design method, design a dynamic tracking controller for a stratospheric airship.

5. A method for controlling the regional coverage of stratospheric airship clusters based on clustering artificial potential field method according to claim 4, characterized in that, Step S41 includes establishing the dynamic equations for the stratospheric airship, as follows: in, In the formula, For the first The derivative of the position and heading angle of a stratospheric airship For the first The current position and heading angle of the stratospheric airship For the first The speed and angular velocity of a stratospheric airship For the first The velocity and angular velocity derivatives of a stratospheric airship , , The first The lateral position coordinates, longitudinal position coordinates, and heading angle of a stratospheric airship For the first The coordinate rotation matrix of a stratospheric airship , , The first The lateral velocity, longitudinal velocity, and yaw rate of a stratospheric airship For the first Mass matrix of a stratospheric airship For the first The nonlinear term matrix of a stratospheric airship model. For the first Aerodynamic matrix of a stratospheric airship For the first The control force of a stratospheric airship.

6. The method for controlling the regional coverage of stratospheric airship clusters based on clustering artificial potential field method according to claim 5, characterized in that, Step S42 includes: The design of the airship's position and attitude controller is as follows: In the formula, For the desired velocity and angular velocity, The derivative of the desired position with respect to the heading angle. For control parameters, This refers to the position and heading angle errors; Design the airship's speed and angular velocity controller, as detailed below: In the formula, It is the desired control. For the error in velocity and angular velocity, These are control parameters.

Images