A multi-unmanned aerial vehicle cooperative observation enhanced yaw angle planning method based on a bidding algorithm

Abstract

The application belongs to the technical field of unmanned aerial vehicles, and discloses a multi-unmanned aerial vehicle cooperative observation enhanced yaw angle planning method based on a bidding algorithm, which comprises the following steps: S1, bidding algorithm problem scenario description and establishment of related symbols; S2, establishment of an optimization problem model of the bidding algorithm; S3, establishment of a bidding algorithm utility calculation model; S4, a bidding process cycle of the bidding algorithm; S5, target observation execution condition inspection; and S6, calculation of a yaw angle and an angular velocity output. The application can realize a multi-unmanned aerial vehicle yaw angle distribution and planning method for enhancing friendly mutual observation and enemy target observation, so that target observation distribution is more rapid and effective, and multi-aircraft cluster positioning is more robust and accurate.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) technology, and in particular relates to a multi-UAV collaborative observation enhancement yaw angle planning method based on an auction algorithm. Background Technology

[0002] Currently, multi-drone collaborative technology has wide applications in various fields such as entertainment, industry, and security. Examples include creating aerial landscapes using multi-drone formations, transporting objects collaboratively, and apprehending illegally intruding drones, suspicious vehicles, and fugitives. During collaborative tasks, swarm drones need to coordinate environmental perception, target perception, swarm localization, and target localization. Visual observation plays a crucial role in localization constraints, and the resulting swarm localization and target localization significantly impact the accuracy of subsequent collaborative operations. In general, yaw angle planning methods that enhance mutual observation among swarm drones can improve the utilization of idle yaw angle observation resources and enhance the robustness of swarm localization and target localization for multi-drone collaborative tasks. This can help swarm drones obtain accurate and robust swarm drone localization and target localization, improving the accuracy and robustness of subsequent task planning and control. However, current technical solutions suffer from insufficient utilization of idle yaw angle observation resources and a lack of observation constraints between drones. Summary of the Invention

[0003] The purpose of this invention is to provide a multi-UAV collaborative observation-enhanced yaw angle planning method based on an auction algorithm to solve the above-mentioned technical problems.

[0004] To address the aforementioned technical problems, the present invention provides a specific technical solution for a multi-UAV cooperative observation-enhanced yaw angle planning method based on an auction algorithm, as follows:

[0005] A multi-UAV collaborative observation-enhanced yaw angle planning method based on an auction algorithm includes multiple UAVs, each equipped with a camera, IMU, other sensors required for the mission, an onboard computer, and flight control and power kits for takeoff. The method utilizes the camera, IMU, and other sensors required for the mission to transmit data to the onboard computer and executes the following steps:

[0006] S1: Description of the auction algorithm problem scenario and establishment of related symbol labels;

[0007] S2: Establish an optimization problem model for the auction algorithm;

[0008] S3: Establishment of the utility calculation model for the auction algorithm;

[0009] S4: The auction process loop of the auction algorithm;

[0010] S5: Target observation execution condition verification;

[0011] S6: Calculate yaw angle and angular velocity output.

[0012] Furthermore, S1 includes problem scenario modeling suitable for multi-UAV collaborative observation to enhance yaw angle planning, and the establishment of symbol labels used in the problem;

[0013] Problem Scenario Description: The auction algorithm simulates an auction to solve the observation relationship allocation problem in multi-UAV collaborative operations. In multi-UAV collaborative operations, there are N observers and M objects to be observed. The problem is to find an allocation scheme S that pairs observers with objects to be observed in pairs, such that the net revenue of all observers who auction the M objects to be observed to the N observers is maximized under this allocation scheme. The number of observers must be less than or equal to the number of objects to be observed, i.e., N≤M.

[0014] Symbols used in the problem:

[0015] Let I={i1,…,i N Let} be the set of observer IDs, J = {j1, ...,j M} represents the set of object numbers to be observed;

[0016] The allocation scheme has a set size of T;

[0017] W = {w ij |i∈I,j∈J} represents the set of utilities that observer i estimates from observing object j;

[0018] G={g ij |i∈I,j∈J} represents the net gain obtained by observer i from observing object j;

[0019] P = {p ij |i∈I,j∈J} represents the bid made by observer i in this round to bid for the observed object j;

[0020] B = {b} j |j∈J} represents the final bid price determined for the observed object j after one round of bidding;

