Unmanned aerial vehicle based on 5G communication and 3D technology and operation method thereof

Abstract

The invention provides an unmanned aerial vehicle based on 5G communication and 3D technology and an operation method thereof.The unmanned aerial vehicle is characterized in that a top plate is fixedly installed at the upper end of an overall frame structure of the unmanned aerial vehicle, multiple sets of supporting frameworks are symmetrically fixed to the periphery of the top plate, and supporting legs making contact with the ground are symmetrically installed at the lower end of the overall frame structure; a plurality of support arms are symmetrically connected to the middle end of the integral frame structure, and positive paddles and negative paddles are mounted at the tail ends of the symmetrically arranged support arms; a chip of the unmanned aerial vehicle adopts a Raspberry Pi development chip, a built-in CMOS chip and a 5G millimeter wave reverse array extremely simple framework, and the unmanned aerial vehicle is built in a flight control mainboard and is connected with a first speed reduction stepping motor and a second speed reduction stepping motor through Dupont lines. And the WIFI chip in the connection interface connected with the VR glasses controls the unmanned aerial vehicle. The 5G communication technology and the 3D technology are combined, the flight stability of the unmanned aerial vehicle is improved, the flight safety is improved, and the unmanned aerial vehicle can well adapt to the complex and severe environment.

Description

Technical Field

[0001] The present invention relates to the technical field of 5G signal transmission and intelligent flight devices, and in particular to a drone based on 5G communication and 3D technology and an operation method thereof. Background Art

[0002] Traditional drone driving refers to directly controlling the flight of the drone through a remote control or ground control station. This method relies on line-of-sight operations, and the operator needs to monitor and control the drone within the line of sight. The operator controls the takeoff, flight direction, speed and landing of the drone through a remote control similar to a game controller. Real-time monitoring is achieved through visual observation and first-person video transmission. FPV technology allows the operator to view the flight status in real time through the drone's camera. However, due to the obstruction of the operating line of sight, there are huge risks in drone operation.

[0003] There are many dangers in drone driving, which require the operator to pay great attention. Drones may collide with obstacles such as buildings, trees, and wires, causing equipment damage or even fire. Collisions with other aircraft may also have serious consequences. Technical failures, battery exhaustion, or operational errors may cause the drone to lose control and fall, endangering the safety of people on the ground and causing property damage. Drones rely on wireless signals for control and communication. Signal interference may cause the drone to lose control, and other electronic devices in the environment may also interfere with the operation of the drone. Bad weather such as strong winds, rain, snow, and lightning will affect the flight stability of the drone and increase the risk of flight. Strong air currents when flying at high altitudes may also cause the drone to lose control. Various sensors and systems of drones may malfunction, such as GPS failure, battery problems, or software errors, which will affect flight safety. Therefore, it is necessary to design a drone based on 5G communication and 3D technology and its operation method. Summary of the invention

[0004] The purpose of the present invention is to overcome the above-mentioned shortcomings and provide a drone based on 5G communication and 3D technology and an operation method thereof. By combining 5G communication technology and 3D technology, the stability of drone flight is improved, the safety of flight is increased, and it can adapt well to complex and harsh environments.

[0005] In order to achieve the above-mentioned technical features, the purpose of the present invention is achieved as follows: a drone based on 5G communication and 3D technology, a top plate is fixedly installed on the upper end of the overall frame structure of the drone, a plurality of groups of supporting frames are symmetrically fixed on the periphery of the top plate, and supporting feet in contact with the ground are symmetrically installed on the lower end of the overall frame structure, and a plurality of supporting arms are symmetrically connected to the middle end of the overall frame structure, and positive propellers and negative propellers are installed at the ends of the symmetrically arranged supporting arms; the chip of the drone adopts a Raspberry Pi development chip, with a built-in CMOS chip and a 5G millimeter wave reverse array minimalist architecture, and is built into the flight control mainboard and connected to the No. 1 reduction stepper motor and the No. 2 reduction stepper motor through a DuPont line. When the drone is operating at high altitude, the drone is controlled by connecting the communication module in the flight control mainboard to the WIFI chip in the connection interface of the VR glasses.

[0006] The propellers of the drone are distributed in an X-shape. When the drone issues a flight command, the positive propeller and the negative propeller rotate in opposite directions.

