阻尼伪逆使系统在任务空间奇异性方面具有一定的鲁棒性
阻尼伪逆
阻尼伪逆是SVD(奇异值分解)逆矩阵的一种有趣替代方法,它使系统在任务空间奇异性方面具有一定的鲁棒性。其主要思想是对任意(可能为奇异的)矩阵 B ∈ R m × n B \in \mathbb{R}^{m \times n} B∈Rm×n,使用以下伪逆矩阵,称为阻尼伪逆(damped-pseudo inverse):
B + ≜ ( B T B + λ 2 I ) − 1 B T B^+ \triangleq (B^T B + \lambda^2 I)^{-1} B^T B+≜(BTB+λ2I)−1BT
其中, λ ∈ R − { 0 } \lambda \in \mathbb{R} - \{0\} λ∈R−{0}是阻尼因子。这个阻尼因子的含义将在下一节中更清楚地解释,但目前只需知道以下结论:
( B T B + λ 2 I ) − 1 (B^T B + \lambda^2 I)^{-1} (BTB+λ2I)−1
始终存在,因此可以使用常规的逆矩阵计算算法来求解阻尼伪逆。
阻尼伪逆与奇异性鲁棒性
从基于雅可比矩阵的机器人控制的角度来看,阻尼因子让我们能够平衡以下两者之间的权衡关系:最优关节速度(在任务误差减少的意义上)和关节速度范数。
如果选择较大的阻尼因子,机器人对任务空间奇异性会更鲁棒,但可能需要更长时间才能达到期望的任务空间值。随着阻尼因子的降低,其行为会更接近于基于SVD的伪逆。如果阻尼因子太小,当机器人接近奇异配置时,关节速度也可能变得危险。
Lesson 7
代码
matlab">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 valueset(h1, 'Visible', 'on');set(h2, 'Visible', 'on');elseset(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 valueset(handles_tn(i), 'Visible', 'on');set(handles_cn(i), 'Visible', 'on');elseset(handles_tn(i), 'Visible', 'off');set(handles_cn(i), 'Visible', 'off');endend
endfunction [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_stepsu = 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 Σ V T V Σ † U T = I m × m A A^\dagger = U \Sigma V^T V \Sigma^\dagger U^T = I_{m \times m} AA†=UΣVTVΣ†UT=Im×m:
第一步:奇异值分解(SVD)
对于矩阵 A ∈ R m × n A \in \mathbb{R}^{m \times n} A∈Rm×n,其中 m ≤ n m \leq n m≤n 且 rank ( A ) = m \text{rank}(A) = m rank(A)=m,我们可以对 A A A 进行奇异值分解:
A = U Σ V T , A = U \Sigma V^T, A=UΣVT,
其中:
- U ∈ R m × m U \in \mathbb{R}^{m \times m} U∈Rm×m:正交矩阵(满足 U T U = I m × m U^T U = I_{m \times m} UTU=Im×m),
- Σ ∈ R m × n \Sigma \in \mathbb{R}^{m \times n} Σ∈Rm×n:对角矩阵,对角线上是 A A A 的奇异值,
- V ∈ R n × n V \in \mathbb{R}^{n \times n} V∈Rn×n:正交矩阵(满足 V T V = I n × n V^T V = I_{n \times n} VTV=In×n)。
第二步:伪逆矩阵的定义
A A A 的伪逆矩阵 A † A^\dagger A† 定义为:
A † = V Σ † U T , A^\dagger = V \Sigma^\dagger U^T, A†=VΣ†UT,
其中 Σ † \Sigma^\dagger Σ† 是 Σ \Sigma Σ 的伪逆,形式如下:
Σ † = [ Σ m − 1 0 0 0 ] , \Sigma^\dagger = \begin{bmatrix} \Sigma_m^{-1} & 0 \\ 0 & 0 \end{bmatrix}, Σ†=[Σm−1000],
其中 Σ m − 1 \Sigma_m^{-1} Σm−1 是 Σ \Sigma Σ 中 m × m m \times m m×m 非零子矩阵的逆。
第三步:计算 A A † A A^\dagger AA†
将 A A A 和 A † A^\dagger A† 的定义代入:
A A † = ( U Σ V T ) ( V Σ † U T ) . A A^\dagger = (U \Sigma V^T)(V \Sigma^\dagger U^T). AA†=(UΣVT)(VΣ†UT).
由于 V T V = I V^T V = I VTV=I,可以简化为:
A A † = U Σ Σ † U T . A A^\dagger = U \Sigma \Sigma^\dagger U^T. AA†=UΣΣ†UT.
第四步:简化 Σ Σ † \Sigma \Sigma^\dagger ΣΣ†
计算 Σ Σ † \Sigma \Sigma^\dagger ΣΣ†:
Σ Σ † = [ Σ m 0 0 0 ] [ Σ m − 1 0 0 0 ] = [ I m 0 0 0 ] . \Sigma \Sigma^\dagger = \begin{bmatrix} \Sigma_m & 0 \\ 0 & 0 \end{bmatrix} \begin{bmatrix} \Sigma_m^{-1} & 0 \\ 0 & 0 \end{bmatrix} = \begin{bmatrix} I_m & 0 \\ 0 & 0 \end{bmatrix}. ΣΣ†=[Σm000][Σm−1000]=[Im000].
第五步:代回 A A † A A^\dagger AA†
将结果代回:
A A † = U [ I m 0 0 0 ] U T . A A^\dagger = U \begin{bmatrix} I_m & 0 \\ 0 & 0 \end{bmatrix} U^T. AA†=U[Im000]UT.
因为 U U U 是正交矩阵,最终结果为:
A A † = I m × m . A A^\dagger = I_{m \times m}. AA†=Im×m.
2. 证明 B T B + λ I B^T B + \lambda I BTB+λI 总是可逆:
第一步:问题背景
给定矩阵 B ∈ R m × n B \in \mathbb{R}^{m \times n} B∈Rm×n,其中 m ≤ n m \leq n m≤n 且 rank ( B ) < m \text{rank}(B) < m rank(B)<m。需要证明 B T B + λ I B^T B + \lambda I BTB+λI 在 λ > 0 \lambda > 0 λ>0 时总是可逆。
第二步:矩阵 B T B B^T B BTB 的特性
矩阵 B T B B^T B BTB 是对称的,且是半正定的,即:
x T ( B T B ) x ≥ 0 , ∀ x ∈ R n . x^T (B^T B) x \geq 0, \quad \forall x \in \mathbb{R}^n. xT(BTB)x≥0,∀x∈Rn.
因此,矩阵 B T B B^T B BTB 的所有特征值 λ i \lambda_i λi 满足 λ i ≥ 0 \lambda_i \geq 0 λi≥0。
第三步:考虑 B T B + λ I B^T B + \lambda I BTB+λI
我们需要证明 B T B + λ I B^T B + \lambda I BTB+λI 是可逆的,即证明其特征值全为非零。
令矩阵 B T B B^T B BTB 的特征分解为:
B T B = Q Λ Q T , B^T B = Q \Lambda Q^T, BTB=QΛQT,
其中:
- Q Q Q 是正交矩阵,
- Λ \Lambda Λ 是 B T B B^T B BTB 的特征值矩阵,对角线上为 λ i ≥ 0 \lambda_i \geq 0 λi≥0。
将 B T B + λ I B^T B + \lambda I BTB+λI 表示为:
B T B + λ I = Q ( Λ + λ I ) Q T . B^T B + \lambda I = Q (\Lambda + \lambda I) Q^T. BTB+λI=Q(Λ+λI)QT.
第四步:分析新的特征值
矩阵 Λ + λ I \Lambda + \lambda I Λ+λI 的特征值是 λ i + λ \lambda_i + \lambda λi+λ,其中 λ > 0 \lambda > 0 λ>0。因此:
λ i + λ > 0 ∀ i . \lambda_i + \lambda > 0 \quad \forall i. λi+λ>0∀i.
第五步:矩阵的可逆性
如果矩阵的所有特征值都为正,则该矩阵一定是可逆的。因此, B T B + λ I B^T B + \lambda I BTB+λI 是可逆的。
结论
- A A † = I m × m A A^\dagger = I_{m \times m} AA†=Im×m,适用于任何 A ∈ R m × n A \in \mathbb{R}^{m \times n} A∈Rm×n 且 rank ( A ) = m \text{rank}(A) = m rank(A)=m 的情况。
- B T B + λ I B^T B + \lambda I BTB+λI 在 λ > 0 \lambda > 0 λ>0 时是可逆的。