Kinodynamic Motion Planning For Multirotor Aerial Vehicles

In recent years,small-sized Multirotor Aerial Vehicles(MAVs)have been used in aerial photography,inspection and monitoring,agricultural spraying,logistics and delivery,and many other fields.As their applications in various fields become broader,the demands for their automation,autonomy,and intelligent operation are getting stronger,and developing autonomous navigation systems for MAVs has become a research hotspot.While stable perception and control can ensure a lower performance bound of the autonomous navigation systems,robust and efficient motion planning determines how well the missions can be accomplished.As the operation scenarios incline to the low-altitude near-ground complex environment,the demands for obstacle avoidance during navigation become higher,and the role of motion planning considering obstacle avoidance,physical vehicle limitations,and task requirements becomes more critical in the overall autonomous navigation system.Three main common situations in MAV autonomous navigation are summarized: 1.Global planning.In this case,a large-scale and accurate static global map can be acquired.The global optimal trajectory planning is performed,and the navigation execution of this trajectory is performed without online sensing and replanning;2.Local planning.There is no a priori map information,and only a small area around the MAV itself can be sensed dynamically in realtime,such that the navigation process requires replanning continuously according to the latest accurate map information.Each replanning is called local trajectory planning;3.Tracking planning.A rough large-scale global map is known,and a global trajectory can be planned accordingly.However,the global map is inaccurate or outdated,and the global trajectory may collide with actual obstacles.Therefore,in tracking the global reference trajectory,there is still a need for dynamic perception to build accurate arounding maps,as well as real-time replanning to avoid actual obstacles.This paper studies motion planning of MAV autonomous navigation in the above situations,which mainly includes:1.For the global planning situation,the planning algorithm should focus on achieving global optimality and quick convergence.Our solution deeply combines local optimization with sampling-based methods.We propose an algorithm called spatio-temporal deformable tree that grows a trajectory tree to explore the solution space and meanwhile deforms simultaneously in the time and space dimensions.It naturally selects a trajectory with better path topology(homotopy class)as the tree grows until approaching the near global optimal.The growth of a tree edge is modeled as solving a two-point boundary value problem with a partially constrained end state,and a constraint relaxation strategy is used to accelerate the solving process.We define the concept of a deformation unit,which only contains one state node in the tree and all the trajectory edges connected to it,accounting for only a small part of the overall trajectory tree.We design objectives expressing the overall tree quality by means of deformation units,and optimize in both time and space dimensions with different combinations of the units.In this way,the overall tree quality is improved effectively at a minimum cost,and thus the convergence is improved.2.For the local planning situation,the planning algorithm should consider the vehicle motion,which is not stationary,and consider whether it can be solved in real-time to cope with the instantly changing environment.Our solution adopts a hierarchical planning framework.It contains a sampling-based kinodynamic front-end module and a following post-process back-end module,which looks for a locally optimal trajectory in each replanning cycle.In the front-end,an approximate topological graph is quickly constructed according to the environment and is used to guide the sampling in the state space,which significantly improves the efficiency of the sampling-based kinodynamic planning.In the back-end,exploiting the front-end legacy,we propose a lightweight post-processing algorithm using bi-level optimization,and the inner layer of which has closed-form solutions,improving the smoothness and continuity with minimal computation resources.The proposed planning framework and algorithm are then applied to a real quadrotor for autonomous navigation tests,verifying the algorithm’s effectiveness in obscure environments such as dense outdoor forests and heavily occluded indoor rooms.3.Based on the above-mentioned hierarchical planning,we propose integrating fast regional optimization and bi-directional search in the global sampling process for narrow space planning.The regional optimization prioritizes exploring limited regional spaces and is solved by a sequence of unconstrained quadratic programming with closed-form solutions for each iteration.To better exploit the front-end legacy,we further incorporate the obstacle information into the objective design of the back-end optimization while still retaining a quadratic programming structure such that efficiency is guaranteed.As a result,the overall performance of the sampling-based front-end and the back-end optimization is enhanced in that an initial feasible solution can be obtained faster,and the success rate of optimization is higher,thus better meeting the demands of real-time replanning.Extensive numerical comparison tests and navigation tests on a real quadrotor are then conducted to verify the effectiveness of the proposed method in dense obstacle and maze-like environments.4.In the case of tracking planning,apart from considering the vehicle motion and the real-time capability,the planning algorithm further needs to respect the space-time fitness with the reference trajectory and the consistency between successive replan cycles.To address the problem of incoherent planning results during successive replan cycles when avoiding obstacles,which leads to frequent path switchings,making the vehicle ”hesitate” to dodge and swing,and increase the risk of collision,this paper defines the τ-topology-coherence relationship between trajectories and proposes sampling-based kinodynamic planning algorithms that improve replan coherence,producing smoother and safer trajectories during the overall navigation process.Considering the conflicting constraints of fitting in the reference trajectory and avoiding obstacles,this paper proposes a trajectory optimization method for tracking replanning that constrains the end-state in the reference trajectory and optimizes it adaptively.We use trajectory classes,penalty functions,and other methods to eliminate constraints and use gradient descent to solve the problem.The maximum step size is calculated according to the τ-topology-coherence relationship during the line search,keeping the resultant optimized trajectory in the same path topology as the initial trajectory.Compared with the traditional fixed end-state method,the proposed method greatly reduces the real-time position tracking error while satisfying the obstacle avoidance constraint.The performance of the proposed trajectory tracking algorithm is verified in a realistic simulated MAV navigation system.