[0007] The ground operator wears VR glasses and observes in real time through the OLED display of the VR glasses. The WIFI chip built into the connection interface receives the image signal transmitted back by the flight control mainboard. The ground operator adjusts the headband to make it fully adapt to the wearer's head. The wearer observes the image information projected onto the optical lens and the information transmitted back by the sensor, and makes timely adjustments to the flight status of the drone.

[0008] The support frame is symmetrically fixedly connected to the top plate and presents a hexagonal symmetrical structure. The support frame is a double-layer structure with a DuPont wire passing through the middle. The support frame is fixedly connected by screws; The top plate is a double-layer structure, with a support arm fixed in the middle by screws and nuts, and a No. 1 reduction stepper motor is symmetrically connected to the support arm. The No. 1 reduction stepper motor is electrically connected to the flight control mainboard placed on the lower top plate through a DuPont line. The interior of the support arm is a hollow structure, and the No. 1 reduction stepper motor is electrically connected to the stepper motor on the other side through the DuPont line passing through the support arm. The support columns are symmetrically fixed on the middle top plate, and the loading platform is fixedly connected to the support columns. The batteries used by the drone are stored between the loading platform and the top plate.

[0009] The output shaft of the stepper motor is connected to the positive propeller or the negative propeller. When the UAV executes a flight command, the positive propeller and the negative propeller rotate simultaneously to generate the same air torque, so that the UAV has an upward movement trend.

[0010] The drone 5G communication system adopts the proposed CMOS reverse array chip, which is based on the Pon array principle. The mixer is the core component of the Pon array. When the subharmonic mixer is used to replace the fundamental mixer in the Pon array, the received RF signal is mixed with the nth harmonic component of the local oscillator signal. The intermediate frequency signal output by the subharmonic mixer is the intermediate frequency signal. After filtering out the high-frequency mixing product, the output signal is the reverse array using the carrier conjugate phase information in the received signal to achieve accurate and fast beam pointing adjustment.

[0011] The reverse array research focuses on the 26 GHz frequency band in the 5G millimeter wave frequency band. All core chips in the reverse array are designed and implemented using domestic 0.18 mm CMOS process; The subharmonic mixer adopts current multiplexing technology. Resistors R1 and R2 provide additional current branches for transistors M1 and M2. By optimizing the resistance value, the AC signal is prevented from directly reaching Vdd through the resistor. Based on the current multiplexing technology, the DC current passing through the switch stage and the load is reduced. The resistance value of the load resistors RL1 and RL2 can reach 1 kW, thereby improving the conversion gain of the down-conversion mixer.

[0012] The mixer adopts current multiplexing technology. Transistors M1~4 form the transconductance stage of the mixer. Among them, PMOS tubes M3~4 increase the effective transconductance and act as multiplexed current sources to provide additional current branches for NMOS tubes M1~2 to reduce the DC current passing through the switch stage; the orthogonal local oscillator signals required by the switch stage NMOS tubes M5~12 are also provided by the local oscillator network. The design of the local oscillator network of the up-conversion subharmonic mixer is the same as that of the down-conversion subharmonic mixer.

[0013] The power amplifier has a single-ended input and a single-ended output. The transistor is biased in the Class A amplifier mode with a bias voltage Vb of 0.95 V. The first stage is an ordinary cascode amplifier stage. The second stage uses transconductance enhancement technology to improve the gain based on the ordinary cascode amplifier stage. C1 and L1 are connected in series to form an inductive impedance, which together with the inductor L2 forms a negative feedback loop required for the transconductance enhancement technology to form a Gmax-core network. The transconductance enhancement technology achieves the purpose of gain enhancement at the expense of circuit stability. The output impedance of the cascode structure is high and has good isolation and stability. Based on this, the transconductance enhancement technology based on the cascode structure is used to ensure circuit stability while improving the gain.

[0014] A method for operating a drone based on 5G communication and 3D technology: The aircraft uses the MPU6050 sensor to calculate the aircraft's attitude, and uses a Kalman filter and a first-order low-pass filter to filter the data, which includes the following steps: S101: Define the value of the initial quaternion as q 0=1,q 1=0, q 2=0, q 3=0; S102: Read the accelerometer value and angular velocity value. The program defines variables as follows: ax , ay , az , gx , gy , gz , convert the gyroscope value to radians; S103: Extract the gravity component in the attitude matrix: ; Where: is the attitude gravity component; is the radian value of each angle; S104: Find the attitude error, perform cross product of the two vectors, and define ex , ey , ez are the three axis error elements: ; Where: V x、 V y、 V z They are X radian system, Y radian system, and Z radian system respectively; S105: complementary filtering, inputting the error into the PID controller and adding it to the angular velocity measured by the gyroscope to correct the angular velocity value; S106: Calculate the attitude angle. The mathematical formula is: ; ; Where: is the X attitude error parameter; is the Y attitude error parameter; is the attitude error angle of X, is the Y attitude error angle, are the Z angles in the attitude error respectively.

