我的毕设题目定为《基于机械臂触觉伺服的物体操控研究》，这个系列主要用于记录做毕设的过程。

轨迹规划是机器人绕不过去的话题，其目的是为了让机器人的运动更加的平滑。对于四足机器人，贝赛尔曲线的应用比较普遍。而对于机械臂，则需要根据场景来选择适合的规划方式，常用的有样条曲线和LSPB等。下面我将给出四种比较常用的轨迹规划方法的推导过错，以及相应的代码，以供读者参考。
注：所有给出的代码主要是为了展示对应算法的编程思路，虽然代码都验证过可行，但是由于还需要关联其它库，所以无法直接运行。也建议读者根据自己的要求进行重新编写。等我的毕设答辩结束后，我会将整份工程进行开源。

规划时间

因为轨迹规划所得的轨迹是关于时间t的函数，在实际应用时，为了保证机械臂运动的平滑，一般要将运动速度设为恒值，即根据过程点的距离来确定规划时间。这里给出一种思路以供参考。
$$\begin{gathered} d=\left|{}^{i+1}p{-}^{i}p\right| \\ T=d/v_{set} \end{gathered}$$
$v_{set}$：人为设定的速度值

代码实现

/**
 * @brief
 * @param p1 point 1
 * @param p2 point 1
 * @param vel motion velocity
 */
template <typename T>
T getDeltaTime(Vec18<T> p1, Vec18<T> p2, T vel) {
    T distance = 0;
    for (int i = 0; i < 3; i++) {
        distance += abs(p1(i) - p2(i));
    }

    /* set delta time refer to distance */
    return distance / vel;
}

1. 线性插值

这种方式假定机械臂以直线的形式在给定过程点之间运动，并且因为拐角的加速度值不可求，所以我们可以假定其一直做匀速运动，加速度恒为零。
$$\begin{gathered} v_{des}(t)=({ \\ }^{i+1}p-{ \\ }^{i}p)/T \\ {p}_{des}(t)={ \\ }^{i}p+v_{des}\ast t(0\le t\le T) \\ {a}_{des}(t)=0 \end{gathered}$$

代码实现

/**
 * @brief  compute Line trajectory
 * @param points  control points（including start point and end point）
 * @param resolution  the point number between two control points
 */
template <typename T>
void LineTrajectory(std::vector<Vec18<T>> points, int resolution) {
    int points_num = points.size();
    Vec18<T> point;
    Vec6<T> pDes, vDes, aDes;

    /* Set acceleration to zero */
    aDes.setZero();

    /* push back start point */
    _trajectory.push_back(points[0]);

    for (int i = 0; i < points_num - 1; i++) {
        T dt = getDeltaTime(points[i], points[i + 1]) / (resolution + 1);
        for (int j = 1; j <= resolution + 1; j++) {
            /* only consider position */
            for (int k = 0; k < 6; k++) {
                vDes(k) = (points[i + 1](k) - points[i](k)) / (resolution + 1);
                pDes(k) = points[i](k) + vDes(k) * j;
            }
            point.block(0, 0, 6, 1) = pDes;
            point.block(6, 0, 6, 1) = vDes * getDeltaTime(points[i], points[i + 1]);
            point.block(12, 0, 6, 1) = aDes;
            _trajectory.push_back(point);
            _deltaT.push_back(dt);
        }
    }
}

2. 贝赛尔曲线

贝赛尔曲线的推导可以参考我之前的文章。
贝赛尔曲线学习
位置
$$\begin{gathered} p_{des}(t)=\sum _{j=0}^n{p}_j{B}_j^n(t) \\ {B}_i^n(t)=\left(\begin{array}{c}n\\ i\end{array}\right)t^i(1-t{)}^i(0\le t\le 1) \end{gathered}$$
$p_j$：轨迹控制点的坐标
$B_i^n(t)$：贝赛尔曲线系数
注：n = 控制点总数(包含起点和终点) - 1

速度，加速度
$$\begin{gathered} v_{des}(t)=\frac{\partial p_{des}(t)}{\partial t} \\ {a}_{des}(t)=\frac{\partial v_{des}(t)}{\partial t} \end{gathered}$$

代码实现

/**
 * @brief  compute Bezier trajectory
 * @param points  control points（including start point and end point）
 * @param resolution  trajectory points number
 */
