机器人动力学与参数辨识学习笔记（一）

---

近来由于课题需要，老师给报名了睿慕课的《快速搭建“机器人动力学-参数辨识-轨迹规划-运动控制”的完整框架》这个课程
→→→快速搭建“机器人动力学-参数辨识-轨迹规划-运动控制”的完整框架

其实我是学习过一些机器人学以及了解过机器人动力学一些知识的，但是考虑到睿慕课学院的课程比较成体系，而且会有一些实践性比较强的练习讲解，所以打算再认真学习学习。

毕竟这是个付费课程，我也没法把课程内容都搬过来，所以这里尽量记录一些我的理解和练习。
那么开始

❤ 2022.8.1 ❤
其实我是8.3才开始学的，但是课程是8.1号开始的，所以凑个整。。。
话说昨天一天都在关心台海局势，无心学习。。。

第一章 概述

△ 机器人控制器整体架构介绍

啊，就是个科普，再加上推销主讲人自家公司的产品，没啥好写的，略

我打算写一个《从零开始搭建CoDeSys机器人》的文章，但考虑到时间精力的问题可能会拖得比较久。。。嗯。。。具体的再说吧。

△ 机器人动力学的作用及重要性

这部分还是以科普为主，主讲人提出了机器人动力学控制的四层塔式架构

△ 机器人动力学应用

○ 动力学方程的四种形式

· 拉格朗日形式

· 牛顿-欧拉形式

上面两种方式是等效的，可以互相转化，拉格朗日形式比较紧凑，但是牛顿-欧拉形式运算速度更快

· 参数分离形式

· 最小参数集

○ 机器人动力学参数辨识

参数辨识主要针对动力学方程的 参数分离形式 及 最小参数集 中的$\mathbf{p}$矩阵，

· 机器人动力学参数辨识流程

○ 轨迹规划

· 最优调速规划

· 最优时间规划

关于轨迹规划这里还看不懂，希望后面能有详细的讲解哈

△ 预备知识及相关教材

预备知识主要是一些很基础的高数、线代计算以及力学、控制论的知识（至少主讲人是这样讲的），这里我就不贴了。

关于教材有这么几本推荐

---

△ 第一章作业 使用Simscape搭建二连杆模型并操作

作业要求使用Simulink中的Simscape模块完成，说到Simscape，其实本应在MBD的课程里学会的，但是。。。反正现在得从头学起

○ Simscape基本操作学习

这部分内容就是官方的教程内容
→→→https://ww2.mathworks.cn/help/physmod/sm/getting-started-with-simmechanics.html

· 建立模型及参数设置

首先在命令窗口输入“smnew”，如果没有安装Simscape模块，Matlab会提示安装，如果安装了则会打开如下所示的Simulink界面

〇 建立简单连杆模型
删除用不到的模块后，使用坐标转换模块和刚体模块组成连杆模型。

刚体模块：

坐标转换模块：

由于刚体坐标系默认位于其重心，需要建立前端至重心，重心至末端两个坐标转换。因此将坐标转
换模块复制后翻转，将两个B端连接至刚体模块处。

为什么要这样我也不知道

然后参考表格设置属性

因为属性设置里面有未定义的变量，所以会变红

〇 将模型设置为子系统
将上述连杆模型的两个坐标转换与刚体选中，右键选择Create Subsystem from Selection将其创建为一个子系统

然后双击打开子系统，在conn1上双击，然后这里选择left，让连接点在左边

将子系统连接到基坐标上

然后

输入参数窗口

点击播放

然后是这样的效果

上面是建立连杆的基础操作，下面用一个实例来练习

· 建立简单倒立摆模型

〇 建立模型
新建一个模板，把刚刚建立的简单连杆模型复制进来，然后在Library Browser中选择Simscape > Multibody >
Joints，添加一个转动关节，并将各模块连接好如图

然后修改基座刚体模块参数：

· 运行简单倒立摆模型

运行之前需要进行一些初始配置

〇 设置重力
由于转动关节默认绕Z轴转动，为了保证重力与转动轴方向垂直，在本教程中将重力改
为沿Y轴方向：双击机制配置模块，设置Uniform Gravity > Gravity参数至[0 -9.81 0]

