【Homework】【7】Learning resources for DQ Robotics in MATLAB

阻尼伪逆使系统在任务空间奇异性方面具有一定的鲁棒性

阻尼伪逆

阻尼伪逆是SVD（奇异值分解）逆矩阵的一种有趣替代方法，它使系统在任务空间奇异性方面具有一定的鲁棒性。其主要思想是对任意（可能为奇异的）矩阵 $B\in {\mathbb{R}}^{m\times n}$，使用以下伪逆矩阵，称为阻尼伪逆（damped-pseudo inverse）：

$$B^{+}\triangleq (B^{T}B+{\lambda }^{2}I{)}^{-1}{B}^T$$

其中， λ ∈ R − { 0 } \lambda \in \mathbb{R} - \{0\} λ∈R−{0}是阻尼因子。这个阻尼因子的含义将在下一节中更清楚地解释，但目前只需知道以下结论：
$$(B^{T}B+{\lambda }^{2}I{)}^{-1}$$

始终存在，因此可以使用常规的逆矩阵计算算法来求解阻尼伪逆。

阻尼伪逆与奇异性鲁棒性

从基于雅可比矩阵的机器人控制的角度来看，阻尼因子让我们能够平衡以下两者之间的权衡关系：最优关节速度（在任务误差减少的意义上）和关节速度范数。

如果选择较大的阻尼因子，机器人对任务空间奇异性会更鲁棒，但可能需要更长时间才能达到期望的任务空间值。随着阻尼因子的降低，其行为会更接近于基于SVD的伪逆。如果阻尼因子太小，当机器人接近奇异配置时，关节速度也可能变得危险。

Lesson 7

代码

clear all;
close all;
clc;
include_namespace_dq;

% 定义参数
damping_values = [0.001, 0.01, 0.1, 0.2, 0.3, 0.4, 0.5, 1];
tau = 0.01;
final_time = 1;
stored_time = 0:tau:final_time;

% 初始条件
theta_i = 0; % Condition 1
td = 7 * j_;
eta = 10;

% 初始化存储
task_space_error_norms = zeros(length(damping_values), length(stored_time));
control_signal_norms = zeros(length(damping_values), length(stored_time));

% 计算每个阻尼值下的任务空间误差和控制信号
for i = 1:length(damping_values)
    damping = damping_values(i);
    [task_space_error_norms(i, :), control_signal_norms(i, :)] = compute_norms(damping, theta_i, td, tau, final_time, eta);
end

% 创建动态显示的窗口
figure;

% 绘制任务空间误差子图
ax1 = subplot(1, 2, 1);
hold on;
handles_tn = []; % 用于存储任务空间误差句柄
for i = 1:length(damping_values)
    h = plot(stored_time, task_space_error_norms(i, :), 'LineWidth', 2, ...
        'DisplayName', sprintf('\\lambda=%.3f', damping_values(i)));
    handles_tn = [handles_tn; h];
end
title('Task-Space Error Norm');
xlabel('Time (s)');
ylabel('Error Norm');
legend show;
hold off;

% 绘制控制信号范数子图
ax2 = subplot(1, 2, 2);
hold on;
handles_cn = []; % 用于存储控制信号范数句柄
for i = 1:length(damping_values)
    h = plot(stored_time, control_signal_norms(i, :), 'LineWidth', 2, ...
        'DisplayName', sprintf('\\lambda=%.3f', damping_values(i)));
    handles_cn = [handles_cn; h];
end
title('Control Signal Norm');
xlabel('Time (s)');
ylabel('Signal Norm');
legend show;
hold off;

% 添加"取消全部显示"复选框
all_checkbox = uicontrol('Style', 'checkbox', ...
                         'String', 'Show All', ...
                         'Position', [20, 400, 150, 20], ...
                         'Value', 1, ...
                         'Callback', @(src, ~) toggle_all_visibility(src.Value, handles_tn, handles_cn));

% 添加每条曲线的复选框控件
for i = 1:length(damping_values)
    uicontrol('Style', 'checkbox', ...
        'String', sprintf('Show \\lambda=%.3f', damping_values(i)), ...
        'Position', [20, 400 - 30 * i, 150, 20], ...
        'Value', 1, ...
        'Callback', @(src, ~) toggle_visibility(src.Value, handles_tn(i), handles_cn(i)));
end


% 辅助函数：单独切换曲线可见性
function toggle_visibility(value, h1, h2)
    if value
        set(h1, 'Visible', 'on');
        set(h2, 'Visible', 'on');
    else
        set(h1, 'Visible', 'off');
        set(h2, 'Visible', 'off');
    end
end

% 辅助函数：切换全部曲线可见性
function toggle_all_visibility(value, handles_tn, handles_cn)
    for i = 1:length(handles_tn)
        if value
            set(handles_tn(i), 'Visible', 'on');
            set(handles_cn(i), 'Visible', 'on');
        else
            set(handles_tn(i), 'Visible', 'off');
            set(handles_cn(i), 'Visible', 'off');
        end
    end
end

