05 | 轨迹规划 ¶

为每个关节计算连续的运动轨迹，使末端执行器在空间中从点 A 移动到点 B

难点：

• 求解方程 ; 公式理解
• 列写方程之后，检查未知数个数和方程个数是否匹配

关节空间规划 ¶

任务描述：给定末端工具坐标系的位置和姿态，逆运动学求解出各个位姿对应的关节角，然后利用插值计算每个关节的运动轨迹。

线性插值 ¶

线性插值是一种简单的轨迹规划方法，通过在起点和终点之间进行线性插值来计算中间点的位置。其公式如下：

其中：

• \(\phi_0\) 为起始位置，
• \(\phi_f\) 为终止位置，
• \(t\) 为当前时间，
• \(T\) 为总时间。

线性插值的优点是计算简单，适用于对路径平滑性要求不高的场景，但其缺点是速度和加速度可能会出现突变。

import numpy as np

def linear_interpolation(start, end, t, duration):
    """
    线性插值轨迹规划
    :param start: 起始点坐标
    :param end: 终点坐标
    :param t: 当前时间
    :param duration: 总时长
    :return: 当前时刻的位置
    """
    if t < duration:
        x_array = start * (1 - t / duration) + end * (t / duration)
        x_angles = inverse_kinematics(x_array)
    else:
        x_angles = inverse_kinematics(end)
    return x_angles

三次多项式：规划位置 & 速度 ¶

这里有四个未知数，所以需要四个约束条件，选择初始位置、初始速度、终止位置、终止速度（一般都是给定的）

用给定的数据求解方程，得到四个系数，然后就可以得到轨迹方程。

三次 + 中间点 ¶

指定中间点速度

每一段都使用三次多项式进行规划。

• 前后两段斜率符号相同：速度取平均值
• 前后两段斜率符号不同：速度取 0

不指定中间点速度

有八个未知数，所以需要八个约束条件，一种方式是

• 位置约束：第一段起点终点，第二段起点终点
• 速度约束：第一段起点速度，第二段终点速度，中间点速度相等
• 加速度联系约束：中间点加速度相等

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22

clc;clear;
%定义时间节点和位置（包含起点、两个中间点、终点）
t_points=[0,1,2,3];%时间参数
y_points=[0,1,-1,2];%Y坐标：起点(0),中间点1(1),中间点2(-1),终点(2)

%拟合三次多项式（4个点需要3次多项式）
poly_y=polyfit(t_points,y_points,3);%Y方向多项式系数

%生成密集插值点
t_dense=linspace(0,3,100);
y_traj=polyval(poly_y,t_dense);%Y轨迹

%绘制结果
figure;
plot(t_dense,y_traj,'b-','LineWidth',1.5);
hold on;
plot(t_points,y_points,'ro','MarkerSize',8,'MarkerFaceColor','r');
xlabel('t');
ylabel('y');
title('多项式轨迹（含两个中间点）');
legend('多项式轨迹','控制点','Location','SouthEast');
grid on;

