Kinodynamic Motion Planning Based On Vector Field For Quadrotors

The quadrotor has the advantages of simple mechanical structure,low manufacturing cost and strong maneuverability,as well as the characteristics of easy hovering,vertical take-off and landing and fixed-point cruise flight.Thanks to these advantages and characteristics,it has been widely used in the fields of earthquake relief,pipeline cruise and agricultural survey.Motion planning is one of the important functions of robot technology,in which the quadrotor needs to plan dynamic and feasible collision-free trajectories when performing mission.The stratification strategy of the motion planning can lead to the generated trajectory which limited to a given homology class,and the rapidly-exploring random tree(RRT)has a lower efficiency of sampling.For these problems,a kinodynamic motion planning algorithm based on vector fields is proposed,which mainly works in the following four aspects.Firstly,on the basis of the quadrotor motion mechanism,the kinematics and dynamics model of the quadrotor based on the Euler angles are derived using the Newton-Euler equation.Linearize the dynamics system of the quadrotor according to the near-hovering method.The double-loop PID control with an inner loop and a outer loop is applied to the small-angle flight control of the quadrotor,and verify the effectiveness of the established quadrotor dynamics model and control model in complex trajectory tracking,thus creating an experimental basis for the research of motion planning.Secondly,on the basis of several commonly used path planning algorithms,according to the characteristics of the higher dimensionality of the quadrotor's planning space,the optimal rapid exploration random tree(RRT*)based on sampling is selected as the path planner.In order to solve the problem of numerical instability when calculating multi-segment or high-order polynomials in polynomial trajectory optimization using quadratic programming,a polynomial trajectory optimization method of unconstrained quadratic programming is proposed.This method uses endpoint derivatives instead of polynomial coefficients as decision variables.Thus,the equation-constrained QP problem is transformed into an unconstrained QP problem.The experimental results show the trajectory optimization effect of the method on the waypoints extracted by any waypoint and path,analyze the influence of the number of waypoints and geometric paths on the trajectory,and reveal that the hierarchical motion planning restricts the trajectory to the homotopy class of the path.Then,on the basis of the concept of kinodynamic motion planning problem and the algorithm framework of kinodynamic motion planning based on RRT*,aiming at the problem of low sampling efficiency of RRT*,presenting a method based on vector field that constructs a cone to constrain the sampling-state,limiting the sampling-state to a set of reachable spaces only in the known state,so that the constrained sampled state is consistent with the local motion trend.According to the quadrotor's dynamic equations,the optimal control principle is used to generate motion primitives,which solves the two-point boundary value problem of kinodynamic motion planning.The state trajectories generated by the motion primitives are constrained by the system power and environment,so the input feasibility and collision detection of the state trajectories are analyzed.Finally,combined with the motion control and kinodynamic motion planning of the quadrotor,the navigation experiment of the quadrotor was carried out,and the dynamic feasibility of the state trajectory planned by the kinodynamic motion planning under complex environment was verified.The experimental results show that the kinodynamic motion planning algorithm proposed in this thesis explores more state nodes with shorter running time under the same number of iterations,and the generated trajectory has a smaller arrival time and lower control costs.