Resonance light-based unmanned aerial vehicle relative positioning system, unmanned aerial vehicle cluster and positioning method
By installing resonant light transmitting and receiving ends on the drone, and combining the central processing module and flight control module, the problems of internal positioning accuracy and computing resources of the drone group are solved, and high-precision relative positioning and attitude measurement are achieved, eliminating the removal of Time difference error improves the accuracy and real-timeness of the relative positioning of the drone group.
Patent Information
- Application Number
- CN202411105974.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-13
- Publication Date
- 2024-12-06
AI Technical Summary
The existing drone relative positioning technology is difficult to deploy in unknown environments, especially the base station-based method is not suitable for internal positioning of drone groups, and the methods based on airborne equipment such as UWB, vision and LiDAR have problems with accuracy and computing resources. .
Using a drone relative positioning system based on resonant light, the centimeter-level accuracy positioning without additional matching algorithm is achieved by installing alternating resonant light emitting and receiving ends on the drone, and using a responsive reflector and image sensor, and combining with central processing The module and flight control module perform relative positioning and attitude estimation, and design direct indirect link optimization algorithms to improve accuracy.
It realizes high-precision relative positioning and attitude measurement within the drone group, eliminates positioning errors caused by time difference, and improves the accuracy and real-timeness of the relative positioning of the drone group.
Smart Images

