# Motion planning using second order Bézier curves

I'm trying to find an algorithm for a motion planning problem. I have $N$ points, $P_1$ to $P_N$, in $k$-dimensional cartesian space, defining $N-1$ segments. The problem is about constructing the fastest motion plan, that is, a function $p(t)$, approximating the path along the segments, bound by some constraints:

• For each segment defined by the points $P_i$ and $P_{i+1}$, let $M_i$ be some point on this segment ($M_i$ is part of the solution, not the input). For now, let's assume it's strictly within the segment. Now, the constraint is that our sought plan $p(t)$ starts with an initial segment from $P_1$ to $M_1$, is followed by $N-2$ second-order Bézier curves, each defined by the control points $(M_i, P_{i+1}, M_{i+1})$, and finally followed by a segment from $M_{n-1}$ to $P_n$.

• Velocity is preserved on boundaries between the curves and the start/end segments. Note that the direction of velocity is implicitly preserved by the above constraint. (because the vector from $P_i$ to $M_i$ is parallel to the one from $M_i$ to $P_{i+1}$)

• Along each of the Bézier curves, as well as on the initial and final segment, the acceleration of the plan $p(t)$ is constant. This means that, if the plan specifies that we arrive to $M_i$ at time $T_i$, and it takes $t_i$ time to traverse the curve that begins there, this part of the motion plan is equal to $p(t)=B_i((t-T_i)/t_i)$, where $B_i$ is standard Bézier formula for this curve. Informally, we cannot manually accelerate along the Bézier curves, all we can do is scale the speed/acceleration across an entire curve.

• The absolute value of acceleration along each dimension $d$ must be no more than $A_d$. The maximum velocity limit may also be defined for each dimension, and I'm not sure whether or not that would make the problem significantly harder.

• The initial and final velocity is zero (or possibly a constant).

Here's a picture of such a path composed of Bézier curves.

Notice that the shape of the path is defined by $N-1$ real numbers $m_i \in(0, 1)$, which define the position of the points $M_i$ as a convex combination of $P_i$ and $P_{i+1}$. The difficulty of this problem is in determining these numbers. The constraints imply that, given $m_i$, the velocities on every point of the plan are determined up to a common factor. Therefore, knowing $m_i$ of the optimal solution, it is not hard to finish the plan by finding the smallest total plan time which does not violate the acceleration constraints on any of the individual components of the plan.

Ideally, the algorithm would work incrementally - given an optimal solution for the first $n$ points, and the next point, it would fix this solution into an optimal plan for the first $n+1$ points.

Some initial work

Let's define $D_i=P_{i+1}-P_i$. Also define $V_i$ to be the velocity of the plan at point $M_i$. But since $V_i$ is parallel to $D_i$, we write $V_i=v_i D_i$, for some $v_i \in \mathbb{R}$. Let's call $v_i$ the relative speeds.

We will now use some knowledge about Bézier curves to express a relationship between the relative velocities on the ends of a single curve, resulting in an equation involving $v_i$ and $v_{i+1}$. Let's forget about our points for a moment and assume we have a quadratic Bézier curve with the control points $(A, B, C)$. If we start with the standard formula for Bézier curves, and factor by $t$, we arrive at:

$$B(t) = A + 2(B-A)t + (A-2B+C)t^2$$

The parameter in this curve is $t \in [0, 1]$, and it is easy to see that $B(0)=A$ and $B(1)=C$. We can also take the derivative of this function and compute it at the ends of the curve:

$$B'(0) = 2(B-A)$$ $$B'(1) = 2(C-B)$$

Now return to the problem of finding a relationship between $v_i$ and $v_{i+1}$. The above formula tells us how to compute the velocities at the start and end of a quadratic Bézier curve, however we have to take into account that the Bézier curves in our plan are scaled proportionally in time. So, assume the plan takes $t_i$ time to traverse the i-th Bézier curve. Then, the initial and final velocities along this curve are given by:

$$V_i = \frac{2(1-m_i)D_i}{t_i}$$ $$V_{i+1} = \frac{2m_{i+1}D_{i+1}}{t_i}$$

since $(1-m_i)D_i$ corresponds to $B-A$ above, and $m_{i+1}D_{i+1}$ corresponds to $C-B$. Then we substitute $V_i=v_i D_i$ and $V_{i+1}=v_{i+1} D_{i+1}$, which turns the vector equations into real equations. Eradicating $t_i$ gives the following:

$$v_{i+1} = \frac{m_{i+1}}{1-m_i}v_i$$

This equation captures many of the constraints of the problem, in terms of real numbers $m_i$ (the relative positions of the splice points) and $v_i$ (the relative velocities at the splice points). In fact, given all $m_i$, this effectively defines the ratios between all $v_i$. Therefore, if after choosing $m_i$ we also choose a value for $v_1$, the plan is completely defined.

Though I don't know yet how this helps with finding the optimal $m_i$, taking into account the other constraints (maximum acceleration, starting and ending velocity). While the last equation looks nice, its non-linearity in terms of $m_i$ is not encouraging.