\(\begin{cases} a_{10} = \phi_0 \\ a_{11} = 0 \\ a_{12} = -\frac{3(\phi_0t_{\textrm{f2}}^2 + \phi_gt_{\textrm{f1}}^2 - \phi_vt_{\textrm{f1}}^2 - \phi_vt_{\textrm{f2}}^2 + 2\phi_0t_{\textrm{f1}}t_{\textrm{f2}} - 2\phi_vt_{\textrm{f1}}t_{\textrm{f2}})}{2t_{\textrm{f1}}^2t_{\textrm{f2}}(t_{\textrm{f1}} + t_{\textrm{f2}})} \\ a_{13} = \frac{\phi_0t_{\textrm{f2}}^2 + 3\phi_gt_{\textrm{f1}}^2 - 3\phi_vt_{\textrm{f1}}^2 - \phi_vt_{\textrm{f2}}^2 + 4\phi_0t_{\textrm{f1}}t_{\textrm{f2}} - 4\phi_vt_{\textrm{f1}}t_{\textrm{f2}}}{2t_{\textrm{f1}}^3t_{\textrm{f2}}(t_{\textrm{f1}} + t_{\textrm{f2}})} \\ a_{20} = \phi_v \\ a_{21} = -\frac{3(\phi_0t_{\textrm{f2}}^2 - \phi_gt_{\textrm{f1}}^2 + \phi_vt_{\textrm{f1}}^2 - \phi_vt_{\textrm{f2}}^2)}{2t_{\textrm{f1}}t_{\textrm{f2}}(t_{\textrm{f1}} + t_{\textrm{f2}})} \\ a_{22} = \frac{3(\phi_0t_{\textrm{f2}} + \phi_gt_{\textrm{f1}} - \phi_vt_{\textrm{f1}} - \phi_vt_{\textrm{f2}})}{t_{\textrm{f1}}t_{\textrm{f2}}(t_{\textrm{f1}} + t_{\textrm{f2}})} \\ a_{23} = -\frac{3\phi_0t_{\textrm{f2}}^2 + \phi_gt_{\textrm{f1}}^2 - \phi_vt_{\textrm{f1}}^2 - 3\phi_vt_{\textrm{f2}}^2 + 4\phi_gt_{\textrm{f1}}t_{\textrm{f2}} - 4\phi_vt_{\textrm{f1}}t_{\textrm{f2}}}{2t_{\textrm{f1}}t_{\textrm{f2}}^3(t_{\textrm{f1}} + t_{\textrm{f2}})} \end{cases}\)

五次多项式：规划位置 & 速度 & 加速度 ¶

思路比较类似。

六个未知数 , 就可以指定起点终点的位置、速度、加速度，然后求解方程。

def quintic_polynomial(start, end, t, duration):
    """
    五次多项式轨迹规划
    :param start: 起始点坐标
    :param end: 终点坐标
    :param t: 当前时间
    :param duration: 总时长
    :return: 当前时刻的位置
    """
    if t < duration:
        t_matrix = np.matrix([
            [0, 0, 0, 0, 0, 1],
            [duration ** 5, duration ** 4, duration ** 3, duration ** 2, duration, 1],
            [0, 0, 0, 0, 1, 0],
            [5 * duration ** 4, 4 * duration ** 3, 3 * duration ** 2, 2 * duration, 1, 0],
            [0, 0, 0, 2, 0, 0],
            [20 * duration ** 3, 12 * duration ** 2, 6 * duration, 2, 0, 0]
        ])
        x_matrix = np.matrix([[start[i], end[i], 0, 0, 0, 0] for i in range(len(start))]).T
        k_matrix = np.linalg.inv(t_matrix) @ x_matrix
        time_vector = np.matrix([t ** 5, t ** 4, t ** 3, t ** 2, t, 1]).T
        x = (k_matrix.T @ time_vector).T.A[0]
    else:
        x = end
    return x

直线段 + 抛物线过渡 ¶

两段形状相同的抛物线，中间连接的直线段是公切线

• 过渡段：抛物线段
• 直线段：顾名思义

给定：起点 \(\phi_0\)、终点 \(\phi_{final}\)、总时间 \(t_{final}\)、加速度 \(\ddot\phi\) 需要求解：过渡时间（抛物线段）\(t_b\)、速度（直线段）\(k_b\)

过渡段的时间间隔，二次方程求根公式求解。舍掉 + 号的解是因为 \(t_b < t_{f}\)

这里相当于初速度为 0 的平抛

def parabolic_transition(start, end, t, duration):
    """
    抛物线过渡插值
    :param start: 起始点坐标
    :param end: 终点坐标
    :param t: 当前时间
    :param duration: 总时长
    :return: 当前时刻的位置
    """
    if t < duration:
        tx = t / duration
        if t < 0.5 * duration:
            tx = 2 * (tx ** 2)
        else:
            tx = 0.5 + 2 * (tx - 0.5) - 2 * (tx - 0.5) ** 2
        x_array = start * (1 - tx) + end * tx
        x_angles = inverse_kinematics(x_array)
    else:
        x_angles = inverse_kinematics(end)
    return x_angles

