Unmanned aerial vehicle path implementation planning method based on rolling EKF

Abstract

The invention provides an unmanned aerial vehicle path implementation planning method based on rolling EKF, and the method provides a new path planning strategy for the UAV in an urban space. Firstly, environment information is sensed and acquired; secondly, performing state estimation and trajectory prediction by using an EKF (Extended Kalman Filter); and finally, the model predicts and controls the rolling optimization trajectory. According to the invention, state estimation and trajectory prediction are carried out by using extended Kalman filtering (EKF), so that noise in a trajectory observed by a sensor is reduced, and the predicted trajectory is close to an actual flight trajectory as much as possible; a time axis is introduced, on the basis of rolling optimization of model prediction control, the flight time of the unmanned aerial vehicle is divided into small time intervals, and a dynamic map is constructed based on the time intervals; and searching a feasible path by using a breadth-first search method, and obtaining an optimal path through an A * algorithm. Simulation results show that the provided obstacle avoidance method can effectively avoid static buildings and dynamic obstacles, and a safe flight path is planned for the unmanned aerial vehicle in the urban space.

Description

Technical Field

[0001] The invention relates to an extended Kalman filter and a model predictive control method, and in particular to a UAV path implementation planning method based on a rolling EKF. Background Art

[0002] With the rapid development of unmanned aerial vehicle (UAV) technology, its application in urban space is increasing, including target tracking, radar mapping, search and rescue, etc. However, the urban space environment is complex, with many static (such as buildings) and dynamic obstacles (such as other drones), which poses a serious challenge to the safe flight of drones. To ensure flight safety, drones need to have effective autonomous obstacle avoidance capabilities.

[0003] Existing obstacle avoidance technologies for drones mainly include model predictive control (MPC) and potential field methods. The MPC method avoids collisions by predicting the trajectory of obstacles and adjusting the speed of the drone in real time, but this method may encounter data disasters over time. The potential field method simulates the obstacle avoidance behavior of animals and guides the drone to avoid obstacles by calculating the potential field around the obstacle, but this method may not be accurate enough in complex environments.

[0004] In addition, there are studies based on different techniques such as collision cone method, hierarchical perception and avoidance method, and rapidly exploring random trees (RRT). Although these methods have certain effects in specific scenarios, their performance may be limited in dynamic and complex urban environments. Summary of the invention

[0005] Aiming at the limitations of existing unmanned aerial vehicle obstacle avoidance technology, the present invention proposes a method for implementing unmanned aerial vehicle path planning based on rolling EKF, which specifically includes the following steps:

[0006] Step 1: Perceive and obtain environmental information;

[0007] Drones use their onboard sensors to obtain information about the environment;

[0008] Step 2: Use EKF for state estimation and trajectory prediction;

[0009] A rectangular coordinate system is constructed for the top view of the drone, and the coordinate origin (0, 0) is set as the position of the drone. The actual trajectory of the drone is an uncertain curve based on the predicted trajectory. At the discrete moment n, the velocity components of the drone on the x-axis and y-axis are:

[0010]

[0011] Where: u x [n] and u y[n] represents the components of the control input of the UAV on the x and y axes, respectively, which is used to change the velocity vector of the UAV; v x [n] and v y [n] is the velocity vector of the drone on the x and y axes at time n; the obstacle position at time n is represented by its distance r from the x axis x [n] and the distance r from the y axis y [n]:

[0012]

[0013] Where: Δ is the small time interval, and the obstacle position at time n-1 is represented by its distance r from the x-axis. x [n-1] and the distance r from the y axis y [n-1]; the obstacle's current position r at time n x [n] and r y [n] represents the position r at the previous moment, that is, at moment n-1 x [n-1] and r y [n-1], and the velocity vector v superimposed on this x [n] and v y [n] is the product of the small time interval Δ;

[0014] In the discrete model of dynamic obstacles, it is assumed that the dynamic obstacle moves at the same speed before time n-1, but changes at time n; then the state vector s[n] is represented as a combination of the position of the drone and the position of the obstacle:

[0015]