function [task_space_error_norm, control_signal_norm] = compute_norms(damping, theta_i, td, tau, final_time, eta)
    include_namespace_dq;
    seven_dof_planar_robot = SevenDofPlanarRobotDH.kinematics();
    translation_controller = DQ_PseudoinverseController(seven_dof_planar_robot);
    translation_controller.set_control_objective(ControlObjective.Translation);
    translation_controller.set_gain(eta);
    translation_controller.set_damping(damping);
    q = ones(7, 1) * theta_i;
    n_steps = length(0:tau:final_time);
    task_space_error_norm = zeros(1, n_steps);
    control_signal_norm = zeros(1, n_steps);
    for step = 1:n_steps
        u = translation_controller.compute_setpoint_control_signal(q, vec4(td));
        task_space_error_norm(step) = norm(translation(seven_dof_planar_robot.fkm(q)) - td).q(1);
        control_signal_norm(step) = norm(u);
        q = q + u * tau;
    end
end

Bonus Homework

1. 证明 $A{A}^{†}=U\Sigma V^{T}V{\Sigma }^{†}{U}^T=I_{m\times m}$：

第一步：奇异值分解（SVD）

对于矩阵 $A\in {\mathbb{R}}^{m\times n}$，其中 $m\le n$ 且 $\text{rank}(A)=m$，我们可以对 $A$ 进行奇异值分解：

$$A=U\Sigma V^T,$$

其中：

• $U\in {\mathbb{R}}^{m\times m}$：正交矩阵（满足 $U^{T}U=I_{m\times m}$），
• $\Sigma \in {\mathbb{R}}^{m\times n}$：对角矩阵，对角线上是 $A$ 的奇异值，
• $V\in {\mathbb{R}}^{n\times n}$：正交矩阵（满足 $V^{T}V=I_{n\times n}$）。

第二步：伪逆矩阵的定义

$A$ 的伪逆矩阵 $A^{†}$ 定义为：

$$A^{†}=V{\Sigma }^{†}{U}^T,$$

其中 ${\Sigma }^{†}$ 是 $\Sigma$ 的伪逆，形式如下：

$${\Sigma }^{†}=\left[\begin{array}{cc}{\Sigma }_m^{-1}& 0\\ 0& 0\end{array}\right],$$

其中 ${\Sigma }_m^{-1}$ 是 $\Sigma$ 中 $m\times m$ 非零子矩阵的逆。

第三步：计算 $A{A}^{†}$

将 $A$ 和 $A^{†}$ 的定义代入：

$$A{A}^{†}=(U\Sigma V^T)(V{\Sigma }^{†}{U}^T).$$

由于 $V^{T}V=I$，可以简化为：

$$A{A}^{†}=U\Sigma {\Sigma }^{†}{U}^T.$$

第四步：简化 $\Sigma {\Sigma }^{†}$

计算 $\Sigma {\Sigma }^{†}$：

$$\Sigma {\Sigma }^{†}=\left[\begin{array}{cc}{\Sigma }_m& 0\\ 0& 0\end{array}\right]\left[\begin{array}{cc}{\Sigma }_m^{-1}& 0\\ 0& 0\end{array}\right]=\left[\begin{array}{cc}{I}_m& 0\\ 0& 0\end{array}\right].$$

第五步：代回 $A{A}^{†}$

将结果代回：

$$A{A}^{†}=U\left[\begin{array}{cc}{I}_m& 0\\ 0& 0\end{array}\right]U^T.$$

因为 $U$ 是正交矩阵，最终结果为：

$$A{A}^{†}=I_{m\times m}.$$

2. 证明 $B^{T}B+\lambda I$ 总是可逆：

第一步：问题背景

给定矩阵 $B\in {\mathbb{R}}^{m\times n}$，其中 $m\le n$ 且 $\text{rank}(B)<m$。需要证明 $B^{T}B+\lambda I$ 在 $\lambda >0$ 时总是可逆。

第二步：矩阵 $B^{T}B$ 的特性

矩阵 $B^{T}B$ 是对称的，且是半正定的，即：

$$x^T(B^{T}B)x\ge 0,\forall x\in {\mathbb{R}}^n.$$

因此，矩阵 $B^{T}B$ 的所有特征值 ${\lambda }_i$ 满足 ${\lambda }_i\ge 0$。

第三步：考虑 $B^{T}B+\lambda I$

我们需要证明 $B^{T}B+\lambda I$ 是可逆的，即证明其特征值全为非零。

令矩阵 $B^{T}B$ 的特征分解为：

$$B^{T}B=Q\Lambda Q^T,$$

其中：

• $Q$ 是正交矩阵，
• $\Lambda$ 是 $B^{T}B$ 的特征值矩阵，对角线上为 ${\lambda }_i\ge 0$。

将 $B^{T}B+\lambda I$ 表示为：

$$B^{T}B+\lambda I=Q(\Lambda +\lambda I)Q^T.$$

第四步：分析新的特征值

矩阵 $\Lambda +\lambda I$ 的特征值是 ${\lambda }_i+\lambda$，其中 $\lambda >0$。因此：

$${\lambda }_i+\lambda >0\forall i.$$

第五步：矩阵的可逆性

如果矩阵的所有特征值都为正，则该矩阵一定是可逆的。因此，$B^{T}B+\lambda I$ 是可逆的。

结论

1. $A{A}^{†}=I_{m\times m}$，适用于任何 $A\in {\mathbb{R}}^{m\times n}$ 且 $\text{rank}(A)=m$ 的情况。
2. $B^{T}B+\lambda I$ 在 $\lambda >0$ 时是可逆的。