中间点 + 抛物线过渡 ¶

给定： 系列点 \(\phi_0, \phi_1,\dots,\phi_{final}\)、各段时间 \(t_{dij}\)、加速度 \(\ddot\phi\)

需要求解： 各段的速度 \(k_{ij}\)，以及过渡段的时间 \(t_{i}\)，直线段的时间 \(t_{ij}\)

中间段计算 ¶

过渡段 \( j \) 加速度

过渡段 \( j \) 时间间隔

直线段 \( jk \) 速度

直线段时间 \( jk \) 间隔

起始段计算 ¶

过渡段 1 加速度

过渡段 1 时间间隔

根据切点速度相等列写方程

求根公式解得：

直线段 12 速度

直线段 12 时间间隔

笛卡尔空间规划 ¶

旋转矩阵和欧拉角不可以插值的原因：插值得到的 R 矩阵不一定属于 SO(3)

等效轴角插值 ¶

等效轴角的表示：\((\hat{k}_{x},\hat{k}_{y},\hat{k}_{z})^{\mathrm{T}}\) 为等效单位转动轴，\(\theta\) 为绕该轴的转动量（这里单位为 °）

对两个等效轴角表示的姿态

插值时，通常应该选择使得 \(\left\|\left(\begin{array}{c} k_{0x} \\ k_{0y} \\ k_{0z} \end{array}\right)-(\theta_{1}+360n)\left(\begin{array}{c} \hat{k}_{1x} \\ \hat{k}_{1y} \\ \hat{k}_{1z} \end{array}\right)\right\|\) 最小的 \(n\)，然后对 \(\left(\begin{array}{c} k_{0x} \\ k_{0y} \\ k_{0z} \end{array}\right) \text { 和 } \left(\theta_{1}+360n\right)\left(\begin{array}{c} \hat{k}_{1x} \\ \hat{k}_{1y} \\ \hat{k}_{1z} \end{array}\right)\) 运用前面的多项式或带抛物线过渡直线段等插值方法。

代码实现 ¶

clc;clear;
%主程序
%初始姿态示例：绕z轴旋转0和π/2
K0 = [0,0,0]; %初始姿态（无旋转）
n_values = [-1,0,1]; %不同n值测试

% 创建3个子图
figure('Position',[100 100 1200 400]);
subplot_handles = zeros(1,3);
h_x = zeros(1,3);
h_y = zeros(1,3);
h_z = zeros(1,3);

% 初始化3个子图
for j = 1:length(n_values)
    subplot_handles(j) = subplot(1,3,j);
    hold on;
    grid on;
    axis equal;
    xlabel('X');
    ylabel('Y');
    zlabel('Z');
    view(3);
    xlim([-1.5,1.5]);
    ylim([-1.5,1.5]);
    zlim([-1.5,1.5]);
    title(['n=',num2str(n_values(j))]);
    
    % 初始化动画对象
    origin = [0;0;0];
    h_x(j) = quiver3(origin(1),origin(2),origin(3),1,0,0,'r','LineWidth',2);
    h_y(j) = quiver3(origin(1),origin(2),origin(3),0,1,0,'g','LineWidth',2);
    h_z(j) = quiver3(origin(1),origin(2),origin(3),0,0,1,'b','LineWidth',2);
end

% 创建动画
while true % 无限循环
    for t = 0:0.01:1 % 减小步长从0.02到0.01使动画更慢
        for j = 1:length(n_values)
            n = n_values(j);
            K1 = (pi/2+2*pi*n)*[0,0,1];
            
            %三次多项式插值
            kt = K0+(3*t^2-2*t^3)*(K1-K0);
            R = rotation_vector_to_matrix(kt);
            
            %更新坐标轴
            x_axis = R*[1;0;0];
            y_axis = R*[0;1;0];
            z_axis = R*[0;0;1];
            
            set(h_x(j),'UData',x_axis(1),'VData',x_axis(2),'WData',x_axis(3));
            set(h_y(j),'UData',y_axis(1),'VData',y_axis(2),'WData',y_axis(3));
            set(h_z(j),'UData',z_axis(1),'VData',z_axis(2),'WData',z_axis(3));
        end
        drawnow;
        pause(0.02); % 添加暂停使动画更慢
    end
