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

ops/2024/11/25 2:34:37/

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

阻尼伪逆

阻尼伪逆是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/ops/136452.html

相关文章

使用线程局部存储解决ffmpeg中多实例调用下自定义日志回调问题

1 问题描述 最近在封装一个库&#xff0c;调用方传入一个URL及对应的回调后就开始执行ffmpeg拉流硬解码硬件格式转换&#xff0c;并将得到的数据帧通过回调传递给调用方&#xff1b;除了数据帧回调外&#xff0c;还有日志回调用来传递一些调试信息。 因为该封装库可能被一个进…

常用Rust日志处理工具教程

在本文中&#xff0c;我想讨论Rust中的日志。通过一些背景信息&#xff0c;我将带您了解两个日志库&#xff1a;env_logger和log4rs。最后&#xff0c;我将分享我的建议和github的片段。 Rust log介绍 log包是Rust中日志API的事实标准&#xff0c;共有五个日志级别&#xff1…

Linux环境开启MongoDB的安全认证

在Linux环境下启动MongoDB并启用安全认证&#xff0c;你需要进行以下步骤&#xff1a; 1.创建MongoDB的配置文件&#xff08;如果还没有的话&#xff09;。 2.编辑配置文件以启用认证。 3.重启MongoDB服务。 以下是具体步骤的示例&#xff1a; 创建MongoDB配置文件&#xf…

基于YOLOv8深度学习的农作物番茄成熟度检测系统研究与实现(PyQt5界面+数据集+训练代码)

本研究详细研究并实现了一种基于深度学习的农作物番茄成熟度检测系统&#xff0c;旨在为现代农业提供智能化、自动化的解决方案。随着农业智能化的发展&#xff0c;传统的农作物成熟度检测方法面临着效率低、依赖人工经验等问题&#xff0c;而本系统通过结合先进的YOLOv8目标检…

Python网络爬虫技术及其应用

Python网络爬虫技术及其应用 在当今数字化时代&#xff0c;互联网已经成为信息传播的主要渠道。海量的数据每天都在互联网上产生&#xff0c;这些数据对于企业决策、市场分析、科学研究等有着极其重要的价值。然而&#xff0c;如何高效地收集并利用这些数据成为了一个挑战。Py…

【MySQL数据库】C#实现MySQL数据库最简单的查询和执行函数

文章目录 前言一、查询方法二、执行方法 前言 C#和MySQL数据库是常见的数据交互&#xff0c;标准的查询和执行方法如下&#xff0c;做个记录。 一、查询方法 private static int QueryTable(string tableName, DateTime today, string stepName){int result 0; // 返回数据…

用Python爬虫“偷窥”1688商品详情:一场数据的奇妙冒险

引言&#xff1a;数据的宝藏 在这个信息爆炸的时代&#xff0c;数据就像是一座座等待挖掘的宝藏。而对于我们这些电商界的探险家来说&#xff0c;1688上的商品详情就是那些闪闪发光的金子。今天&#xff0c;我们将化身为数据的海盗&#xff0c;用Python这把锋利的剑&#xff0…

ROS VRRP软路由双线组网方式

虚拟路由冗余协议 Virtual Router Redundancy Protocol (VRRP)&#xff0c;MikroTik RouteROS VRRP 协议遵循 RFC 2338。 VRRP 协议是保证访问一些资源不会中断&#xff0c;即通过多台路由器组成一个网关集合&#xff0c;如果其中一台路由器出现故障&#xff0c;会自动启用另外…