基于抛物线过渡（梯形加减速）的空间直线插补算法与空间圆弧插补算法（Matlab）

写在前面

学习代码都记录在个人github上，欢迎关注~

Matlab机器人工具箱版本9.10

机械臂用的是五自由度的，我测试时发现逆解精度存在一些问题，目前还没找到是求解析解时出错还是编程过程有问题，还是算法本身考虑不周到，欢迎有研究过的大神们批评指正！感谢~

在这里插入图片描述

常规插补算法

假设机器人末端由P1点沿直线运动到P2点， $\left(x_{1}, y_{1}, z_{1}\right)$ 和 $\left(\alpha(_{1}, \beta_{1}, \gamma_{1}\right)$ 分别为起点P1的位置和RPY角， $\left(x_{2}, y_{2}, z_{2}\right)$ 和 $\left(\alpha_{2}, \beta_{2}, \gamma_{2}\right)$ 分别为终点P2的位置和RPY角， $\left(x_{i}, y_{i}, z_{i}\right)$ 和 $\left(\alpha_{i}, \beta_{i}, \gamma_{i}\right)$ 分别为中间插补点Pi的位置和RPY角。

各个插值点位置坐标向量和RPY角度向量可以表示为：
$\left\{\begin{array}{l}{x=x_{1}+\lambda \Delta x} \\ {y=y_{1}+\lambda \Delta y} \\ {z=z_{1}+\lambda \Delta z}\end{array}\right.（1）$

$\left\{\begin{array}{l}{\alpha=\alpha_{1}+\lambda \Delta \alpha} \\ {\beta=\beta_{1}+\lambda \Delta \beta} \\ {\gamma=\gamma_{1}+\lambda \Delta \gamma}\end{array}\right.（2）$

其中， $(x, y, z)$ 和 $(\alpha, \beta, \gamma)$ 表示插值点的位置坐标向量和RPY角度向量， $\lambda$ 为归一化因子，采用抛物线过渡的线性函数（即梯形加减速）以保证整段轨迹上的位移和速度都连续。 $(\Delta x, \Delta y, \Delta z)$ 和 $(\Delta \alpha, \Delta \beta, \Delta \gamma)$ 分别表示首末位置间的位置和RPY角度的增量，其求解为：
$\left\{\begin{array}{l}{\Delta x=x_{2}-x_{1}} \\ {\Delta y=y_{2}-y_{1}} \\ {\Delta z=z_{2}-z_{1}} \\ {\Delta \alpha=\alpha_{2}-\alpha_{1}} \\ {\Delta \beta=\beta_{2}-\beta_{1}} \\ {\Delta \gamma=\gamma_{2}-\gamma_{1}}\end{array}\right.（3）$
所以问题被转变为对归一化因子 $\lambda$ 的求解，求解方法后面会讲。下面先了解记录两种常见的插补算法：直线插补和空间圆弧插补。

直线插补算法

一般最简单的做法为，已知空间直线的起点S位置坐标为 $x_{1},y_{1}, z_{1})$ ，终点D位置坐标为 $x_{2}, y_{2}, z_{2})$ 和插补次数N，则有
$\left\{\begin{array}{l}{\Delta x=\left(x_{2}-x_{1}\right) /(N+1)} \\ {\Delta y=\left(y_{2}-y_{1}\right) /(N+1)} \\ {\Delta z=\left(z_{2}-z_{1}\right) /(N+1)}\end{array}\right.（4）$
对于该直线上任一点i(1<= i <= N)有
$\left\{\begin{array}{l}{x_{i}=x_{1}+\Delta x \cdot i} \\ {y_{i}=y_{1}+\Delta y \cdot i} \\ {z_{i}=z_{1}+\Delta z \cdot i}\end{array}\right.（5）$
这种方法很简单，对于机械臂的运动而言只需要加上RPY角的变化即可，此处不详述。

基于抛物线过渡（梯形加减速）的空间直线插补的核心在于归一化参数 $\lambda$ 的计算，同时加入速度规划的物理条件和约束。因此得到的直线插补算法需要初始化的参数有机械臂末端线速度 $v_{s}$ 、加速度 $a_{a}$ 、减速度 $a_{d}$ ，需要计算得到的插值参数有总位移S和插值点数N。

对于空间直线插补而言，总位移计算公式如下：
$S_{e}=\sqrt{\left(x_{2}-x_{1}\right)^{2}+\left(y_{2}-y_{1}\right)^{2}+\left(z_{2}-z_{1}\right)^{2}}（6）$
插值点数的计算公式如下：
$\mathrm{N}=P_{n} \frac{S_{e}}{V_{s}}（7）$
其中， $P_{n}$ 为插值参数，可以根据情况进行调整。从公式（7）可以看出，插值点数N和总位移 $S_{e}$ 成正比，与机械臂末端的线速度 $v_{s}$ 成反比。也就是说，如果总位移越大，线速度越小，则插值点数应该越多。原因在于，如果位移越大，在速度不变的情况下，一定要保证更多的插值点才能对规划曲线进行有效的拟合。同样的，如果位移不变，为了得到大的运动速度，则要降低插值点数，因为插值过程是一个等时间间隔的过程，插值点数越多，意味着所需时间越长。

空间圆弧插补算法

1.圆心和半径

在这里插入图片描述

如图所示，{O-XYZ}为机器人基座坐标系。P1、P2、P3为空间中任意不共线的三个点，其中P1 $x_{1}, y_{1}, z_{1})$ 、P2 $x_{2}, y_{2}, z_{3})$ 、P3 $x_{3}, y_{3}, z_{3})$ 为三点在基座标系中的坐标。那么这三个点可以唯一确定一个外接圆，该园所在平面 $M_{1}$ 的方程如下：
$\left|\begin{array}{ccc}{x-x_{3}} & {y-y_{3}} & {z-z_{3}} \\ {x_{1}-x_{3}} & {y_{1}-y_{3}} & {z_{1}-z_{3}} \\ {x_{2}-x_{3}} & {y_{2}-y_{3}} & {z_{2}-z_{3}}\end{array}\right|=0 （8）$
该外接圆方程的一般形式如下：
$A_{1} x+B_{1} y+C_{1} z+D_{1}=0（9）$
其中，

$A_{1} = (y_{1} - y_{3})(z_{2} - z_{3}) - (y_{2} - y_{3})(z_{1} - z_{3})$

$B_{1} = (x_{2}-x_{3})(z_{1} - z_{3}) - (x_{1} - x_{3})(z_{2} - z_{3})$

$C_{1} = (x_{1} - x_{3})(y_{2} - y_{3}) - (x_{2} - x_{3})(y_{1} - y_{3})$

$D_{1} = -(A_{1}\cdot x_{3} + B_{1} \cdot y_{3} + C_{1} \cdot z_{3})$

P1P2的垂直平分面M2的方程如下：
$A_{2} x+B_{2} y+C_{2} z+D_{2}=0（10）$
其中，

$A_{2} = x_{2} - x_{1}$

$B_{2} = y_{2} - y_{1}$

$C_{2} = z_{2} - z_{1}$

$D_{2} = \left[\left(x_{2}^{2}-x_{1}^{2}\right)+\left(y_{2}^{2}-y_{1}^{2}\right)+\left(z_{2}^{2}-z_{1}^{2}\right)\right] /2$

P2P3的垂直平分面M3的方程如下：
$A_{3} x+B_{3} y+C_{3} z+D_{3}=0（11）$
其中，

$A_{3} = x_{3} - x_{2}$

$B_{3} = y_{3} - y_{2}$

$C_{3} = z_{3} - z_{2}$

$D_{3} = -\left[\left(x_{3}^{2}-x_{2}^{2}\right)+\left(y_{3}^{2}-y_{2}^{2}\right)+\left(z_{3}^{2}-z_{2}^{2}\right)\right] /2$

综上，联立M1、M2、M3三个平面的方程，可得到如下算式：
$\left|\begin{array}{lll}{A_{1}} & {B_{1}} & {C_{1}} \\ {A_{2}} & {B_{2}} & {C_{2}} \\ {A_{3}} & {B_{3}} & {C_{3}}\end{array}\right|\left|\begin{array}{l}{x} \\ {y} \\ {z}\end{array}\right|=\left|\begin{array}{c}{-D_{1}} \\ {-D_{2}} \\ {-D_{3}}\end{array}\right|（12）$
公式（12）有且仅有唯一一组解，可解得三点外接圆圆心P0的位置坐标为 $x_{0}, y_{0}, z_{0})$ ，根据圆心坐标可求得外接圆半径为
$r=\sqrt{\left(x_{1}-x_{0}\right)^{2}+\left(y_{1}-y_{0}\right)^{2}+\left(z_{1}-z_{0}\right)^{2}}（13）$

