智能车圆弧轨迹规划

圆弧轨迹速度梯形轨迹规划

1. 点到点轨迹规划简介

点到点轨迹规划是让机器人（如机械臂、智能车）在规定时间$T$内完成一段轨迹（如直线、曲线）。轨迹规划跟路径规划概念不同，轨迹规划是对时间$T$做规划，也映射到了速度、加速度、加加速度等。路径规划只是规划一条路径，起点到目标点，对时间不做要求，但是对路径的平滑度、距离最短、能量最少、甚至是时间最优（以某个速度完成这段路径）。他们最大的差异在于是否对时间$T$做规划

PTP（Point to Point,点到点）轨迹规划是已知$起点{\theta }_{start}运动到终点{\theta }_{end}$ ，在时间$T$内完成运动。 这里会引入中间变量$s(t)$，它是时间的函数，定义域为$[0,T]$，值域为$[0,1]$。当$s=0,\theta ={\theta }_{start}。当s=1,\theta ={\theta }_{end}$。
因此角度值$\theta 在规定时间T内从{\theta }_{start}到{\theta }_{end}完成运动，\theta 关于s的函数如下$

$$\begin{array}{rl}\theta (s)& ={\theta }_{start}+({\theta }_{end}-{\theta }_{start})s,s\in \left[0,1\right]\\ s& =s(t),t\in \left[0,T\right]\end{array}\text{\hspace{1em}(1)}$$
$将\theta 对t进行求偏导得到速度$：
$$\begin{array}{rl}\frac{\partial \theta }{\partial t}& =\frac{\partial \theta }{\partial s}\ast \frac{\partial s}{\partial t}\\ \because \frac{\partial \theta }{\partial s}& ={\theta }_{end}-{\theta }_{start}\\ \therefore \frac{\partial \theta }{\partial t}& =({\theta }_{end}-{\theta }_{start})\frac{\partial s}{\partial t}\end{array}\text{\hspace{1em}(2)}$$

$将\theta 对t进行求二阶偏导得到加速度$：
$$\begin{array}{r}\frac{{\partial }^2\theta }{\partial t^2}=({\theta }_{end}-{\theta }_{start})\frac{{\partial }^{2}s}{\partial t^2}\end{array}\text{\hspace{1em}(3)}$$
因为${\theta }_{start}和{\theta }_{end}$是已知的，所以速度和加速度随时间的变化取决于$s=s(t),t\in [0,T]$。一般会有三次多项式、五次多项式、梯形曲线、S型曲线做规划。具体可以参考下面链接点到点轨迹规划。

2. 圆弧轨迹梯形规划

2.1 速度梯形规划

