傅里叶级数轨迹

以傅里叶级数表示轨迹的位置，并求导得到速度和加速度的形式。

$x(t)=\sum_{k=0}^n{a_k}sin{k{\omega}t}+{b_k}cos{k{\omega}t}$
$\dot{x}(t)=k{\omega}\sum_{k=1}^n{a_k}cos{k{\omega}t}-{b_k}sin{k{\omega}t}$
$\ddot{x}(t)=k^2{\omega}^2\sum_{k=1}^n-{a_k}sin{k{\omega}t}-{b_k}cos{k{\omega}t}$

$a_k$ 和 $b_k$ 作为参数， $k=0,1,...,n$ 。其中 $a_0$ 取任意值均对轨迹无影响，而 $b_0$ 表示位置的直流分量。

边界条件

对于轨迹来说，以基频的一个周期作为轨迹的时长。起始的时间点为 $t=0$ ，结束的时间点为 $t=\frac{2\pi}{\omega}$ 。

指定起始和终止的位置 $q$ ，而起点和终点的速度和加速度均为零。

$x|_{t=0}=x|_{t=\frac{2\pi}{\omega}}=\sum_{k=0}^n{b_k}=q$
$\dot{x}|_{t=0}=\dot{x}|_{t=\frac{2\pi}{\omega}}=\sum_{k=1}^{n}k{a_k}=0$
$\ddot{x}|_{t=0}=\ddot{x}|_{t=\frac{2\pi}{\omega}}=\sum_{k=1}^{n}k^2{b_k}=0$

计算不独立的系数

存在3条边界条件，令 $a_n$ 、 $b_n$ 、 $b_0$ 不独立。指定相互独立的 $a_k$ 和 $b_k$ 组合，其中 $k=1,2,...,n-1$ 。

$a_n=-\frac{1}{n}\sum_{k=1}^{n-1}k{a_k}$
$b_n=-\frac{1}{n}\sum_{k=1}^{n-1}k^2{b_k}$
$b_0=q-\sum_{k=1}^{n}{b_k}$

python代码

def forier_trajectory(s, a, b, f, t) :

    n = len(a)
    assert(n == len(b))

    # 使得起点和终点的位置速度加速度为零的约束条件
    an = 0
    bn = 0
    b0 = s
    for i in range(n):
        k = i + 1
        an -= k * a[i]
        bn -= k * k * b[i]
        b0 -= b[i]
    a.append(an / (n + 1))
    b.append(bn / (n + 1) / (n + 1))
    b0 -= b[n]

    q = b0
    dq = 0
    ddq = 0

    for i in range(n + 1) :
        w = 2 * math.pi * (i + 1) * f
        q += a[i] * math.sin(w * t)
        q += b[i] * math.cos(w * t)
        dq += a[i] * math.cos(w * t) * w
        dq += - b[i] * math.sin(w * t) * w
        ddq += - a[i] * math.sin(w * t) * w * w
        ddq += - b[i] * math.cos(w * t) * w * w
    return q, dq, ddq

效果

取 $n=5$ ，令起点和终点的位置为零，多组随机的系数，得到如下结果。