MPC-based four-rotor collision avoidance method with limited load bearing and computing power
Through the MPC-based method, a dynamic model of the quadrotor UAV is established and a collision avoidance trajectory is designed. Combined with the control algorithm of the variable prediction step size mechanism, the collision avoidance problem of the quadrotor UAV in an obstacle environment is solved, and the collision avoidance problem of the quadrotor UAV is realized. It effectively avoids obstacles and reduces the calculation burden.
Patent Information
- Application Number
- CN202411208429.X
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-30
- Publication Date
- 2024-12-06
AI Technical Summary
In the design of four-rotor drone collision avoidance in an unknown obstacle environment, depth cameras and 3D lidars are bulky and have high computing requirements, making it difficult to effectively avoid collisions under limited load and computing capabilities, and millimeter-wave radar only output points. Target information, it is difficult to deal with multi-constraint problems.
The four-rotor collision avoidance method based on MPC's limited load-bearing and computing capabilities is optimized to track the planned collision avoidance trajectory and reduce the model prediction control algorithm with variable prediction step size mechanism. Calculate the amount and improve control accuracy.
The four-rotor drone with limited load-bearing and computing power can effectively avoid obstacles, reduce the computing burden, and maintain good tracking effect in a multi-constrained environment.
Smart Images

Figure CN119088070A_ABST
Abstract
Description
Technical field:
[0001] The invention belongs to the technical field of unmanned aerial vehicle collision avoidance, and in particular relates to a four-rotor collision avoidance method with limited load and computing power based on MPC. Background technology:
[0002] With the rapid development of intelligent agents, quadcopters have attracted the interest of many research institutions and technology companies. Due to their flexibility and convenience, quadcopters are widely used in disaster relief, environmental monitoring, surveillance, exploration, military operations and other fields. In an unknown obstacle environment, if there are obstacles on the flight trajectory, it is necessary to perform online collision avoidance design on the quadcopter. The collision avoidance of quadcopters is usually completed online through two steps: detection planning and trajectory control.
[0003] For detection planning, the detection sensor must have a certain estimation accuracy for obstacles and meet the carrying capacity of the quadrotor. As we all know, depth cameras and 3D lidars are too bulky and require high computing power, which is disadvantageous for quadrotors with limited carrying and computing power. When the load of the quadrotor is required to be as light as possible, millimeter-wave radar can be used as a priority to detect obstacles for the quadrotor. Millimeter-wave radar can immediately output the direction and distance of the obstacle, thereby saving the computing power of the onboard computer installed on the quadrotor. Based on the obstacle position, the trajectory planning method of the quadrotor is used to give a collision avoidance trajectory. In addition, it should be noted that the millimeter-wave radar only outputs the point target information closest to the quadrotor, and the computing power is limited by the onboard computer.
[0004] The main task of collision avoidance trajectory control for quadrotors is how to design an effective controller so that the quadrotor can accurately track the planned collision avoidance trajectory. Many nonlinear controllers for quadrotors only use the current state to track the reference trajectory, and it is difficult to handle multi-constraint problems in the collision avoidance process. MPC not only uses future states for control, but can also handle multi-constraint problems, which provides a basis for collision avoidance trajectory control for quadrotors. It should be noted that MPC usually requires powerful computing power, which is challenging for the onboard computer installed on the quadrotor. Summary of the invention:
[0005] The purpose of the present invention is to propose a quadrotor collision avoidance method with limited load and computing power based on MPC for situations where a quadrotor drone with limited load and computing power may be threatened by obstacles, so that the quadrotor with limited load and computing power can smoothly avoid obstacles and reach the target position.
[0006] The technical solution of the present invention is: a quadrotor collision avoidance method based on the limited load and computing power of MPC, the method comprising the following steps:
[0007] Step 1: Establish and discretize the dynamic model of the quadrotor drone.
[0008] Step 2: Design collision avoidance trajectory based on information from the millimeter-wave radar.
[0009] Step 3: Design a model predictive control algorithm that introduces a variable prediction step size mechanism.
[0010] Furthermore, the dynamic model of the quad-rotor drone is established and discretized as follows:
[0011] According to Newton's second law and Euler's equation, the quad-rotor drone is modeled and analyzed. The dynamic model of the quad-rotor drone is as follows:
[0012]
[0013] Where x(t), y(t), z(t) are the positions of the quadrotor drone in space; p, q, r are the angular velocities of the quadrotor drone rotating around the coordinate axes x, y, z of the ground coordinate system; I x ,I y ,I z is the moment of inertia of the quadcopter UAV rotating around its axis; m is the mass of the quadcopter UAV; g is the acceleration of gravity; U1, U2, U3, and U4 are the control inputs;
[0014] The quadrotor model is divided into a position loop system and an attitude loop system. The attitude loop system is implemented using a cascade PID controller. The expected roll angle and pitch angle of the attitude loop system are expressed as:
[0015]
[0016] in, and θ d (t) is the expected value of the attitude loop system, u1(t) and u2(t) represent the virtual control inputs in the X and Y directions, respectively;
[0017] Taking the X direction as an example for analysis, the tracking error is expressed as follows:
[0018]
[0019] Among them, x d (t) is the reference position, x(t) is the real-time position, ε1(t) is the position tracking error, and ε2(t) is the velocity tracking error; the tracking error system is expressed as follows:
[0020]
[0021] In addition, x(t) and Rewrite it as a vector The error is expressed as ε e (t)=[ε1(t),ε2(t)] T ; In the X direction, the kinematic model is:
[0022]
[0023] The tracking error system is discretized as follows
[0024]
[0025] Where T0 is the sampling period.
[0026] Further, the collision avoidance trajectory is designed according to the information of the millimeter wave radar, as follows:
[0027] The collision avoidance trajectory is planned based on the detected obstacle information. The collision avoidance trajectory planning is divided into four stages: initial stage, deviation stage, relaxation stage and regression stage. In the initial stage, the collision avoidance trajectory is based on the target position X p =[x p ,y p ] T Initial reference trajectory; in the away phase, from the real-time position X(t) = [x(t), y(t)] T The direction vector to the target position is expressed as
[0028]
[0029] Then the rotation matrix C2 is introduced as
[0030]
[0031] Reference speed It is expressed as
[0032] V d (t) = k v C2E(t)
[0033] Among them, k v is the speed constant; by the reference speed V d (t) Integrate to get the reference position X d (t) = [x d (t),y d (t)] T , when the distance between the quadrotor drone and the obstacle is greater than or equal to the safety distance d in the Y direction safeWhen the collision avoidance trajectory enters the relaxation stage, the quadrotor drone keeps a safe distance from the obstacle in the Y direction and moves at a constant speed in the X direction. After passing the obstacle, the quadrotor drone enters the return stage. In the return stage, the quadrotor drone moves toward the initial trajectory, and its acceleration is expressed as follows
[0034]
[0035] Where a is the acceleration constant, t d is the time when the quadrotor reaches the midpoint of the regression phase; if the quadrotor reaches the initial reference trajectory, it re-enters the original phase; in addition, according to the switching time between any two phases, the entire area is divided into a normal area Φ and a switching area Ψ; the neighborhood of the switching point between the two phases is called the switching area, and the other areas are called normal areas.
[0036] Furthermore, the model predictive control algorithm that introduces the variable prediction step size mechanism is specifically as follows:
[0037] The variable prediction step size MPC is used to control the quadrotor drone in the whole area; in the normal area Φ, the cost function is as follows
[0038]
[0039] Among them, T n is the prediction time domain, P, Q and R are three positive definite matrices;
[0040] Let t sk To predict the switching time in the time domain; in the switching region Ψ, the cost function is expressed as
[0041]
[0042] Among them, T s is the prediction time domain; by adding the quadcopter state and control input to the cost function, the quadcopter state changes in the direction of approaching the target state faster; then, the optimization problem is designed as
[0043]
[0044] stε(t k |t k )=ε(t k ),ε(t sk |t sk )=ε(t sk )
[0045]
[0046] ε e (tk +T n |t k )∈Ω,ε e (t sk +T s |t sk )∈Ω,
[0047] in
[0048]
[0049] The current state information of the quadrotor drone is obtained through the positioning module installed on it. After solving the optimization problem, the first control quantity is only applied in the first sampling period, and the process is repeated at the beginning of the next sampling period to achieve rolling optimization.
[0050] Beneficial Effects
[0051] Compared with the prior art, the present invention has the following advantages:
[0052] 1. Considering the limited load of the quadcopter and the computing power of its onboard computer, a millimeter-wave radar sensor that is as light as possible is selected to detect obstacle information. This not only ensures the load limit, but also effectively reduces the computing burden of the onboard computer.
[0053] 2. Considering that millimeter-wave radar can only output point target information and cannot determine the shape of obstacles, a simple collision avoidance trajectory planning method is designed to minimize the calculation amount of the planning part while avoiding collisions with obstacles.
[0054] 3. Since the planned collision avoidance trajectory is not completely smooth, the general control algorithm cannot achieve a good tracking effect. Therefore, a variable prediction step model predictive control method is designed to improve the control accuracy and avoid collisions while reducing the solution time as much as possible. Description of the drawings:
[0055] Figure 1 A control structure diagram of a four-rotor collision avoidance method with limited load and computing power based on MPC of the present invention;
[0056] Figure 2 The basic structure diagram of the quad-rotor drone of the present invention;
[0057] Figure 3 A schematic diagram of the detection results of the sensor used in the present invention;
[0058] Figure 4 A schematic diagram of a collision avoidance trajectory of the present invention;
[0059] Figure 5 It is a normal area and a switching area diagram of the present invention;
[0060] Figure 6 For the present invention k ,t sk , T n and T s The relationship diagram between
[0061] Figure 7 It is the collision avoidance trajectory tracking diagram of the present invention;
[0062] Figure 8 This is a tracking error result diagram of the present invention; Specific implementation method:
[0063] The present invention is described in detail below with reference to the accompanying drawings and specific embodiments.
[0064] The present invention provides a four-rotor collision avoidance method with limited load and computing capability based on MPC. Figure 1 The control structure diagram of a four-rotor collision avoidance method with limited load and computing power based on MPC provided by the present invention. The desired position of the four-rotor UAV is planned online based on the detected obstacle information. Then, according to the switching mechanism and the planned collision avoidance trajectory, a suitable prediction step size is selected to complete the position control of the four-rotor. After that, the attitude of the four-rotor is further controlled by using a cascade PID controller in the attitude loop system. Specifically, the following steps are included:
[0065] Step 1, Figure 2 The basic structure diagram of the quad-rotor drone of the present invention is shown in Figure 1. The quad-rotor drone is modeled and analyzed according to Newton's second law and Euler's equation. The dynamic model of the quad-rotor drone is as follows:
[0066]
[0067] Where x(t), y(t), z(t) are the positions of the quadrotor drone in space; p, q, r are the angular velocities of the quadrotor drone rotating around the coordinate axes x, y, z of the ground coordinate system; I x ,I y ,I z is the moment of inertia of the quadcopter UAV rotating around its axis; m is the mass of the quadcopter UAV; g is the acceleration of gravity; U1, U2, U3, and U4 are the control inputs;
[0068] The quadrotor model is divided into a position loop system and an attitude loop system. The attitude loop system is implemented using a cascade PID controller. The expected roll angle and pitch angle of the attitude loop system are expressed as:
[0069]
[0070] in, and θ d(t) is the expected value of the attitude loop system, u1(t) and u2(t) represent the virtual control inputs in the X and Y directions, respectively;
[0071] Taking the X direction as an example for analysis, the tracking error is expressed as follows:
[0072]
[0073] Among them, x d (t) is the reference position, x(t) is the real-time position, ε1(t) is the position tracking error, and ε2(t) is the velocity tracking error; the tracking error system is expressed as follows:
[0074]
[0075] In addition, x(t) and Rewrite it as a vector The error is expressed as ε e (t)=[ε1(t),ε2(t)] T ; In the X direction, the kinematic model is:
[0076]
[0077] The tracking error system is discretized as follows
[0078]
[0079] Where T0 is the sampling period.
[0080] Step 2, Figure 3 is a schematic diagram of the detection results of the sensor used in the present invention, Figure 4 This is a schematic diagram of the collision avoidance trajectory. Figure 5 It is a diagram of the normal area and the switching area. The collision avoidance trajectory is designed based on the information of the millimeter wave radar, as follows:
[0081] The collision avoidance trajectory is planned based on the detected obstacle information. The collision avoidance trajectory planning is divided into four stages: initial stage, deviation stage, relaxation stage and regression stage. In the initial stage, the collision avoidance trajectory is based on the target position X p =[x p ,y p ] T Initial reference trajectory; in the away phase, from the real-time position X(t) = [x(t), y(t)] T The direction vector to the target position is expressed as
[0082]
[0083] Then the rotation matrix C2 is introduced as
[0084]
[0085] Reference speed It is expressed as
[0086] V d (t) = k v C2E(t)
[0087] Among them, k v is the speed constant; by the reference speed V d (t) Integrate to get the reference position X d (t) = [x d (t),y d (t)] T , when the distance between the quadrotor drone and the obstacle is greater than or equal to the safety distance d in the Y direction safe When the collision avoidance trajectory enters the relaxation stage, the quadrotor drone keeps a safe distance from the obstacle in the Y direction and moves at a constant speed in the X direction. After passing the obstacle, the quadrotor drone enters the return stage. In the return stage, the quadrotor drone moves toward the initial trajectory, and its acceleration is expressed as follows
[0088]
[0089] Where a is the acceleration constant, t d is the time when the quadrotor reaches the midpoint of the regression phase; if the quadrotor reaches the initial reference trajectory, it re-enters the original phase; in addition, according to the switching time between any two phases, the entire area is divided into a normal area Φ and a switching area Ψ; the neighborhood of the switching point between the two phases is called the switching area, and the other areas are called normal areas.
[0090] Step 3, Figure 6 t k ,t sk , T n and T s The model predictive control algorithm that introduces the variable prediction step size mechanism is specifically as follows:
[0091] The variable prediction step size MPC is used to control the quadrotor drone in the whole area; in the normal area Φ, the cost function is as follows
[0092]
[0093] Among them, T n is the prediction time domain, P, Q and R are three positive definite matrices;
[0094] Let t skTo predict the switching time in the time domain; in the switching region Ψ, the cost function is expressed as
[0095]
[0096] Among them, T s is the prediction time domain; by adding the quadcopter state and control input to the cost function, the quadcopter state changes in the direction of approaching the target state faster; then, the optimization problem is designed as
[0097]
[0098] stε(t k |t k )=ε(t k ),ε(t sk |t sk )=ε(t sk )
[0099]
[0100] ε e (t k +T n |t k )∈Ω,ε e (t sk +T s |t sk )∈Ω,
[0101] in
[0102]
[0103] The current state information of the quadrotor drone is obtained through the positioning module installed on it. After solving the optimization problem, the first control quantity is only applied in the first sampling period, and the process is repeated at the beginning of the next sampling period to achieve rolling optimization.
[0104] Example
[0105] The present invention is based on a quad-rotor drone and other equipment as a collision avoidance experimental platform, which is mainly composed of a quad-rotor drone, a remote controller, a laptop computer, a router, a millimeter wave radar, two brackets and a pair of wireless data transmission equipment.
[0106] In order to solve the problem of collision avoidance with minimum load of quadrotor aircraft, this paper selects millimeter wave radar for obstacle detection, which can output information of 8 target points from obstacles, and the nearest target point is regarded as the obstacle that the quadrotor aircraft needs to avoid. If the obstacle detected last time is close to the obstacle detected currently, the two obstacles are regarded as one obstacle to avoid repeated planning.
[0107] The flight altitude is set to 1.5 meters, and the two brackets are used as two unknown obstacles. The quadcopter is used to complete the collision avoidance experiment to verify the effectiveness of the highlighted method. The network RTK service is used to further improve the positioning accuracy. The coordinate system is established with the initial position of the quadcopter as the origin, the east direction as the X-axis, and the north direction as the Y-axis. After the quadcopter is taken off using the remote control, the flight mode is switched from the fixed-point mode to the offboard mode, and the quadcopter flies from the initial position to the target position (15m, 0m). Finally, Figure 7 The collision avoidance trajectory is shown in the figure. Figure 7 It can be seen that the method of the present invention can enable a quadcopter with limited load and computing power to reach the target position while avoiding all obstacles smoothly. The tracking error results of the position are as follows Figure 8 As shown in the figure, it can be seen that the tracking error of the quadrotor drone fluctuates only near the switching points of each stage, and the tracking error at other positions is almost 0. Therefore, the collision avoidance method designed by the present invention for quadrotor drones with limited load and computing power is effective.
Claims
1. A quadrotor collision avoidance method with limited load and computing power based on MPC, characterized in that: The following steps are involved: Step 1: Establish and discretize the dynamic model of the quadrotor drone. Step 2: Design collision avoidance trajectory based on information from the millimeter-wave radar. Step 3: Design a model predictive control algorithm that introduces a variable prediction step size mechanism.
2. A collision avoidance method for a quadrotor drone based on MPC with minimum load and limited computing power according to claim 1, characterized in that: In step 1, the dynamic model of the quadrotor drone is established and discretized as follows: According to Newton's second law and Euler's equation, the quad-rotor drone is modeled and analyzed. The dynamic model of the quad-rotor drone is as follows: Where x(t), y(t), z(t) are the positions of the quadrotor drone in space; p, q, r are the angular velocities of the quadrotor drone rotating around the coordinate axes x, y, z of the ground coordinate system; I x ,I y ,I z is the moment of inertia of the quadcopter UAV rotating around its axis; m is the mass of the quadcopter UAV; g is the acceleration of gravity; U1, U2, U3, and U4 are the control inputs; The quadrotor model is divided into a position loop system and an attitude loop system. The attitude loop system is implemented using a cascade PID controller. The expected roll angle and pitch angle of the attitude loop system are expressed as: in, and θ d (t) is the expected value of the attitude loop system, u1(t) and u2(t) represent the virtual control inputs in the X and Y directions, respectively; Taking the X direction as an example for analysis, the tracking error is expressed as follows: Among them, x d (t) is the reference position, x(t) is the real-time position, ε1(t) is the position tracking error, and ε2(t) is the velocity tracking error; the tracking error system is expressed as follows: In addition, x(t) and Rewrite it as a vector The error is expressed as ε e (t)=[ε1(t),ε2(t)] T ; In the X direction, the kinematic model is: The tracking error system is discretized as follows Where T0 is the sampling period.
3. A quadrotor collision avoidance method with limited load and computing power based on MPC according to claim 2, characterized in that: In step 2, the collision avoidance trajectory is designed according to the information of the millimeter wave radar, as follows: The collision avoidance trajectory is planned based on the detected obstacle information. The collision avoidance trajectory planning is divided into four stages: initial stage, deviation stage, relaxation stage and regression stage. In the initial stage, the collision avoidance trajectory is based on the target position X p =[x p ,y p ] T The initial reference trajectory of In the distance phase, from the real-time position X(t) = [x(t), y(t)] T The direction vector to the target position is expressed as Then the rotation matrix C2 is introduced as Reference speed It is expressed as V d (t)=k v C2E(t) Among them, k v is the speed constant; by the reference speed V d (t) Integrate to get the reference position X d (t) = [x d (t),y d (t)] T , when the distance between the quadrotor drone and the obstacle is greater than or equal to the safety distance d in the Y direction safe When the collision avoidance trajectory enters the relaxation stage, the quadrotor drone keeps a safe distance from the obstacle in the Y direction and moves at a constant speed in the X direction. After passing the obstacle, the quadrotor drone enters the return stage. In the return stage, the quadrotor drone moves toward the initial trajectory, and its acceleration is expressed as follows Where a is the acceleration constant, t d is the time when the quadrotor reaches the midpoint of the regression phase; if the quadrotor reaches the initial reference trajectory, it re-enters the original phase; in addition, according to the switching time between any two phases, the entire area is divided into a normal area Φ and a switching area Ψ; the neighborhood of the switching point between the two phases is called the switching area, and the other areas are called normal areas.
4. A quadrotor collision avoidance method with limited load and computing power based on MPC according to claim 3, characterized in that: In step 3, the model predictive control algorithm that introduces the variable prediction step size mechanism is specifically as follows: The variable prediction step size MPC is used to control the quadrotor drone in the whole area; in the normal area Φ, the cost function is as follows Among them, T n is the prediction time domain, P, Q and R are three positive definite matrices; Let t sk To predict the switching time in the time domain; in the switching region Ψ, the cost function is expressed as Among them, T s is the prediction time domain; by adding the quadcopter state and control input to the cost function, the quadcopter state changes in the direction of approaching the target state faster; then, the optimization problem is designed as s.t.ε(t k |t k )=ε(t k ),ε(t sk |t sk )=ε(t sk ) ε e (t k +T n |t k )∈Ω,ε e (t sk +T s |t sk )∈Ω, in The current state information of the quadrotor drone is obtained through the positioning module installed on it. After solving the optimization problem, the first control quantity is only applied in the first sampling period, and the process is repeated in the next sampling period to achieve rolling optimization.