2.坐标变换

在以圆心P0为原点的基础上建立新的坐标系{P0-X0Y0Z0}。新坐标系中的Z0为平面M1的法矢量，从上文可得Z0轴的方向余弦为：
$a_{x}=A_{1} / L, \quad a_{y}=B_{1} / L， \quad a_{z}=C_{1} / L（14）$
其中， $L=\left(A_{1}^{2}+B_{1}^{2}+C_{1}^{2}\right)^{1 / 2}$

此处，规定新坐标系{P0-X0Y0Z0}的X0为P0P1的矢量方向，故可得X0轴的方向余弦如下：
$n_{i}=\frac{\left(i_{1}-i_{0}\right)}{\sqrt{\left(x_{1}-x_{0}\right)^{2}+\left(y_{1}-y_{0}\right)^{2}+\left(z_{1}-z_{0}\right)^{2}}}（15）$
其中，i= x， y， z。

确定Z0和X0的方向后，根据右手定则即可确定Y0的方向，如下：
$\boldsymbol{Y}_{1}=\boldsymbol{X}_{1} \times \boldsymbol{Z}_{1}（16）$
在得到新坐标系三个轴的方向余弦后，根据旋转矩阵的定义，可得到新坐标系{P0-X0Y0Z0}相对于基座标系{O-XYZ}的齐次变换矩阵
$T=\left[\begin{array}{cccc}{X_{1}} & {Y_{1}} & {Z_{1}} & {P_{1}} \\ {0} & {0} & {0} & {1}\end{array}\right]（17）$
因此，可通过齐次变换矩阵T将基座标系内的空间圆弧变换到新坐标系内的平面圆弧，简化求解过程。