〇 更改倒立摆初始位置
由于连杆位置由关节决定，双击转动关节，设置State Targets > Position改变位置

〇 设置求解器

以上是资料里面写的，但是我找了半天没找到这个Modeling标签，于是我找了matlab官网的教程，英文原版也是这么写的
→→→https://ww2.mathworks.cn/help/physmod/sm/gs/model-pendulum.html

这就很奇怪，然后我在我的Simulink（2018b版本）里面找了半天相关的设置，找到了这个


看来官方教程的版本和我的不太一样。。。可能是我的版本太旧了吧。。。
不过总算是找到了

〇 更新模型
在菜单标签页Modeling中点击Update Model

这个选项在我的里面也没有，不过不影响

最后运行
这个杆就开始摆起来了（开摆。。。）

· 获取简单倒立摆模型运动状态

〇 添加传感器模块
在上一节建立的倒立摆模型中，双击转动关节模块，在Sensing菜单中选择输出Position与Velocity，模块会增加两个信号端口q与w，输出为倒立摆的关节位置与速度。

然后在模型中添加如下两钟模块

双击To Workspace模块，分别设置两个模块的Variable Name为q与w，并通过PS-Simlink Converter模块，连接至关节模块的相应信号端口q与w，将关节位置与速度输出至MATLAB主工作空间。

说起来，虽然我以前用过Simulink来做仿真，但是还是第一次知道Simulink向Matlab输送变量需要这样设置

· 分析简单倒立摆模型

〇 得到位置和速度的变化曲线

figure; % 打开一个新图像 
hold on; 
plot(q); % 倒立摆关节位置曲线 
plot(w); % 倒立摆关节速度曲线

结果

〇 关节在0°作为初始角度的相图

figure; 
plot(q.data, w.data);

结果

奇怪 为啥我跟材料里给出的不太一样。。。

然后修改转动关节的初始值可以改变图形

〇 为转动关节增加阻尼
双击转动关节模块，修改State Targets > Position为0°，并设置Internal Mechanics >Damping为8e-5(N*m)/(deg/s)，为倒立摆增加阻尼系数，增加运动过程中的能量消耗，会使得倒立摆震荡幅度随时间逐渐减小

figure; % 打开一个新图像 
hold on; 
plot(q); % 倒立摆关节位置曲线 
plot(w); % 倒立摆关节速度曲线

这时候再这样

figure; 
plot(q.data, w.data);

嗯。。。有意思

· 控制简单倒立摆模型

通过关节对其施加力或力矩来控制模型的运动状态：

双击转动关节模块，设置Actuation > Torque为Provided by Input，模块会增加一个用于力矩输入的信号端口

然后添加如下两种模块

连接

双击Sine Wave模块，设置Amplitude为0.06，此时模块会产生-0.06N至0.06N的正弦力矩

双击转动关节模块，修改State Targets > Position为0°

然后还是重复上面的绘图指令

figure; % 打开一个新图像 
hold on; 
plot(q); % 倒立摆关节位置曲线 
plot(w); % 倒立摆关节速度曲线

figure; 
plot(q.data, w.data);

好了 教程到此结束！

---

○ 第一章作业

作业要求


二连杆如图所示

· 搭建二连杆模型

新建Simscape模板，然后直接把刚才建立的简单连杆模型复制进来，添加旋转关节，另外根据要求，基坐标和世界坐标有个转换关系，所以连接如图

【！！！】这里其实写错了，基坐标的转换矩阵位置不对，后面已经改正，这里为保证真实记录就不改了

然后就是设置属性。

在这里遇到一个问题，教程里面给出的是长宽高尺寸和密度，也就是说认为连杆是均质的，但是作业里面给出了杆长和重心位>置以及各截面的惯量，那么该怎么配置呢？
经过百度我找到了这个教程
→→→Simscape Multibody Solid和Rigid Transform模块说明 说明
需要更改一下属性inertia→type为custom，然后在里面设置就可以