[0015] The present invention has the following beneficial effects: 1. The present invention combines 5G communication technology with 3D technology to improve the stability of drone flight and increase flight safety, thereby adapting well to complex and harsh environments.

[0016] 2. The present invention has six arms symmetrically connected at the middle end, and the six rotors provide higher stability and wind resistance, especially in severe weather conditions. When the drone performs a flight command, the symmetrical positive and negative propellers 207 rotate in opposite directions. The main reason for designing the propellers of the drone to rotate in different directions is to balance the torque, maintain flight stability, provide good control performance, and improve overall flight efficiency.

[0017] 3. In the present invention, the ground operator wears VR glasses, and observes the image signal transmitted back by the UAV flight control mainboard received by the built-in WIFI chip of the connection interface through the OLED display in real time. The ground operator adjusts the headband to make it fully adapt to the wearer's head. The wearer observes the image information projected on the optical lens and the information transmitted back by the sensor, and adjusts the flight status of the UAV in time to ensure that the body is in a balanced state.

[0018] 4. The subharmonic mixer of the present invention adopts current multiplexing technology to solve the problem of current demand contradiction.

[0019] 5. The present invention benefits from the current multiplexing technology, which reduces the DC current passing through the switch stage and the load. The resistance of the load resistors RL1 and RL2 can reach 1 kW, which directly improves the conversion gain of the down-conversion mixer.

[0020] 6. The transconductance enhancement technology of the present invention achieves the purpose of gain enhancement at the expense of circuit stability. The output impedance of the common source and common gate structure is high and has good isolation and stability. Therefore, the use of the transconductance enhancement technology based on the common source and common gate structure can ensure circuit stability while improving gain. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0022] Figure 1 This is a side view of the VR glasses of the present invention.

[0023] Figure 2 This is a front view of the VR glasses of the present invention.

[0024] Figure 3 This is a top view of the drone of the present invention.

[0025] Figure 4 It is a front view of the UAV of the present invention.

[0026] Figure 5 It is a side view of the drone of the present invention.

[0027] In the figure: VR glasses 100, OLED display 101, optical lens 102, sensor 103, head display 104, headband 105, connection interface 106; UAV 200, top plate 201, supporting frame 202, supporting foot 203, nut 204, screw 205, positive propeller 206, negative propeller 207, stepper motor 208, arc workpiece 209, support arm 210, No. 1 reduction stepper motor 211, baffle 212, flight control main board 213, No. 2 reduction stepper motor 214, fixing part 215, clamp 216, fork joint 216, gimbal camera 218, supporting column 219, and stage 220. DETAILED DESCRIPTION The embodiments of the present invention are further described below in conjunction with the accompanying drawings.

[0028] Embodiment 1: See also Figure 1-5 , a drone based on 5G communication and 3D technology and its operation method: including an overall frame structure of a drone 200, a top plate 201 and a support frame 202 symmetrically fixedly connected thereto are fixedly installed on the upper end of the overall frame structure, and support feet 203 in contact with the ground are symmetrically fixedly installed on the lower end. The hollow design of the fixed support structure ensures that there is sufficient space for the internal software. The shell structure of the drone is made of lightweight and strong materials, such as carbon fiber and aluminum alloy, which provides sufficient strength to support other components while keeping the weight light. Six arms 210 are symmetrically connected at the middle end, and the six rotors provide higher stability and wind resistance, especially in severe weather conditions. When the drone performs a flight command, the symmetrical positive blade 206 and the negative blade 207 rotate in opposite directions. The main reason for designing the rotation direction of the drone propeller to be different is to balance the torque, maintain flight stability, provide good control performance, and improve overall flight efficiency. This design enables the drone to achieve stable and controllable flight under a variety of flight conditions, while providing redundancy and fault tolerance. The drone blades are distributed in an X-shape, and the drone chip uses a Raspberry Pi development chip with a built-in CMOS chip and a 5G millimeter wave reverse array minimalist architecture, which is built into the flight control mainboard 213 and connected to the first reduction stepper motor 211 and the second reduction stepper motor 214 through DuPont wires. When the drone is operating at high altitude, the drone can be controlled by connecting the WiFi chip in the connection interface 106 through the communication module in the flight control mainboard 213.

