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

news/2024/11/23 17:12:31/

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

阻尼伪逆

阻尼伪逆是SVD(奇异值分解)逆矩阵的一种有趣替代方法,它使系统在任务空间奇异性方面具有一定的鲁棒性。其主要思想是对任意(可能为奇异的)矩阵 B ∈ R m × n B \in \mathbb{R}^{m \times n} BRm×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} ARm×n,其中 m ≤ n m \leq n mn 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} URm×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} VRn×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}, Σ=[Σm1000],

其中 Σ m − 1 \Sigma_m^{-1} Σm1 Σ \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][Σm1000]=[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} BRm×n,其中 m ≤ n m \leq n mn 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)x0,xRn.

因此,矩阵 B T B B^T B BTB 的所有特征值 λ i \lambda_i λi 满足 λ i ≥ 0 \lambda_i \geq 0 λi0

第三步:考虑 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 λi0

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+λ>0i.

第五步:矩阵的可逆性

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


结论

  1. 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} ARm×n rank ( A ) = m \text{rank}(A) = m rank(A)=m 的情况。
  2. B T B + λ I B^T B + \lambda I BTB+λI λ > 0 \lambda > 0 λ>0 时是可逆的。

http://www.ppmy.cn/news/1549329.html

相关文章

ElasticSearch学习篇18_《检索技术核心20讲》LevelDB设计思想

目录 一些常见的设计思想以及基于LSM树的LevelDB是如何利用这些设计思想优化存储、检索效率的。 几种常见的设计思想 索引和数据分离减少磁盘IO读写分离分层思想 LevelDB的设计思想 读写分离设计分层设计与延迟合并LRU缓存加速检索 几种常见设计思想 索引与数据分离 索引…

【openwrt】Openwrt系统的reboot流程

reboot流程 应用层reboot流程内核reboot流程参考 当我们在openwrt系统的终端中敲下 reboot指令后,系统需要进行一系列动作后&#xff0c;才会真正的进行硬件重启。而这一系列的动作可以分为——应用程序的停止和内核的停止。在Openwrt系统中&#xff0c;应用程序的停止是1号进…

C++ 并发专题 - 线程安全的单例模式

一&#xff1a;概述&#xff1a; 在C编程中&#xff0c;call_once 是一种机制&#xff0c;用于确保某个函数或代码段在多线程环境下仅被调用一次。这种机制常用于初始化资源、配置全局变量或执行只需执行一次的逻辑。在 C11 标准中&#xff0c;std::call_once 是由标准库提供的…

Java SE 与 Java EE:基础与进阶的探索之旅

在编程世界中&#xff0c;Java语言以其跨平台、面向对象、丰富的类库等特点&#xff0c;成为了众多开发者和企业的首选编程语言。而Java SE与Java EE&#xff0c;作为Java平台的两个重要组成部分&#xff0c;各自承载着不同的使命&#xff0c;同时又紧密相连&#xff0c;共同构…

Elasticsearch客户端在和集群连接时,如何选择特定的节点执行请求的?

大家好&#xff0c;我是锋哥。今天分享关于【Elasticsearch客户端在和集群连接时&#xff0c;如何选择特定的节点执行请求的&#xff1f;】面试题。希望对大家有帮助&#xff1b; Elasticsearch客户端在和集群连接时&#xff0c;如何选择特定的节点执行请求的&#xff1f; 100…

前端知识点---rest(javascript)

文章目录 前端知识点---rest(javascript)rest的用法基本语法特点使用场景与扩展运算符&#xff08;spread&#xff09;区别小练习 前端知识点—rest(javascript) rest出现于ES2015 function doSum(a,b, ...args) //示例中的args就是一个rest参数 //它会将后续的所有参数存储…

AI技术在电商行业中的应用面临哪些挑战?

AI技术在电商行业的应用尽管具有巨大潜力&#xff0c;但也面临以下主要挑战&#xff1a; 挑战 1. 数据质量与隐私问题 数据量与质量不均&#xff1a;电商平台需要大量高质量数据来训练AI模型&#xff0c;但数据可能不完整、不准确&#xff0c;或存在偏差&#xff0c;影响AI预…

概率论和数理统计知识点汇总——第二章随机变量的分布与数字特征

2.1 随机变量及其分布 1.随机变量的概念 定义2.1 定义在概率空间(Ω,P)上,取值为实数的函数xx(ω)(w∈Ω)称为(Ω,P)上的一个随机变量.) 基本事件:Xa 复合事件:X 2.离散型随机变量的概率分布 定义&#xff1a;X的全部可能取值只有有限个或可数无穷多个 性质&#xff1a; 3…