移动机器人运动规划–基于硬约束和贝塞尔曲线

移动机器人运动规划–基于硬约束和贝塞尔曲线

前段时间学了软约束和硬约束下的轨迹优化，对贝塞尔曲线有了一定的认识，根据Online Safe Trajectory Generation For Quadrotors Using Fast Marching Method and Bernstein Basis Polynomial一文做的笔记记录如下。

---

---

一、贝塞尔曲线的特性

贝塞尔曲线较早被应用于计算机图形学，2010左右在机器人导航方面有广泛的应用。
它是由若干个控制点按一定权重组合而成的曲线。其表达式为
$B(t)=c_0{b}_n^0(t)+c_1{b}_n^1(t)+...+c_n{b}_n^n(t)$
其主要的特点在于
1）曲线始终通过第一个控制点和最后一个控制点
2）曲线始终被约束在控制点组成的凸包（多边形）内，convex hull性质
3）曲线的各阶导数，仍是一个贝塞尔曲线，可被原来的控制点线性表示，hodograph性质
4）曲线上的点参数化的被时间$t\in [0,1]$表示

二、基于贝塞尔曲线的轨迹优化

贝塞尔曲线要求时间$t\in [0,1]$，而我们分配某段的时间不会设置定值1，往往会根据这一段所经历的距离除以速度得到一个大概的期望时间T。即T不等于1，怎么办？
参考文献给出的办法是将时间缩放到1，这个只会对Q矩阵产生影响。用一个尺度因子将$t$缩放到任意时间。从起点到终点的每段轨迹可以表示为
f μ ( t ) = { s 1 ⋅ ∑ i = 0 n c μ 1 i b n i ( t s 1 ) , t ∈ [ 0 , T 1 ] s 2 ⋅ ∑ i = 0 n c μ 2 i b n i ( t s 2 ) , t ∈ [ 0 , T 2 ] ⋮ s n ⋅ ∑ i = 0 n c μ n i b n i ( t s n ) , t ∈ [ 0 , T n ] （ 2 ） f_\mu(t)= \begin{cases} s_1 \cdot \sum_{i=0}^n c_{\mu1}^i b_n^i (\frac{t}{s_1}), & t\in[0,T_1] \\ s_2 \cdot \sum_{i=0}^n c_{\mu2}^i b_n^i (\frac{t}{s_2}), & t\in[0,T_2] \\ \vdots\\ s_n \cdot \sum_{i=0}^n c_{\mu n}^i b_n^i (\frac{t}{s_n}),& t\in[0,T_n] \\ \end{cases} （2） fμ​(t)=⎩ ⎨ ⎧​s1​⋅∑i=0n​cμ1i​bni​(s1​t​),s2​⋅∑i=0n​cμ2i​bni​(s2​t​),⋮sn​⋅∑