※【这里我非常细心的发现了，材料里给出的转动惯量向量是**[Ixx,Iyy,Izz,Ixy,Ixz,Iyz]**，而要求的输入格式是 Moments of Inertia:[Ixx,Iyy,Izz];Products of Inertia:[Iyz,Ixz,Ixy]，是资料里面给错了还是就是需要调整一下呢？我觉得可以问一问老师】
※※【但是我看了数据。。。那两个都是0.。。】

而且因为这里直接输入了质量，所以不能再用之前的密度来计算了，要把之前的变量删掉，不然会报错，参考默认设置，直接输入1000（反正用不到）（应该吧。。。）

然后是重力

然后调整下基座的尺寸

关于基坐标和世界坐标的变换
关于怎么设置，虽然没查资料，但我觉得是这样

运行

奇怪，怎么是朝向Z+方向的。。。我再调整下

【！！！】这里其实是因为我把坐标转换模块的方向弄错了，已经在后面改过来了，但是为了保证过程真实，这里就不改了

嗯，这样就对了

这样看不出来是两个杆，我换个颜色

好，完成！

· 将三维模型赋予二连杆

根据之前MBD课程的课件，在Solid模块里选择from file

这里我选择from file之后报错了，大概意思是因为改变了物理尺寸，所以重心位置不对了，这里我选择custom试试

然后连杆1

连杆2

运行

emmm。。。。有点小问题，我猜是我的坐标转换模块位置放错了，甚至方向也放错了，怪不得我直接按照要求的参数做不成功，改一改

然后参数改成和资料里一样

运行

诶嘿，还是有问题
经过观察，发现是第一个连杆的模型重心和参数不匹配，模型的坐标原点并不在中间

将几何中心修改成重心位置试试
连杆1：

连杆2：

运行

嗯 这样就对了
好，搞定！

· 将二连杆模型封装为子系统

题目要求里说输入为控制（位置/力矩）指令 c，输出为关节角度q、关节速度 qp、关节加速度 qpp、关节力矩 qt 与末端位姿，这里没说要用哪个关节作为控制，而且后面的题目有要求对两个关节同时控制，所以我就把两个关节都引出来
子系统如下：

主系统如下：

那么，问题来了，怎么输出末端位姿。。。
我当然知道末端位姿是可以算出来的，但是。。。懒。。。
所以我找了一些资料，知道这个模块可以计算出坐标

资料在这里
→→→Simscape Multibody – 使用Transform Sensor 测量运动
但是文章里只说可以显示坐标，没说可以显示欧拉角，我来试试

查了一下帮助文档，坐标转换传感器模块是可以显示角度的，在这里选择Axis即可，但是和题目要求的YPR是不是等同我也不太确定
【这里可以问问老师】

修改后的主系统如下，我把角度和坐标放在了一个向量里面

搞定！

· 关节位置控制

题目要求将关节 Actuation 设置为位置控制，Damping 设置为 1N*m，控制指令输入设置为幅值 0.5， 频率 1Hz 的正弦信号进行仿真，输出位置及力矩图像（提示：需要更改 Simulink-PS Converter 输入为二阶 Filter Input）
在这里没有说对哪个关节施加控制，那我就当成对两个关节同时控制了。
把关节输入修改为位置控制

修改关节阻尼

添加正弦输入模块和转换模块

修改正弦输入参数

然后按照提示修改转换模式

【为什么要这样设置呢？？？】

设置运行时间10s，然后运行

这里报了个错

看了一下，大概是关节力矩设置的问题，我把输入模式改为位置输入之后，就把力矩输入选了none，看来是不行的，于是选成了如下所示

就好了~

再运行
OK~
这样的效果

然后把位置和力矩输出出来

figure;
plot(q1,'r')
hold on
plot(t1,'b')
legend('位置','力矩')
title('关节1位置和力矩')

figure;
plot(q2,'r')
hold on
plot(t2,'b')
legend('位置','力矩')
title('关节2位置和力矩')

得到曲线如图


搞定！

· 关节力矩控制

〇 将关节 Actuation 设置为力矩控制，Damping 设置为 0.1N*m，控制指令输入设置为常数 0.0，输出关节位置图像

首先修改关节输入模式

修改阻尼

修改模型

运行程序

figure;
plot(q1,'r')
hold on;
plot(q2,'b')
title('关节1、关节2位置图像')
legend('关节1','关节2')

得到曲线