[0016] The above formula can be expressed as: s[n] = As[n-1] + u[n], where: s[n] is the state vector at time n, A is the matrix coefficient, and u[n] is the control input vector;

[0017] The sensing signal is W, which contains the disturbance of the distance r between the UAV and the dynamic obstacle. r [n], and W, which contains the perturbation of the angle θ between the angular drone and the dynamic obstacle θ [n] Noise information:

[0018]

[0019] Where: W r [n] is the distance disturbance noise information, W θ [n] is the angle disturbance noise information, is the distance noise W r[n] is the distance sensing signal based on the actual value of the current distance sensing signal. is the angular noise W θ [n] is the angle sensing signal based on the actual value of the current angle sensing signal;

[0020] Modify the drone's sensing signal to:

[0021] x[n]=h(s[n])+w[n] (5)

[0022] Where: x[n] and h(s[n]) represent the drone sensing signal vector and the actual measurement value vector respectively, w[n] is the disturbance vector, and its definitions are as follows:

[0023]

[0024] The filtering model using the EKF function is:

[0025]

[0026] Where: a(s[n-1]) represents the historical input function of the EKF filter model, a(s[n-1]) = {s[n-1], s[n-2], ..., s[nk]}, represents the historical input from time nk to time n-1; Bu[n] represents the product of the control input vector u[n] and the corresponding weight coefficient matrix B, and the weight coefficient matrix B is assigned according to the control input vector;

[0027] EKF converts nonlinear equations into linear equations through Taylor series expansion. a(s[n-1]) and h(s[n]) are linearized at the state vector at time n-1 using Taylor series expansion. The Jacobian matrices A[n-1] and H(n) of the UAV flight state at time n are:

[0028]

[0029] Wherein, A[n-1] represents the Jacobian matrix obtained by linearizing the historical input function a(s[n-1]) of the EKF filter model, and H[n] represents the Jacobian matrix obtained by linearizing the measured value h(s[n]), where a is the abbreviation of a(s[n-1]); Represents the estimated value of the drone sensing signal s[n-1] at time n-1 and the information at time n-1 obtained based on s[n-1];

[0030] To simplify obstacles, a two-dimensional horizontal space is selected from the three-dimensional space; a time axis is introduced for dynamic obstacles, and a time-obstacle dynamic map TODM is constructed in the three-dimensional space;

[0031] In the top view plane rectangular coordinate system constructed above, TODM consists of the state<x,y,v,θ,t> The composition is: x and y represent the horizontal and vertical coordinates of the dynamic obstacle relative to the position of the drone, v is the flight speed of the drone, θ is the flight direction of the drone, and t is the flight time of the drone. There are a limited number of dynamic obstacles D_obstacle in the environment. i =(x i ,y i , v i ,θ i , t), where: In the plane rectangular coordinate system, with the drone as the coordinate origin, the horizontal and vertical coordinate positions (x i ,y i ), speed v i and direction θ i ; Assume that the update rate of TODM is a small time period Δt TEDM , for dynamic obstacles:

[0032]

[0033] Indicates that in a small time period Δt TEDM In the case of infinitesimal velocity v i and direction θ i and in a small time period [t, t+Δt TEDM ] remains unchanged, where Respectively represent t+Δt TEDM The speed and direction of the i-th dynamic obstacle at the moment; then the movement trend within this time period can be predicted and located;

[0034] The drone can sense the TEDM ] time period and convert it into TODM; this is because it is the minimum value of the time period, so the drone can only avoid obstacles in [t, t+Δt TEDM ]The obstacles within the time period can avoid all obstacles;

[0035] Step 3: Model predictive control rolling optimization trajectory;

[0036] The concept of timeline is introduced into the UAV, and rolling optimization is performed under the model predictive control MPC algorithm. This process divides the flight time of the UAV into multiple small time intervals. In each time interval, the UAV will build a time-obstacle dynamic diagram TODM based on the current state and predicted environmental information according to the process in step 2.

[0037] Using this dynamic map, the drone uses the breadth-first search (BFS) algorithm to explore all possible path nodes to ensure that a safe path is found that avoids all obstacles. Subsequently, the drone uses the A algorithm to evaluate these paths and select the optimal path node.