[0021] F={f ij Let |i∈I,j∈J} be the set of flag variables indicating whether observer i is assigned to the observed object j. If the assignment relation holds, then f ij =1, otherwise f ij =0.

[0022] Furthermore, step S2 includes the following steps:

[0023] When the allocation scheme is S, the formula for calculating the net benefit for all observers is:

[0024]

[0025] In addition, there are constraints:

[0026] 1) If each observer is assigned at most one object to be observed, then:

[0027]

[0028] 2) Each observer must eventually be assigned to an object to be observed, then:

[0029] T = N (3)

[0030] To maximize the net gain for all observers, we establish an optimization problem using equations (1)-(3):

[0031]

[0032] T = N.

[0033] Furthermore, based on the principle of establishing observation relationships in multi-UAV cooperation, S3 designs the following utility items:

[0034] 1) Distance cost term: the distance d between observer i and the observed object j. ij The closer the observation, the better the results; therefore, the following method is adopted. As a distance utility, where δ is a very small positive number to prevent the denominator from being zero;

[0035] 2) Target priority term: The number of times the observed object j has been observed in the last 5 assignments, f. j The fewer the number, the higher the priority in this allocation; therefore, the following approach is adopted. As a priority utility for objectives;

[0036] 3) Yaw angle cost term: The angle θ between the original yaw angle of observer i and the yaw angle required to observe the object j. ij The smaller the value, the lower the cost; therefore, we use w. yaw =cos(θ) ij As a yaw angle effect;

[0037] 4) Observer idleness term: the shortest distance d between observer i and the edge of the field or an obstacle. min.i The closer the object is, the more likely observer i is to be in a non-idle state, and the more difficult it is to allocate observation resources to the object to be observed. Therefore, we adopt... As the observer's idleness utility, where d crash For a predefined safe distance, d max The maximum distance that a drone can reach from the edge of the site is defined by d, which, after pre-screening by observers, ensures...min.i >d crash , Therefore w leisure ∈(0,1];

[0038] Combining the above utility terms, the formula for calculating the utility of observer i towards observed object j can be listed as follows:

[0039]

[0040] Where α1, α2, and α3 are the weight parameters for each utility term.

[0041] Furthermore, step S4 includes the following steps:

[0042] First, initialize the parameters by setting the initial bid P of all observers for the object to be observed and the initial bid B of all objects to be observed to 0; set the relaxation complementarity parameter ∈ = 0.01, set a set of bidders X, and initialize X = I;

[0043] Next, the bidding process loop begins, iterating through each bidder i in the set of bidders X:

[0044] 1) Calculate the utility set π of observer i for each observed object j. i ={w ij -p ij} j∈J ;

[0045] 2) Calculate the maximum benefit π in this utility set. max,i =max{π i}, Maximum benefit to be observed object j * and the second largest return π max2,i =max{π i |j≠j * The second largest gain is the object to be observed, j. ** ;

[0046] 3) Update the observer i's view on the observation object j that will maximize the gain. * The price is The purpose is to determine the object j to be observed. * The price quoted for increasing the scarcity of observer i;

[0047] 4) If the observed object j * If quotes have been received from other observers, then those observers are placed in set X;

[0048] 5) Complete the quote for observer i and remove observer i from set X.

[0049] Perform steps 1)-5) above for each observer i in set X. When there are no more unassigned observers in set X, the condition T=N is satisfied, and the loop ends. The assignment scheme S at this time is the final scheme.

[0050] Furthermore, step S5 includes the following steps:

[0051] According to the allocation scheme given by the auction algorithm Check whether observer i meets the validity criteria:

[0052] 1) Safety Time Condition: Before flying close to the edge of the field or an obstacle, observer i needs sufficient time to change its yaw angle from the observed object j back to the yaw angle of the observation environment; otherwise, the observation will not be performed. Judgment condition:

[0053] a max (t out -t turn )≥v now (6)

[0054] Where a max v is the maximum acceleration of observer i. now Let t be the current velocity of observer i. out This refers to the time buffer before observer i flies close to the edge of the field or a safe distance from obstacles. The time required for observer i to perform an observation of the object j and then return to its original position;

[0055] If the condition shown in equation (6) is true, the test is passed; otherwise, the test is not passed and the observation is not performed.

[0056] 2) Effective observation distance conditions: The distance between observer i and the object j to be observed cannot exceed the maximum observation distance of 3m, and there cannot be any obstacles obstructing the observation between them;