〇 将关节 Actuation 设置为力矩控制，Damping 设置为 0.1N*m，关节一控制指令输入设置为常数 1.0，关节二控制指令输入设置为常数 0.3，输出关节位置图像
和上面差不多，改一下参数

曲线

感觉没什么变化呀

好吧，第一章的作业就完了，下面等公布答案之后再来订正。

○ 第一章作业答案及分析

第一章作业基本都是比较基础的操作练习，参考答案也是给了一个最终完成的模型，这里比较一下答案的实现方式和我自己做的之间的差异。
首先答案中将整个二连杆模型和基座都封装在了一个子系统里，只把输入信号和输出信号引了出来，我只是把二连杆封装了起来，相对来说答案的处理更简洁

打开子系统，可以看到答案的结构和我自己摸索做出来的基本是相同的，证明了我的思路和操作是没有问题的，但是答案中结构和排布逻辑更简洁明了，值得学习。

打开连杆模型，答案的模块比教程中多了一个Reference Frame的模块，不太清楚是做什么用的，可能以后会用到吧。

然后我尝试运行参考答案模型，但是出现了问题。
报错：

Error in ‘TwoLinkRobot/Robot/PS-Simulink Converter’: Initialization commands cannot be evaluated.
Caused by:
Unit name, ‘inherit’, is not recognized
Component:Simulink | Category:Block error

我百度了一下，感觉是matlab版本的问题。

→→→Why do I receive an error when saving a Simscape model as a previous version and trying to compile it in that version?

大概意思就是从matlab2019b版本开始用了某种新的机制，所以用新版本生成的文件在老版本里就运行不了了。
还好matlab可以支持多版本共存，于是我安装了2021b版本。
于是能正常运行了。

和我不一样的是，参考答案里并没有把数据传输到matlab里面去，而是用scope模块直接显示曲线。

结果是差不多的啦~

---

❤ 2022.8.20 ❤

第二章 机器人动力学建模

参考教材：

△ 理论力学回顾

○ 牛顿力学

· 质点平动

· 固定轴转动

· 刚体一般运动：质心平动+绕质心轴转动

○ 分析力学

· 牛顿力学（传统几何）→ 分析力学（解析几何）

· 刚体一般运动：质心平动+绕质心轴转动

〇 例子1

〇 例子2

△ 机器人动力学

分类：
〇 连杆（转动）动力学：多自由度，强耦合，高度非线性

〇 关节动力学：单自由度，解耦形式

○ 连杆动力学

· 局部DH坐标系

后面的页数是在教材的位置

※ 建立DH参数的时候要注意转角的正负号定义（按照上上图）

· 连杆惯性参数集

、

可以在solidworks等软件里查看

· 小结

□ 基于拉格朗日方程

针对一个机械臂

建立动力学方程步骤

DH参数示例

· 拉格朗日方程

$\mathrm{H}(\mathrm{q})$：3x3
$\mathrm{C}(\mathrm{q},\mathrm{q})$：3x3
$\mathrm{G}(\mathrm{q})$：重力矩阵 3x1

关于具体参数的示例：
〇 三连杆

〇 六连杆

□ 基于牛顿-欧拉方程

· 牛顿-欧拉和拉格朗日法的区别

牛顿-欧拉方法比拉格朗日法运算量小

【其实我没太看懂。。。】

□ 基于线性分离

将惯性参数集与状态变量这两个矩阵分开
惯性参数集就是前面的10个参数矩阵
状态变量矩阵是由机器人的运动状态决定的

证明过程参考教材

〇 惯性参数集可能遇到的问题

举例说明：
例1：垂直单摆模型

例2：水平单摆模型

结论：针对不同的运动状态，惯性参数集是确定的，但是最小惯性参数集是不一定的

□ 基于最小参数集

举例说明，PUMA560机器人的前三关节最小惯性参数集共有15个参数，其中有一部分是其他参数的线性组合

· 确定最小惯性参数集的方法

先列出基于惯性参数集的动力学方程，再确定一个C矩阵，使其与惯性参数集p相乘得到最小惯性参数集，然后再得到基于最小惯性参数集的动力学方程。
这里Yr满秩，如果不满秩则要用下面的方法降维处理。【这里没听懂。。。】