template <typename T>
void BezierTrajectory(std::vector<Vec18<T>> points, int resolution) {
    int poinets_num = points.size(); /* the number of control points */
    T dt = 1 / (T)resolution;
    Vec18<T> point;
    T pDes, vDes, aDes;

    /* push back start point */
    _trajectory.push_back(points[0]);

    /* get motion time */
    T motion_T = 0;
    for (int k = 0; k < poinets_num - 1; k++) {
        motion_T += getDeltaTime(points[k], points[k + 1]);
    }
    motion_T /= resolution;

    /* apply Bezier algorithm */
    point.setZero();
    for (int i = 1; i <= resolution; i++) {
        /* only consider position */
        for (int j = 0; j < 6; j++) {
            pDes = 0;
            vDes = 0;
            aDes = 0;
            for (int k = 0; k < poinets_num; k++) {
                pDes += CombinatorialNumber(poinets_num - 1, k) * points[k](j) * pow(dt * i, k) *
                        pow((1 - dt * i), poinets_num - 1 - k);
                vDes += CombinatorialNumber(poinets_num - 1, k) * points[k](j) *
                        FirstDerivation(dt * i, poinets_num - 1, k);
                aDes += CombinatorialNumber(poinets_num - 1, k) * points[k](j) *
                        SecondDerivation(dt * i, poinets_num - 1, k);
            }
            point(j) = pDes;
            point(j + 6) = vDes;
            point(j + 12) = aDes;
        }

        /* Add to trajectory */
        _trajectory.push_back(point);
        _deltaT.push_back(motion_T);
    }
}

/**
 * @brief return the first derivation value of t^i((1-t)^(n-i))
 *
 */
template <typename T>
T FirstDerivation(T t, T n, T i) {
    T value = 0;
    if ((i - 1) >= 0)
        value += i * pow(t, i - 1) * pow(1 - t, n - i);
    if ((n - i - 1) >= 0)
        value += -(n - i) * pow(t, i) * pow(1 - t, n - i - 1);
    return value;
}

/**
 * @brief return the second derivation value of t^i((1-t)^(n-i))
 *
 */
template <typename T>
T SecondDerivation(T t, T n, T i) {
    T value = 0;
    if ((i - 2) >= 0 && (n - i) >= 0)
        value += i * (i - 1) * pow(t, i - 2) * pow(1 - t, n - i);
    if ((i - 1) >= 0 && (n - i - 1) >= 0)
        value += -2 * i * (n - i) * pow(t, i - 1) * pow(1 - t, n - i - 1);
    if (i >= 0 && (n - i - 2) >= 0)
        value += (n - i) * (n - i - 1) * pow(t, i) * pow(1 - t, n - i - 2);
    return value;
}

/* compute Statistics combination number */
int CombinatorialNumber(int n, int i)
{
    int out;
    out = factorial(n);
    out /= factorial(i) * factorial(n-i);
}

3. LSPB曲线(抛物线-直线-抛物线)

LSPB是机械臂最常用的轨迹规划算法之一，但要注意的是，LSPB得到的轨迹不经过中间点，
并且过渡区间的加速度需要在符合要求的区间内自行定义。一般来说，加速度的取值要足够大，以保证各路径段有足够长的直线区段。

只有起始点和终点的情况

参数定义
$$\begin{gathered} t_h:时间中点 \\ {\theta }_h:位置中点 \\ {\theta }_b:过渡区段终点位置 \\ \ddot{\theta }:过渡区段加速度 \end{gathered}$$
过渡区终点速度等于直线部分速度
$$\ddot{\theta }{t}_b=\frac{{\theta }_h-{\theta }_b}{t_h-t_b}$$
运动学公式