[0057] Observation pairs {i,j} that do not meet either condition 1) or 2) above are removed from allocation scheme S, and are not executed in subsequent steps or included in the subsequent observation frequency p. j .

[0058] Furthermore, step S6 includes the following steps:

[0059] Calculate the expected yaw angle for each observer i in the allocation scheme S. And perform the turn with a constant angular velocity ω, let the current yaw angle of observer i be ω. The positions of observer i and observed object j in the world coordinate system are pos, respectively. i pos j Then the expected yaw angle of observer i for:

[0060]

[0061] in Let be the X-axis direction vector in the world coordinate system, which is related to pos j The included angle is the absolute yaw angle that observer i wants to align with.

[0062] Execute from constant angular velocity ω arrive The direction of rotation is determined by calculating the cross product to determine the direction of rotation due to angular velocity:

[0063]

[0064]

[0065] in Let be the X-axis direction vector of the observer i's body coordinate system, and let be the vector through which pos... j The sign of k3 obtained from the cross product can determine which direction the angular velocity will reach the destination faster. The final result is the angular velocity ω of observer i during the turning process.

[0066] After the desired yaw angle and angular velocity are output, the yaw angle plan is output to the UAV controller to merge the position plan and yaw angle plan, and calculate the pose for subsequent execution.

[0067] The multi-UAV cooperative observation-enhanced yaw angle planning method based on an auction algorithm of the present invention has the following advantages:

[0068] (1) This invention provides a solution for the allocation of observation relationships that maximizes the collective observation utility by designing an auction algorithm to optimize the problem.

[0069] (2) This invention quantifies the attractiveness of the observation target to each observer by using utility terms such as distance cost, target priority, and yaw angle cost, thereby improving the utility differentiation of different observation assignments in actual cluster observation.

[0070] (3) This invention quantifies the richness of the observer's own idle observation resources by using the observer idleness utility term, thereby reducing the observation allocation rights of non-idle observers.

[0071] (4) This invention ensures the safety and effectiveness of the observation behavior by verifying the conditions for the execution of target observation.

[0072] This invention enables a multi-UAV yaw angle allocation and planning method to enhance mutual observation with friendly forces and observation of enemy targets, making target allocation faster and more effective, and multi-UAV swarm positioning more robust and accurate. Attached Figure Description

[0073] Figure 1 This is a flowchart illustrating the present invention. Detailed Implementation

[0074] To better understand the purpose, structure, and function of this invention, the following description, in conjunction with the accompanying drawings, provides a more detailed account of a multi-UAV collaborative observation enhancement yaw angle planning method based on an auction algorithm.

[0075] Example 1: Several drones were prepared, each equipped with a camera, IMU, other sensors required for the mission, an onboard computer, and flight control and power kits for takeoff. The drones were started, and data was transmitted from the sensors to the onboard computer. A multi-drone collaborative observation-enhanced yaw angle planning method based on an auction algorithm was implemented, such as... Figure 1 As shown, perform the following steps:

[0076] S1: Description of the bidding algorithm problem scenario and establishment of related symbol labels, including problem scenario modeling applicable to multi-UAV collaborative observation to enhance yaw angle planning, and establishment of the symbol labels used in the problem.

[0077] Problem Scenario Description: An auction algorithm simulates an auction to solve the observation relationship allocation problem in multi-UAV collaborative operations. In multi-UAV collaborative operations, there are N observers and M objects to be observed. The problem is to find an allocation scheme S that pairs observers with objects to be observed, such that the net profit of all observers is maximized when the M objects to be observed are auctioned to the N observers. Since the observer UAVs only include our own UAVs, while the object UAVs include both our own and enemy UAVs, the number of observers must be less than or equal to the number of objects to be observed, i.e., N ≤ M.

[0078] Symbols used in the problem:

[0079] Let I={i1,…,i N Let} be the set of observer IDs, J = {j1, ...,j M} represents the set of object numbers to be observed;

[0080] The allocation scheme has a set size of T;

[0081] W = {w ij |i∈I,j∈J} represents the set of utilities that observer i estimates from observing object j;

[0082] G={g ij |i∈I,j∈J} represents the net gain obtained by observer i from observing object j;

[0083] P = {p ij|i∈I,j∈J} represents the bid made by observer i in this round to bid for the observed object j;