3.转换为平面圆弧问题

根据齐次变换矩阵，P1、P2、P3在新坐标系下的坐标Q1、Q2、Q3可通过下面公式计算得到：
$\left|Q_{i}, 1\right|^{\mathrm{T}}=T^{-1}\left|P_{i}, 1\right|^{\mathrm{T}}(i=1,2,3)（18）$
在平面内进行圆弧插补，实际上就是对圆心角进行插补，因此核心就是求出圆弧的圆心角，同时还需要注意顺逆时针的问题。

4.圆心角和插值点数N

由于atan2函数返回的是原点至点(x,y)的方位角，即与 x 轴的夹角。也可以理解为复数 x+yi 的辐角。返回值的单位为弧度。在数学坐标系中，结果为正表示从 X 轴逆时针旋转的角度，结果为负表示从 X 轴顺时针旋转的角度。此时为了判断圆弧是顺时针还是逆时针，需要将角度值转换成0~2 $\pi$ 的范围。

假设 $\angle P_{1} P_{0} P_{2}$ 为 $P_{0}P_{2}$ 与 $P_{0}X_{0}$ 正向的夹角，有
$\angle P_{1} P_{0} P_{2}=\operatorname{atan} 2\left(P_{2 y}, P_{2 x}\right)+n * 2 \pi（19）$
其中 $P_{2y}、P_{2x}$ 是P2在新坐标系下的坐标值。此时存在如下两种情况：