end

%旋转向量到旋转矩阵的函数
function R = rotation_vector_to_matrix(k)
    % 这个函数实现了罗德里格斯公式，将旋转向量转换为旋转矩阵
    % 输入k是旋转向量，包含了旋转轴方向和旋转角度(角度在向量的模长中)
    
    % 1. 计算旋转角度theta(弧度)，即旋转向量的模长
    theta = norm(k);
    
    % 2. 如果旋转角度为0，直接返回单位矩阵(不旋转)
    if theta == 0
        R = eye(3);
        return;
    end
    
    % 3. 计算单位旋转轴向量ne
    ne = k/theta;  % 归一化得到单位向量
    
    % 4. 构造ne的叉积矩阵K，用于后续计算
    % K = [ne]_× 是ne的叉积矩阵，满足K*v = ne × v
    K = [0,-ne(3),ne(2);
        ne(3),0,-ne(1);
        -ne(2),ne(1),0];
        
    % 5. 使用罗德里格斯公式计算旋转矩阵
    % R = I + sin(θ)[k]_× + (1-cos(θ))[k]_×^2
    % 其中I是单位矩阵，[k]_×是叉积矩阵，θ是旋转角度
    R = eye(3) + sin(theta)*K + (1-cos(theta))*(K*K);
end

Slerp ｜ 四元数球面线性插值 ¶

注意到单位四元数 \(r\) 和 -\(r\) 表示三维空间中的同一姿态。一般应该选取最短路径进行球面线性插值。

因此如果两四元数的夹角为钝角，则可通过将其中一个四元数取负，再对得到的两个夹角为锐角的四元数进行球面线性插值。

初始两个四元数如果是 \(\pi\) 的话，就说明两个四元数对应的是同一个姿态。

如果

代码实现 ¶

def slerp(q1, q2, t):
    """
    球面线性插值（Slerp）
    :param q1: 起始四元数
    :param q2: 终止四元数
    :param t: 插值因子，范围 [0, 1]
    :return: 插值后的四元数
    """
    dot_product = np.dot(q1, q2)
    dot_product = np.clip(dot_product, -1.0, 1.0)
    theta_0 = np.arccos(dot_product)
    sin_theta_0 = np.sin(theta_0)
    
    if sin_theta_0 < 1e-6:
        return q1
    
    theta = theta_0 * t
    s1 = np.sin(theta) / sin_theta_0
    s0 = np.cos(theta) - dot_product * s1
    return s0 * q1 + s1 * q2

需要下载 matlab 的 robotics toolbox 和

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99

%初始化
clear;clc;close all;
%定义初始和结束四元数（绕Y轴旋转90度）
theta=pi/3;%90度
r0=quaternion(1,0,0,0);%初始姿态（无旋转）
r1=quaternion(cos(theta/2),0,sin(theta/2),0);%绕Y轴旋转theta

% 创建两个子图
h = figure('Position',[100 100 1200 400]);
subplot_handles = zeros(1,2);
h_x = zeros(1,2);
h_y = zeros(1,2);
h_z = zeros(1,2);