[0084] B = {b} j |j∈J} represents the final bid price determined for the observed object j after one round of bidding;

[0085] F={f ij Let |i∈I,j∈J} be the set of flag variables indicating whether observer i is assigned to the observed object j. If the assignment relation holds, then f ij =1, otherwise f ij =0.

[0086] S2: Establish an optimization problem model for the auction algorithm.

[0087] When the allocation scheme is S, the formula for calculating the net benefit for all observers is:

[0088]

[0089] In addition, there are constraints:

[0090] 1) If each observer is assigned at most one object to be observed, then:

[0091]

[0092] 2) Each observer must eventually be assigned to an object to be observed, then:

[0093] T = N (3)

[0094] To maximize the net gain for all observers, we establish an optimization problem using equations (1)-(3):

[0095]

[0096] S3: Establishment of the utility calculation model for the auction algorithm. Based on the principle of establishing observation relationships in multi-UAV cooperation, the following utility terms are designed:

[0097] 5) Distance cost term: the distance d between observer i and the observed object j. ij The closer the observation, the better the results; therefore, the following method is adopted. As a distance utility, where δ is a tiny positive number to prevent the denominator from being zero.

[0098] 6) Target priority: The number of times the observed object j has been observed in the last 5 assignments, f. j The fewer the number, the higher the priority in this allocation; therefore, the following approach is adopted. As a priority of the goal.

[0099] 7) Yaw angle cost term: The angle θ between the original yaw angle of observer i and the yaw angle required to observe the object j. ij The smaller the value, the lower the cost; therefore, we use w. yaw =cos(θ) ij As a yaw angle effect.

[0100] 8) Observer Idleness Term: The shortest distance d between observer i and the edge of the field or an obstacle. min.i The closer the object is, the more likely observer i is to be in a non-idle state, and the more difficult it is to allocate observation resources to the object to be observed. Therefore, we adopt... As the observer's idleness utility. Where d crash For a predefined safe distance, d max This represents the maximum distance that a drone can reach from the edge of the field. After pre-screening by observers, d can be guaranteed. min.i >d crash , Therefore w leisure ∈(0,1).

[0101] Combining the above utility terms, the formula for calculating the utility of observer i towards observed object j can be listed as follows:

[0102]

[0103] Where α1, α2, and α3 are the weight parameters for each utility term.

[0104] S4: Auction process loop of the auction algorithm.

[0105] First, initialize the parameters by setting the initial bids P for all observers and B for all observed objects to 0; set the relaxation complementarity parameter ∈ = 0.01. Establish a set of bidders X, initialized to X = I.

[0106] Next, the bidding process loop begins, iterating through each bidder i in the set of bidders X:

[0107] 6) Calculate the utility set π of observer i for each observed object j. i ={w ij -p ij} j∈J ;

[0108] 7) Calculate the maximum benefit π in this utility set. max,i =max{π i}, Maximum benefit to be observed object j * and the second largest return π max2,i =max{π i |j≠j *The second largest gain is the object to be observed, j. ** ;

[0109] 8) Update the observer i's view on the observation object j with the maximum benefit. * The price is The purpose is to determine the object j to be observed. * The price quoted for increasing the scarcity of observer i;

[0110] 9) If the observed object j * If quotes have been received from other observers, then those observers are placed in set X;

[0111] 10) Complete the bid for observer i and remove observer i from set X. Perform steps 1)-5) above for each observer i in set X. When there are no more unassigned observers in set X (satisfying the condition T=N), end the loop. The allocation scheme S at this time is the final scheme.

[0112] S5: Target observation execution condition verification. Based on the allocation scheme given by the auction algorithm. Check whether observer i meets the validity criteria:

[0113] 1) Safety Time Condition: Before flying close to the edge of the field or an obstacle, observer i needs sufficient time to change its yaw angle from the observed object j back to the yaw angle of the observation environment; otherwise, the observation will not be performed. Judgment Condition:

[0114] a max (t out -t turn )≥v now (6)

[0115] Where a max v is the maximum acceleration of observer i. now Let t be the current velocity of observer i. out This refers to the time buffer before observer i flies close to the edge of the field or a safe distance from obstacles. The time required for observer i to perform an observation of the object j and then return to its original position.

[0116] If the condition shown in equation (6) is true, the test is passed; otherwise, the test is not passed and the observation is not performed.

