写在前面

学习代码都记录在个人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角度向量可以表示为：
$$\{\begin{array}{l}x=x_1+\lambda \Delta x\\ y=y_1+\lambda \Delta y\\ z=z_1+\lambda \Delta z\end{array}（1）$$

$$\{\begin{array}{l}\alpha ={\alpha }_1+\lambda \Delta \alpha \\ \beta ={\beta }_1+\lambda \Delta \beta \\ \gamma ={\gamma }_1+\lambda \Delta \gamma \end{array}（2）$$

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

直线插补算法

一般最简单的做法为，已知空间直线的起点S位置坐标为$(x_1,y_1,z_1)$，终点D位置坐标为$(x_2,y_2,z_2)$和插补次数N，则有
$$\{\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}（4）$$
对于该直线上任一点i(1<= i <= N)有
$$\{\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}（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,a_y=B_1/L，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的方向，如下：
$${\mathbf{Y}}_1={\mathbf{X}}_1\times {\mathbf{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=\mathrm{atan}2\left(P_{2y},P_{2x}\right)+n\ast 2\pi （19）$$
其中$P_{2y}、P_{2x}$是P2在新坐标系下的坐标值。此时存在如下两种情况：

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

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

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

通过比较$\angle P_1{P}_0{P}_2$和$\angle P_1{P}_0{P}_3$的大小可以确定圆弧的方向，同时可以求出圆弧圆心角$\Omega$：

1. 当$\angle P_1{P}_0{P}_2<\angle P_1{P}_0{P}_3$时，圆弧为逆时针，圆心角$\Omega =\angle P_1{P}_0{P}_3$;
2. 当$\angle P_1{P}_0{P}_2\ge \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=2T_1+\frac{s_e-2S_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{2s_{1\lambda }}{T_{1\lambda }^2}（28）$$

结合梯形加减速的位移公式，可以推导出归一化因子$\lambda$的计算公式如下：
$$\lambda =\{\begin{array}{cc}\frac{1}{2}{a}_{\lambda }{t}^2& \left(0\le t\le 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\le 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\le 1\right)\end{array}（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.

写在前面

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

Matlab机器人工具箱版本9.10

在前面的博文中：

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

基于单位四元数的姿态插补（Matlab）

我使用了抛物线过渡（也就是梯形加减速）作为空间插补算法的速度规划方法，但是梯形曲线的缺点也很明显，即加速度不连续，速度过渡不平滑，容易造成机械系统冲击。下面我尝试了S型加减速曲线的规划方法并结合到空间插补算法中，仿真效果还可以，加速度曲线连续，更柔和，适用于精度速度要求更高的场合。

空间直线插补：

空间圆弧插补：

直观插补：

带约束S型速度规划

预备知识：

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

参照基于抛物线过渡（梯形加减速）的空间直线插补算法与空间圆弧插补算法（Matlab）中归一化参数思想，将归一化参数计算函数中的梯形加减速方法换成S型加减速法。此时，定义归一化时间算子：
$$\lambda (t)=\frac{s(t)-s(0)}{L}$$
其中，$t\in [0,T]$,$s(t)$是S型曲线中位移在全局时间坐标下的函数，$L$是机器人执行器末端经过的距离。

空间直线插补与空间圆弧插补

位置插补与姿态插补需要分开讨论。

位置插补

假设机器人末端由P1点沿直线运动到P2点，$\left(x_1,y_1,z_1\right)$分别为起点P1的位置，$\left(x_2,y_2,z_2\right)$为终点P2的位置，$\left(x_i,y_i,z_i\right)$为中间插补点Pi的位置。

各个插值点位置坐标向量为：
$$\{\begin{array}{l}x=x_1+\lambda \Delta x\\ y=y_1+\lambda \Delta y\\ z=z_1+\lambda \Delta z\end{array}$$
其中，$\lambda$是归一化时间因子，$(\Delta x,\Delta y,\Delta z)$表示首末位置间的位置增量，其解为：
$$\{\begin{array}{l}\Delta x=x_2-x_1\\ \Delta y=y_2-y_1\\ \Delta z=z_2-z_1\end{array}$$

基于单位四元数的姿态插补

预备知识：

基于单位四元数的姿态插补（Matlab）

已知起点S、终点D的齐次变换矩阵（位姿）分别为$T_S$和$T_D$，则姿态插补步骤如下：

1. 从齐次变换矩阵中提取出各自的旋转矩阵$R_S$和$R_D$，并将其转换为四元数$Q_s$和$Q_d$;

2. 姿态四元数插补公式为：
   $$Q_k(t)=\mathrm{Slerp}\left(Q_s,Q_d,t\right)=\frac{\sin [(1-\lambda (t))\theta ]}{\sin \theta }{Q}_s+\frac{\sin [\lambda (t)\theta ]}{\sin \theta }{Q}_d$$
   式中，$\theta =arccos(Q_s\cdot Q_d)$；

3. 将四元数$Q_k(t)$转换为旋转矩阵$R_k$，将$R_k$和位置插补得到的$P_k$组合得到各插值点对应的齐次变换矩阵$T_k$；

4. 带入逆运动学计算，得到各关节角度变量，进行插补运动。

规划插补流程

基于带约束S型加减速曲线的插补算法流程图如下：

Matlab实现代码

和前面博文的联系非常紧密，部分函数并未展出。

% 归一化处理
% S型加减速曲线
% 输入参数：机械臂末端运动总位移（角度）pos 
%          机械臂末端线速度（角速度）初始v0，终止v1，最大速度vmax
%          最大加减速度amax、最大加加速度jmax
%          插补周期t
% 返回值： 插值点数N
function [lambda, N] = Normalization_S(pos, v0, v1, vmax, amax, jmax, t)
% S曲线参数计算（S型速度规划，又称七段式轨迹）
% function para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax)
q0 = 0; q1 = pos;
para = STrajectoryPara(q0, q1, v0, v1, vmax, amax, jmax); % 获取S曲线参数
% 总的规划时间
T = para(1) + para(2) + para(3)
% 等时插补
N = ceil(T / t);
j = 1;
for i = 0 : t: T
   q(j) = S_position(i, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
%    qd(j) = S_velocity(i, para(1), para(2), para(3), para(4), para(5), para(6), para(7), para(8), para(9), para(10), para(11), para(12), para(13), para(14), para(15), para(16));
   lambda(j) = q(j) / pos;
   j = j + 1;
end
end

% 空间单一直线位置插补与单元四元数姿态插补 + 梯形加减速归一化处理/S型加减速曲线归一化处理
% 参数：起点S位置， 终点D位置
%      起点S的单元四元数Qs，终点的单元四元数Qd
%      起始速度v0，终止速度v1，最大速度vmax，最大加速度amax，最大加加速度jmax
%      插补周期t
% 返回值：插值点位置、单元四元数、插值点数
function [x y z qk N] = SpaceLine_Q(S, D, Qs, Qd, v0, v1, vmax, amax, jmax, t)
x1 = S(1); y1 = S(2); z1 = S(3);
x2 = D(1); y2 = D(2); z2 = 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);
% 求归一化参数：S型加减速曲线
% function lambda = Normalization_S(pos, v0, v1, vmax, amax, jmax, N)
[lambda, N] = Normalization_S(s, v0, v1, vmax, amax, jmax, t);
delta_x = x2 - x1;
delta_y = y2 - y1;
delta_z = z2 - z1;
% 计算两个四元数之间的夹角
dot_q = Qs.s*Qd.s + Qs.v(1)*Qd.v(1) + Qs.v(2)*Qd.v(2) + Qs.v(3)*Qd.v(3);
if (dot_q < 0)
    dot_q = -dot_q;
end
 
for i = 1: N
    % 位置插补
    x(i) = x1 + delta_x*lambda(i);
    y(i) = y1 + delta_y*lambda(i);
    z(i) = z1 + delta_z*lambda(i);
    % 单位四元数球面线性姿态插补
    % 插值点四元数
    if (dot_q > 0.9995)
        k0 = 1-lambda(i);
        k1 = lambda(i);
    else
        sin_t = sqrt(1 - power(dot_q, 2));
        omega = atan2(sin_t, dot_q);
        k0 = sin((1-lambda(i)*omega)) / sin(omega);
        k1 = sin(lambda(i)*omega) / sin(omega);
    end
    qk(i) = Qs * k0 + Qd * k1;
end

end

% 空间单一圆弧位置插补与单位四元数姿态插补 + 梯形加减速归一化处理/S型加减速曲线归一化处理
% 参数： 起点S位置, 中间点M位置, 终点D位置
%       起点S和终点D的单位四元数Qs_,Qd_
%       起始速度v0，终止速度v1，最大速度vmax，最大加速度amax，最大加加速度jmax
%       插值周期t
% 返回值：插值点位置、单元四元数、插值点数
% 方便起见，角速度和角加速度均为角度制
function [x y z qk N] = SpaceCircle_Q(S, M, D, Qs_, Qd_, v0, v1, vmax, amax, jmax, t)
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);

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 = 1; %插补参数，增加插值点数，避免过小
% ws = ws*pi / 180; % 角度换成弧度
% a = a*pi / 180;
% N = ceil(P*theta / ws);
% % 求归一化参数：梯形加减速曲线
% lambda = Normalization(theta, ws, a, N);
% 求归一化参数：S型加减速曲线
% function lambda = Normalization_S(pos, v0, v1, vmax, amax, jmax, N)
[lambda, N] = Normalization_S(theta, v0, v1, vmax, amax, jmax, t);

% 插补原理: 在新平面上进行插补（简化）
% 在新坐标系下z1_,z2_,z3_均为0，即外接圆在新坐标系的XOY平面内
% 此时转化为平面圆弧插补问题
delta_ang = theta;
% 计算两个四元数之间的夹角
dot_q = Qs_.s*Qd_.s + Qs_.v(1)*Qd_.v(1) + Qs_.v(2)*Qd_.v(2) + Qs_.v(3)*Qd_.v(3);
if (dot_q < 0)
    dot_q = -dot_q;
end

for i = 1: N
    % 位置插补
    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);
    % 单位四元数球面线性姿态插补
    % 插值点四元数
    if (dot_q > 0.9995)
        k0 = 1-lambda(i);
        k1 = lambda(i);
    else
        sin_t = sqrt(1 - power(dot_q, 2));
        omega = atan2(sin_t, dot_q);
        k0 = sin((1-lambda(i))*omega) / sin(omega);
        k1 = sin(lambda(i)*omega) / sin(omega);
    end
    qk(i) = Qs_ * k0 + Qd_ * k1;
end

end

不足之处

1. 圆弧的姿态插补只严格通过了起始姿态和终点姿态，没有经过中间姿态；
2. 无法达到时间最优。

参考文献

[1]刘鹏飞,杨孟兴,宋科,段晓妮.‘S’型加减速曲线在机器人轨迹插补算法中的应用研究[J].制造业自动化,2012,34(20):4-8+11.

[2]李振娜,王涛,王斌锐,郭振武,陈迪剑.基于带约束S型速度曲线的机械手笛卡尔空间轨迹规划[J].智能系统学报,2019,14(04):655-661.

[3]王添. 基于单位四元数机器人多姿态轨迹平滑规划[D].天津工业大学,2018.

[4]季晨. 工业机器人姿态规划及轨迹优化研究[D].哈尔滨工业大学,2013.

                                                                    圆弧插补算法

                                                                          前瞻算法

                                                                          插补算法