[0029] Furthermore, the ground operator wears VR glasses 100, and observes the image signal transmitted back by the drone flight control mainboard 213 received by the built-in WIFI chip of the connection interface 106 through the OLED display 101 in real time. The ground operator adjusts the headband 105 to make it fully adapt to the wearer's head. The wearer observes the image information projected on the optical lens 102 and the information transmitted back by the sensor 103, and adjusts the flight status of the drone in time to ensure that the body is in a balanced state.

[0030] Furthermore, the upper part of the drone 200 is composed of a top plate 201 and a support frame 202. The support frame 202 is symmetrically fixedly connected to the middle top plate 201 and presents a hexagonal symmetrical structure. The support frame 202 is a double-layer structure, and the double-layer structure provides additional support and rigidity through two independent layers. The outer layer and the inner layer jointly bear the external pressure and stress, and improve the strength and durability of the overall structure. A DuPont line passes through the middle, and the support frame 202 is fixedly connected by screws 205 and nuts 204. The middle top plate 201 is a double-layer structure, in the middle of which a mechanical arm 210 is fixedly connected by screws 205 and nuts 204. A No. 1 reduction stepper motor 211 is symmetrically connected to the support arm 210. The No. 1 reduction stepper motor 211 is electrically connected to the flight control mainboard 213 on the lower side of the top plate 201 through a DuPont line, which greatly improves the space utilization of the drone. The interior of the support arm 210 is a hollow structure. The No. 1 reduction stepper motor 211 passes through the support arm 210 through a DuPont line and is electrically connected to the stepper motor 208 on the other side, which increases the overall space utilization of the drone body. The support column 219 is symmetrically fixed on the middle top plate 201, and the loading platform 220 is fixedly connected to the support column 219. The battery used by the drone can be stored between the loading platform 220 and the top plate 220.

[0031] Furthermore, a stepper motor 208 is connected to the end of the support arm 210, and its upper end is connected to the blades. The blades are divided into a positive blade 206 and a negative blade 207, and are symmetrically distributed on the support arm 210. When the UAV executes a flight command, the positive blade 206 and the negative blade 207 rotate at the same time to generate the same air torque, causing the UAV to have an upward movement trend.

[0032] Furthermore, the UAV 5G communication system adopts the proposed CMOS reverse array chip based on the Pon array principle, and the mixer is the core device of the Pon array. When the subharmonic mixer is used to replace the fundamental mixer in the Pon array, the received radio frequency (RF) signal is mixed with the nth harmonic component of the local oscillator (LO) signal, and the intermediate frequency signal output by the subharmonic mixer is vIF = VRF cos(ωRFt + ϕRF) VLO cos(nωLOt). After filtering out the high-frequency mixing products from the intermediate frequency signal, the output signal is vIF(conj) = 1 / 2 VRFVLO {cos[(nωLO − ωRF)t − ϕRF]} The reverse array utilizes the carrier conjugate phase information in the received signal to achieve accurate and fast beam pointing adjustment.

[0033] Furthermore, the reverse array research of this product focuses on the 26 GHz band in the 5G millimeter wave band. All core chips in the reverse array are designed and implemented using domestic low-cost 0.18 mm CMOS process.

[0034] Furthermore, the subharmonic mixer designed in the present invention adopts current multiplexing technology to solve this current demand contradiction problem. Resistors R1 and R2 (both 300 W) provide additional current branches for transistors M1 and M2, and optimize the resistance to avoid the AC signal directly reaching Vdd through the resistors. Benefiting from the current multiplexing technology, the DC current passing through the switch stage and the load is reduced, and the resistance of the load resistors RL1 and RL2 can reach 1 kW, which directly improves the conversion gain of the down-conversion mixer.