[0117] 2) Observation effective distance conditions: The distance between observer i and the object to be observed j cannot exceed the farthest observation distance of 3m, and there cannot be any obstacles obstructing the observation between them.

[0118] Observation pairs {i,j} that do not meet either condition 1) or 2) above are removed from allocation scheme S, and are not executed in subsequent steps or included in the subsequent observation frequency p. j .

[0119] S6: Calculate the yaw angle and angular velocity output. Calculate the expected yaw angle for each observer i in allocation scheme S. It then performs a turn with a constant angular velocity ω. Let the current yaw angle of observer i be ω. The positions of observer i and observed object j in the world coordinate system are pos, respectively. i pos j Then the expected yaw angle of observer i is... for:

[0120]

[0121] in Let be the X-axis direction vector in the world coordinate system, which is related to pos j The included angle is the absolute yaw angle that observer i wants to align with.

[0122] Execute from constant angular velocity ω arrive The direction of rotation can be determined by calculating the cross product to determine the direction of rotation due to angular velocity:

[0123]

[0124]

[0125] in Let be the X-axis direction vector of the observer i's body coordinate system, and let be the vector through which pos... j The sign of k3 obtained from the cross product can determine which direction the angular velocity will reach the destination faster. The final result is the angular velocity ω of observer i during the turning process.

[0126] After the desired yaw angle and angular velocity are output, the yaw angle plan is output to the UAV controller to merge the position plan and yaw angle plan, and calculate the pose for subsequent execution.

[0127] This invention is not limited to the embodiments described above. Within the scope of knowledge possessed by those skilled in the art, various modifications can be made without departing from the spirit of this invention. For example, it can be used for multi-robot yaw angle planning based on assisted direction-sensitive ranging for any purpose.

[0128] It is understood that the present invention has been described through some embodiments, and those skilled in the art will recognize that various changes or equivalent substitutions can be made to these features and embodiments without departing from the spirit and scope of the invention. Furthermore, under the teachings of the present invention, these features and embodiments can be modified to adapt to specific situations and materials without departing from the spirit and scope of the invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are within the protection scope of the present invention.

Claims

1. A multi-UAV collaborative observation-enhanced yaw angle planning method based on an auction algorithm, comprising multiple UAVs, each UAV equipped with a camera, IMU, other sensors required for the mission, an onboard computer, and flight control and power kits required for takeoff, characterized in that... The method utilizes cameras, IMUs, and other sensors required for the mission to transmit data to the onboard computer and performs the following steps: S1: Description of the auction algorithm problem scenario and establishment of related symbol labels; S2: Establish an optimization problem model for the auction algorithm; S3: Establishment of the utility calculation model for the auction algorithm; Based on the principle of establishing observation relationships in multi-UAV cooperation, S3 designs the following utility items: 1) Distance cost term, observer With the object to be observed Distance between The closer the observation, the better the results; therefore, the following method is adopted. As a distance utility, among which To prevent a very small positive number with a denominator of zero; 2) Target priority item: the object to be observed Number of observations in the last 5 allocations The fewer the number, the higher the priority in this allocation; therefore, the following approach is adopted. As a priority utility for objectives; 3) Yaw angle cost term: Observer The original yaw angle and the observed object The angle between the required yaw angles The smaller the value, the lower the cost, therefore we adopt... As a yaw angle effect; 4) Observer idleness term: Observer Shortest distance between the edge of the field or an obstacle The closer, the better for the observer. The more active a state is, the more difficult it is to allocate observation resources to the object being observed; therefore, the following approach is adopted. As observer idleness utility, among which For a predefined safe distance, This is the maximum distance that a drone can reach from the edge of the site, and it is pre-screened by observers to ensure... Therefore ; By combining the above utility terms, the observer can be listed. Treating the observed object Utility calculation formula: in These are the weighting parameters for each utility item; S4: The auction process loop of the auction algorithm; S5: Target observation execution condition verification; S6: Calculate yaw angle and angular velocity output.