Figure CN119087451A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of wireless positioning of unmanned aerial vehicles, and in particular to a group of unmanned aerial vehicles system and a relative positioning method based on improved resonant light. Background Art
[0002] Existing relative positioning technologies for drones can be divided into those based on base stations and those based on airborne devices. Base station-based technologies include GPS, motion capture systems, and UWB (UWB base stations triangulate targets with UWB tags), which are difficult to deploy in unknown environments. Airborne-based technologies can be roughly divided into UWB, vision, and LiDAR. As an airborne device, UWB can only be used for relative distance measurement between drones. If you want to know the direction information, you need to cooperate with other means, and the positioning accuracy is often above decimeters; the cost of using visual methods is low, and relative positioning usually requires the configuration of target detection modules, which requires certain computing resources, and is limited by the accuracy affected by illumination, pixel resolution, etc.; LiDAR-based relative positioning has a high centimeter-level accuracy and is not affected by factors such as illumination, but requires a large computing power to identify drones from 3D point clouds for positioning. Resonant light positioning has centimeter-level accuracy, and pairs of transmitters and receivers are installed on two drones respectively, which can complete positioning without additional matching algorithms, and requires less computing power resources.
[0003] Chinese invention patent application 201910294606.3 discloses a tracking and positioning system based on separated resonant light, which uses a separated resonant cavity long cavity laser structure with a resonant adapter unit. The resonant light beam can always remain coincident during the back and forth reflection process, and can provide the distance and relative position information of the tracking target, and can achieve laser tracking and positioning at a distance of 0-50 meters. However, this solution is a positioning and tracking method that requires a base station and is not suitable for relative positioning within a drone group. Summary of the invention
[0004] The purpose of the present invention is to overcome the defects of the above-mentioned prior art and provide a UAV relative positioning system, UAV cluster and positioning method based on resonant light.
[0005] The purpose of the present invention can be achieved by the following technical solutions:
[0006] As a first aspect of the present invention, a relative positioning system for a drone based on resonance light is provided, wherein the drone positioning system is provided with a resonance light positioning module, including a resonance light transmitting end and a receiving end that alternately surround the drone, wherein the transmitting end and the receiving end are respectively provided with a retroreflector and an image sensor;
[0007] The retro-reflector includes a cat's eye and a semi-transparent mirror. A portion of the incident light beam will pass through the semi-transparent mirror, and the other portion of the incident light will be reflected back along the original path, so that the resonant light beam will continuously transmit back and forth between the transmitting end and the receiving end;
[0008] The image sensor includes an attenuation sheet and a CMOS image sensor. The light beam passes through the semi-transparent and semi-reflective mirror and hits the CMOS image sensor via the attenuation sheet. The CMOS image sensor converts the light beam into a digital value and outputs a light spot image.
[0009] The transmitting end and / or the receiving end is also provided with a distance measuring module for measuring the distance between the receiving end and the transmitting end.
[0010] As a preferred technical solution, the transmitting end is also provided with a gain medium, which is fixed at the pupil of the retro-reflector and is used to amplify the passing light beam.
[0011] As a preferred technical solution, the UAV positioning system further includes:
[0012] The central processing module is used to estimate the angles involved in the image sensor in the resonant light module, estimate the relative positioning and attitude between drones, and run the cluster positioning method;
[0013] Communication module; used to transmit data between drone groups;
[0014] The flight control module is used to control the flight of the UAV; the flight control module has a built-in inertial measurement unit, which is used to measure the 3-axis acceleration and angular velocity of the UAV to obtain the real-time flight data of the UAV.
[0015] As a second aspect of the present invention, a drone cluster is provided, wherein each drone in the cluster is provided with the drone relative positioning system based on resonant light as described above, and the data flow between the drones and the cluster is as follows:
[0016] A steady-state positioning beam is formed between the matched resonant light positioning modules, that is, between the resonant light transmitting end of one drone and the resonant light receiving end of another drone;
[0017] The distance, direction angle and corresponding receiving end number of the receiving end relative to the transmitting end are obtained at the transmitting end; the direction angle of the transmitting end relative to the receiving end is obtained at the receiving end;
[0018] The information related to the positioning of the drone is transmitted to the processing unit, and the resonance light positioning information of the drone matched with it received by the communication unit is also transmitted to the processing unit. After the processing unit calculates the relative rotation and relative position, it is packaged together with the drone number and broadcasted to the communication unit;
[0019] The inertial measurement unit calculates the current speed and acceleration of the drone and also transmits it to the communication unit for broadcast.
[0020] As a third aspect of the present invention, a relative positioning method is provided, which is applied to the drone cluster as described above, and the specific steps include:
[0021] Deploy UAVs equipped with relative positioning systems and calculate initial data of the UAVs, including using resonance light positioning to calculate the relative distance between two adjacent UAVs that form positioning resonance light, as well as the direction angle of the receiving end relative to the transmitting end and the direction angle of the transmitting end relative to the receiving end;
[0022] For each UAV, the relative positioning results of all direct links and indirect links are calculated, including the positioning vector between two adjacent UAVs and the positioning vector between the UAV to be optimized and the central UAV;
[0023] Performing delay time correction on the indirect link, updating the positioning vector, and further obtaining a corrected positioning vector;
[0024] Based on the offset angle and |θ of each link in the positioning link i |As well as the length of each link and the optimization weight of each link, an objective function is constructed to minimize the difference between the positioning estimate and the positioning vector of each link, and the estimated relative position is obtained by optimizing the objective function.
[0025] As a preferred technical solution, the relative direction angle between the receiving end and the transmitting end resonant light beam is derived and calculated using the light spot obtained by the image sensor:
[0026] Calculate the center of mass of the light spot based on the digital value of the light field intensity obtained by CMOS
[0027]
[0028] Wherein, the subscript i∈(1,2), i=1 represents the transmitting end, i=2 represents the receiving end; I represents the digital value of the light field intensity obtained by CMOS;
[0029] The direction angles are calculated as follows:
[0030]
[0031] Where f represents the focal length of the semi-transparent and semi-reflective mirror.
[0032] As a preferred technical solution, the positioning vector x between the two adjacent drones ij And the positioning vector p of any UAV n relative to the central UAV n It is expressed as follows:
[0033] x ij =[T ij (0,3),T ij (1,3),T ij (2,3)]
[0034] p n =[T n (0,3),T n (1,3),T n (2,3)]
[0035]
[0036] Among them, p represents the above O represents a 1x3 matrix filled with all zeros; Respectively represent the calibration matrix between the resonant light coordinate system and the drone body coordinate system in drone i and drone j; R represents the rotation matrix between the receiving end of drone j relative to the transmitting end of drone i, which is expressed as:
[0037]
[0038] As a preferred technical solution, the delay time correction for the indirect link is specifically as follows:
[0039]
[0040] in, and is x ij Sending time UAV i and UAVs j Relative velocity and acceleration, δt s Indicates sending x ij The transmission delay t of all links between the UAV to the UAV that needs positioning optimization trans , the delay time t of resonant light localization establishment rbs and calculation processing delay t deal sum.
[0041] As a preferred technical solution, the objective function is expressed as:
[0042]
[0043] Among them, p i Represents the positioning vector of each link of the UAV to be optimized; Indicates that the optimization is performed to obtain the positioning estimate; w i is the optimization weight of each link, expressed as follows:
[0044]
[0045] Where i represents the ID of the positioning link p, M represents the number of positioning links p; |θ i | represents the sum of the offset angles of each link x in each positioning link p, |l i | represents the length and length of each link x in each positioning link p; κ1 and κ2 are respectively guaranteed to |θ i | and |l i |with n i Keep the normalization coefficients of the same magnitude.
[0046] As an optimal technical solution, the method comprises the central UAV performing a traversal algorithm to determine the optimization order of other UAVs, so that the UAV with the least number of shortest connection links with the central UAV is optimized first; and the links with the number of positioning link hops within a certain threshold are selected for optimization.
[0047] Compared with the prior art, the present invention has the following beneficial effects:
[0048] 1) The present invention proposes a system for simultaneously obtaining relative position and posture using resonant light. The transmitting end is composed of a gain medium, a reply reflector, a distance measurement module and an image sensor, and the receiving end is composed of a reply reflector and an image sensor. A corresponding positioning calculation method is designed to calculate the distance from the transmitting end to the receiving end based on the principle of time of flight. At the same time, the light spot collected by the image sensor is used to estimate the arrival angle, and the angle of the transmitting end relative to the receiving end, and the angle of the receiving end relative to the transmitting end, are calculated. Compared with the original resonant light structure that can only obtain relative displacement, it is now possible to obtain relative position and relative posture at the same time.
[0049] 2) The present invention proposes a direct indirect link optimization algorithm that takes time calibration into consideration. By correcting the transmission delay, the resonance light positioning establishment delay and the calculation processing delay, the positioning error caused by the time difference on the indirect link can be eliminated, thereby further improving the relative positioning accuracy of the drone swarm while ensuring real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS
[0050] Figure 1 It is a schematic diagram of the functional modules and data flow of the drone of the present invention;
[0051] Figure 2 It is a schematic diagram of the resonant light positioning structure of the present invention;
[0052] Figure 3 Schematic diagram of the relationship between the azimuth angle of the resonant light and the adjacent UAVs;
[0053] Figure 4 Schematic diagram of the direct indirect link optimization algorithm;
[0054] Figure 5This is a flow chart of the method for relative positioning of a swarm of drones based on an improved resonant optical structure according to the present invention. DETAILED DESCRIPTION
[0055] The present invention is described in detail below in conjunction with the accompanying drawings and specific embodiments. This embodiment is implemented based on the technical solution of the present invention, and provides a detailed implementation method and specific operation process, but the protection scope of the present invention is not limited to the following embodiments.
[0056] Example 1
[0057] The present invention mainly improves the resonant light positioning system so that it can obtain relative position and posture at the same time, integrates it on the UAV, designs a set of matching cluster positioning optimization algorithms, and completes the high-precision relative positioning of the UAV swarm in three-dimensional space.
[0058] First, a system that uses resonant light to simultaneously obtain relative position and posture is proposed. The transmitter consists of a gain medium, a retroreflector, a ranging module, and an image sensor, and the receiver consists of a retroreflector and an image sensor. According to the time-of-flight principle, the distance from the transmitter to the receiver is calculated. At the same time, the light spot collected by the image sensor is used to estimate the arrival angle, and the angle of the transmitter relative to the receiver and the angle of the receiver relative to the transmitter are calculated. Combining the above two aspects, the relative distance and posture of the receiver relative to the transmitter are derived.
[0059] Second, a UAV positioning system with integrated resonant light structure is designed. A certain number of resonant light transmitters and receivers are installed alternately on the UAVs to obtain the omnidirectional relative posture between adjacent UAVs. At the same time, a direct indirect link optimization algorithm considering time calibration is proposed to further improve the relative positioning accuracy of the UAV swarm while ensuring real-time performance.
[0060] Hardware level
[0061] Overall structure:
[0062] The drone is equipped with a resonant light positioning module, a central processing module, a communication module, and a flight control module. The resonant light positioning module is a resonant light transmitter and receiver that alternately surround a circle. Because the resonant light positioning system uses a separate transmitter and receiver, and can automatically align within a certain field of view to complete positioning, as long as a certain number of resonant light positioning units are installed, the omnidirectional positioning of the drone can be completed, that is, the positioning of the 360° omnidirectional horizontal direction and the resonant light field of view angle range in the vertical direction. The central processing module uses an Intel-NUC onboard computer to estimate the angle involved in the image sensor in the resonant light module, the relative positioning and attitude estimation between drones, and the operation of the cluster positioning optimization algorithm. The communication module uses WiFi communication to transmit necessary data between drone groups. The flight control module uses PX4 to control the flight of the drone. At the same time, PX4 has a built-in inertial measurement unit (IMU), which can measure the 3-axis acceleration and angular velocity of the drone, making it easy to obtain real-time flight data of the drone.
[0063] Data flow:
[0064] Data flow between drones and clusters Figure 1 As shown, first, a steady-state positioning beam is formed between the matched resonant light units (i.e., between the resonant light transmitting end of one drone and the resonant light receiving end of another drone). At the transmitting end, the distance l, direction angles α, β, and the corresponding receiving end number id of the receiving end relative to the transmitting end can be obtained; at the receiving end, the direction angles α, β of the transmitting end relative to the receiving end can be obtained. Then, the local positioning related information is transmitted to the processing unit, and at the same time, the resonant light positioning information of the other machine matched with it received by the communication unit is also transmitted to the processing unit. After the processing unit calculates the relative rotation R and relative position p, it is packaged together with the drone number to the communication unit for broadcast. The inertial measurement unit calculates the speed and acceleration of the current drone, and also transmits it to the communication unit for broadcasting, which is used to correct the error caused by the delay in the indirect relative positioning in the optimization algorithm.
[0065] Resonant light positioning structure:
[0066] The resonant light positioning unit includes a transmitting end and a receiving end separated in space, such as Figure 2 As shown, the transmitting end is composed of a gain medium, a retroreflector, an image sensor and a ranging module; the receiving end is composed of a retroreflector and an image sensor.
[0067] The gain medium is fixed at the pupil of the cat's eye retro-reflector, which can amplify the light beam passing through it; the retro-reflector consists of a cat's eye Ls and a semi-transparent and semi-reflective mirror M, which can reflect the incident light back along the original path, so the resonant light beam will continuously transmit back and forth between the transmitting end and the receiving end; the image sensor consists of an attenuation plate Att and a CMOS image sensor, a part of the light beam passes through the semi-transparent and semi-reflective mirror M and hits the CMOS image sensor via the attenuation plate Att, and the CMOS is used to convert the light beam into a digital value and output a spot image; the ranging module uses the TOF ranging principle to obtain the distance from the receiving end to the transmitting end.
[0068] The offset of the light spot formed by the resonant light obtained by the image sensor at the transmitting end is proportional to the offset position of the receiving end relative to the transmitting end. Therefore, the incident angle α1 and β1 of the receiving end relative to the transmitting end can be estimated by using the centroid of the light spot; similarly, the light spot obtained by the image sensor at the receiving end can also be used to obtain the direction angles α2 and β2 of the transmitting end relative to the receiving end. Combined with the distance L obtained by the ranging module at the transmitting end, the relative displacement and relative rotation between the transmitting end and the receiving end can be derived.
[0069] Example 2
[0070] As a second embodiment of the present invention, the present invention also provides a positioning method between drones based on the resonant light positioning system as described in the above embodiment.
[0071] Relative posture calculation based on resonant light:
[0072] In order to estimate the four directional angles of the resonant light beam, the light spot obtained by the image sensor can be used for derivation and calculation. First, the centroid of the light spot is calculated based on the digital value of the light field intensity obtained by CMOS.
[0073]
[0074] Among them, the subscript i∈(1,2), i=1 represents the transmitting end, and i=2 represents the receiving end. I represents the digital value of the light field intensity obtained by CMOS. Therefore, the direction angle can be calculated as follows:
[0075]
[0076] Where f represents the focal length of the semi-transparent and semi-reflective mirror.
[0077] Assuming UAV i A transmitter and UAV j A positioning beam is formed between a certain receiving end of the UAV, from which it can be deduced that the UAV j The receiving end is relative to the UAV i The position of the transmitter p = (XT ,Y T ,Z T ):
[0078]
[0079] In addition to obtaining relative displacement, it is also necessary to obtain relative attitude, that is, relative rotation. Figure 3 As shown, a drone is alternately equipped with a transmitter and a receiver of resonant light. Through previous calculations, the two direction angles α1β1 of the receiver relative to the transmitter, and the two direction angles α2β2 of the transmitter relative to the receiver have been obtained. Figure 3 As shown in the figure, because the four wings of the quadcopter are evenly stressed and parallel to the ground when the quadcopter is in stable flight, the pitch angle α1=α2, the roll angle (rotation along the z-axis) is 0, and only the yaw angle is different. The relative yaw angle of the two drones in the figure is β1+β2+π. Therefore, UAV j The receiving end is relative to the UAV i The rotation matrix between the transmitters can be expressed as:
[0080]
[0081] By pre-calibration, the homogeneous transformation matrix of each resonant light coordinate system on the UAV relative to the UAV body coordinate system is obtained. j Relative to UAV i The location is as follows:
[0082]
[0083] The square brackets represent the homogeneous transformation between the resonant light units matched by the two drones, and p represents the above O represents a 1x3 matrix filled with all zeros; Represents UAV i The calibration matrix between the resonance light coordinate system and the drone body coordinate system, Same reason.
[0084] Therefore, the homogeneous transformation of any UAV in the cluster relative to the base UAV can be expressed as:
[0085]
[0086] Furthermore, the relative position x between any two drones can be obtained ij , and the position p of any drone n relative to the base drone n ,as follows:
[0087] x ij =[T ij(0,3),T ij (1,3),T ij (2,3)]
[0088] p n =[T n (0,3),T n (1,3),T n (2,3)]
[0089] Drone swarm positioning optimization algorithm:
[0090] The relative positioning of a drone swarm is to calculate the position of each drone in the swarm relative to the reference drone. The resonant light positioning in the previous section can obtain the relative position and attitude of two adjacent drones in the swarm, and then the positioning optimization can be performed to obtain more accurate drone positioning information. The attitude of any drone relative to the reference drone can be obtained through a direct link or derived through a multi-hop indirect link.
[0091] like Figure 4 As shown, the position of UAV2 relative to UAV0 can be obtained through three resonant optical links as shown in the figure, one of which is the result of direct measurement and the other two are the results of indirect measurement. For the indirect link p1, the two measurement values x 01 and x 12 , their initial values are obtained as described in the previous section. It is worth noting that x 01 First calculated from UAV0, packaged and sent to UAV1, then x 12 Then obtained by UAV1, together with x 01 are sent to the UAV for calculation, so x 01 and x 12 There is a certain time difference between the obtained time and the optimized time. This time difference includes the transmission delay t trans , the delay time t of resonant light localization establishment rbs and calculation processing delay t deal , they are all in milliseconds. Therefore, assuming that the speed of the drone remains unchanged during this delay, the displacement change can be calculated to eliminate this error, as shown below:
[0092]
[0093]
[0094] in, and is x ij Sending time UAV i and UAVs j Relative velocity and acceleration, δt sIndicates sending x ij The sum of the three time delays of all links between the UAV to the UAV that needs positioning optimization.
[0095] The above correction can eliminate the positioning error caused by the time difference on the indirect link, so the corrected positioning vector p can be obtained. According to the different positioning vectors p obtained by the UAV to be optimized i , optimize to get a more accurate positioning estimate
[0096]
[0097] where w i is the optimization weight of each link, expressed as follows:
[0098]
[0099] Where i represents the ID of the positioning link p, and M represents the number of positioning links p. Because in indirect measurement, the link is segmented into n i The more, the greater the cumulative error will be, |θ i | represents the sum of the offset angles of each link x in each positioning link p, |l i | represents the length and length of each link x in each positioning link p. These three variables are inversely proportional to the positioning error, and f(x) is selected to describe this relationship. κ1 and κ2 are respectively guaranteed to be |θ i | and |l i |with n i Keep the normalization coefficients of the same magnitude.
[0100] Example 3
[0101] like Figure 5 As shown, this example provides a relative positioning optimization method between drone groups based on an improved resonant light structure. The scheme is described as follows:
[0102] S1. Deploy the drone according to the description of the overall structure and equip it with the corresponding hardware system; and calculate the initial data that the drone needs to obtain, including the relative distance and two direction angles between two adjacent drones (forming positioning resonance light) calculated by using resonance light positioning;
[0103] S2. Determine the optimization order of the drone group. The central drone performs a traversal algorithm to determine the optimization order of other drones, ensuring that the drone with the least number of shortest connection links with the central drone is optimized first;
[0104] S3. For each UAV, calculate the relative positioning results of all direct links and indirect links, including the positioning vector x between two adjacent UAVs and the positioning vector p between the UAV to be optimized and the center UAV;
[0105] S4, perform delay time correction, update x←x+δx, and further obtain all corrected positioning vectors p. The specific calculation method is shown in the drone positioning algorithm described in Example 2;
[0106] S5: Select the link whose positioning link hop count is within a certain threshold for optimization, that is, n i <α l , to prevent the indirect positioning links with more hops from affecting the overall optimization results. After experimental verification, all direct positioning links and no more than 2 hops (i.e., α l The best optimization effect is achieved by optimizing the indirect positioning link with a value of ≤2.
[0107] S6. Calculate the |l on the selected link i | and |θ i |Variable, further calculate the optimization weight w i , minimize the difference of the objective function and solve the relative position to be estimated
[0108] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.
Claims
1. A UAV relative positioning system based on resonant light, characterized in that: The UAV positioning system is provided with a resonant light positioning module, including a resonant light transmitting end and a receiving end that alternately surround the UAV, and the transmitting end and the receiving end are respectively provided with a retroreflector and an image sensor; The retro-reflector includes a cat's eye and a semi-transparent mirror. A portion of the incident light beam will pass through the semi-transparent mirror, and the other portion of the incident light will be reflected back along the original path, so that the resonant light beam will continuously transmit back and forth between the transmitting end and the receiving end; The image sensor includes an attenuation sheet and a CMOS image sensor. The light beam passes through the semi-transparent and semi-reflective mirror and hits the CMOS image sensor via the attenuation sheet. The CMOS image sensor converts the light beam into a digital value and outputs a light spot image. The transmitting end and / or the receiving end is also provided with a distance measuring module for measuring the distance between the receiving end and the transmitting end.
2. The UAV relative positioning system based on resonance light according to claim 1, characterized in that: The transmitting end is also provided with a gain medium, which is fixed at the pupil of the retro-reflector and is used to amplify the passing light beam.
3. The UAV relative positioning system based on resonance light according to claim 1, characterized in that: The UAV positioning system further comprises: The central processing module is used to estimate the angles involved in the image sensor in the resonant light module, estimate the relative positioning and attitude between drones, and run the cluster positioning method; Communication module; used to transmit data between drone groups; The flight control module is used to control the flight of the UAV; the flight control module has a built-in inertial measurement unit, which is used to measure the 3-axis acceleration and angular velocity of the UAV to obtain the real-time flight data of the UAV.
4. A drone cluster, characterized in that: Each drone in the cluster is provided with a drone relative positioning system based on resonant light as described in any one of claims 1 to 3, and the data flow between the drones and the cluster is as follows: A steady-state positioning beam is formed between the matched resonant light positioning modules, that is, between the resonant light transmitting end of one drone and the resonant light receiving end of another drone; The distance, direction angle and corresponding receiving end number of the receiving end relative to the transmitting end are obtained at the transmitting end; the direction angle of the transmitting end relative to the receiving end is obtained at the receiving end; The information related to the positioning of the UAV is transmitted to the processing unit, and the resonance light positioning information of the UAV matched with it received by the communication unit is also transmitted to the processing unit. After the processing unit calculates the relative rotation and relative position, it is packaged together with the UAV number and broadcasted to the communication unit; The inertial measurement unit calculates the current speed and acceleration of the drone and also transmits it to the communication unit for broadcast.
5. A relative positioning method, characterized in that: The method is applied to the drone cluster as claimed in claim 4, and the specific steps include: Deploy UAVs equipped with relative positioning systems and calculate initial data of the UAVs, including using resonance light positioning to calculate the relative distance between two adjacent UAVs that form positioning resonance light, as well as the direction angle of the receiving end relative to the transmitting end and the direction angle of the transmitting end relative to the receiving end; For each UAV, the relative positioning results of all direct links and indirect links are calculated, including the positioning vector between two adjacent UAVs and the positioning vector between the UAV to be optimized and the central UAV; Performing delay time correction on the indirect link, updating the positioning vector, and further obtaining a corrected positioning vector; Based on the offset angle and |θ of each link in the positioning link i |As well as the length of each link and the optimization weight of each link, an objective function is constructed to minimize the difference between the positioning estimate and the positioning vector of each link, and the estimated relative position is obtained by optimizing the objective function.
6. A relative positioning method according to claim 5, characterized in that: The relative direction angle between the receiving end and the transmitting end resonant light beam is derived and calculated using the light spot obtained by the image sensor: Calculate the center of mass of the light spot based on the digital value of the light field intensity obtained by CMOS Wherein, the subscript i∈(1,2), i=1 represents the transmitting end, i=2 represents the receiving end; I represents the digital value of the light field intensity obtained by CMOS; The direction angles are calculated as follows: Where f represents the focal length of the semi-transparent and semi-reflective mirror.
7. A relative positioning method according to claim 6, characterized in that: The positioning vector x between the two adjacent drones ij And the positioning vector p of any UAV n relative to the central UAV n It is expressed as follows: x ij =[T ij (0,3),T ij (1,3),T ij (2,3)] p n =[T n (0,3),T n (1,3),T n (2,3)] Among them, p represents the above O represents a 1x3 matrix filled with all zeros; Respectively represent the calibration matrix between the resonant light coordinate system and the drone body coordinate system in drone i and drone j; R represents the rotation matrix between the receiving end of drone j relative to the transmitting end of drone i, which is expressed as:
8. A relative positioning method according to claim 5, characterized in that: The delay time correction for the indirect link is specifically as follows: in, and is x ij Sending time UAV i and UAVs j Relative velocity and acceleration, δt s Indicates sending x ij The transmission delay t of all links between the UAV to the UAV that needs positioning optimization trans , the delay time t of resonant light localization establishment rbs and calculation processing delay t deal sum.
9. A relative positioning method according to claim 8, characterized in that: The objective function is expressed as: Among them, p i Represents the positioning vector of each link of the UAV to be optimized; Indicates that optimization is performed to obtain positioning estimation; w i is the optimization weight of each link, expressed as follows: Where i represents the ID of the positioning link p, M represents the number of positioning links p; |θ i | represents the sum of the offset angles of each link x in each positioning link p, |l i | represents the length and length of each link x in each positioning link p; κ1 and κ2 are respectively guaranteed to |θ i | and |l i |with n i Keep the normalization coefficients of the same magnitude.
10. A relative positioning method according to claim 5, characterized in that: The method comprises the following steps: the central UAV performs a traversal algorithm to determine the optimization order of other UAVs, so that the UAV with the least number of shortest connection links with the central UAV is optimized first; and the links with the number of positioning link hops within a certain threshold are selected for optimization.
Citation Information
Patent Citations
Tracking and positioning system based on separating cavity
CN110058256A