[0035] Furthermore, the mixer also uses current multiplexing technology. Transistors M1-4 form the transconductance stage of the mixer, in which PMOS tubes M3-4 provide additional current branches for NMOS tubes M1-2 as multiplexed current sources while increasing the effective transconductance, thereby reducing the DC current passing through the switch stage. The orthogonal local oscillator signal required by the switch-stage NMOS tubes M5-12 is also provided by the local oscillator network. The design of the local oscillator network of the up-conversion subharmonic mixer is the same as that of the down-conversion subharmonic mixer. The power amplifier has a single-ended input and a single-ended output. The transistor is biased in the class A amplifier mode, and the bias voltage Vb is 0.95 V. The first stage is a common common source and common gate amplifier stage. The second stage uses transconductance enhancement (gm-boosting) technology to increase the gain based on the common common source and common gate amplifier stage. C1 and L1 are connected in series to form an inductive impedance, which forms a negative feedback loop required by the transconductance enhancement technology with the inductor L2 to form a Gmax-core network. Transconductance enhancement technology achieves the purpose of gain improvement at the expense of circuit stability. The output impedance of the common source and common gate structure is high and has good isolation and stability. Therefore, the use of transconductance enhancement technology based on the common source and common gate structure can improve the gain while ensuring circuit stability.

[0036] Embodiment 2: The aircraft in the invention uses the MPU6050 sensor to calculate the attitude of the aircraft, and uses the designed Kalman filter and first-order low-pass filter to filter the data. The algorithm includes the following steps: S101: Define the value of the initial quaternion as q 0=1, q 1=0, q 2=0, q 3=0; S102: Read the accelerometer value and angular velocity value. The program defines variables as follows: ax , ay , az , gx , gy , gz , convert the gyroscope value to radians; S103: Extract the gravity component in the attitude matrix: ; Where: is the attitude gravity component; is the radian value of each angle; S104: Find the attitude error, perform cross product of the two vectors, and define ex , ey , ez are the three axis error elements: ; Where: V x、 V y、 V z They are X radian system, Y radian system, and Z radian system respectively; S105: complementary filtering, inputting the error into the PID controller and adding it to the angular velocity measured by the gyroscope to correct the angular velocity value; S106: Calculate the attitude angle. The mathematical formula is: ; ; Where: is the X attitude error parameter; is the Y attitude error parameter; is the attitude error angle of X, is the Y attitude error angle, are the Z angles in the attitude error respectively.

Claims

1. A drone based on 5G communication and 3D technology, characterized in that: A top plate (201) is fixedly mounted on the upper end of the overall frame structure of the drone (200), a plurality of groups of support frames (202) are symmetrically fixed on the periphery of the top plate (201), support feet (203) in contact with the ground are symmetrically mounted on the lower end of the overall frame structure, a plurality of support arms (210) are symmetrically connected to the middle end of the overall frame structure, and positive propellers (206) and negative propellers (207) are mounted at the ends of the symmetrically arranged support arms (210); the chip of the drone (200) adopts a Raspberry Pi development chip, with a built-in CMOS chip and a 5G millimeter wave reverse array minimalist architecture, and is built into a flight control mainboard (213) and connected to a first reduction stepping motor (211) and a second reduction stepping motor (214) via a DuPont line; when the drone is operating at high altitude, the drone is controlled by connecting the WIFI chip in the connection interface (106) of the VR glasses (100) via a communication module in the flight control mainboard (213).

2. The drone based on 5G communication and 3D technology according to claim 1, characterized in that: The propeller blades of the drone are distributed in an X-shaped manner. When the drone executes a flight command, the positive propeller (206) and the negative propeller (207) rotate in opposite directions.

3. The drone based on 5G communication and 3D technology according to claim 1, characterized in that: The ground operator wears the VR glasses (100), and observes in real time through the OLED display (101) of the VR glasses (100), the image signal transmitted back by the flight control mainboard (213) received by the built-in WIFI chip of the connection interface (106), and the ground operator adjusts the headband (105) to make it fully adapt to the wearer's head, and the wearer observes the image information projected on the optical lens (102) and the information transmitted back by the sensor (103), and timely adjusts the flight status of the unmanned aerial vehicle.

4. The drone based on 5G communication and 3D technology according to claim 3, characterized in that: The support frame (202) is symmetrically fixedly connected to the top plate (201) and presents a hexagonal symmetrical structure. The support frame (202) is a double-layer structure with a Dupont wire passing through the middle. The support frame (202) is fixedly connected by screws (205) and nuts (204); The top plate (201) is a double-layer structure, with a support arm (210) fixedly connected in the middle thereof by screws (205) and nuts (204), a first reduction stepping motor (211) being symmetrically connected to the support arm (210), the first reduction stepping motor (211) being electrically connected to a flight control mainboard (213) placed on the lower top plate (201) via a DuPont line, the inside of the support arm (210) being a hollow structure, the first reduction stepping motor (211) passing through the support arm (210) via a DuPont line to be electrically connected to a stepping motor (208) on the other side, a support column (219) being symmetrically fixed on the top plate (201) in the middle, a loading platform (220) being fixedly connected to the support column (219), and a storage battery used by the drone being stored between the loading platform (220) and the top plate (201).

5. The drone based on 5G communication and 3D technology according to claim 4, characterized in that: The output shaft of the stepper motor (208) is connected to the positive propeller (206) or the negative propeller (207), and when the UAV executes a flight command, the positive propeller (206) and the negative propeller (207) rotate simultaneously to generate the same air torque, so that the UAV has an upward movement trend.

6. The drone based on 5G communication and 3D technology according to claim 2, characterized in that: The drone 5G communication system adopts the proposed CMOS reverse array chip, which is based on the Pon array principle. The mixer is the core component of the Pon array. When the subharmonic mixer is used to replace the fundamental mixer in the Pon array, the received RF signal is mixed with the nth harmonic component of the local oscillator signal. The intermediate frequency signal output by the subharmonic mixer is the intermediate frequency signal. After filtering out the high-frequency mixing product, the output signal is the reverse array using the carrier conjugate phase information in the received signal to achieve accurate and fast beam pointing adjustment.

7. The drone based on 5G communication and 3D technology according to claim 6, characterized in that: The reverse array research focuses on the 26 GHz frequency band in the 5G millimeter wave frequency band. All core chips in the reverse array are designed and implemented using domestic 0.18 mm CMOS process; The subharmonic mixer adopts current multiplexing technology. Resistors R1 and R2 provide additional current branches for transistors M1 and M2. By optimizing the resistance value, the AC signal is prevented from directly reaching Vdd through the resistor. Based on the current multiplexing technology, the DC current passing through the switch stage and the load is reduced. The resistance value of the load resistors RL1 and RL2 can reach 1 kW, thereby improving the conversion gain of the down-conversion mixer.

8. The drone based on 5G communication and 3D technology according to claim 7, characterized in that: The mixer adopts current multiplexing technology. Transistors M1~4 form the transconductance stage of the mixer. Among them, PMOS tubes M3~4 increase the effective transconductance and act as multiplexed current sources to provide additional current branches for NMOS tubes M1~2 to reduce the DC current passing through the switch stage; the orthogonal local oscillator signals required by the switch stage NMOS tubes M5~12 are also provided by the local oscillator network. The design of the local oscillator network of the up-conversion subharmonic mixer is the same as that of the down-conversion subharmonic mixer.

9. The drone based on 5G communication and 3D technology according to claim 8, characterized in that: The power amplifier has a single-ended input and a single-ended output. The transistor is biased in the Class A amplifier mode with a bias voltage Vb of 0.95 V. The first stage is an ordinary cascode amplifier stage, and the second stage uses transconductance enhancement technology to improve the gain based on the ordinary cascode amplifier stage. C1 and L1 are connected in series to form an inductive impedance, which together with the inductor L2 forms a negative feedback loop required for the transconductance enhancement technology to form a Gmax-core network. The transconductance enhancement technology achieves the purpose of gain improvement at the expense of circuit stability. The output impedance of the cascode structure is high and has good isolation and stability. Based on this, the transconductance enhancement technology based on the cascode structure is used to ensure circuit stability while improving the gain.

10. The method for operating a drone based on 5G communication and 3D technology according to any one of claims 1 to 9, characterized in that: The aircraft uses the MPU6050 sensor to calculate the aircraft's attitude, and uses a Kalman filter and a first-order low-pass filter to filter the data, which includes the following steps: S101: Define the value of the initial quaternion as q 0=1, q 1=0, q 2=0, q 3=0; S102: Read the accelerometer value and angular velocity value. The program defines variables as follows: ax , ay , az , gx , gy , gz , convert the gyroscope value to radians; S103: Extract the gravity component in the attitude matrix: ； Where: is the attitude gravity component; is the radian value of each angle; S104: Find the attitude error, perform cross product of the two vectors, and define ex , ey , ez are the three axis error elements: ； Where: V x、 V y、 V z They are X radian system, Y radian system, and Z radian system respectively; S105: complementary filtering, inputting the error into the PID controller and adding it to the angular velocity measured by the gyroscope to correct the angular velocity value; S106: Calculate the attitude angle. The mathematical formula is: ； ； Where: is the X attitude error parameter; is the Y attitude error parameter; is the attitude error angle of X, is the Y attitude error angle, are the Z angles in the attitude error respectively.

Images