CN117826866A - UWB ranging-based multi-unmanned aerial vehicle co-location and formation control method - Google Patents
UWB ranging-based multi-unmanned aerial vehicle co-location and formation control method Download PDFInfo
- Publication number
- CN117826866A CN117826866A CN202311852956.XA CN202311852956A CN117826866A CN 117826866 A CN117826866 A CN 117826866A CN 202311852956 A CN202311852956 A CN 202311852956A CN 117826866 A CN117826866 A CN 117826866A
- Authority
- CN
- China
- Prior art keywords
- unmanned aerial
- aerial vehicle
- machine
- collar
- slave
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 230000015572 biosynthetic process Effects 0.000 title claims abstract description 37
- 239000011159 matrix material Substances 0.000 claims abstract description 26
- 238000013528 artificial neural network Methods 0.000 claims description 17
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000014509 gene expression Effects 0.000 claims description 6
- 238000012549 training Methods 0.000 claims description 4
- 238000013461 design Methods 0.000 abstract description 4
- 238000010586 diagram Methods 0.000 description 11
- 238000004891 communication Methods 0.000 description 10
- 238000005259 measurement Methods 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 5
- 230000001276 controlling effect Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 4
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 3
- 230000006872 improvement Effects 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000011478 gradient descent method Methods 0.000 description 2
- 241000084490 Esenbeckia delta Species 0.000 description 1
- 230000004913 activation Effects 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000036544 posture Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
- 230000003313 weakening effect Effects 0.000 description 1
Abstract
The invention discloses a multi-unmanned aerial vehicle co-location and formation control method based on UWB ranging, which belongs to the field of unmanned aerial vehicle location and control, and the method obtains the relative position coordinates of the multi-unmanned aerial vehicle through solving the relative distance between unmanned aerial vehicles under the GNSS refusing environment without a positioning system such as GPS, namely obtains a distance matrix by obtaining the current inter-machine distance of the multi-unmanned aerial vehicle, solves the uniquely determined relative position coordinates of the multi-unmanned aerial vehicle under the collar machine system, and realizes the multi-unmanned aerial vehicle co-location of the multi-unmanned aerial vehicle under the GNSS refusing environment; in addition, the invention combines the task execution scene and the task execution requirement of the multi-unmanned aerial vehicle system with the flight characteristics of the four-rotor unmanned aerial vehicle, and designs a multi-four-rotor unmanned aerial vehicle formation control method based on UWB ranging. Aiming at the environment in which the relative distance of the unmanned aerial vehicles can be obtained, the multi-unmanned aerial vehicle formation can be used in the environment in which the global position coordinates cannot be obtained and the relative distance of the multi-unmanned aerial vehicles can only be measured, and the application scene of the multi-unmanned aerial vehicle system is expanded.
Description
Technical Field
The invention belongs to the field of unmanned aerial vehicle positioning and control, and particularly relates to a multi-unmanned aerial vehicle co-positioning and formation control method based on UWB ranging.
Background
With the development and improvement of unmanned aerial vehicle technology, the practical application range is wider and wider, and task demands are more complicated and diversified, so that a single unmanned aerial vehicle has certain limitation. Therefore, research on the cooperative control problem of multiple unmanned aerial vehicles needs to be developed, so that the multiple unmanned aerial vehicle system can execute more complex cooperative tasks of multiple unmanned aerial vehicles, such as search and rescue, inspection, cooperative striking or trapping.
When the unmanned aerial vehicle executes the cooperative task, a task execution scene and task requirements need to be defined. Depending on whether or not there is a Global Navigation Satellite System (GNSS) that can be classified into a usable outdoor broad environment and a unusable GNSS (Global Navigation Satellite System) that rejects environments such as a tunnel, a dense forest, an airspace of signal interference, etc., an environment where the positioning signal is weak or no positioning signal. Under the environment, in order to solve the problem of co-location among multiple unmanned aerial vehicles, a depth vision-based co-location method is mostly adopted at home and abroad at present, or the multiple unmanned aerial vehicles are located by additionally arranging a base station. However, the visual algorithm has a problem of being greatly influenced by the illumination of the environment, and the base station is additionally arranged, so that the limited environment is large.
In the prior art, it is difficult to find a multi-unmanned aerial vehicle co-location and formation control algorithm which considers the yaw angle and the relative position relationship of unmanned aerial vehicles in an environment where only distance information is known. The existing cooperative system based on distance mostly realizes formation by regarding unmanned aerial vehicles as mass points and directly controlling the relative distance between multiple unmanned aerial vehicles, has the problem that the orientation of the head of the unmanned aerial vehicle cannot be obtained, and has the problem that multiple solutions cause the formation rotation of the multiple unmanned aerial vehicle system under an inertial system. This greatly limits the use scenarios of part of the external sensors carried by the unmanned aerial vehicle fuselage.
Disclosure of Invention
Aiming at the defects or improvement demands of the prior art, the invention provides a multi-unmanned-plane co-positioning and formation control method based on UWB ranging, which can realize the co-positioning of a multi-unmanned-plane system in an environment that the relative distance of the multi-unmanned-plane system can only be measured and the global position coordinate can not be obtained.
To achieve the above object, according to a first aspect of the present invention, there is provided a multi-unmanned aerial vehicle co-location method based on UWB ranging, including:
s1, by minimizing the objective functionObtaining the position coordinates of each unmanned aerial vehicle;
wherein, unmanned aerial vehicle's quantity is N+1, and the neck machine sets up two range finding modules, and N all set up a range finding module for N slave machines, totally N+2 range finding module, name UWBi respectively (i epsilon U uwb ,U uwb = {0,1,.. u i (i∈U uwb ) Wherein P is u 0 、P u 1 Position coordinates of two ranging modules arranged on the collar machine; the ranging module is a UWB ranging module; n+1 unmanned aerial vehicles are designated as unmanned aerial vehicles i (i e U, u= {0, 1..once., N }), where i is 0 represents the lead machine, the remaining unmanned aerial vehicles are slaves, all slaves constitute the set f= {1, 2..the N }, d ij (i,j∈U uwb ) Distance d between UWBi and UWBj obtained by ranging module arranged on unmanned aerial vehicle ij ＞0；
S2, converting the position coordinates of the unmanned aerial vehicles into a collar machine body coordinate system, and obtaining the relative position coordinates of the unmanned aerial vehicles in the collar machine body coordinate system.
According to a second aspect of the present invention, there is provided a multi-unmanned aerial vehicle co-location method based on UWB ranging, comprising:
inputting the distance acquired by any unmanned aerial vehicle in the unmanned aerial vehicle set to be positioned into a pre-trained neural network to obtain the relative position coordinates of each unmanned aerial vehicle in the unmanned aerial vehicle set to be positioned under the collar machine body coordinate system;
the distance comprises the distance between the ranging module on any unmanned aerial vehicle and the ranging module on other unmanned aerial vehicles in the unmanned aerial vehicle to be positioned and the distance between the ranging module on any two unmanned aerial vehicles except the unmanned aerial vehicle in the unmanned aerial vehicle to be positioned and the ranging module on other unmanned aerial vehicles;
the neural network is obtained by training with the distance acquired by any unmanned aerial vehicle in the unmanned aerial vehicle with the known position coordinates as input and the relative position coordinates of each unmanned aerial vehicle in the unmanned aerial vehicle with the known position coordinates under the collar machine body coordinate system as output;
the neural network has a loss function of:
wherein, unmanned aerial vehicle's quantity is N+1, and the neck machine sets up two range finding modules, and N all set up a range finding module for N slave machines, totally N+2 range finding module, name UWBi respectively (i epsilon U uwb ,U uwb = {0,1,.. u i (i∈U uwb ) Wherein P is u 0 、P u 1 Position coordinates of two ranging modules arranged on the collar machine; the ranging module is a UWB ranging module; n+1 unmanned aerial vehicles are designated as unmanned aerial vehicles i (i e U, u= {0, 1..once., N }), where i is 0 represents the lead machine, the remaining unmanned aerial vehicles are slaves, all slaves constitute the set f= {1, 2..the N }, d ij (i,j∈U uwb ) Distance d between UWBi and UWBj obtained by ranging module arranged on unmanned aerial vehicle ij ＞0。
According to a third aspect of the present invention, there is provided a multi-unmanned aerial vehicle formation control method based on UWB ranging, comprising:
a1, acquiring a two-dimensional relative motion equation of the collar machine and the slave machine;
a2, respectively constructing linear models of the slave machine and the collar machine to represent the states of the slave machine and constructing fixed time and speed observers for each slave machine according to the linear models; wherein the status includes position, speed;
a3, constructing a fixed time controller of each slave machine according to the two-dimensional relative motion equation, the linear model, the fixed time and speed observer and the position coordinates of each unmanned aerial vehicle relative collar machine acquired by adopting the method according to the first aspect or the second aspect;
and A4, acquiring expected control acceleration of each slave machine based on the fixed time controller, thereby obtaining expected attitude angles and expected thrust of each slave machine so as to realize control of the state of the slave machine.
According to a fourth aspect of the present invention, there is provided a multi-unmanned co-location system based on UWB ranging, comprising: a computer readable storage medium and a processor;
the computer-readable storage medium is for storing executable instructions;
the processor is configured to read executable instructions stored in the computer readable storage medium and perform the method according to the first or second aspect.
According to a fifth aspect of the present invention, there is provided a multi-unmanned aerial vehicle formation control system based on UWB ranging, comprising: a computer readable storage medium and a processor;
the computer-readable storage medium is for storing executable instructions;
the processor is configured to read executable instructions stored in the computer readable storage medium and perform the method according to the third aspect.
According to a fifth aspect of the present invention there is provided a computer readable storage medium storing computer instructions for causing a processor to perform the positioning method of the first or second aspect or the control method of the third invention.
In general, the above technical solutions conceived by the present invention, compared with the prior art, enable the following beneficial effects to be obtained:
1. according to the multi-unmanned aerial vehicle co-location method based on UWB (Ultra Wide Band) ranging, provided by the invention, under the GNSS refusing environment without a positioning system such as GPS (global positioning system), the relative position coordinates of the multi-unmanned aerial vehicle are obtained through solving the relative distance between unmanned aerial vehicles, namely, the distance matrix is obtained through obtaining the current inter-machine distance of the multi-unmanned aerial vehicle, the uniquely determined relative position coordinates of the multi-unmanned aerial vehicle under the collar machine system are solved, and the data of an optical flow, an inertial measurement unit and a down-looking laser radar are fused, so that the multi-unmanned aerial vehicle co-location of the multi-unmanned aerial vehicle under the GNSS refusing environment is realized.
2. According to the multi-unmanned aerial vehicle co-location method based on UWB ranging, the problems that convergence is slow and solving difficulty is high in a traditional optimization-based algorithm such as a nonlinear least square method and a gradient descent method are considered as the number of unmanned aerial vehicles increases, and the optimization problem from a distance matrix to a relative position coordinate is solved by combining a lightweight neural network and utilizing the characteristics of low calculation resource consumption and high calculation speed of the lightweight neural network; the co-location process is accelerated by incorporating artificial intelligence algorithms.
3. The invention designs a multi-four-rotor unmanned aerial vehicle formation control method based on UWB ranging by combining a multi-unmanned aerial vehicle system task execution scene, task execution demands and flight characteristics of the four-rotor unmanned aerial vehicle. Aiming at the environment in which the relative distance of the unmanned aerial vehicles can be obtained, the multi-unmanned aerial vehicle formation can be used in the environment in which the global position coordinates cannot be obtained and the relative distance of the multi-unmanned aerial vehicles can only be measured, and the application scene of the multi-unmanned aerial vehicle system is expanded.
4. According to the multi-four-rotor unmanned aerial vehicle formation control method based on UWB ranging, a distance matrix is obtained by obtaining the current inter-plane distance of the multi-unmanned aerial vehicle, the uniquely determined relative position coordinates of the multi-unmanned aerial vehicle under a collar machine system are solved, and a control instruction is solved by combining the relative position and speed of the multi-unmanned aerial vehicle and flight state information transmitted by an adjacent machine, so that the flight state of each unmanned aerial vehicle is cooperatively controlled; the multi-unmanned aerial vehicle system completes the co-location and formation control within a fixed time under the environment that the relative distance of the multi-unmanned aerial vehicle can only be measured and the global position coordinates can not be obtained.
5. According to the multi-four-rotor unmanned aerial vehicle formation control method based on UWB ranging, when the state observer of each slave is designed, the influence of the convergence speed of the observer on the control effect is considered, the speed observer meeting the convergence of fixed time is designed, when the controller of each slave is designed, the convergence speed of the formation error is considered, the fixed time controller is designed, the consistency condition of a multi-unmanned aerial vehicle system in fixed time can be met, and the formation task is completed.
Drawings
FIG. 1 is a block diagram of a quad-rotor unmanned helicopter system;
FIG. 2 is a block diagram of a multiple unmanned system;
FIG. 3 is a schematic diagram of a co-located multi-solution problem;
FIG. 4 is a schematic diagram of a collar machine fixed two-point co-location according to an embodiment of the present invention;
FIG. 5 is a schematic diagram illustrating installation of a UWB ranging module of a collar machine according to an embodiment of the present invention;
FIG. 6 is a schematic diagram of co-location provided by an embodiment of the present invention;
fig. 7 is a schematic diagram of a node communication topology structure of an unmanned aerial vehicle according to an embodiment of the present invention;
fig. 8 is a schematic diagram of a neural network according to an embodiment of the present invention.
FIG. 9 is a schematic diagram of a dequeue algorithm according to an embodiment of the present invention;
Detailed Description
The present invention will be described in further detail with reference to the drawings and examples, in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention. In addition, the technical features of the embodiments of the present invention described below may be combined with each other as long as they do not collide with each other.
The unmanned aerial vehicle is a four-rotor unmanned aerial vehicle, and a hardware system of the four-rotor unmanned aerial vehicle is described below.
The four-rotor unmanned aerial vehicle hardware system for cooperative formation control is shown in fig. 1 and 2, wherein fig. 1 is a hardware connection diagram of a single unmanned aerial vehicle, and mainly comprises six parts, namely an upper computer, a lower computer, a sensor module, a power module, an executing mechanism and an external safety guarantee. The sensor module provides pose information of the unmanned aerial vehicle for the lower computer, wherein the pose information comprises a three-dimensional position P and quaternion information describing a pose angle. The lower computer transmits the state quantity of the unmanned aerial vehicle to the upper computer, and the expected control quantity is calculated by combining the control requirement and transmitted to the lower computer for execution. The actuating mechanism provides power for unmanned aerial vehicle's flight. The power module provides power support for onboard systems such as an upper computer, a lower computer, a sensor module, an executing mechanism and the like. The upper computer, the lower computer and the sensor module in the present invention will be described in detail.
(1) Sensor module
The sensor modules used in the present invention according to application and performance requirements are: magnetometer, gyroscope, laser ranging radar, optical flow, accelerometer, and remote control receiver. Wherein the magnetometer, gyroscope and accelerometer are integrated sensors built in the flight control board. The magnetometer provides machine head orientation information for the system, and the gyroscope and accelerometer data are fused to provide attitude angle and triaxial acceleration information for the unmanned aerial vehicle system. The optical flow and the laser range radar are external sensors, wherein the optical flow is a low-resolution lower camera, the speed information in the horizontal plane of the unmanned plane system can be estimated by combining the operation optical flow algorithm with the unmanned plane height, and the accurate height information can be obtained after the laser range radar is arranged. The remote control receiver and the remote controller in the external safety guarantee module are a set of signal receiving and transmitting equipment, and the unmanned aerial vehicle can be forced to jump from a program control mode to a manual control mode by poking a control rod in the remote controller by a flying hand, so that the flight safety of an unmanned aerial vehicle system is ensured in sequence.
(2) Lower computer
The invention uses PIXHWAKmini6 c as a lower computer, namely a flight control board, according to application and performance requirements. The data fusion algorithm of the sensor is operated in the module, the accurate state information of unmanned aerial vehicle systems such as positions, speeds, accelerations and postures is obtained by fusing data of built-in sensors such as magnetometers, gyroscopes and accelerometers and external sensors such as optical flows and laser range radars, and the received control instructions from the upper computer are combined with the state information to generate PWM signals for controlling the rotating speed of the motor. The PWM signal is then transmitted to an electronic governor for controlling the rotational speed of the motor.
(3) Upper computer
The invention uses NVIDIAJetson TX2 NX as an upper computer according to application and performance requirements. The upper computer is used as a comprehensive module, a set of state machine system is operated in the board, information sent by modules such as the lower computer and UWB is received, and a desired control instruction is generated by combining the current flight task and is transmitted to the flight control board for execution. The control instruction in the invention is the expected attitude angle and the expected thrust of the unmanned aerial vehicle, and the unmanned aerial vehicle system can be controlled most flexibly and safely on the basis of the expected attitude angle and the expected thrust.
On the basis of a single unmanned aerial vehicle, a multi-unmanned aerial vehicle system is built, the multi-unmanned aerial vehicle system interacts based on the ROS, and information transmitted by the adjacent unmanned aerial vehicles can be acquired between unmanned aerial vehicles with communication links. According to the invention, each unmanned aerial vehicle is provided with UWB ranging sensors, and two UWB ranging sensors are arranged on the unmanned aerial vehicle.
The embodiment of the invention provides a multi-unmanned aerial vehicle co-location method based on UWB ranging, which comprises the following steps:
s1, by minimizing the objective functionObtaining the position coordinates of each unmanned aerial vehicle;
wherein, unmanned aerial vehicle's quantity is N+1, and the neck machine sets up two range finding modules, and N all set up a range finding module for N slave machines, totally N+2 range finding module, name UWBi respectively (i epsilon U uwb ,U uwb = {0,1,.. u i (i∈U uwb ) Wherein P is u 0 、P u 1 Position coordinates of two ranging modules arranged on the collar machine; the ranging module is a UWB ranging module; n+1 unmanned aerial vehicles are designated as unmanned aerial vehicles i (i e U, u= {0, 1..once., N }), where i is 0 represents the lead machine, the remaining unmanned aerial vehicles are slaves, all slaves constitute the set f= {1, 2..the N }, d ij (i,j∈U uwb ) Distance d between UWBi and UWBj obtained by ranging module arranged on unmanned aerial vehicle ij ＞0；
S2, converting the position coordinates of the unmanned aerial vehicles into a collar machine body coordinate system, and obtaining the relative position coordinates of the unmanned aerial vehicles in the collar machine body coordinate system.
Specifically, according to the application and performance requirements, the distance measuring mode used by the invention is UWB, the model of the distance measuring module is Nooploop LinkTrack P-AS, and the module can provide distance measurement with centimeter-level precision in the range of 40 meters. And a DRMode distributed ranging mode is adopted, so that the distance information between a certain module and other ranging modules can be obtained.
Considering that the problem of multiple solutions of the topology exists for the whole multi-unmanned aerial vehicle system under the condition that only the distance between the single machine and the single machine is known, as shown in fig. 3, the problem of multiple solutions exists because the specific orientation of the slave machine relative to the master machine is not known. In order to solve the unique solution under the system of the collar machine, the invention uses two distance measuring modules with known distances on the collar machine, as shown in fig. 4, so that the unique solution can be obtained by adopting a differential idea according to different distances between the slave machine and the two distance measuring modules of the collar machine, and the sensor on the collar machine is installed as shown in fig. 5, wherein (1) and (2) are respectively two identical UWB distance measuring modules, and the distance between the two distance measuring modules is known to be 0.4 meter. After the coordinates of each module are obtained through solving, the position coordinates of each ranging module obtained through solving are converted into the machine body coordinate system of the collar machine through rotating and translating of the coordinate system, and the unique relative position coordinates of each module in the machine body system of the collar machine can be obtained. Thus, the relative position coordinates of the multiple unmanned aerial vehicles can be obtained, and formation control can be performed.
Considering that the classical method for obtaining the node coordinate position from the node distance information is mostly based on a full-connected network, namely, the method is solved integrally after the distances among all nodes are known, and the method has a great limitation. The n+1 unmanned aerial vehicle is named as unmanned aerial vehicle i (i epsilon U, U= {0, 1..once., N }), wherein i represents the collar machine when being 0, the rest unmanned aerial vehicles are slaves, all slaves form a set F= {1, 2..once., N }, and the relative position coordinate of the slave i (i epsilon F) under the collar machine system is set as P i ＝(X i ,Y i ). Each slave machine is provided with a distance measuring module, and the collar machine is provided with two measuring modulesThe distance modules are n+2 distance measuring modules, which are named UWBi (i. Epsilon. U) uwb ,U uwb = {0,1,.. u i (i∈U uwb ) Wherein P is u 0 、P u 1 The position coordinates of the two ranging modules are arranged on the collar machine. The invention adopts an optimization-based method to solve the problem, namely, the relative position coordinate matrix is needed to be solvedSo that the cost functionMinimum, where d ij I, j satisfy d as measured by UWB for the distance between UWBi and UWBj ij And (3) carrying out rotary translation on the coordinate system obtained by solving until the coordinate system coincides with the collar machine system. Assuming that the centroid of the collar machine is at P u 0 、P u 1 The center of the slave machine coincides with the position of the self-assembled ranging sensor, so that the relative position coordinate P of each unmanned aerial vehicle under the collar machine system can be obtained i . The invention adopts the measurement adjustment method to optimize the measurement error generated by the measurement accuracy reason of the UWB ranging module, thereby weakening the measurement error to the measured value d ij Is a function of (a) and (b).
According to the invention, each unmanned aerial vehicle knows the distance information (acquired by UWB) with other unmanned aerial vehicles, and according to the triangle positioning theory, the unmanned aerial vehicle can solve and obtain the relative position coordinates of each unmanned aerial vehicle by only combining the distance information (transmitted to the unmanned aerial vehicle by communication mode) of the other unmanned aerial vehicles and the distance information of the other unmanned aerial vehicles, which are measured by the other unmanned aerial vehicles.
As shown in fig. 6, for the number (4) machine, distance information with other unmanned aerial vehicles can be measured, and after receiving data measured from the number (2) machine and the number (5) machine, a distance information structure shown in the lower right corner of fig. 6 can be obtained, and it can be found that the structure is rigid. On the basis, the unique relative position coordinate solving result can be obtained by building the machine system of the collar machine in the center of the collar machine. The distance matrix of the unmanned aerial vehicle is simplified to a certain extent, and the calculation difficulty is reduced.
The machine body coordinate system of the collar machine takes the mass center of the collar machine as an origin, the machine head direction of the collar machine is a Y axis, and the right side or the left side of the machine body of the collar machine is an X axis.
For the whole multi-unmanned aerial vehicle system, the relative position coordinates can be obtained only by ensuring that the input degree of each unmanned aerial vehicle node of the communication topological structure is more than or equal to 2, so that distributed control is performed. One such communication topology is shown in fig. 7.
Along with the increase of the number of unmanned aerial vehicles, the distance matrix is more complex, and the problems of slow convergence and high solving difficulty occur in the traditional optimization-based algorithm such as a nonlinear least square method, a gradient descent method and the like. Based on the above, the invention combines a lightweight neural network and solves the problem of optimizing the distance matrix to the relative position coordinates by utilizing the properties of low calculation resource consumption and high calculation speed, namely, the embodiment of the invention provides a multi-unmanned aerial vehicle co-location method based on UWB ranging, which comprises the following steps:
inputting the distance (preferably represented in a distance matrix) acquired by any unmanned aerial vehicle in the unmanned aerial vehicle group to be positioned into a pre-trained neural network to obtain the relative position coordinates of each unmanned aerial vehicle in the unmanned aerial vehicle group to be positioned under the body coordinate system of the collar machine;
the distance comprises the distance between the ranging module on any unmanned aerial vehicle and the ranging module on other unmanned aerial vehicles in the unmanned aerial vehicle to be positioned and the distance between the ranging module on any two unmanned aerial vehicles except the unmanned aerial vehicle in the unmanned aerial vehicle to be positioned and the ranging module on other unmanned aerial vehicles;
the neural network is obtained by training with a distance matrix acquired by any unmanned aerial vehicle in an unmanned aerial vehicle set with a known position coordinate as input and the relative position coordinate of each unmanned aerial vehicle in the unmanned aerial vehicle set with the known position coordinate under a collar machine body coordinate system as output; it can be appreciated that in the unmanned aerial vehicle with known position coordinates, the position coordinates of each unmanned aerial vehicle are randomly generated.
The neural network has a loss function of:
wherein, unmanned aerial vehicle's quantity is N+1, and the neck machine sets up two range finding modules, and N all set up a range finding module for N slave machines, totally N+2 range finding module, name UWBi respectively (i epsilon U uwb ,U uwb = {0,1,.. u i (i∈U uwb ) Wherein P is u 0 、P u 1 Position coordinates of two ranging modules arranged on the collar machine; the ranging module is a UWB ranging module; n+1 unmanned aerial vehicles are designated as unmanned aerial vehicles i (i e U, u= {0, 1..once., N }), where i is 0 represents the lead machine, the remaining unmanned aerial vehicles are slaves, all slaves constitute the set f= {1, 2..the N }, d ij (i,j∈U uwb ) Distance d between UWBi and UWBj obtained by ranging module arranged on unmanned aerial vehicle ij ＞0。
Specifically, as shown in fig. 8, the neural network structure used in the present invention inputs distance information that can be acquired by the unmanned aerial vehicle i, the number is the product of the total number n+2 of ranging modules and the sum of the number of incoming degrees+1, that is, the data size is (n+2) × (number of incoming degrees+1), and after passing through a hidden layer, two-dimensional relative position coordinates of n+1 points (position coordinates of all unmanned aerial vehicles in the collar machine system) are output, and the data size is 2×1. The activation function uses a sigmoid function, and the back propagation loss function isThis is also an optimized cost function, with gradient descent for updating network parameters.
When the training set is constructed, the method randomly generates N+2 points with known coordinates, and obtains a distance matrix based on the N+2 points. According to a preset communication topological structure, only a part of a complete distance matrix can be obtained for the unmanned aerial vehicle i, the known part of the distance matrix is used as input of a neural network, and the midpoint coordinates (the centroid position of the collar machine) of the first point and the second point and the total N+1 coordinates (the centroid position of the slave machine) of the rest N coordinates are used as expected output. Therefore, a neural network can be trained based on the neural network structure, and the problem of the invention that the co-location is quickly solved is solved.
The network used by the invention has a simple structure, can solve and output in real time, and ensures the requirement on the control period in formation control.
The embodiment of the invention provides a multi-unmanned aerial vehicle formation control method based on UWB ranging, which comprises the following steps:
a1, acquiring a two-dimensional relative motion equation of the collar machine and the slave machine;
a2, respectively constructing linear models of the collar machine and the slave machines to represent the states of the collar machine and the slave machines, and constructing a fixed time speed observer for each slave machine according to the linear models to observe the speed of the collar machine; wherein the status includes position, speed;
a3, constructing a fixed time controller of each slave according to the two-dimensional relative motion equation, the linear model, the fixed time and speed observer and the position coordinates of each unmanned aerial vehicle relative collar acquired by adopting the method as set forth in any one of claims 1-3;
and A4, acquiring expected acceleration of each slave machine based on the fixed time controller, thereby obtaining expected attitude angles and expected thrust of each slave machine so as to realize control of the state of the slave machine.
Specifically, after the relative position coordinates of the multiple unmanned aerial vehicles are obtained based on inter-aircraft distances, the formation control is completed by adopting a collar-driven distributed cooperative control algorithm. The algorithm designates one unmanned aerial vehicle in the formation as a collar machine, and other unmanned aerial vehicles as slave machines, and the slave machines actively follow the collar machine to form the formation by controlling the relative positions of the slave machines and the collar machine. And the method is improved on the basis of the traditional distributed observer controller algorithm, and the effect of completing formation tasks within fixed time is achieved.
Let n+1 unmanned aerial vehicles and number i (i e U, u= {0, 1..once., N }) where i=0 this unmanned aerial vehicle is the lead aircraft and the remaining unmanned aerial vehicles are slaves, all slaves constitute the set f= {1, 2..once., N }). Start executionThe height of the collar machine is consistent with that of the slave machine during formation task, the six-degree-of-freedom problem of the multi-unmanned aerial vehicle is reduced to a motion problem in a two-dimensional plane, so that a relative motion equation based on the collar machine is solved, a schematic diagram is shown in fig. 9, wherein the left upper corner thickened unmanned aerial vehicle is the collar machine, and the right lower corner is the slave machine. With northeast-north coordinate system as inertial system x w Oy w The position coordinate of the collar machine is p L ＝(x L ,y L ) T The position coordinate of the slave machine is p F ＝(x F ,y F ) T The yaw angle of the collar machine is set as psi L The yaw angle of the slave machine is phi F . The machine system is characterized in that the position of the collar machine is taken as an origin, the machine head direction is taken as a Y axis, and the right side of the machine body is taken as an X axis.
When flying at a small angle, namely the pitch angle and the roll angle of the unmanned aerial vehicle are regarded as 0, the motion equation of the collar machine and the slave machine in the two-dimensional plane is as follows:
wherein V is LX 、V LY Representing the velocity component of the collar machine along the coordinate axis under the own machine system, V FX 、V FY Representing the velocity component of the slave machine along the coordinate axis under the system of the slave machine.
P i ＝(X i ,Y i ) T The relative position coordinates of the positions of the i-number slaves in the body coordinate system of the collar machine are represented, and according to the geometric relationship, the relative positions of the i-number slaves (i epsilon F) of the slave machines can be represented as follows:
and (3) deriving the (2) and substituting the derivative into the (1), so that a two-dimensional relative motion equation based on the collar machine can be obtained as follows:
thus, when the yaw angles of all the unmanned aerial vehicles are equal and 0, i.e., ψ F ＝ψ L When=0, (3) can be simplified to
Setting the expected position coordinates of the slave i (i epsilon F) relative to the collar machine asThe relative expected speed ish i (t)＝[h ip T (t) h iv T (t)] T According to the consistency theorem, to complete the formation task and keep the formation stable, the requirement of
Wherein xi i ＝[p i T v i T ] T i (i.e.U) represents the state quantity of the unmanned plane.
The invention adopts a double integral linear model which considers that resistance exists and is proportional to speed to establish a state expression of the master machine and the slave machine. It can be understood that the state expressions of the master machine and the slave machine can also be established by adopting other linear models such as a single integral linear model and the like.
Assuming that the yaw angles of the unmanned aerial vehicles are the same and unchanged, and that the collar is in a constant-speed cruising state, i.e. the acceleration is 0, a state space expression is constructed for the collar
For the collar machine, there is
For slave i (i.e.F), a state space expression is constructed
For the slave, there is
Wherein,α v is a constant, and represents the influence of resistance on the state of the unmanned aerial vehicle, the resistance is positively correlated with the speed of the unmanned aerial vehicle, u i (t) is the control quantity of the slave, and is determined by a controller designed later, I q For the identity matrix of q-order, q is the spatial dimension, and 2 is taken in the invention, namely only the control in the horizontal direction is considered.
For the multi-unmanned aerial vehicle system, not all slaves can acquire the state information of the collar machine, and in order to realize distributed control, an observer is required to be designed on the slaves to observe the state information such as the position, the speed and the like of the collar machine. (wherein the positional information is obtained by the positioning method according to any one of the above embodiments)
Constructing a matrix H=L+delta according to a communication topological structure, wherein L is a Laplacian matrix of a communication topology between slaves, and is the difference between an input degree matrix and the communication matrix, namely L=D-w, and w is when the unmanned aerial vehicle i (i epsilon F) can acquire the information of the unmanned aerial vehicle j ij =1, otherwise w ij =0, diagonal matrix d=diag { D } 1 ...d N }，Delta is used to describe the communication topology between the slave and the neck machine, i.e. delta when the slave i (i e F) can obtain the information of the neck machine ii The remaining elements in Δ are 0.
The invention aims at the problem that the fixed time speed observer for observing the state of the collar machine is designed as follows
Wherein the method comprises the steps ofη 0v For state information of the neck machine, i.e. eta 0v ＝v 0 ，v 0 Speed of collar machine, eta iv (i epsilon F) is the observed value of the speed of the slave machine to the collar machine, mu is an adjustable parameter, and the sig function represents sig (x) a ＝sign(x)|x| a a∈R，/>
Unlike the conventional observer for multiple unmanned aerial vehicles, the observer adopted by the invention can ensure that the observed speed value converges within a fixed time, and when μ= (2λ) in (10) min_H R) -1 P v Wherein P is v For Riccati equation A v T P v +P v A v -P v R -1 P v +i=0, R is a real coefficient matrix, in the present invention R is I q . And a 1 、a 2 At > 0, the observed error fixed time convergence can be expressed as
Wherein lambda is max_H Represents the maximum value, lambda, of the eigenvalues of matrix H min_H Representing the minimum value of the eigenvalues of matrix H.
The invention combines LQR control algorithm to design control quantity, designs a fixed time controller for each slave i (i E F) as
Wherein the method comprises the steps ofAnd->K is an adjustable parameter.
Different from classical LQR, the fixed time controller designed by the invention can ensure that the multi-unmanned aerial vehicle system reaches a consistency condition in fixed time to complete a formation task. When K= (2λ) min_H R) -1 B T P u Wherein P is u For Riccati equation A T P u +P u A-P u BR -1 B T P u Solution of +i=0, and a 1 、a 2 At > 0, the problem can be demonstrated to meet the consistency condition in a fixed time, i.e
The relative position observer, the fixed time and speed observer and the fixed time formation controller designed by combining the invention can ensure that the multi-unmanned-plane system is in T u +T o And meeting consistency conditions in time to complete formation tasks.
The embodiment of the invention provides a multi-unmanned aerial vehicle co-location system based on UWB ranging, which comprises the following components: a computer readable storage medium and a processor;
the computer-readable storage medium is for storing executable instructions;
the processor is configured to read executable instructions stored in the computer readable storage medium and perform the positioning method according to any of the embodiments described above.
The embodiment of the invention provides a multi-unmanned aerial vehicle formation control system based on UWB ranging, which is characterized by comprising the following steps: a computer readable storage medium and a processor;
the computer-readable storage medium is for storing executable instructions;
the processor is configured to read executable instructions stored in the computer readable storage medium and execute the control method according to any one of the above embodiments.
An embodiment of the present invention provides a computer-readable storage medium, where the computer-readable storage medium stores computer instructions for causing a processor to execute the positioning method according to any one of the embodiments or the control method according to any one of the embodiments.
It will be readily appreciated by those skilled in the art that the foregoing description is merely a preferred embodiment of the invention and is not intended to limit the invention, but any modifications, equivalents, improvements or alternatives falling within the spirit and principles of the invention are intended to be included within the scope of the invention.
Claims (10)
1. The multi-unmanned aerial vehicle co-location method based on UWB ranging is characterized by comprising the following steps:
s1, by minimizing the objective functionObtaining the position coordinates of each unmanned aerial vehicle;
wherein, unmanned aerial vehicle's quantity is N+1, and the neck machine sets up two range finding modules, and N all set up a range finding module from the machine, totally N+2 range finding module, and the name is UWBi respectively, i epsilon U uwb ,U uwb = {0,1,.. u i ，i∈U uwb Wherein P is u 0 、P u 1 Position coordinates of two ranging modules arranged on the collar machine; the ranging module is a UWB ranging module; n+1 unmanned aerial vehicles are named unmanned aerial vehicles i, i e U, U={0, 1..the., N }, wherein i represents the collar machine when 0, the remaining drones are slaves, all slaves make up the set f= {1, 2..the., N }, d ij Distance d between UWBi and UWBj obtained by ranging module arranged on unmanned aerial vehicle ij ＞0，i,j∈U uwb ；
S2, converting the position coordinates of the unmanned aerial vehicles into a collar machine body coordinate system, and obtaining the relative position coordinates of the unmanned aerial vehicles in the collar machine body coordinate system.
2. The method of claim 1, wherein the collar machine body coordinate system uses a center of mass of the collar machine as an origin, a machine head direction of the collar machine as a Y axis, and a right side or a left side of a machine body of the collar machine as an X axis.
3. The multi-unmanned aerial vehicle co-location method based on UWB ranging is characterized by comprising the following steps:
inputting the distance acquired by any unmanned aerial vehicle in the unmanned aerial vehicle set to be positioned into a pre-trained neural network to obtain the relative position coordinates of each unmanned aerial vehicle in the unmanned aerial vehicle set to be positioned under the collar machine body coordinate system;
the distance comprises the distance between the ranging module on any unmanned aerial vehicle and the ranging module on other unmanned aerial vehicles in the unmanned aerial vehicle to be positioned and the distance between the ranging module on any two unmanned aerial vehicles except the unmanned aerial vehicle in the unmanned aerial vehicle to be positioned and the ranging module on other unmanned aerial vehicles;
the neural network is obtained by training with the distance acquired by any unmanned aerial vehicle in the unmanned aerial vehicle with the known position coordinates as input and the relative position coordinates of each unmanned aerial vehicle in the unmanned aerial vehicle with the known position coordinates under the collar machine body coordinate system as output;
the neural network has a loss function of:
wherein, unmanned aerial vehicle's quantity is N+1, and the neck machine sets up two range finding modules, N slave machinesA distance measuring module is arranged, and the total of N+2 distance measuring modules are respectively named UWBi, i epsilon U uwb ,U uwb = {0,1,.. u i ，i∈U uwb Wherein P is u 0 、P u 1 Position coordinates of two ranging modules arranged on the collar machine; the ranging module is a UWB ranging module; n+1 unmanned aerial vehicles are named unmanned aerial vehicles i, i e U, u= {0, 1..the N }, wherein i is 0 represents the lead machine, the rest unmanned aerial vehicles are slaves, and all slaves form a set f= {1, 2..the N }, d ij Distance d between UWBi and UWBj obtained by ranging module arranged on unmanned aerial vehicle ij ＞0，i,j∈U uwb 。
4. The multi-unmanned aerial vehicle formation control method based on UWB ranging is characterized by comprising the following steps:
a1, acquiring a two-dimensional relative motion equation of the collar machine and the slave machine;
a2, respectively constructing linear models of the slave machine and the collar machine to represent the states of the slave machine and constructing fixed time and speed observers for each slave machine according to the linear models; wherein the status includes position, speed;
a3, constructing a fixed time controller of each slave according to the two-dimensional relative motion equation, the linear model, the fixed time and speed observer and the position coordinates of each unmanned aerial vehicle relative collar acquired by adopting the method as set forth in any one of claims 1-3;
and A4, acquiring expected control acceleration of each slave machine based on the fixed time controller, thereby obtaining expected attitude angles and expected thrust of each slave machine so as to realize control of the state of the slave machine.
5. The method of claim 4, wherein the fixed time speed observer of each slave is:
wherein eta iv The observation value of the speed of the i number slave machine to the collar machine is shown, and mu is an adjustable parameter;w when slave i can acquire information of slave j ij =1, otherwise w ij ＝0，η jv Is the observation value eta of the j number slave machine to the collar machine speed 0v For the speed of the collar machine, delta when the slave machine i can obtain the information of the collar machine ii =1, otherwise Δ ii ＝0；a 1 、a 2 B are adjustable coefficients, and the expression of the sig function is sig (x) a ＝sign(x)|x| a ，a∈R；P v For Riccati equation A v T P v +PA v -P v R -1 P v Solution of +i=0, ++i =0>α v Is constant, I q The q-order identity matrix and R is a real coefficient matrix.
6. The method of claim 4 or 5, wherein the fixed time controller of each slave is:
wherein, K is an adjustable parameter,P i 、P 0 the relative position coordinates of the i-number slave machine and the collar machine under the collar machine body coordinate system are v i Speed of slave machine with number i, eta iv As the observed value of the i number slave machine to the speed of the collar machine, delta when the slave machine i can obtain the information of the collar machine ii =1, otherwise Δ ii ＝0；a 1 、a 2 B are adjustable coefficients, and the expression of the sig function is sig (x) a ＝sign(x)|x| a ，a∈R；h i ＝[h ip T h iv T ] T For the expected formation vector of the slave i relative to the master, h ip To expect relative position vector, h iv To expect relative velocity vector, P u For Riccati equation A T P u +P u A-P u BR -1 B T P u Solution of +i=0, ++i =0>α v Is constant, I q The q-order identity matrix and R is a real coefficient matrix.
7. The method of claim 4, wherein the linear model is a double integral model.
8. UWB ranging-based multi-unmanned aerial vehicle co-location system, which is characterized by comprising:
a computer readable storage medium and a processor;
the computer-readable storage medium is for storing executable instructions;
the processor is configured to read executable instructions stored in the computer readable storage medium and perform the method of any one of claims 1-3.
9. UWB ranging-based multi-unmanned aerial vehicle formation control system is characterized by comprising:
a computer readable storage medium and a processor;
the computer-readable storage medium is for storing executable instructions;
the processor is configured to read executable instructions stored in the computer readable storage medium and perform the method of any one of claims 4-7.
10. A computer-readable storage medium storing computer instructions for causing a processor to perform the positioning method according to any one of claims 1-3 or the control method according to any one of claims 4-7.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311852956.XA CN117826866A (en) | 2023-12-29 | 2023-12-29 | UWB ranging-based multi-unmanned aerial vehicle co-location and formation control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311852956.XA CN117826866A (en) | 2023-12-29 | 2023-12-29 | UWB ranging-based multi-unmanned aerial vehicle co-location and formation control method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117826866A true CN117826866A (en) | 2024-04-05 |
Family
ID=90507354
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311852956.XA Pending CN117826866A (en) | 2023-12-29 | 2023-12-29 | UWB ranging-based multi-unmanned aerial vehicle co-location and formation control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117826866A (en) |
-
2023
- 2023-12-29 CN CN202311852956.XA patent/CN117826866A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110262553B (en) | Fixed-wing unmanned aerial vehicle formation flying method based on position information | |
| Scaramuzza et al. | Vision-controlled micro flying robots: from system design to autonomous navigation and mapping in GPS-denied environments | |
| Herissé et al. | Landing a VTOL unmanned aerial vehicle on a moving platform using optical flow | |
| Tomic et al. | Toward a fully autonomous UAV: Research platform for indoor and outdoor urban search and rescue | |
| Cheng et al. | Time-optimal UAV trajectory planning for 3D urban structure coverage | |
| Zhu et al. | Model of collaborative UAV swarm toward coordination and control mechanisms study | |
| Cai et al. | An overview on development of miniature unmanned rotorcraft systems | |
| De et al. | Control design for an under-actuated UAV model | |
| Hérissé et al. | A terrain-following control approach for a vtol unmanned aerial vehicle using average optical flow | |
| EP1901153A1 (en) | Control system for unmanned 4-rotor-helicopter | |
| US20210247782A1 (en) | Control of vehicle movement by application of geometric algebra and state and error estimation | |
| Montenegro et al. | A review on distributed control of cooperating mini UAVs | |
| CN113625774A (en) | Multi-unmanned aerial vehicle cooperative positioning system and method for local map matching and end-to-end distance measurement | |
| D'Amato et al. | Attitude and position estimation for an UAV swarm using consensus Kalman filtering | |
| Bodi et al. | Reinforcement learning based UAV formation control in GPS-denied environment | |
| CN113110593A (en) | Flight formation cooperative self-adaptive control method based on virtual structure and estimation information transmission | |
| Herisse et al. | A nonlinear terrain-following controller for a vtol unmanned aerial vehicle using translational optical flow | |
| Xian et al. | Hovering control of a nano quadrotor unmanned aerial vehicle using optical flow | |
| Wang et al. | Formation flight of unmanned rotorcraft based on robust and perfect tracking approach | |
| Hua | Contributions to the automatic control of aerial vehicles | |
| Chen et al. | A moving target tracking control and obstacle avoidance of quadrotor UAV based on sliding mode control using artificial potential field and RBF neural networks | |
| Zhang et al. | Multi-sensory motion estimation and control of a mini-quadrotor in an air-ground multi-robot system | |
| CN117826866A (en) | UWB ranging-based multi-unmanned aerial vehicle co-location and formation control method | |
| Mueller | A dynamics-agnostic state estimator for unmanned aerial vehicles using ultra-wideband radios | |
| Tingting et al. | Formation control of multiple UAVs via pigeon inspired optimisation |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination |