【自适应和反应式机器人控制】编程练习 1.1：计算最优轨迹 + 编程练习 1.3：基于三阶多项式的闭式时变轨迹发生器

2.1 编程练习 1.1：计算最优轨迹

书籍对应：编程练习 Ex1.1.2，第8页

此练习的目的是让读者熟悉应用于机械臂运动的基本优化方法。打开chp1_ex1.2.m代码文件，该代码生成一个四关节机械臂的绘图。编辑代码以完成以下任务：

1. 编写理论练习中冗余机械臂的优化问题，该机械臂在三维空间中运动，因此有一个三维（3D）吸引点。假设关节是依次连接的。尝试将目标位置移动到空间中的不同位置，并比较三个优化的轨迹解。你能找到一个使（b）和（c）的解相同的配置吗？

   • ( a ) 最小化到达目标的时间 $T_f$
   • ( b ) 跟随笛卡尔空间中的最短路径
   • ( c ) 跟随关节空间中的最短路径

使用MATLAB的模型预测控制类

我们使用MATLAB中的模型预测控制（Model Predictive Control, MPC）类来处理数值优化。你只需要定义与书中每个问题对应的代价函数。为此，可以将不同组合的变量X和U填入cost变量中，分别表示最优轨迹和最优输入，按行向量存储。

公式定义如下：

$$\mathbf{X}=\left[\begin{array}{c}{\mathbf{X}}_0\\ ⋮\\ {\mathbf{X}}_N\end{array}\right]=\left[\begin{array}{cccc}{q}_0^1& q_0^2& q_0^3& q_0^4\\ ⋮& ⋮& ⋮& ⋮\\ q_N^1& q_N^2& q_N^3& q_N^4\end{array}\right]$$

$$\mathbf{U}=\left[\begin{array}{c}{\mathbf{U}}_0\\ ⋮\\ {\mathbf{U}}_N\end{array}\right]=\left[\begin{array}{ccccc}{\dot{q}}_0^1& {\dot{q}}_0^2& {\dot{q}}_0^3& {\dot{q}}_0^4& T_f\\ ⋮& ⋮& ⋮& ⋮& ⋮\\ {\dot{q}}_N^1& {\dot{q}}_N^2& {\dot{q}}_N^3& {\dot{q}}_N^4& T_f\end{array}\right]$$

解释：

X表示机械臂的最优轨迹，其值为机械臂各个关节的位置信息（角度等）。U表示最优输入，包含关节的速度信息和达到目标所需的总时间 ( T_f )。通过定义合适的代价函数，MPC可以计算出最优轨迹和输入，使得机械臂满足上述不同的优化目标。

在这里，$[q^1,q^2,q^3,q^4]$ 表示机器人的四个关节位置，而 $[{\dot{q}}^1,{\dot{q}}^2,{\dot{q}}^3,{\dot{q}}^4]$ 表示四个关节的速度。$T_f$ 是轨迹的终止时间。该轨迹由 $N$个点组成，可以通过以下代码获取：

% 轨迹中的路径点数量:
N = data.PredictionHorizon

需要修改的三个函数分别是 minimumTime()、minimumTaskDistance() 和 minimumJointDistance()，它们位于文件底部。你可以尝试不同的代价函数，并在MATLAB中运行相应的部分，查看它对最终轨迹的影响。
注意：某些目标函数可能需要几秒钟才能解决。

你可以使用雅可比矩阵 $J(q)$来根据关节速度计算小位移$\delta x$： $\delta x=J(q)\ast \dot{q}\ast \delta t$。可以使用以下MATLAB函数来计算雅可比矩阵：

% 输入 4x1 关节位置向量 q，输出 3x4 雅可比矩阵：
jacobian = robot.fastJacobian(q);

解释：