这个方法看着眼熟。。。
用这些方法处理不满秩的Y使其满秩。【？？？】

□ 连杆动力学仿真

---

○ 关节动力学

□ 刚性关节模型

· 刚性关节动力学建模方法

将关节当做刚性来处理

$I$：电流
${\tau }_f$：摩擦力，是建立关节模型最麻烦的一项。

· 摩擦力模型

在低速时，摩擦力更为复杂。。。

---

· 完整的动力学方程

${\mathbf{\tau }}_m$：电机力矩（关节）
${\mathbf{\tau }}_f$：关节摩擦力
$J^T$ ：外力

〇 线性分离形式的动力学方程

□ 柔性关节模型

· 柔性关节动力学建模方法

· 柔性关节完整动力学方程

△ 动力学正问题与逆问题

· 逆问题

已知状态，求力矩

现在计算机功能很强大，不用花里胡哨的优化。

· 正问题

已知当前状态，求下一个状态

引入状态变量，把二阶微分方程转换成一阶微分方程

最后一行这个公式是数值迭代法

给关节输入一个力矩，判断机器人如何运动，做仿真比较常用

〇 柔性关节的运动学正问题

---

△ 其他形式的动力学

· 连杆动力学

· 静态动力学

· 动态动力学

△ 深入学习

---

△ *拉格朗日迭代法

〇 经典力学

〇 推导出拉格朗日方程的两种途径

【关于达朗贝尔原理和哈密顿原理，找资料再学习学习】

○ 拉格朗日直接法

先表示出系统的总动能和总势能

将总动能和总势能带入拉格朗日方程，再排列组合，就得到机器人的动力学方程

○ 拉格朗日迭代法

如果连杆数量越多，计算就会越来越复杂，所以可以用拉格朗日迭代法

〇 列出系统的动能方程，拆分变量

首先将系统的总动能和势能分解成单个连杆的动能和势能，此时会得到一些未知量，这些量是与机械臂状态相关的量，会随着机械臂关节角的变化而变化。
分别是：连杆质心位置、质心的速度、质心角速度、转动惯量。
这些量的坐标系必须是惯性坐标系。

然后把这些量拆分成与关节角有关的坐标转换和与关节角无关的物理量

重心位置：质心相对于连杆坐标系的位置，经过基坐标喜到连杆坐标系的齐次变换，【这里讲的不清楚】
惯性矩阵：连杆自身的惯性矩阵一般建立在重心坐标系下的，用上面的公式可以转换到基坐标系下
重心的速度及角速度：对应的雅克比矩阵乘以关节角速度得到

移动关节和旋转关节的雅克比求解公式，也是和机械臂的状态相关的。
【这里也讲得不清楚，说的莫名其妙】

〇 将拆分后的变量带入系统动能方程

此时表达式就只和关节角度有关了
得到的惯性矩阵经过观察其形式发现其转秩是其本身，通过分析可知，动能方程只在速度为0的时候为0，因此惯性矩阵是个对称正定矩阵。

将动能由矩阵的形式转换成累加的形式，再带入拉格朗日量中

再带入拉格朗日方程

拉格朗日方程第一项：惯性项，第二项：科氏力（科里奥利力）与离心力项，第三项：重力项
【这里讲的也乱七八糟的】

化简

将科里奥利项转换成一个矩阵乘以一个角速度的形式

〇 总结

---

△ 第二章作业

○ 问答题

1. 求取二连杆机构的标准 DH 参数表（使用符号表示）

   【我的答案】
   

我记得DH模型是要固定一个位姿的呀，这也没给出图示，参考PPT给出的示例，大概就是这样吧

〇 参考答案

我写的对吧，应该是对的吧

2. 机器人动力学建模常用的两种方法是什么？请简述两者之间的联系与区别

   【我的答案】
   拉格朗日法和牛顿-欧拉法。
   联系是他们可以互相转化，是同一个方程的不同形式。
   区别是牛顿-欧拉法的运算速度更快。

大概还有其他的点，想不起来了

〇 参考答案

我写对了

