Probabilistic Inference Based Simultaneous Trajectory Estimation And Planning For Micro Aerial Vehicles

Micro Aerial Vehicles(MAVs)have a wide range of applications in detection,inspection,and search-rescue tasks.In many cases,MAVs are required to work autonomously in unknown,dynamic,and obstacle-cluttered environments.In addition,due to the limitation of airborne resources,it poses a huge challenge to their abilities of state estimation and motion planning.In most existing works,state estimation and motion planning work in a serial mode,which leads to extra data transmission and storage consumption,on the other hand,it is difficult to pass the uncertainty effectively between these two modules.To this end,we consider a system-level lightweight integration for MAVs,study a simultaneous trajectory estimation and planning algorithm via probability inference,which unifies state estimation and motion planning under the theoretical framework of probability inference,so that they can be combined properly and promote each other.Firstly,we study on continuous-time trajectory estimation.The trajectory estimation for MAVs is modeled as a maximum a posteriori(MAP)problem expressed on a factor graph,which is then converted into nonlinear least square optimization.The Gaussian Process(GP)continuous-time trajectory representation is introduced,so that we can further utilize the sparsity of matrices and incremental characteristics to improve efficiency.Then,we study on continuous-time trajectory planning.It is regarded as a probabilistic inference problem,in which the GP trajectory representation is still adopted.Constraints(e.g.,obstacle avoidance)are defined as a series of binary events.By solving the corresponding MAP problem,the trajectory with the highest probability for MAVs to reach the target safely is found.Then the method is further extended to multi-MAV formation trajectory planning cases,and the simulation and physical experiments are performed to demonstrate the practicability and efficiency of the algorithm.Finally,the above two modules are integrated into a whole,yielding a simultaneous trajectory estimation and planning algorithm,in which estimation and planning are modelled as a whole MAP problem expressed on a single factor graph.In this way,the trajectory estimation and planning can be performed simultaneously.The result of a simple simulation experiment is given to illustrate the feasibility.