$${\theta }_b={\theta }_0+\frac{1}{2}\ddot{\theta }{t}_b^2$$
联立得
$$t_b=\frac{t_m}{2}-\frac{\sqrt{{\ddot{\theta }}^2{t}_m^2-4\ddot{\theta }({\theta }_f-{\theta }_0)}}{2\ddot{\theta }}$$
加速度要满足的条件
$$\begin{gathered} \ddot{\theta }\ge \frac{4\ddot{\theta }({\theta }_f-{\theta }_0)}{t_m^2} \\ {t}_m=t_f-t_0 \end{gathered}$$
根据运动学公式，即可得到各个分段中的轨迹位置，速度及加速度（这里给出位置的公式，速度和加速度简单求导即可）
$$p_{des}=\{\begin{array}{l}{\theta }_0+\frac{1}{2}\ddot{\theta }{t}^2,t\le t_b\\ {\theta }_b+(\ddot{\theta }{t}_b)t,t_b\le t\le t_f-t_b\\ {\theta }_f-{\theta }_b+(\ddot{\theta }{t}_b)t-\frac{1}{2}\ddot{\theta }{t}^2,t_f-t_b\le t\le t_f\end{array}$$

带有中间点的情况

对于内部路径点
$$\begin{gathered} \dot{{\theta }_{jk}}=\frac{{\theta }_k-{\theta }_j}{t_{djk}} \\ \ddot{{\theta }_k}=SGN(\dot{{\theta }_{kl}}-\dot{{\theta }_{jk}})\left|\ddot{{\theta }_k}\right| \\ {t}_k=\frac{\dot{{\theta }_{kl}}-\dot{{\theta }_{jk}}}{\ddot{{\theta }_k}} \\ {t}_{jk}=t_{djk}-\frac{1}{2}{t}_j-\frac{1}{2}{t}_k \end{gathered}$$
对于第一个路径段，令线性区段速度的两个表达式相等
$$\frac{{\theta }_2-{\theta }_1}{t_{d12}-\frac{1}{2}{t}_1}=\ddot{{\theta }_1}{t}_1$$
由此即可解出其余量
$$\begin{gathered} \ddot{{\theta }_1}=SGN({\theta }_2-{\theta }_1)\left|\ddot{{\theta }_1}\right| \\ \ddot{{\theta }_k}=t_{d12}-\sqrt{t_{d12}^2-\frac{2({\theta }_2-{\theta }_1)}{\ddot{{\theta }_1}}} \\ \dot{{\theta }_{12}}=\frac{{\theta }_2-{\theta }_1}{t_{d12}-\frac{1}{2}{t}_1} \\ {t}_{12}=t_{d12}-t_1-\frac{1}{2}{t}_2 \end{gathered}$$
对于最后一个路径段，与一个路径点处理类似
$$\frac{{\theta }_{n-1}-{\theta }_n}{t_{d(n-1)n}-\frac{1}{2}{t}_n}=\ddot{{\theta }_n}{t}_n$$
由此即可解出其余量
$$\begin{gathered} \ddot{{\theta }_n}=SGN({\theta }_{n-1}-{\theta }_n)\left|\ddot{{\theta }_n}\right| \\ \ddot{{\theta }_n}=t_{d(n-1)n}-\sqrt{t_{d(n-1)n}^2+\frac{2({\theta }_n-{\theta }_{n-1})}{\ddot{{\theta }_n}}} \\ \dot{{\theta }_{(n-1)n}}=\frac{{\theta }_n-{\theta }_{n-1}}{t_{d(n-1)n}-\frac{1}{2}{t}_n} \\ {t}_{(n-1)n}=t_{d(n-1)n}-t_n-\frac{1}{2}{t}_{n-1} \end{gathered}$$
加速度要满足的条件
$$\begin{gathered} t_{d12}^2-\frac{2({\theta }_2-{\theta }_1)}{\ddot{{\theta }_1}}\ge 0 \\ {t}_{d(n-1)n}^2+\frac{2({\theta }_n-{\theta }_{n-1})}{\ddot{{\theta }_n}}\ge 0 \end{gathered}$$

代码实现

/**
 * @brief  compute LSPB trajectory
 * @param points  control points（including start point and end point）
 * @param resolution  the point number between two control points
 */
template <typename T>
void pathPlanner<T>::LSPBTrajectory(std::vector<Vec18<T>> points, int resolution) {
    int len = points.size();

    std::vector<Vec18<T>> trajectory(resolution * (len - 1) + len);
    ;

    /* push back start point */
    trajectory[0] = points[0];

    std::vector<T> acc(len);
    std::vector<T> deltaT(len - 1);

    /* define acceleration and deltaT to constant value */
    for (int i = 0; i < len; i++) {
        acc[i] = 150.;
        if (i != len - 1)
            deltaT[i] = getDeltaTime(points[i], points[i + 1]);
    }

    std::vector<T> Vd(len - 1); /* line velocity between two points */
    std::vector<T> Td(len - 1); /* line motion time between two points */
    std::vector<T> Ti(len);     /* nearby time of a point */

    T dp, a_;
    T pDes, vDes, aDes;
    for (int j = 0; j < 6; j++) {
        if (len == 1)
            return;

        /* no middle points */
        else if (len == 2) {
            dp = points[1](j) - points[0](j);
            a_ = sign(dp) * abs(acc[0]);
            assert(abs(a_) >= 4 * abs(dp / pow(deltaT[0], 2)));
            Ti[0] = 0.5 * deltaT[0] - sqrt((pow(a_ * deltaT[0], 2) - 4 * a_ * dp)) / (2 * a_);
            Ti[1] = Ti[0];
            Vd[0] = a_ * Ti[0];
            Td[0] = deltaT[0] - 2 * Ti[0];
        }

        /* have middle points */
        else {
            /* compute start time and velocity */
            dp = points[1](j) - points[0](j);
            a_ = sign(dp) * abs(acc[0]);
            assert((pow(deltaT[0], 2) - 2. * dp / a_) >= 0);
            Ti[0] = deltaT[0] - sqrt(pow(deltaT[0], 2) - 2. * dp / a_);
            Vd[0] = dp / (deltaT[0] - 0.5 * Ti[0]);

            /* compute velocity of middle points */
            for (int i = 1; i < len - 2; i++) {
                dp = points[i + 1](j) - points[i](j);
                Vd[i] = dp / deltaT[i];
            }

            //* compute end time and velocity */
            dp = points[len - 1](j) - points[len - 2](j);
            a_ = sign(-dp) * abs(acc[len - 1]);
            assert((pow(deltaT[len - 2], 2) + 2. * dp / a_) >= 0);
            Ti[len - 1] = deltaT[len - 2] - sqrt(pow(deltaT[len - 2], 2) + 2. * dp / a_);
            Vd[len - 2] = dp / (deltaT[len - 2] - 0.5 * Ti[len - 1]);

            /* compute time of middle points */
            for (int i = 1; i < len - 1; i++) {
                a_ = sign(Vd[i] - Vd[i - 1]) * abs(acc[i]);
                Ti[i] = (Vd[i] - Vd[i - 1]) / a_;
            }

            /* compute delta time of middle points */
            for (int i = 1; i < len - 2; i++) {
                Td[i] = deltaT[i] - 0.5 * Ti[i] - 0.5 * Ti[i + 1];
            }

            /* supplemental data */
            Td[len - 2] = deltaT[len - 2] - Ti[len - 1] - 0.5 * Ti[len - 2];
            Td[0] = deltaT[0] - Ti[0] - 0.5 * Ti[1];
        }

        T dt, t_;
        for (int i = 0; i < len - 1; i++) {
            /* delta time between two points */
            dt = deltaT[i] / (T)(resolution + 1);

            /* start and end point */
            double start = points[i](j);
            double end = points[i + 1](j);

            for (int k = 1; k <= resolution + 1; k++) {
                /* define value refer to time range */
                t_ = k * dt;

                /* judge start or end point */
                T Tb1 = (i == 0) ? Ti[i] : Ti[i] / 2.;
                T Tm = Tb1 + Td[i];
                T Tb2 = Tm + (i == len - 2) ? Ti[i + 1] : Ti[i + 1] / 2.;

                if (t_ <= Tb1) {
                    pDes = start + 0.5 * acc[i] * pow(t_, 2);
                    vDes = acc[i] * t_;
                    aDes = acc[i];
                } else if (t_ > Tb1 && t_ < Tm) {
                    pDes = start + 0.5 * acc[i] * pow(Ti[i], 2) + Vd[i] * (t_ - Tb1);
                    vDes = Vd[i];
                    aDes = 0;
                } else {
                    pDes = start + 0.5 * acc[i] * pow(Ti[i], 2) + Vd[i] * Td[i] + Vd[i] * (t_ - Tm) -
                           0.5 * acc[i] * pow((t_ - Tm), 2);
                    vDes = -acc[i] * (t_ - Tm);
                    aDes = -acc[i];
                }

                trajectory[i * (resolution + 1) + k](j) = pDes;
                trajectory[i * (resolution + 1) + k](j + 6) = vDes;
                trajectory[i * (resolution + 1) + k](j + 12) = aDes;

                /* set delta time */
                if (!j) {
                    _deltaT.push_back(dt);
                }
            }
        }
    }

    /* Add to trajectory */
    for (int i = 0; i < resolution * (len - 1) + len; i++) {
        _trajectory.push_back(trajectory[i]);
    }
}

4. 三次样条法

根据过程点列写方程，并写成矩阵形式。

三次样条表示形式

$$\begin{gathered} \theta (t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3 \\ \dot{\theta }(t)=a_1+2a_{2}t+3a_3{t}^2 \\ \ddot{\theta }(t)=2a_2+6a_{3}t \end{gathered}$$

根据过程点列写方程

在一个时间段内，每个三次曲线的起始时刻$t=0$。终止时刻$t=t_{fi}(1\le i\le n)$，过程点为${\theta }_i$

要求解的参数
$$\begin{gathered} X={\left[\begin{array}{ccccccccc}{a}_{10}& a_{11}& a_{12}& a_{13}& \dots & a_{n0}& a_{n1}& a_{n2}& a_{n3}\end{array}\right]}^T \\ n：过程点个数-1 \end{gathered}$$
初末速度为0
$$\begin{gathered} 0=a_{11} \\ 0=a_{n1}+2a_{n2}{t}_{fn}+3a_{n3}{t}_{fn}^2 \end{gathered}$$
位置满足设定值
$$\begin{gathered} {\theta }_i=a_{i0} \\ {\theta }_{i+1}=a_{i0}+a_{i1}{t}_{fi}+a_{i2}{t}_{fi}^2+a_{i3}{t}_{fi}^3 \end{gathered}$$

中间点速度和加速度一致
$$\begin{gathered} a_{i1}+2a_{i2}{t}_{fi}+3a_{i3}{t}_{fi}^2=a_{(i+1)1} \\ 2a_{i2}+6a_{i3}{t}_{fi}=2a_{(i+1)2} \end{gathered}$$
联立即可求解。

代码实现

/**
 * @brief  compute Spline trajectory
 * @param points  control points（including start point and end point）
 * @param resolution  the point number between two control points
 */
template <typename T>
void SplineTrajectory(std::vector<Vec18<T>> points, int resolution) {
    T pDes, vDes, aDes, a_;
    int len = points.size();

    /* Set start point */
    std::vector<Vec18<T>> trajectory(resolution * (len - 1) + len);
    trajectory[0] = points[0];

    /* set delta time */
    std::vector<T> dT(len - 1);
    for (int i = 0; i < len - 1; i++) {
        //    dT[i] = 1;
        dT[i] = getDeltaTime(points[i], points[i + 1]);
    }

    const int M_SIZE = 4 * (len - 1);
    /* deine variable matrix */
    Eigen::MatrixXd A;
    A.setZero(M_SIZE, M_SIZE);
    Eigen::VectorXd X(M_SIZE);
    Eigen::VectorXd b(M_SIZE);
    Eigen::VectorXd solve(M_SIZE);

    for (int j = 0; j < 6; j++) {
        int index = 0;

        /* Set end velocity to zero */
        A(index, 1) = 1;
        b(index) = 0;
        index++;

        A(index, 4 * (len - 2) + 1) = 1;
        A(index, 4 * (len - 2) + 2) = 2 * dT[len - 2];
        A(index, 4 * (len - 2) + 3) = 3 * pow(dT[len - 2], 2);
        b(index) = 0;
        index++;
        for (int i = 0; i < len - 1; i++) {
            /* position */
            A(index, i * 4 + 0) = 1;
            b(index) = points[i](j);
            index++;
            A(index, i * 4 + 0) = 1;
            A(index, i * 4 + 1) = dT[i];
            A(index, i * 4 + 2) = pow(dT[i], 2);
            A(index, i * 4 + 3) = pow(dT[i], 3);
            b(index) = points[i + 1](j);
            index++;

            /* velocity */
            if (i != 0) {
                A(index, (i - 1) * 4 + 1) = 1;
                A(index, (i - 1) * 4 + 2) = 2 * dT[i - 1];
                A(index, (i - 1) * 4 + 3) = 3 * pow(dT[i - 1], 2);
                A(index, i * 4 + 1) = -1;
                b(index) = 0;
                index++;
            }
            /* acceleration */
            if (i != 0) {
                A(index, (i - 1) * 4 + 2) = 2;
                A(index, (i - 1) * 4 + 3) = 6 * dT[i - 1];
                A(index, i * 4 + 2) = -2;
                b(index) = 0;
                index++;
            }
        }

        /* solve function */
        X = A.colPivHouseholderQr().solve(b);

        double dt, t_;
        for (int i = 0; i < len - 1; i++) {
            /* delta time between two points */
            dt = dT[i] / (resolution + 1);

            for (int k = 1; k <= resolution + 1; k++) {
                /* substitute into cubic polynomial */
                t_ = k * dt;
                pDes = X(i * 4) + X(i * 4 + 1) * t_ + X(i * 4 + 2) * pow(t_, 2) + X(i * 4 + 3) * pow(t_, 3);
                vDes = X(i * 4) + X(i * 4 + 1) + 2 * X(i * 4 + 2) * t_ + 3 * X(i * 4 + 3) * pow(t_, 2);
                aDes = 2 * X(i * 4 + 2) + 6 * X(i * 4 + 3) * t_;
                trajectory[i * (resolution + 1) + k](j) = pDes;
                trajectory[i * (resolution + 1) + k](j + 6) = vDes;
                trajectory[i * (resolution + 1) + k](j + 12) = aDes;

                /* set delta time */
                if (!j) {
                    _deltaT.push_back(dt);
                }
            }
        }
    }

    /* Add to trajectory */
    for (int i = 0; i < resolution * (len - 1) + len; i++) {
        _trajectory.push_back(trajectory[i]);
    }
}