3. 将课程中的刚性关节动力学与连杆动力学联立，写成一个完整的动力学方程
   $$(H(q)+J_m)\ddot{q}+C(\dot{q},q)+G(q)+{\tau }_f+J^{T}F={\tau }_m$$

这个我记得PPT里面给出来了，直接抄上去么？
是的，答案就是这个。。。】

4. 请简述机器人动力学模型的精度受什么因素影响较大

   【我的答案】
   受机器人的惯性参数集以及关节的转动惯量是否准确影响较大。

你想呀，要是这些基本参数都没弄对，那肯定精度不够呀，是吧。

〇 参考答案

啊这。。还是去问问可莉吧

○ 编程题

看到这些题，就像是老师刚讲完1+1，然后就让证明哥德巴赫猜想。。。
说起来，我不应该一道都不会，但是我确实是一道都不会。。。
直接搬答案和我的理解吧。。。

1. 在函数模板ForwordKinematicTemplate.m基础上完成二连杆正向运动学求解，并将函数及文件重命名为ForwardKinematic

函数模板如下：

% 二连杆正向运动学
function pe = ForwardKinematicTemplate(u)
%% 输入定义
q1=u(1);
q2=u(2);

%% 参数设置
LWx=0.0; % 第一关节相对基座x方向位移
LWz=0.0; % 第一关节相对基座z方向位移
L1=0.0;
L2=0.0;

%% 正运动学（欧拉角x-y-z）
x=0.0;
y=0.0;
z=0.0;
roll=0.0;
pitch=0.0;
yaw=0.0;

%% 输出定义
pe=[x;y;z;roll;pitch;yaw];

end

〇 参考答案

% 二连杆正向运动学
function pe = ForwardKinematic(u)
%% 输入定义
q1=u(1);
q2=u(2);

%% 参数设置
LWx=0.085;
LWz=0.075;
L1=0.4;
L2=0.4;

%% 正运动学（欧拉角x-y-z）
x=LWx;
y=L2*cos(q1 + q2) + L1*cos(q1);
z=LWz + L2*sin(q1 + q2) + L1*sin(q1);
roll=pi/2;
pitch=- q1 - q2;
yaw=pi/2;

%% 输出定义
pe=[x;y;z;roll;pitch;yaw];

end

〇 分析解析
串联机器人的正向运动学比较简单。
根据作业说明中对欧拉角的定义

可知，

2. 在 SIMULINK 中利用二连杆模型验证ForwardKinematic函数：在同一张图上表示连杆末端位置的函数输出与模型输出（旋转不做要求），并比较图像（提示：利用Interpreted MATLABFunction模块将函数加入模型中，同时将二连杆关节设置为位置控制，输入正弦指令，比较函数与模型输出）

3. 在函数模板JacobianTemplate.m基础上完成二连杆雅可比矩阵求解，并将函数及文件重命名为Jacobian

4. 在 SIMULINK 中利用二连杆模型验证Jacobian函数：在同一张图上表示连杆末端速度与角速度的函数求解（通过雅可比矩阵计算）与模型输出，并比较图像

5. 在脚本模板GenerateDynamicsTemplate.m基础上，使用任意一种动力学建模方程，计算二连杆动力学模型，并将文件重命名为GenerateDynamics（提示：符号变量常用函数：simplify()、diff()、*subs()*等）

PS:6-10题为进阶题，可选做（作为加分题）

6. 将上述动力学模型复制至函数模板DynamicsTemplate.m中，并将函数及文件重命名为Dynamics

7. 在 SIMULINK 中验证Dynamics函数：在同一张图上表示连杆关节角加速度的函数输出与模型输出，并比较图像（提示：将二连杆关节设置为力矩控制，关节Damping设置为零，输入力矩指令，比较函数与模型输出）

8. 在函数模板YrThetaTemplate.m的基础上，利用上述所得二连杆动力学模型，完成其最小惯性参数集计算，并将函数及文件重命名为YrTheta

9. 在函数模板InverseDynamicsTemplate.m基础上，利用最小惯性参数集完成二连杆逆向动力学模型计算，并将函数及文件重命名为InverseDynamics

10. 在 SIMULINK 中验证InverseDynamics函数：在同一张图上表示连杆关节力矩的函数输出与模型输出，并比较图像