2. The multi-UAV cooperative observation enhancement yaw angle planning method based on auction algorithm according to claim 1, characterized in that, S1 includes problem scenario modeling for multi-UAV collaborative observation-enhanced yaw angle planning and the establishment of symbol labels used in the problem; Problem Scenario Description: An auction algorithm simulates an auction to solve the problem of allocating observation relationships in multi-UAV collaborative operations. Multi-UAV collaborative operations present... One observer, Given several objects to be observed, the problem is to find an allocation scheme. Pair observers with objects to be observed in pairs, such that under this assignment scheme... The object to be observed was auctioned to To maximize the net benefit of all observers for an observer, the number of observers must be less than or equal to the number of objects being observed. ; Symbols used in the problem: set up Assign a set of numbers to the observers. Number the objects to be observed; The allocation scheme has a set size of . ; For the observer Estimate the observed object The set of utilities obtained; For the observer The object to be observed was observed. The net income obtained; For the observer In order to bid for the object to be observed The bids made in this round; After one round of bidding, the object to be observed The final bid price; For the observer Whether to assign to the object to be observed The set of flag variables, if the assignment relation holds, then ,otherwise .

3. The multi-UAV cooperative observation enhancement yaw angle planning method based on auction algorithm according to claim 1, characterized in that, S2 includes the following steps: When the allocation scheme is S, the formula for calculating the net benefit for all observers is: In addition, there are constraints: 1) If each observer is assigned at most one object to be observed, then: 2) Each observer must eventually be assigned to an object to be observed, then: To maximize the net benefit for all observers, the joint... Establish an optimization problem: 。 4. The multi-UAV cooperative observation enhancement yaw angle planning method based on auction algorithm according to claim 1, characterized in that, S4 includes the following steps: First, initialize the parameters, giving all observers their initial quotes for the observed objects. and the initial quotes for all objects to be observed Initialize all parameters to 0; set the relaxation complementarity parameter. Set up a set of bidders ,initialization ; Next, the bidding process loop begins, iterating through the set of bidders. Each bidder : 1) Calculate the observer For each object to be observed Utility set ; 2) Calculate the maximum benefit within this utility set. Maximum benefit to be observed and second largest return Second largest benefit subject to observation ; 3) Update the observer For the object to be observed with the maximum benefit The price is The purpose is to determine the object to be observed. For the observer The higher the scarcity, the higher the corresponding price. 4) If the observed object If we have received quotes from other observers, we will add those observers to the set. middle; 5) Complete the observer The quote will be observed by the observer. From the set Take it out from the middle; For sets Each observer Perform steps 1)-5) above, when the set When there are no more unassigned observers, the condition is satisfied. The condition is met, the loop ends, and the allocation scheme at this point is determined. This is the final solution.

5. The multi-UAV cooperative observation enhancement yaw angle planning method based on auction algorithm according to claim 1, characterized in that, S5 includes the following steps: According to the allocation scheme given by the auction algorithm Test observer Does it meet the valid conditions? 1) Safe time condition: Observer Sufficient time is needed to adjust the yaw angle from the observed object before approaching the edge of the field or obstacles. Turn back to the yaw angle of the observation environment; otherwise, do not perform this observation. Judgment condition: in For the observer Maximum acceleration, For the observer Current speed, For the observer The time allowance before flying close to the edge of the field or to a safe distance from obstacles. For the observer Execute the object to be observed The time required to return to the original position after observation; like If the condition shown in the formula is true, the test passes; otherwise, the test fails and the observation is not performed. 2) Effective observation distance condition: observer With the object to be observed The distance between them cannot exceed the farthest observation distance. Furthermore, there must be no obstacles obstructing the view between them; Observation pairs that do not meet either condition 1) or 2) above All were eliminated from the allocation scheme. Subsequent steps will not be performed, and the data will not be included in subsequent observation frequencies. .

6. The multi-UAV cooperative observation enhancement yaw angle planning method based on auction algorithm according to claim 1, characterized in that, S6 includes the following steps: Calculate the allocation scheme Each observer Expected yaw angle and with a constant angular velocity Execute redirection, set observer The current yaw angle is Observer With the object to be observed Their positions in the world coordinate system are respectively , Then the observer Expected yaw angle for: in Let X be the direction vector of the world coordinate system, which is related to... The included angle is for the observer Desired absolute yaw angle for alignment ; With constant angular velocity Execute from arrive The direction of rotation is determined by calculating the cross product to determine the direction of rotation due to angular velocity: in For the observer The X-axis direction vector of the body coordinate system, through its intersection with... Cross product The sign (positive or negative) indicates which direction the angular velocity will reach the destination faster; the final result is the observer's result. Angular velocity during steering. ; After the desired yaw angle and angular velocity are output, the yaw angle plan is output to the UAV controller to merge the position plan and yaw angle plan, and calculate the pose for subsequent execution.

Images