• N = data.PredictionHorizon 用于获取轨迹中路径点的数量。
• 文件中定义了三个不同的代价函数 minimumTime()、minimumTaskDistance() 和 minimumJointDistance()，分别用于实现不同的优化目标：最小化时间、最小化任务空间中的距离和最小化关节空间中的距离。可以通过修改这些代价函数来观察对轨迹的影响。
• 使用雅可比矩阵 $J(q)$可以将关节速度转换为笛卡尔空间中的小位移，这在计算机器人的实际路径时非常有用。
• robot.fastJacobian(q) 是MATLAB中的一个函数，用于快速计算给定关节位置下的雅可比矩阵。

通过这些函数和工具，您可以生成并优化机器人的运动轨迹。

chp1_ex1_2

这段代码实现了一个控制机器人手臂的优化控制任务，目的是计算从当前配置到目标位置的最优轨迹。在代码中，定义了三个优化目标：最小时间、最小笛卡尔空间距离和最小关节空间距离。每个优化任务都需要定义其对应的成本函数。下面是每个任务成本函数的详细补充和解释。

补全代码-实现每个任务的成本函数，以满足优化目标

%% %%%%%%%%%%%%% User defined cost functions %%%%%%%%%%%%% %%
%% ------ Write your code below for Question 2 ------
%  vvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvvv %%

% Task 1: Minimum time 
% This function minimizes the time scaling parameter Tf to minimize
% trajectory time
function cost = minimumTime(X, U, e, data, robot, target)
    % The cost is simply the final time, which we want to minimize.
    % U(5) is the final time of the trajectory, representing the overall
    % time needed to reach the target position.
    cost = U(5); % Minimize the total trajectory time
end

% Task 2: Minimum distance in task space 
% This function integrates dx = J*dq to minimize Cartesian trajectory length
% You can obtain the Jacobian J at configuration q using
% J = robot.fastJacobian(q)
% USE THE SQUARE OF THE NORM FOR NUMERICAL STABILITY
function cost = minimumTaskDistance(X, U, e, data, robot, target)
    % Initialize cost
    cost = 0;
    % Loop over the prediction horizon
    for i = 1:data.PredictionHorizon
        % Get the joint angles (state) for the current timestep
        q = X(i, :)';
        % Get the joint velocities (input) for the current timestep
        dq = U(i, 1:4)';
        % Compute the Jacobian J at the current configuration
        J = robot.fastJacobian(q);
        % Calculate the Cartesian velocity dx = J * dq
        dx = J * dq;
        % Increment the cost by the square of the norm of dx for stability
        cost = cost + norm(dx)^2;
    end
end

% Task 3: Minimum distance in joint space 
% This function integrates dq to minimize joint trajectory length
% USE THE SQUARE OF THE NORM FOR NUMERICAL STABILITY
function cost = minimumJointDistance(X, U, e, data, robot, target)
    % Initialize cost
    cost = 0;
    % Loop over the prediction horizon
    for i = 1:data.PredictionHorizon
        % Get the joint velocities (input) for the current timestep
        dq = U(i, 1:4)';
        % Increment the cost by the square of the norm of dq for stability
        cost = cost + norm(dq)^2;
    end
end

解释每个任务的成本函数

1. Task 1: Minimum Time

   • 目标：最小化到达目标位置的时间。
   • 实现：时间由控制输入的最后一个分量 ( U(5) ) 表示，这个分量代表整个轨迹的最终时间。因此，成本函数 minimumTime 仅仅返回 ( U(5) ) 的值，以最小化总时间。
   • 公式：cost = U(5);

2. Task 2: Minimum Distance in Task Space

   • 目标：最小化机器人在笛卡尔空间（任务空间）中的移动距离。
   • 实现：
     • 使用雅可比矩阵 $J$ 将关节速度 $dq$ 转换为任务空间速度 $dx$。
     • 计算每个时间步中 $dx$的平方范数并累加到成本函数中。
     • 使用平方范数可以提高数值稳定性。
   • 公式：
     $$\text{cost}=\sum _{i=1}^{\text{PredictionHorizon}}\Vert J(q)\cdot dq{\Vert }^2$$

3. Task 3: Minimum Distance in Joint Space

   • 目标：最小化机器人在关节空间中的移动距离。
   • 实现：
     • 直接计算每个时间步中关节速度 $dq$的平方范数并累加到成本函数中。
     • 同样地，使用平方范数可以提高数值稳定性。
   • 公式：
     $$\text{cost}=\sum _{i=1}^{\text{PredictionHorizon}}\Vert dq{\Vert }^2$$

其他说明

• data.PredictionHorizon 表示优化过程中预测的时间步数。
• robot.fastJacobian(q) 是用来计算在配置 $q$ 下的雅可比矩阵的函数。
• norm() 函数用于计算向量的范数，使用平方范数有助于数值优化的稳定性。

使用平方范数有助于数值优化的稳定性

如果不使用平方，第3步求解会出错

Slack variable unused or zero-weighted in your custom cost function.
All constraints will be hard. OPTIMIZATION SUCCESSFUL
--------- Reached target in 0.93489 seconds ---------
Elapsed time is 0.374180 seconds.
Press space to continue…
Slack variable unused or zero-weighted in your custom cost function. All constraints will be
hard. OPTIMIZATION SUCCESSFUL
--------- Reached target in 5 seconds ---------
Elapsed time is 0.731092 seconds. Press space to continue…
Slack variable unused or zero-weighted in your custom cost function. All constraints will be
hard.
Warning: Solver failed. Try relaxing some of your constraints In
MPC4DOF/solveOptimalTrajectory (line 146) In chp1_ex1_2 (line 54)
--------- Reached target in 1.5 seconds --------- Elapsed time is 0.343741 seconds.

chp1_ex1_1-编程练习 1.3：闭式轨迹 [可选]

书籍对应：编程练习 Ex1.1.1，第8页

1. 基于三阶多项式计算一个时间依赖的闭形式轨迹生成器。
   • 您可以在 chp1_ex1_1.m 的第51行添加您的实现代码。需要手动创建一个在笛卡尔空间的轨迹，该轨迹从 initial_position 开始，到达 target_position，基于三阶多项式（参见图2）。

计算基于三阶多项式的闭式时变轨迹发生器

通过插值计算，每个时间点的位置信息会逐步从初始位置过渡到目标位置，以确保运动平稳。

原理和公式解释

在这个三阶多项式轨迹生成中，我们希望找到一个平滑的函数，使得机器人的位置从初始位置过渡到目标位置。三阶多项式的形式如下：