% 初始化两个子图
for j = 1:2
    subplot_handles(j) = subplot(1,2,j);
    hold on;
    grid on;
    axis equal;
    xlabel('X');
    ylabel('Y');
    zlabel('Z');
    view(3);
    xlim([-1.5,1.5]);
    ylim([-1.5,1.5]);
    zlim([-1.5,1.5]);
    if j == 1
        title('r0到r1的四元数插值路径');
    else
        title('r0到-r1的四元数插值路径');
    end
    
    % 初始化动画对象
    origin = [0;0;0];
    h_x(j) = quiver3(origin(1),origin(2),origin(3),1,0,0,'r','LineWidth',2);
    h_y(j) = quiver3(origin(1),origin(2),origin(3),0,1,0,'g','LineWidth',2);
    h_z(j) = quiver3(origin(1),origin(2),origin(3),0,0,1,'b','LineWidth',2);
end

% 创建动画帧
filename = 'quaternion_animation.gif';
for t = 0:0.02:1
    % 第一个子图：r0到r1的插值
    q_r1 = slerp(r0,r1,t);
    x_r1 = quatrotate(compact(q_r1),[1,0,0]);
    y_r1 = quatrotate(compact(q_r1),[0,1,0]);
    z_r1 = quatrotate(compact(q_r1),[0,0,1]);
    
    set(h_x(1),'UData',x_r1(1),'VData',x_r1(2),'WData',x_r1(3));
    set(h_y(1),'UData',y_r1(1),'VData',y_r1(2),'WData',y_r1(3));
    set(h_z(1),'UData',z_r1(1),'VData',z_r1(2),'WData',z_r1(3));
    
    % 第二个子图：r0到-r1的插值
    q_neg_r1 = slerp(r0,-r1,t);
    x_neg_r1 = quatrotate(compact(q_neg_r1),[1,0,0]);
    y_neg_r1 = quatrotate(compact(q_neg_r1),[0,1,0]);
    z_neg_r1 = quatrotate(compact(q_neg_r1),[0,0,1]);
    
    set(h_x(2),'UData',x_neg_r1(1),'VData',x_neg_r1(2),'WData',x_neg_r1(3));
    set(h_y(2),'UData',y_neg_r1(1),'VData',y_neg_r1(2),'WData',y_neg_r1(3));
    set(h_z(2),'UData',z_neg_r1(1),'VData',z_neg_r1(2),'WData',z_neg_r1(3));
    
    drawnow;
    
    % 捕获当前帧并写入GIF
    frame = getframe(h);
    im = frame2im(frame);
    [imind,cm] = rgb2ind(im,256);
    if t == 0
        imwrite(imind,cm,filename,'gif','Loopcount',inf,'DelayTime',0.02);
    else
        imwrite(imind,cm,filename,'gif','WriteMode','append','DelayTime',0.02);
    end
end

function q_interp = slerp(q0,q1,t)
    %将四元数转换为数组格式
    q0_compact=compact(q0);
    q1_compact=compact(q1);
    %计算点积（四元数分量直接相乘）
    
    dot_prod=sum(q0_compact.*q1_compact);
    %限制点积范围
    dot_prod=min(max(dot_prod,-1),1);
    theta=acos(dot_prod);
    %处理θ为0的情况
    if theta<eps
        q_interp=q0;
        return;
    end
    %计算插值系数
    k0=sin((1-t)*theta)/sin(theta);
    k1=sin(t*theta)/sin(theta);
    %线性组合并重新构造四元数
    q_interp_compact=k0*q0_compact+k1*q1_compact;
    q_interp=quaternion(q_interp_compact);
    q_interp=normalize(q_interp);
end

Slerp 拓展：对角速度有约束 ¶

由于单位四元数球面线性插值方法 (Slerp) 的旋转角速度是定值， 此时直接应用 Slerp 公式进行规划是不行的。结合 Slerp 公式，给出上述要求下相应的姿态

题型 ¶

解方程比较麻烦

多项式参数计算：时间、速度 ¶

抛物线过渡计算 ¶

已知起点角度为 138°，中间点到达的点角度为 158°，时间为 5.5s，点角度为 10°，时间为 31.5s。需要计算中间点。中间点关于中点对称。采用带有抛物线过渡的线性规划来实现。过渡段（第二个中间点到第三个中间点）的持续时间为 16s。

matlab 求解方程 ¶

• 方程写出来，丢给 gpt