当 $\operatorname{atan} 2\left(P_{2 y}, P_{2 x}\right)<0$ 时， $n = 1$ ；
否则， $n = 0$ 。

同理，假设 $\angle P_{1} P_{0}P_{3}$ 为 $P_{0}P_{3}$ 与 $P_{0}X_{0}$ 正向的夹角，有
$\angle P_{1} P_{0} P_{3}=\operatorname{atan} 2\left(P_{3 y}, P_{3 x}\right)+n * 2 \pi（20）$
其中 $P_{3y}、P_{3x}$ 是P2在新坐标系下的坐标值。此时存在如下两种情况：

当 $\operatorname{atan} 2\left(P_{3 y}, P_{3 x}\right)<0$ 时， $n = 1$ ；
否则， $n = 0$ 。

通过比较 $\angle P_{1} P_{0} P_{2}$ 和 $\angle P_{1} P_{0} P_{3}$ 的大小可以确定圆弧的方向，同时可以求出圆弧圆心角 $\Omega$ ：

当 $\angle P_{1} P_{0} P_{2}<\angle P_{1} P_{0} P_{3}$ 时，圆弧为逆时针，圆心角 $\Omega=\angle P_{1} P_{0} P_{3}$ ;
当 $\angle P_{1} P_{0} P_{2} \geq \angle P_{1} P_{0} P_{3}$ 时， $\Omega=2\pi - \angle P_{1} P_{0} P_{3}$ ;

对应基于抛物线过渡（梯形加减速）的空间圆弧插补而言，需要初始化的运动参数有机械臂末端角速度 $\omega_{s}$ 和角加速度a，需要计算的插值参数为圆心角 $\Omega$ 和插值点数N。上文已经求得圆心角，插值点数计算公式如下：
$\mathrm{N}=P_{n} \frac{\Omega}{\omega_{s}}（21）$

归一化因子 $\lambda$ 的求解

预备知识

机器人学回炉重造（6）：关节空间规划方法——梯形加减速（与抛物线拟合的线性函数）、S型曲线规划

设机械臂在匀速运动时的线速度为 $v_{s}$ ，抛物线段的加减速度均为 $a$ ，可以计算出加速阶段和减速阶段的时间和位移为
$T_{1}=\frac{V_{s}}{a}（22）$

$S_{1}=\frac{1}{2} a T_{1}^{2}（23）$

设机械臂末端运动的总位移为 $S_{e}$ ，总时间为：
$T_{e}=2 T_{1}+\frac{s_{e}-2 S_{1}}{V_{s}}（24）$
将位移、速度和加速度进行归一化处理得到上述计算量的归一化参数：
$S_{1 \lambda}=\frac{s_{1}}{s_{e}}（25）$

$T_{1 \lambda}=\frac{T_{1}}{T_{e}}（26）$

$T_{2 \lambda}=1-T_{1 \lambda}（27）$

$a_{\lambda}=\frac{2 s_{1 \lambda}}{T_{1 \lambda}^{2}}（28）$