[0038] The drone constantly determines whether it has reached the mission point or the target point; if it has reached the predetermined location, the drone will terminate the algorithm and perform the corresponding task; if it has not yet arrived, the drone will continue to perform path planning and optimization, repeating steps one to three.

[0039] The method of the present invention first uses EKF to perform state estimation and trajectory prediction to reduce the noise in the trajectory observed by the sensor and make the predicted trajectory closer to the actual flight trajectory. Then, by introducing the time axis under the framework of model predictive control, the flight time of the UAV is divided into small time intervals, and a time-obstacle dynamic diagram (TODM) is constructed based on these time intervals. The breadth-first search method is used to explore feasible paths, and the A* algorithm is applied to determine the optimal path. The real-time UAV path planning method of the present invention not only improves the accuracy of state estimation and trajectory prediction, and enhances the robustness of the obstacle avoidance strategy, but also realizes real-time path planning and optimization by combining TODM and path planning algorithms, ensuring that the UAV can quickly adapt to dynamic changes in complex urban environments and plan a safe and efficient flight route. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 is a flow chart of the steps of the present invention;

[0041] Figure 2 A schematic diagram of the relationship between the predicted trajectory, actual trajectory, and observed trajectory of the drone described in the present invention;

[0042] Figure 3 This is a schematic diagram of the rolling optimization principle of model predictive control;

[0043] Figure 4 Shows the process of creating a rasterized dynamic map;

[0044] Figure 5 The unmanned state simulation results based on EKF are shown; (a) represents the estimation of the distance between the obstacle and the UAV, (b) is the estimation of the relative speed between the obstacle and the UAV, and (c) shows the estimation of the direction of the velocity vector;

[0045] Figure 6 A schematic diagram showing safe path planning within the drone's perception area;

[0046] Figure 7 Shows several key moments when the drone is performing real-time path planning. DETAILED DESCRIPTION

[0047] The present invention will be described in detail below with reference to the accompanying drawings.

[0048] like Figure 1 As shown in the figure, the flowchart of the steps of real-time UAV path planning based on rolling extended Kalman filter (EKF) describes in detail the whole process of UAV path planning, which is as follows.

[0049] Step 1: Perceive and obtain environmental information;

[0050] The drone needs to rely on its onboard sensors to perceive the surrounding environment information, including the location and movement trend of static and dynamic obstacles. Through this information, the drone can have a preliminary understanding of the surrounding environment. Obtaining environmental information through onboard sensors is well known to those skilled in the art and will not be repeated here.

[0051] Step 2: Use EKF for state estimation and trajectory prediction;

[0052] The drone uses the extended Kalman filter algorithm to filter these perception data. The EKF algorithm can effectively reduce the noise in the trajectory observed by the sensor and provide more accurate state estimation and trajectory prediction data. The algorithm is well known to those skilled in the art and will not be repeated here. State estimation involves the current position, speed, acceleration and other parameters of the drone, while trajectory prediction predicts the possible future flight path of the drone based on the current state and environmental information, which is described in detail below.

[0053] In the plane rectangular coordinate system constructed for the top view of the drone, the origin (0, 0) is set as the position of the drone. During the flight of the drone, it will inevitably be affected by uncertain factors, so the true trajectory is an uncertain curve based on the predicted trajectory. At the discrete moment n, the velocity components of the drone on the x-axis and y-axis are:

[0054]

[0055] Where: u x [n] and u y [n] represents the components of the control input of the UAV on the x and y axes, respectively, which is used to change the velocity vector of the UAV; v x [n] and v y [n] are the velocity vectors of the drone on the x and y axes at time n. Therefore, the obstacle position at time n can be expressed as its distance r from the x axis. x [n] and the distance r from the y axis y [n]:

[0056]

[0057] Where: Δ is the small time interval, and the obstacle position at time n-1 can be expressed as its distance r from the x-axis x [n-1] and the distance r from the y axis y [n-1]. The obstacle’s current position r at time n x [n] and r y [n] can be expressed as the position r at the previous moment, that is, at moment n-1 x [n-1] and r y [n-1], and the velocity vector v superimposed on this x [n] and v y [n] is the product of the small time interval Δ.