$$p(t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3$$
其中：

• $a_0$、$a_1$、$a_2$ 和 $a_3$是多项式的系数。
• $t$是时间变量。

条件设置

使用满足初始和最终位置以及速度的边界条件。

1. 初始位置条件：当 $t=0$时，位置为初始位置 $p(0)=\text{initialPosition}$。
2. 终点位置条件：当 $t=T_f$时，位置为目标位置 $p(T_f)=\text{targetPosition}$。
3. 初始速度为 0：当 $t=0$时，速度 $p^{\prime }(0)=0$。
4. 最终速度为 0：当 $t=T_f$ 时，速度$p^{\prime }(T_f)=0$。

其中 $T_f$是总时间长度。

利用条件求解系数

% 使用符号工具箱推导三阶多项式轨迹生成的公式
syms a0 a1 a2 a3 t T_f initialPosition targetPosition

% 定义三阶多项式 p(t) = a0 + a1*t + a2*t^2 + a3*t^3
p = a0 + a1 * t + a2 * t^2 + a3 * t^3;

% 计算速度 p'(t)
p_dot = diff(p, t);

% 设定边界条件：
% 1. 初始位置条件：p(0) = initialPosition
eq1 = subs(p, t, 0) == initialPosition;

% 2. 初始速度条件：p'(0) = 0
eq2 = subs(p_dot, t, 0) == 0;

% 3. 终点位置条件：p(T_f) = targetPosition
eq3 = subs(p, t, T_f) == targetPosition;

% 4. 最终速度条件：p'(T_f) = 0
eq4 = subs(p_dot, t, T_f) == 0;

% 解方程组，求解 a0, a1, a2, a3
solutions = solve([eq1, eq2, eq3, eq4], [a0, a1, a2, a3]);

% 展示结果
disp('a0 = '); disp(solutions.a0);
disp('a1 = '); disp(solutions.a1);
disp('a2 = '); disp(solutions.a2);
disp('a3 = '); disp(solutions.a3);


% 为验证目的，可以将结果代入并观察
% 定义具体的初始位置、目标位置、总时间，并生成时间序列
initialPosition_val = 0;
targetPosition_val = 10;
T_f_val = 5;
time = linspace(0, T_f_val, 50);

% 计算 a2 和 a3 的值
a0_val = subs(solutions.a0, [initialPosition, targetPosition, T_f], [initialPosition_val, targetPosition_val, T_f_val]);
a1_val = subs(solutions.a1, [initialPosition, targetPosition, T_f], [initialPosition_val, targetPosition_val, T_f_val]);
a2_val = subs(simplified_a2, [initialPosition, targetPosition, T_f], [initialPosition_val, targetPosition_val, T_f_val]);
a3_val = subs(simplified_a3, [initialPosition, targetPosition, T_f], [initialPosition_val, targetPosition_val, T_f_val]);

% 计算轨迹
cartesianTrajectory = double(a0_val + a1_val * time + a2_val * time.^2 + a3_val * time.^3);

% 绘制轨迹
figure;
plot(time, cartesianTrajectory);
xlabel('Time [s]');
ylabel('Position');
title('Smooth Trajectory from Initial to Target Position based on Third-Order Polynomial');
grid on;

第一步：初始位置条件

当 $t=0$时，轨迹位置为初始位置：
$$p(0)=a_0=\text{initialPosition}$$
因此得到$a_0=\text{initialPosition}$。

第二步：初始速度条件

速度函数是多项式对时间$t$的一阶导数：
$$p^{\prime }(t)=a_1+2a_{2}t+3a_3{t}^2$$
当 $t=0$ 时，初始速度为 0：
$$p^{\prime }(0)=a_1=0$$
因此，得到 $a_1=0$。

到目前为止，我们有：
$$p(t)=a_0+a_2{t}^2+a_3{t}^3$$
其中$a_0=\text{initialPosition}$和 $a_1=0$。

第三步：终点位置条件

当$t=T_f$时，轨迹位置为目标位置：
$$p(T_f)=a_0+a_2{T}_f^2+a_3{T}_f^3=\text{targetPosition}$$
代入 $a_0=\text{initialPosition}$：
$$\text{initialPosition}+a_2{T}_f^2+a_3{T}_f^3=\text{targetPosition}$$
移项得到：
$$a_2{T}_f^2+a_3{T}_f^3=\text{targetPosition}-\text{initialPosition}$$

第四步：最终速度条件

当 $t=T_f$时，速度为 0：
$$p^{\prime }(T_f)=a_1+2a_2{T}_f+3a_3{T}_f^2=0$$
代入$a_1=0$，简化为：
$$2a_2{T}_f+3a_3{T}_f^2=0$$
解出 $a_2$与 $a_3$ 的关系：
$$a_2=-\frac{3}{2}{a}_3{T}_f$$

联立方程解 $a_2$和 $a_3$

我们现在有两个方程：
$a_2{T}_f^2+a_3{T}_f^3=\text{targetPosition}-\text{initialPosition}$
$a_2=-\frac{3}{2}{a}_3{T}_f$

将第二个方程代入第一个方程：
$$\left(-\frac{3}{2}{a}_3{T}_f\right)T_f^2+a_3{T}_f^3=\text{targetPosition}-\text{initialPosition}$$
$$-\frac{3}{2}{a}_3{T}_f^3+a_3{T}_f^3=\text{targetPosition}-\text{initialPosition}$$
$$\frac{-3+2}{2}{a}_3{T}_f^3=\text{targetPosition}-\text{initialPosition}$$
$$-\frac{1}{2}{a}_3{T}_f^3=\text{targetPosition}-\text{initialPosition}$$
$$a_3=-\frac{2(\text{targetPosition}-\text{initialPosition})}{T_f^3}$$

将 $a_3$的值代入 $a_2=-\frac{3}{2}{a}_3{T}_f$：
$$a_2=-\frac{3}{2}\left(-\frac{2(\text{targetPosition}-\text{initialPosition})}{T_f^3}\right)T_f$$
$$a_2=\frac{3(\text{targetPosition}-\text{initialPosition})}{T_f^2}$$

最终结果

因此，我们得到：
$$a_0=\text{initialPosition}$$
$$a_1=0$$。
$$a_2=\frac{3(\text{targetPosition}-\text{initialPosition})}{T_f^2}$$
$$a_3=-\frac{2(\text{targetPosition}-\text{initialPosition})}{T_f^3}$$

chp1_ex1_1代码实现

在代码中，将三维空间中的每个轴（$x$, $y$, $z$）分别使用三阶多项式插值公式来计算：

for i = 1:3
    a0 = initialPosition(i);  % 初始位置
    a1 = 0;                   % 初始速度为0
    a2 = 3 * (targetPosition(i) - initialPosition(i)) / (5^2);  % 二次项系数
    a3 = -2 * (targetPosition(i) - initialPosition(i)) / (5^3); % 三次项系数
    cartesianTrajectory(i, :) = a0 + a1 * time + a2 * time.^2 + a3 * time.^3;
end

在这个循环中，代码针对 $x$、$y$、$z$ 坐标分别计算各自的三阶多项式轨迹，最终生成平滑的轨迹数组 cartesianTrajectory。在此代码中 $T_f=5$ 秒
a0 是起始位置，a1 是初始速度为0，a2 和 a3 的系数用于保证在5秒内到达目标位置。

Press space to continue…
Warning: The provided robot configuration violates the predefined joint limits.
In robotics.manip.internal.warning (line 19)
In robotics.manip.internal/RigidBodyTree/validateConfigurationWithLimits (line 1705)
In inverseKinematics/solve (line 318)
In inverseKinematics/stepImpl (line 163)
In RobotisWrapper/computeInverseKinematics (line 95)
In chp1_ex1_1 (line 76)

该警告信息指出，在尝试计算逆运动学解（将笛卡尔坐标的轨迹转换为关节角度的轨迹）时，机器人当前的某些关节配置超出了其预定义的关节限制。

具体解释

1. 逆运动学求解：
   在 computeInverseKinematics 函数中，代码尝试通过逆运动学方法将笛卡尔空间的路径（cartesianTrajectory）转换为关节空间路径（即各关节角度的变化轨迹）。逆运动学求解通常涉及将给定的末端执行器位置和姿态转换为每个关节的角度。

2. 关节限制：
   多数机器人在设计时对关节的活动范围有一定的限制（例如旋转角度的上限和下限）。这些限制是为了保证机械结构的安全和控制的稳定性。超出关节限制可能会导致机械损坏或控制失误。

3. 警告来源：
   警告来自于 MATLAB 的 RigidBodyTree 类的 validateConfigurationWithLimits 方法，表明逆运动学求解过程中计算出的某些关节角度值超出了预设的关节活动范围。

解决方法

您可以尝试以下几种方法来解决此警告：

1. 检查目标位置：
   确保 targetPosition 是在机器人的工作空间内的合理位置。如果目标位置太远或不在机器人可达的范围内，逆运动学求解可能会遇到困难，从而导致关节角度超出限制。

2. 调整初始和目标位置：
   尝试将 initialPosition 和 targetPosition 设置在更靠近中心的位置，避免它们位于机器人关节限制的边界区域。

3. 使用关节限制优化器：
   如果您的机器人类提供了优化选项，可以设置逆运动学求解器考虑关节限制，从而自动避免生成超出关节限制的解。

4. 调试代码：
   如果有调试需求，可以在逆运动学求解过程中打印或记录关节角度，检查哪些关节角度超过了限制，从而可以进一步手动调整目标位置或轨迹。