梯形规划是指 $\dot{s}$的曲线是梯形。如图所示
s为分段函数,即：
$$s(t)=\{\begin{array}{ll}\frac{1}{2}a{t}^2& 0\le t\le \frac{v}{a}\\ vt-\frac{v^2}{2a}& \frac{v}{a}<t\le T-\frac{v}{a}\\ \frac{2avT-2v^2-a^2(t-T{)}^2}{2a}& T-\frac{v}{a}<t\le T\end{array}\text{\hspace{1em}(4.1)}$$
$\dot{s}$为：
$$\dot{s}(t)=\{\begin{array}{ll}at& 0\le t\le \frac{v}{a}\\ v& \frac{v}{a}<t\le T-\frac{v}{a}\\ -a(t-T)& T-\frac{v}{a}<t\le T\end{array}\text{\hspace{1em}(5.1)}$$
这里的$v,a,T$均有约束条件：
$s(T)=\frac{avT-v^2}{a}=1$，且加速段 $t_a=\frac{v}{a}\le \frac{T}{2},a\ge 0$

• 指定$v,a$，$T=\frac{a+v^2}{av}$，且需要满足$\frac{v^2}{a}\le 1$
• 指定$v,T$，$a=\frac{v^2}{vT-1}$，且需要满足$1<vT\le 2$
• 指定$a,T$，$v=\frac{1}{2}(aT-\sqrt{a}\sqrt{a{T}^2-4})$，且需要满足$a{T}^2\ge 4$

若起始速度$v_0$和终点速度$v_e$不为0,则梯形规划分段函数为：
$$s(t)=\{\begin{array}{ll}{v}_{0}t+\frac{1}{2}a{t}^2& 0\le t\le \frac{v-v_0}{a}\\ vt-\frac{(v-v_0{)}^2}{2a}& \frac{v-v_0}{a}<t\le T-\frac{v-v_e}{a}\\ \frac{2avT-(v-v_0{)}^2-(v-v_e{)}^2-a^2(t-T{)}^2+2a{v}_e(t-T)}{2a}& T-\frac{v-v_e}{a}<t\le T\end{array}\text{\hspace{1em}(4.2)}$$
$\dot{s}$为：
$$\dot{s}(t)=\{\begin{array}{ll}{v}_0+at& 0\le t\le \frac{v-v_0}{a}\\ v& \frac{v-v_0}{a}<t\le T-\frac{v-v_e}{a}\\ v_e-a(t-T)& T-\frac{v-v_e}{a}<t\le T\end{array}\text{\hspace{1em}(5.2)}$$
这里的$v,a,T$均有约束条件：
$s(T)=\frac{2avT-(v-v_0{)}^2-(v-v_e{)}^2}{2a}=1$，且加速段和减速度段 $t_a+t_b=\frac{2v-v_0-v_e}{a}\le T,a\ge 0$

• 指定$v,a$，$T=\frac{2a+(v-v_0{)}^2+(v-v_e{)}^2}{2av}$，且需要满足$\frac{2v^2-v_0^2-v_e^2}{2a}\le 1$
• 指定$v,T$，$a=\frac{(v-v_0{)}^2+(v-v_e{)}^2}{2(vT-1)}$，且需要满足$1<vT\le \frac{(v_0^2+v_e^2)T-2(v_0+v_e)+4v}{2v}$
• 指定$a,T$，$v=\frac{1}{2}(v_0+v_e+aT-\sqrt{(aT+v_0+v_e{)}^2-2v_0^2-2v_e^2-4a})$，且需要满足$(aT+v_0+v_e{)}^2-2v_0^2-2v_e^2-4a)\ge 0$

2.2 圆弧轨迹规划

以需要让小车转360°一个圈为例，让小车以特定的最大角速度$\dot{\theta }$绕半径$R$旋转360°，进行轨迹规划。
已知起始角度为$0°$,目标角度为$360°$。
因此可以得到每个时刻转动角速度$\dot{\theta }$的值为：
$$\begin{gathered} \dot{\theta }=360\dot{s} \\ \dot{s}是关于速度梯形规划\text{\hspace{1em}(6)} \end{gathered}$$
因为限制了最大角速度$\dot{\theta }$，因此，在$\dot{s}$梯形规划中，$v=\dot{s}max=\frac{\dot{\theta }}{360}$ (注意这里的$\dot{\theta }单位为deg/s$)，设$a=v^2+1$, 在已知$a,v$可以得到规划周期$T$.
通过公式(5)对$\dot{s}$进行梯形速度规划，相当于对$\dot{\theta }$进行梯形速度规划，从公式(6)可以得出该结论。
从跟随圆弧路径原理这一篇文章我们得知$\dot{\theta }与\dot{x},\dot{y}$的关系如下：
$$\begin{array}{rl}\dot{x}& =-Rsin\theta \dot{\theta }\\ \dot{y}& =Rcos\theta \dot{\theta }\end{array}\text{\hspace{1em}(7)}$$
最后通过公式(5)(6)(7)，我们得到在时间$t$下规划出来的瞬时速度并将速度下发给智能车让小车进行圆弧轨迹规划。

3. 圆弧轨迹规划进阶

如何让小车按着圆弧切线方向进行轨迹规划或者是朝着车头刷锅进行轨迹规划。甚至是让小车进行圆弧轨迹规划的同时，小车可以圆弧运动，也能到达期望的角度（车与世界坐标系的夹角$\alpha$）小车可以边自旋，边绕圆，像陀螺运动。

3.1 车头绕圆弧切线和刷锅运动

从跟随圆弧路径原理这一篇文章我们得知：
$$\begin{array}{rl}{\dot{x}}_r& =-Rsin(\theta -\alpha )\dot{\theta }\\ {\dot{y}}_r& =Rcos(\theta -\alpha )\dot{\theta }\\ {\dot{\theta }}_r& =\dot{\theta }\end{array}\text{\hspace{1em}(8)}$$
$\theta 是圆弧角，\alpha 是机器人坐标系与圆弧世界坐标系的夹角。$见下图：
当智能车切线圆弧运动，$\theta =\alpha$，公式(8)变成：
$$\begin{array}{rl}{\dot{y}}_r& =R\dot{\theta }\\ {\dot{\theta }}_r& =\dot{\theta }\end{array}\text{\hspace{1em}(9)}$$
通过上述章节2.2，对$\dot{\theta }$进行轨迹规划即可。 刷锅运动轨迹规划原理一样，$\alpha -\theta =\frac{\pi }{2}$。

3.2 小车圆弧运动且能达到期望角度

由于小车的角速度和线速度是非耦合的关系，当小车在圆弧运动时，期望绕一圈后，小车已经转了特定角度。 例如，小车圆弧运动360°的同时，小车的方向与原来的方向已经旋转了$\beta$ ,如90°。
因为对圆弧进行了轨迹规划，已知在$T$时间内，小车会走完圆弧轨迹，因此小车旋转$\beta$也对应在该规划的$T$时间内。
$$\begin{array}{rl}\because \dot{\theta }& =({\theta }_{end}-{\theta }_{start})\dot{s}\\ \therefore \dot{\beta }& =\beta \dot{s}\end{array}\text{\hspace{1em}(10)}$$
$\dot{s}$ 属于梯形规划，得到运动周期$T$。通过公式(10)，得到了关于自转角度$\beta$的规划。因此关于$({\dot{x}}_r,{\dot{y}}_r,{\dot{\theta }}_r)$解算公式如下：
$$\begin{array}{rl}{\dot{x}}_r& =-Rsin(\theta -\alpha -\gamma )\dot{\theta }\\ {\dot{y}}_r& =Rcos(\theta -\alpha -\gamma )\dot{\theta }\\ {\dot{\theta }}_r& =\dot{\theta }+\dot{\beta }\\ \gamma & ={\gamma }_{last}+{\dot{\theta }}_{r}t\end{array}\text{\hspace{1em}(11)}$$
通过下发$({\dot{x}}_r,{\dot{y}}_r,{\dot{\theta }}_r)$三组速度即可达到期望的运动状态。

3.3 小车圆弧运动且像陀螺旋转

跟上一个小节3.2一样，小车陀螺旋转就是多了一个固定自转角速度$\dot{\beta }$，当规划开始与结束的时候$\dot{\beta }=0$，期间速度恒定，就能让小车边走圆弧边陀螺运动，解算公式与公式(11)一样，但是$\dot{\beta }$是没有经过轨迹规划的。而小车走圆弧是经过轨迹规划的。