[0058] In the discrete model of dynamic obstacles, it is assumed that the dynamic obstacle moves at the same speed before (including) time n-1, but changes at time n. Then the state vector s[n] can be designed with the combination of the position of the drone and the position of the obstacle:

[0059]

[0060] The above formula can be expressed as: s[n] = As[n-1] + u[n], where: s[n] is the state vector at time n, A is the matrix coefficient, and u[n] is the control input vector.

[0061] The sensing signal is W, which contains the disturbance of the distance r between the UAV and the dynamic obstacle. r [n], and W, which contains the perturbation of the angle θ between the angular drone and the dynamic obstacle θ [n] Noise information:

[0062]

[0063] Where: W r [n] is the distance disturbance noise information, W θ [n] is the angle disturbance noise information, is the distance noise W r [n] is the distance sensing signal based on the actual value of the current distance sensing signal. is the angular noise W θ [n] is the angle sensing signal based on the original image, and θ[n] is the actual value of the current angle sensing signal.

[0064] The drone's sensing signal can be modified to:

[0065] x[n]=h(s[n])+w[n] (17)

[0066] Where: x[n] and h(s[n]) represent the drone sensing signal vector and the actual measurement value vector respectively, w[n] is the disturbance vector, and its definitions are as follows:

[0067]

[0068] The filter model using the EKF function can be designed as:

[0069]

[0070] Wherein: a(s[n-1]) represents the historical input function of the EKF filtering model, a(s[n-1]) = {s[n-1], s[n-2], …, s[nk]}, represents the historical input from time nk to time n-1; Bu[n] represents the product of the control input vector u[n] and the corresponding weight coefficient matrix B, and the weight coefficient matrix B is assigned according to the control input vector.

[0071] EKF converts nonlinear equations into linear equations through Taylor series expansion. a(s[n-1]) and h(s[n]) can be linearized at the state vector at time n-1 using Taylor series expansion, and the Jacobian matrices A[n-1] and H(n) of the UAV flight state at time n are designed:

[0072]

[0073] Among them, A[n-1] represents the Jacobian matrix obtained by linearizing the historical input function a(s[n-1]) of the EKF filter model, and H[n] represents the Jacobian matrix obtained by linearizing the measured value h(s[n]), where a is the abbreviation of a(s[n-1]). Represents the estimated value of the drone sensing signal s[n-1] at time n-1 and the information at time n-1 obtained based on s[n-1] This is due to the angle perturbation noise information W θ [n] and distance sensing signal W r The existence of [b] causes a deviation between the estimated value and the sensed signal.

[0074] Drones must avoid not only static buildings but also dynamic obstacles in a very limited area in urban space. Therefore, traditional obstacle avoidance methods are not suitable for complex environments. In order to simplify obstacles, a two-dimensional horizontal space is selected from the three-dimensional space. This is because the static buildings in the urban space remain unchanged. For dynamic obstacles, the time axis is introduced to construct a time-obstacle dynamic map TODM in the three-dimensional space.

[0075] In the top - view plane rectangular coordinate system constructed above, TODM consists of the state <x, y, v, θ, t), where: x and y respectively represent the abscissa and ordinate of the dynamic obstacle relative to the UAV position with the UAV position as the origin, v is the flight speed of the UAV, θ is the flight direction of the UAV (i.e., the angle between the flight direction of the UAV and the positive x - axis), and t is the flight time of the UAV. There are a finite number of dynamic obstacles D_obstacle in the environment i =<x i , y i , v i , θ i , t>, where: in the plane rectangular coordinate system, with the UAV as the coordinate origin, the abscissa and ordinate positions (x i , y i ), speed v i and direction θ i of the i - th dynamic obstacle. Let the update rate of TODM be the small time period Δt TEDM , for the dynamic obstacle:

[0076]