结合梯形加减速的位移公式，可以推导出归一化因子 $\lambda$ 的计算公式如下：
$\lambda=\left\{\begin{array}{cc}{\frac{1}{2} a_{\lambda} t^{2}} & {\left(0 \leq t \leq T_{1 \lambda}\right)} \\ {\frac{1}{2} a_{\lambda} T_{1 \lambda}^{2}+a_{\lambda} T_{1 \lambda}\left(t-T_{1 \lambda}\right)} & {\left(T_{1 \lambda}<t \leq T_{2 \lambda}\right)} \\ {\frac{1}{2} a_{\lambda} T_{1 \lambda}^{2}+a_{\lambda} T_{1 \lambda}\left(t-T_{1 \lambda}\right)-\frac{1}{2} a_{\lambda}\left(t-T_{2 \lambda}\right)^{2}} & {\left(T_{2 \lambda}<t \leq 1\right)}\end{array}\right.（29）$
式中，t表示时间， $t = i / N (0 < = t < = 1)$ ，其中N表示总的插值点数，i = 1、2、3、……、N-1、N。每个插值点的时间值，都有一个 $\lambda$ 与之对应，结合公式（1）（2）（3）可以得到各个插值点位置坐标向量和RPY角度向量，最后就是解逆运动学方程，得到每个插值点对应的关节变量，生成轨迹。

Matlab实现代码

预备知识：

五自由度机械臂正逆运动学算法（Ｃ语言+Matlab）

模型是我买的五自由度机械臂，之前已经推导过逆运动学的解析解，这里直接拿来用。

先是预备程序：

高斯列主元消去法，解圆心方程；
五自由度逆运动学解析解推导（前面博客有）；
归一化因子推导。

%% 高斯列主元消去法
function x = Gauss_lie(n, A, b)
for k = 1: n-1
    a_max = abs(A(k, k));
    cnt = k;
    for i = k: n
        if (abs(A(i, k)) > a_max)
            a_max = abs(A(i, k));
            cnt = i; % 确定下标i
        end
    end
    if (A(cnt, k) == 0)
        fprintf('Gauss_lie: no unique solution\n');
        return;
    end
    % 换行
    if (cnt ~= k)
        t = 0; s = 0;
        for j = k: n
            t = A(k, j);
            A(k, j) = A(cnt, j);
            A(cnt, j) = t;
            s = b(cnt);
            b(cnt) = b(k);
            b(k) = s;
        end
    end
    % step 5
    for i = k+1: n
        L(i) = A(i, k) / A(k, k);
        for j = k+1: n
            A(i, j) = A(i, j) - L(i)*A(k, j);
        end
        b(i) = b(i) - L(i)*b(k);
    end
end
for i = 1: n
    if (A(i, i) == 0)
        fprintf('Gauss_lie no unique solution\n');
        return;
    end
end
% 回代
x(n) = b(n) / A(n, n);
for i = n-1: -1: 1
    sum_a = 0;
    for j = i+1: n
        sum_a = sum_a + A(i, j)*x(j);
    end
    x(i) = (b(i) - sum_a) / A(i, i);
end

end

% 归一化处理
% 梯形加减速曲线
% 输入参数：机械臂末端运动总位移（角度）pos 
%          机械臂末端线速度（角速度）vel
%          加速度减速度accl（设定加减速段accl相同）
%          插值点数N
function lambda = Normalization(pos, vel, accl, N)
% 加减速段的时间和位移
T1 = vel / accl;
S1 = (1/2) * accl * T1^2;
% 总时间
Te = 2*T1 + (pos - 2*S1) / vel;
% 归一化处理
S1_ = S1 / pos;
T1_ = T1 / Te;
T2_ = 1 - T1_;
accl_ = 2*S1_ / T1_^2;
% lambda求解公式
for i = 0: N
    t = i / N;
    if (t >= 0 && t <= T1_)
        lambda(i+1) = (1/2) * accl_ * t^2;
    elseif (t > T1_ && t <= T2_)
        lambda(i+1) = (1/2)*accl_*T1_^2 + accl_*T1_*(t - T1_);
    elseif (t > T2_ && t <= 1)
        lambda(i+1) = (1/2)*accl_*T1_^2 + accl_*T1_*(T2_ - T1_) + (1/2)*accl_*power(t - T2_, 2);
    end
end
            
end

核心程序：

% 空间直线位置插补与RPY角姿态插补 + 梯形加减速归一化处理
% 参数：起点S位置， 终点D位置, 末端线速度vs， 加减速度a
%      起点S的RPY角、终点D的RPY角
% 返回值：插值点（不包括起点S和终点D）
function [x y z alp beta gama N] = SpaceLine(S, D, S_, D_, vs, a)
x1 = S(1); y1 = S(2); z1 = S(3);
x2 = D(1); y2 = D(2); z2 = D(3);
alp1 = S_(1); beta1 = S_(2); gama1 = S_(3);
alp2 = D_(1); beta2 = D_(2); gama2 = D_(3);
P = 1; % 插值参数，增加插值点数，避免过小
% 总位移S
s = sqrt(power(x2 - x1, 2) + power(y2 - y1, 2) + power(z2 - z1, 2))
% 插值点数N
N = ceil(P*s / vs)
% 求归一化参数
% function lambda = Normalization(pos, vel, accl, N)
lambda = Normalization(s, vs, a, N)
delta_x = x2 - x1;
delta_y = y2 - y1;
delta_z = z2 - z1;
delta_alp = alp2 - alp1;
delta_beta = beta2 - beta1;
delta_gama = gama2 - gama1;
for i = 1: N+1
    x(i) = x1 + delta_x*lambda(i);
    y(i) = y1 + delta_y*lambda(i);
    z(i) = z1 + delta_z*lambda(i);
    alp(i) = alp1 + delta_alp*lambda(i);
    beta(i) = beta1 + delta_beta*lambda(i);
    gama(i) = gama1 + delta_gama*lambda(i);
end
end

% 空间圆弧位置插补与RPY角姿态插补 + 梯形加减速归一化处理
% 参数： 起点S位置和RPY角, 中间点M位置, 终点D位置和RPY角，末端角速度，角加减速度
% 方便起见，角速度和角加速度均为角度制
function [x y z alp beta gama N] = SpaceCircle(S, M, D, S_, D_, ws, a)
x1 = S(1); x2 = M(1); x3 = D(1);
y1 = S(2); y2 = M(2); y3 = D(2);
z1 = S(3); z2 = M(3); z3 = D(3);
alp1 = S_(1); beta1 = S_(2); gama1 = S_(3);
alp2 = D_(1); beta2 = D_(2); gama2 = D_(3);

A1 = (y1 - y3)*(z2 - z3) - (y2 - y3)*(z1 - z3);
B1 = (x2 - x3)*(z1 - z3) - (x1 - x3)*(z2 - z3);
C1 = (x1 - x3)*(y2 - y3) - (x2 - x3)*(y1 - y3);
D1 = -(A1*x3 + B1*y3 + C1*z3);

A2 = x2 - x1;
B2 = y2 - y1;
C2 = z2 - z1;
D2 = -((x2^2 - x1^2) + (y2^2 - y1^2) + (z2^2 - z1^2)) / 2;

A3 = x3 - x2;
B3 = y3 - y2;
C3 = z3 - z2;
D3 = -((x3^2 - x2^2) + (y3^2 - y2^2) + (z3^2 - z2^2)) / 2;
A = [A1, B1, C1; A2, B2, C2; A3, B3, C3]
b = [-D1, -D2, -D3]'
% 圆心
C = Gauss_lie(3, A, b)
x0 = C(1); y0 = C(2); z0 = C(3);
plot3(x0, y0, z0, 'bo')
hold on
% 外接圆半径
r = sqrt(power(x1 - x0, 2) + power(y1 - y0, 2) + power(z1 - z0, 2));
% 新坐标系Z0的方向余弦
L = sqrt(A1^2 + B1^2 + C1^2);
ax = A1 / L; ay = B1 / L; az = C1 / L;
% 新坐标系X0的方向余弦
nx = (x1 - x0) / r;
ny = (y1 - y0) / r;
nz = (z1 - z0) / r;
% 新坐标系Y0的方向余弦
o = cross([ax, ay, az], [nx, ny, nz]);
ox = o(1);
oy = o(2);
oz = o(3);
% 相对于基座标系{O-XYZ}， 新坐标系{C-X0Y0Z0}的坐标变换矩阵
T = [nx ox ax x0;
     ny oy ay y0;
     nz oz az z0;
      0  0  0  1]
T_ni = T^-1
% 求在新坐标系{C-X0Y0Z0}下S、M和D的坐标
S_ = (T^-1)*[S'; 1]
M_ = (T^-1)*[M'; 1]
D_ = (T^-1)*[D'; 1]
x1_ = S_(1) , y1_ = S_(2), z1_ = S_(3)
x2_ = M_(1), y2_ = M_(2), z2_ = M_(3)
x3_ = D_(1), y3_ = D_(2), z3_ = D_(3)
% 判断圆弧是顺时针还是逆时针，并求解圆心角
if (atan2(y2_, x2_) < 0)
    angle_SOM = atan2(y2_, x2_) + 2*pi;
else
    angle_SOM = atan2(y2_, x2_);
end
if (atan2(y3_, x3_) < 0)
    angle_SOD = atan2(y3_, x3_) + 2*pi;
else
    angle_SOD = atan2(y3_, x3_);
end
% 逆时针
if (angle_SOM < angle_SOD)
    flag = 1;
    theta = angle_SOD % 圆心角
end
% 顺时针
if (angle_SOM >= angle_SOD)
    flag = -1;
    theta = 2*pi - angle_SOD % 圆心角
end
% 插补点数N
P = 2; %插补参数，增加插值点数，避免过小
ws = ws*pi / 180; % 角度换成弧度
a = a*pi / 180;
N = P*theta / ws;
% 求归一化参数
lambda = Normalization(theta, ws, a, N);

% 插补原理: 在新平面上进行插补（简化）
% 在新坐标系下z1_,z2_,z3_均为0，即外接圆在新坐标系的XOY平面内
% 此时转化为平面圆弧插补问题
delta_ang = theta;
delta_alp = alp2 - alp1
delta_beta = beta2 - beta1;
delta_gama = gama2 - gama1;
for i = 1: N+1
    x_(i) = flag * r * cos(lambda(i)*delta_ang);
    y_(i) = flag * r * sin(lambda(i)*delta_ang);
    P = T*[x_(i); y_(i); 0; 1];
    x(i) = P(1);
    y(i) = P(2);
    z(i) = P(3);
    alp(i) = alp1 + delta_alp*lambda(i);
    beta(i) = beta1 + delta_beta*lambda(i);
    gama(i) = gama1 + delta_gama*lambda(i);
end
% % figure(1)
% % plot(x_, y_)
% 插补原理： 在原圆弧上进行插补
% 圆弧上任一点处沿前进方向的切向量
% x(1) = x1; y(1) = y1; z(1) = z1;
% for i = 1: N+1
%     m(i) = flag*(ay*(z(i) - z0) - az*(y(i) - y0));
%     n(i) = flag*(az*(x(i) - x0) - ax*(z(i) - z0));
%     l(i) = flag*(ax*(y(i) - y0) - ay*(x(i) - x0));
%     delta_s = delta_ang * r;
%     E = delta_s / (r*sqrt(ax^2 + ay^2 + az^2));
%     G = r / sqrt(r^2 + delta_s^2);
%     x(i+1) = x0 + G*(x(i) + E*m(i) - x0);
%     y(i+1) = y0 + G*(y(i) + E*n(i) - y0);
%     z(i+1) = z0 + G*(z(i) + E*l(i) - z0);
% end 

end

测试主程序

% Standard DH
% five_dof robot
% 在关节4和关节5之间增加一个虚拟关节，便于逆运动学计算
clear;
clc;
th(1) = 0; d(1) = 0; a(1) = 0; alp(1) = pi/2;
th(2) = 0; d(2) = 0; a(2) = 1.04;alp(2) = 0;
th(3) = 0; d(3) = 0; a(3) = 0.96; alp(3) = 0;
th(4) = 0; d(4) = 0; a(4) = 0; alp(4) = 0;
th(5) = pi/2; d(5) = 0; a(5) = 0; alp(5) = pi/2;
th(6) = 0; d(6) = 0; a(6) = 0; alp(6) = 0;
th(7) = 0; d(7) = 1.63; a(7) = 0.28; alp(7) = 0;
% DH parameters  th     d    a    alpha  sigma
L1 = Link([th(1), d(1), a(1), alp(1), 0]);
L2 = Link([th(2), d(2), a(2), alp(2), 0]);
L3 = Link([th(3), d(3), a(3), alp(3), 0]);
L4 = Link([th(4), d(4), a(4), alp(4), 0]);
L5 = Link([th(5), d(5), a(5), alp(5), 0]); 
L6 = Link([th(6), d(6), a(6), alp(6), 0]);
L7 = Link([th(7), d(7), a(7), alp(7), 0]);
robot = SerialLink([L1, L2, L3, L4, L5, L6, L7]); 
robot.name='MyRobot-5-dof';
robot.display() 

% 起点关节角[0 90 30 60 90 0 0]*pi/180
% 终点关节角[0 90 60 60 90 0 0]*pi/180
theta_S = [0 120 100 40 90 0 0]*pi/180;
theta_M = [60 100 100 30 90 0 0]*pi/180;
theta_D = [100 120 100 40 90 0 0]*pi/180;
% robot.teach();
% robot.plot(theta_D); 
% hold on
T_S = robot.fkine(theta_S)    %起点末端执行器位姿
T_M = robot.fkine(theta_M)
T_D = robot.fkine(theta_D)
% ik_T = five_dof_ikine(T_D)
[S_RPY(1) S_RPY(2) S_RPY(3)] = RPY_angle(T_S); % 起点对应的RPY角
[D_RPY(1) D_RPY(2) D_RPY(3)] = RPY_angle(T_D); % 终点对应的RPY角
S = T_S(1: 3, 4); % 起点对应的位置坐标
M = T_M(1: 3, 4);
D = T_D(1: 3, 4); % 终点对应的位置坐标
vs = 0.06; a = 0.02; % 直线插补速度参数
ws = 5; a = 2.5; % 空间圆弧插补速度参数
% [x y z alp beta gama N] = SpaceLine(S, D, S_RPY, D_RPY, vs, a); % 直线插补，得到插值点（不包括起点和终点）
[x y z alp beta gama N] = SpaceCircle(S', M', D', S_RPY, D_RPY, ws, a);
plot3(x, y, z)
hold on
th1(1) = theta_S(1); th2(1) = theta_S(2); th3(1) = theta_S(3);
th4(1) = theta_S(4); th5(1) = theta_S(5); th6(1) = theta_S(6); th7(1) = theta_S(7);
% t = [0 0 0 0 0];
T = {1, N};
for i = 1: N+1
    R = RPY_rot(alp(i), beta(i), gama(i));
    R(1, 1:3);
    R(2, 1:3);
    R(3, 1:3);
    T{i} = [R(1, 1:3), x(i);
            R(2, 1:3), y(i);
            R(3, 1:3), z(i);
            0 0 0 1];
     theta = five_dof_ikine(T{i});
     th = theta(2, 1:5); % 简单取1行逆解
     th1(i+1) = th(1);
     th2(i+1) = th(2);
     th3(i+1) = th(3);
     th4(i+1) = th(4);
     th5(i+1) = pi/2;
     th6(i+1) = th(5);
     th7(i+1) = 0;
end
for i = 1: N+1
    T_ = robot.fkine([th1(i), th2(i), th3(i), th4(i), th5(i), th6(i), th7(i)]);
    traj = T_(1: 3, 4);
    a(i) = traj(1);
    b(i) = traj(2);
    c(i) = traj(3);
    plot3(traj(1), traj(2), traj(3), 'r*');
    hold on
    robot.plot([th1(i), th2(i), th3(i), th4(i), th5(i), th6(i), th7(i)])
end

参考文献

[1]杨淞. 一种六自由度机械臂的运动控制系统设计[D].上海交通大学,2014.

[2]陈国良,黄心汉,王敏.机械手圆周运动的轨迹规划与实现[J].华中科技大学学报(自然科学版),2005(11):69-72.

[3]卓扬娃,白晓灿,陈永明.机器人的三种规则曲线插补算法[J].装备制造技术,2009(11):27-29.

[4]林威,江五讲.工业机器人笛卡尔空间轨迹规划[J].机械工程与自动化,2014(05):141-143.

[5]Chen Z, Wang H, Lu X, et al. Kinematics analysis and application of 5-DOF manipulator with special joint[C]//2017 Chinese Automation Congress (CAC). IEEE, 2017: 7421-7426.