State Estimation And Motion Planning For Dynamic Systems On Lie Group Based On Gaussian Process

The autonomous capability of space service robots and planetary rovers is an important guarantee to complete tasks when people do not control the closed loop,which involves state estimation and motion planning.For example,the autonomous docking of space robots and target spacecraft requires estimating the relative pose,planning a reasonable path for the maintenance of the robot arm,and real-time positioning of the patrol for autonomous navigation of extraterrestrial objects.In this paper,we study the application of Gaussian Process in state estimation and motion planning,the same continuous-time trajectory representation,the similar factor graph problem structure and the maximum posteriori probability inference framework shared by them.Firstly,the prior of Gaussian Process in vector space is introduced.For state estimation and motion planning,they are essentially trajectory optimization problem.Based on the Gaussian Process,the trajectory is expressed as a continuous-time form,and the sparse kernel matrix is obtained by the constant-velocity prior of the Gaussian Process,which makes the interpolation of the Gaussian Process only need constant time.The workspace of the robot is on Lie group space and the vector space is trivial Lie group.Then the prior of Gaussian Process is extended to non-trivial Lie group.Then,the problem of state estimation based on Gaussian Process is described mathematically.Gaussian Process is used to describe the continuous time trajectory of the robot,and the problem to be solved is modeled as simultaneous trajectory estimation and mapping.If only the trajectory is estimated,it is a special case of this problem.By constructing sparse kernel matrix,trajectory described by Gaussian Processes can interpolate the trajectory quickly,and then parameterize the trajectory with a small number of states.Sparsity greatly speeds up the solution of the problem.The problem is expressed as a factor graph,and Bayesian tree is used to further realize fast incremental updating.Experiments on several datasets show that the proposed algorithm can obtain high estimation accuracy by comparing the estimated trajectory with the ground truth trajectory.The motion planning problem is viewed as probabilistic inference,and the dual of optimization and inference is used to transform the trajectory optimization problem into factor graph for inference.For obstacle avoidance,we construct Gaussian Process prior factor for trajectory smoothing and obstacle factor.Due to the use of a small number of support state parameterized trajectories,obstacle avoidance detection is needed for trajectory interpolation.Obstacles are further categorized into conventional obstacle factors and interpolation obstacle factors.Bayesian tree is used for incremental inference of factor graphs.The problem of re-planning is also studied by using the incremental characteristics of Bayesian tree.The experimental results on mobile platform manipulator and stationary base manipulator verify the adaptability and effectiveness of the algorithm.