[0077] represents that in the case of an infinitesimal small time period Δt TEDM , the speed v i and direction θ i remain unchanged within the small time period [t, t + Δt TEDM , where respectively represent the speed and direction of the i - th dynamic obstacle at the moment t + Δt TEDM . Then the motion trend within this time period can be predicted and located. If the area that the obstacle (possibly) reaches is regarded as the obstacle area, then only by avoiding the obstacle area can a collision - free path be designed later. [t, t + Δt TEDM Applying this idea to the path planning in urban space, a safe flyable path is generated while avoiding dynamic obstacles, and TODM is constructed by perceiving obstacles.

[0078] The UAV can predict the obstacles within the time period [t, t + Δt TEDM within its sensing range and convert them into TODM. This is because it is the minimum value of the time period, so the UAV can avoid all obstacles only by avoiding the obstacles within the time period [t, t + Δt TEDM .

[0079] Step 3: Model Predictive Control Rolling Optimization Trajectory;

[0080] After obtaining accurate state estimation and trajectory prediction data, the drone introduces the concept of timeline and performs rolling optimization under the model predictive control (MPC) algorithm (MPC algorithm is well known to those skilled in the art). This process divides the flight time of the drone into multiple small time intervals. In each time interval, the drone will build a time-obstacle dynamic map (TODM) based on the current state and predicted environmental information according to the process in step 2. TODM is a dynamically updated map that reflects the relative position and motion state of the drone and obstacles in a specific time interval.

[0081] Using this dynamic map, the drone uses the breadth-first search (BFS) algorithm (BFC algorithm is well known to those skilled in the art) to explore all possible path nodes to ensure that a safe path is found that avoids all obstacles. Subsequently, the drone uses the A algorithm (A* algorithm is well known to those skilled in the art) to evaluate these paths and select the optimal path nodes. The A algorithm is an efficient path search algorithm that selects the best path by evaluating the cost of the path and the distance to the target.

[0082] During the entire path planning process, the drone will constantly determine whether it has reached the mission point or the target point. If it has reached the predetermined location, the drone will terminate the algorithm and perform the corresponding task, such as dropping supplies, taking photos, etc. If it has not arrived, the drone will continue to perform path planning and optimization, repeating steps 1 to 3, and adjusting the flight path in real time to ensure that the mission is completed safely and efficiently.

[0083] In addition, the path planning process of UAV is an iterative process, which requires continuous state estimation, trajectory prediction, map construction and path search to adapt to the uncertainty and dynamic changes of the environment. This UAV path implementation planning method based on rolling EKF not only improves the autonomous obstacle avoidance capability of UAV in complex urban environments, but also enhances the flight safety and mission execution efficiency of UAV through real-time path planning and optimization.

Claims

1. A UAV path planning method based on rolling EKF, characterized in that: The specific steps include: Step 1: Perceive and obtain environmental information; Drones use their onboard sensors to obtain information about the environment; Step 2: Use EKF for state estimation and trajectory prediction; A rectangular coordinate system is constructed for the top view of the drone, and the coordinate origin (0, 0) is set as the position of the drone. The actual trajectory of the drone is an uncertain curve based on the predicted trajectory. At the discrete moment n, the velocity components of the drone on the x-axis and y-axis are: Where: u x [n] and u y [n] represents the components of the control input of the UAV on the x and y axes, respectively, which is used to change the velocity vector of the UAV; v x [n] and v y [n] is the velocity vector of the drone on the x and y axes at time n; the obstacle position at time n is represented by its distance r from the x axis x [n] and the distance r from the y axis y [n]: Where: Δ is the small time interval, and the obstacle position at time n-1 is represented by its distance r from the x-axis. x [n-1] and the distance r from the y axis y [n-1]; the obstacle's current position r at time n x [n] and r y [n] represents the position r at the previous moment, that is, at moment n-1 x [n-1] and r y [n-1], and the velocity vector v superimposed on this x [n] and v y [n] is the product of the small time interval Δ; In the discrete model of dynamic obstacles, it is assumed that the dynamic obstacle moves at the same speed before time n-1, but changes at time n; then the state vector s[n] is represented as a combination of the position of the drone and the position of the obstacle: The above formula can be expressed as: s[n] = As[n-1] + u[n], where: s[n] is the state vector at time n, A is the matrix coefficient, and u[n] is the control input vector; The sensing signal is W, which contains the disturbance of the distance r between the UAV and the dynamic obstacle. r [n], and W, which contains the perturbation of the angle θ between the angular drone and the dynamic obstacle θ [n] Noise information: Where: W r [n] is the distance disturbance noise information, W θ [n] is the angle disturbance noise information, is the distance noise W r [n] is the distance sensing signal based on the actual value of the current distance sensing signal. is the angular noise W θ [n] is the angle sensing signal based on the actual value of the current angle sensing signal; Modify the drone's sensing signal to: x[n]=h(s[n])+w[n] (5) Where: x[n] and h(s[n]) represent the drone sensing signal vector and the actual measurement value vector respectively, w[n] is the disturbance vector, and its definitions are as follows: The filtering model using the EKF function is: Where: a(s[n-1]) represents the historical input function of the EKF filter model, a(s[n-1]) = {s[n-1], s[n-2], ..., s[nk]}, represents the historical input from time nk to time n-1; Bu[n] represents the product of the control input vector u[n] and the corresponding weight coefficient matrix B, and the weight coefficient matrix B is assigned according to the control input vector; EKF converts nonlinear equations into linear equations through Taylor series expansion. a(s[n-1]) and h(s[n]) are linearized at the state vector at time n-1 using Taylor series expansion. The Jacobian matrices A[n-1] and H(n) of the UAV flight state at time n are: Wherein, A[n-1] represents the Jacobian matrix obtained by linearizing the historical input function a(s[n-1]) of the EKF filter model, and H[n] represents the Jacobian matrix obtained by linearizing the measured value h(s[n]), where a is the abbreviation of a(s[n-1]); Represents the estimated value of the drone sensing signal s[n-1] at time n-1 and the information at time n-1 obtained based on s[n-1]; To simplify obstacles, a two-dimensional horizontal space is selected from the three-dimensional space; a time axis is introduced for dynamic obstacles, and a time-obstacle dynamic map TODM is constructed in the three-dimensional space; In the top view plane rectangular coordinate system constructed above, TODM consists of the state<x,y,v,θ,t> The composition is: x and y represent the horizontal and vertical coordinates of the dynamic obstacle relative to the position of the drone, v is the flight speed of the drone, θ is the flight direction of the drone, and t is the flight time of the drone. There are a limited number of dynamic obstacles D_obstacle in the environment. i = <x i ,y i , v i ,θ i , t>, where: In the plane rectangular coordinate system, with the drone as the coordinate origin, the horizontal and vertical coordinate positions (x i ,y i ), speed v i and direction θ i ; Assume that the update rate of TODM is a small time period Δt TEDM , for dynamic obstacles: Indicates that in a small time period Δt TEDM In the case of infinitesimal velocity v i and direction θ i and in a small time period [t, t+Δt TEDM ] remains unchanged, where Respectively represent t+Δt TEDM The speed and direction of the i-th dynamic obstacle at the moment; then the movement trend within this time period can be predicted and located; The drone can sense the TEDM ] time period and convert it into TODM; this is because it is the minimum value of the time period, so the drone can only avoid obstacles in [t, t+Δt TEDM ]The obstacles within the time period can avoid all obstacles; Step 3: Model predictive control rolling optimization trajectory; The concept of timeline is introduced into the UAV, and rolling optimization is performed under the model predictive control MPC algorithm. This process divides the flight time of the UAV into multiple small time intervals. In each time interval, the UAV will build a time-obstacle dynamic diagram TODM based on the current state and predicted environmental information according to the process in step 2. Using this dynamic map, the drone uses the breadth-first search (BFS) algorithm to explore all possible path nodes to ensure that a safe path is found that avoids all obstacles. Subsequently, the drone uses the A algorithm to evaluate these paths and select the optimal path node. The drone constantly determines whether it has reached the mission point or the target point; if it has reached the predetermined location, the drone will terminate the algorithm and perform the corresponding task; if it has not yet arrived, the drone will continue to perform path planning and optimization, repeating steps one to three.

Images