Abstract-We consider the task of planning smooth trajectories for robot motion. In this paper we make two contributions. First we present a method for cubic spline optimization; this technique lets us simultaneously plan optimal task-space trajectories and fit cubic splines to the trajectories, while obeying many of the same constraints imposed by a typical motion planning algorithm. The method uses convex optimization techniques, and is therefore very fast and suitable for real-time re-planning and control. Second, we apply this approach to the tasks of planning foot and body trajectory for a quadruped robot, the "LittleDog," and show that the proposed approach improves over previous work on this robot.
I. INTRODUCTIONIn this paper we consider the task of planning smooth trajectories for robot motion. This is one of the fundamental tasks of robotics, and has received a great deal of attention over the past several decades. One strategy that has proven particularly effective for this task is the use of smooth, parametrized splines to describe trajectories, either in jointspace or task-space. Cubic splines in particular are ubiquitous in robotic applications [1], as they provide a simple means of generating smooth (twice differentiable) trajectories for robot motion.Cubic splines for robot trajectories are typically employed as follows. First, one uses a high-level planning algorithm to generate a series of kinematically feasibly waypoints that the robot should pass through on its way to the goal. Next, one fits the parameters of a cubic spline that passes through all these points; the smoothness of the resulting cubic spline leads to a smoother motion of the robot than would be obtained, for example, by a linear spline that just interpolated between the waypoints. We refer to this as the "two-phase" approach, since the planning and spline fitting are done in separate phases.However, despite their advantages, cubic splines also suffer from a number of drawbacks. The chief problem is that in the typical two-phase application of cubic splines, the high-level waypoint planning is done separate from the cubic spline fitting procedure, which can lead to poor trajectories. To convey this intuition, consider the simple planning task shown in Figure 1 (a): the objective is to move a double pendulum, actuated at both joints, from the start to the goal, while avoiding the obstacle. Figure 1 (b) shows a possible output from a typical planner (for example, a randomized tree planner [2]) and the corresponding cubic spline fit to these waypoints. Due to the stochastic nature of the planner, the waypoints do not lead to a particularly nice final trajectory. Existing trajectory optimization techniques [3] can help mitigate this problem to some degree, but they usually