Matlab 机器人动力学参数辨识仿真（1）单摆的动力学参数辨识

Matlab 机器人动力学参数辨识仿真（1）单摆的动力学参数辨识

0. 前言

本文我们研究怎么辨识一个单摆的动力学参数，通过对这个最简单的例子的研究我们可以感受一下机器人动力学参数辨识的流程，在后续的文章里面我们慢慢的进化到辨识复杂多关节的机械臂的动力学参数。 这里不妨回顾一下动力学参数辨识的理论步骤：

1. 推导结构的逆动力学方程
2. 对动力学方程进行线性化处理
3. 激励运动得到扭矩，关节位置，关节速度，关节加速度数据
4. 利用最小二乘法得到辨识出来的参数

本文用到的软件有：

1. Matlab R2019b
2. SolidWorks 2018

本文的所有模型文件与代码开源在我的github：https://github.com/julis-wolala/Pendulum-Parameter-Identification 记得给个star⭐

1. 理论准备

1.1 确定研究对象

我们的研究对象很简单，是一个单摆，即一个旋转电机固连一根连杆。下图是它的结构简图以及我附上的坐标系。（这里我用的是SDH建系法）
它的初始位置是连杆1水平的时候。其中l_{c1}代表转轴至质心的距离，l代表连杆的长度。最后一个工具坐标系我没标轴号，是因为本文用不到这个坐标系（其实是懒）

1.2 推导动力学模型

对这步很熟的可以直接跳过这节，先贴出结论：

\left(m L_{c_{1}}^{2}+I_{zz}\right) \ddot{q}_{1}+m g L_{c_{1}} \cos q_{1}=\tau

这里求逆动力学方程我用的欧拉-拉格朗日方程法。对于这种极其简单的结构，可以直接写出结果，为了练习动力学方程推导能力，这里我们一步一步解答，过程如下：

• 首先列出需要用到的一些结论
• 求解惯性项D(q)
• 求解离心力和科氏力项C(q)以及重力项G(q)
• 得到最终结果

\left(m L_{c_{1}}^{2}+I_{zz}\right) \ddot{q}_{1}+m g L_{c_{1}} \cos q_{1}=\tau

1.3 线性化处理

机械臂的动力学方程的参数具有线性的性质，因此可以改写为：

D(q)+C(q, \dot{q}) \dot{q}+g(q)=: Y(q, \dot{q}, \ddot{q}) \Theta=\tau

按照上面的思路我们改写上一节得到的动力学方程，得到：

Y(q, \dot{q}, \ddot{q}) \Theta=\tau

其中

Y(q, \dot{q}, \ddot{q})=\left[\begin{array}{ll} \ddot{q_1} & \cos q_{1} \end{array}\right]

以及

\Theta=\left[\begin{array}{c} m L_{c_{1}}^{2}+I_{z z} \\ m g L_{c_{1}} \end{array}\right]

\Theta是需要辨识的动力学参数,Y(q, \dot{q}, \ddot{q})是可以测量到的部分，也叫观测矩阵。有了线性关系方程之后，根据最小二乘法的知识我们就可以求出动力学参数。于是我们给定一个运动激励，测量出在不同时刻下关节位置，速度，加速度以及扭矩，将其写成矩阵形式：
然后用最小二乘法得到

1.4 轨迹激励

到这里，我们只需要知道几个轨迹点下关节的位置，速度，加速度和关节扭矩就能算出动力学参数集。那么问题来了，我要激励什么样的轨迹？数据点要采多少？才能很好的辨识我们的动力学参数呢？对于本文研究的单摆结构，上述问题并不突出。但是对于复杂机械臂，激励轨迹的选择以及数据点的采集就很讲究了。有兴趣的可以去研究，学废了记得教我~ 本文为了简化讨论，我们把

j_{1}=1.7 \sin (t)

当作我们的激励信号

2. 仿真

2.1 模型搭建

2.1.1 画机械设计图

我们用solidworks简单画一个单摆结构，然后转换成urdf格式导入到matlab里面。十分简单的结构，只需要两个零件即可。

2.1.2 导入Matlab

先导成URDF格式，然后导入到Matlab simscape里面，这里过程不再赘述。导入到simscape的代码如下：

smimport('Pendulum_SLDASM.urdf');

我们跑看看模型，一开始会Joint limit报错，只需要把关节模块里面的Joint limit 上的勾勾去掉就好。接着运行模型，模型动了起来，确认我们的Simscape模型无误

2.1.3 计算参考惯量矩阵

有了模型之后，为了让后面辨识出来的参数有个参考，我们在Solidworks里面查看连杆的质量参数
然后用这些参数在Matlab里面计算我们的参考惯量矩阵，代码如下：

% Theta_sw is inertia matrix get from solidworks 
% tau_sw is torque calculated from solidworks inertia
syms m L_c I_zz Theta_sw tau_sw
m = 14.39*10^(-3);%kg
L_c = 50.59*10^(-3);%m
I_zz = 13822.02*10^(-9);%kg*m2
g = 9.81;%N/kg
Theta_sw = [m*L_c^2+I_zz;m*g*L_c]
1000*Theta_sw

这里需要进行一下单位换算。最后的结果放大了1000倍是为了方便我们观察数据，输出为
我们后面希望辨识结果能够尽可能地接近这两个数字。

2.2 添加激励与传感器

根据第一节的理论知识，我们需要给模型添加一个运动激励，然后测量关节位置，关节加速度以及关节扭矩来进行后续的辨识过程。因此我们修改关节配置，添加输入接口和关节传感器接口
然后添加激励输入，以及关节位置，加速度，和扭矩传感器，新的结构图如下
其中激励输入的配置为
（请把频率改成1，不是0.1）我们再一次运行，发现系统报错
这是因为添加了S-PS Convertor之后需要修改一下S-PS以及matlab Solver配置才行，不需要慌，解决方法：

1.把simulink-PS converter模块里面的Input Handling参数分别改成

• Filter input，derivatives calculated
• Second-order filtering
• 时间常数随意

2.模型配置参数（Modeling configuration parameter）那块把solver改成ode23s

改完之后再一次运行，模型可以正常运动，且Scope里面有数据曲线。

2.3 数据采集

2.3.1 把数据导入Matlab工作空间

模型跑通了，数据图像也有了，现在要怎么把Simscape仿真结果提取出来用来计算呢？Matlab自带的Data Inspector很方便我们进行数据获取。首先我们告诉matlab我们在关心哪些数据，我们框住关心的输入输出信号，然后点击Log Signals
信号流上面出现了wifi标志说明这些数据已经开始被监听了。运行模型，待Data Inspector变黄，点击就可以看到提取出来的数据图像
然后我们可以把数据导入到工作空间里给后续的辨识过程使用。操作过程很简单，选中想要提取的数据，点击export，改一下名字，ok就行
我们需要操作四次，把所有用到的信息导出。导完之后观察我们的工作空间，数据都已经进来了
我们不难发现加速度一开始的时候有个很高的冲激，为了避免这里的数据点对结果产生干扰，我们用中间往后的数据点进行我们的辨识。这里我们有2000多个数据点，于是我们可以选择使用1000至2000范围内的数据。

2.3.2 确定数据单位

需要的数据都有了，终于可以开始辨识了？慢着，我们还得知道我们Matlab里面测量到的数据的单位，我们去到DEBUG-Information Overlay- 点击Unit
新的模型数据线上面出现了单位
至此，数据准备工作完成，我们可以开始写代码辨识了

2.4 进行参数辨识

2.4.1 提取数据点构造观测矩阵与扭矩矩阵

我这里的数据采集范围是1000至2000.然后通过更改步长来改变数据采集数目。

% Retrive data for identification
StartDataindex = 1000;
EndDataindex = 2000;
step = 10;
numOfData = (EndDataindex-StartDataindex)/step+1
% Y and tau
Y = zeros(numOfData,2);
tau = zeros(numOfData,1);
for n=1:1:numOfData
   Y(n,1)=getdatasamples(Joint1Acce,step*(n-1)+StartDataindex);
   Y(n,2)=cos(getdatasamples(Joint1Pos,step*(n-1)+StartDataindex));
   tau(n,1) = getdatasamples(Joint1Torq,step*(n-1)+StartDataindex);
end

2.4.2 得到辨识结果

theta=inv((Y.')*Y)*(Y.')*tau
1000*theta
% Use identified inertia to calculate torque to verify your result
% tau_esti
tau_esti = Y*theta;

为了方便比较同样也把结果放大了1000倍
发现辨识出来的参数与之前的参考结果相差很小

2.5 检查辨识效果

我们用辨识出来的参数进行扭矩计算，然后与测量出来的扭矩进行比较

error = tau_esti-tau;
index=1:1:numOfData;
% Measuerd torque VS Estimated torque
plot(index,tau,index,tau_esti)
title('Measuerd torque VS Estimated torque')
xlabel('Index');
ylabel('Torque(Nm)');
% Error 
plot(error)
title('Torque error');
xlabel('Index');
ylabel('Torque(Nm)');

输出结果
扭矩大概在10^(-3)这个量级，误差在10^(-6)这个量级，可以判断此次辨识结果基本成功

2.6 总结

可见即便对于一个很简单的结构，即便我们忽略了很多实际情况下的条件让辨识十分理想化，机器人的动力学辨识过程依旧不简单。大家可以自行调整激励信号以及模型，看看辨识结果会如何变化。辨识结果受到的影响很多，激励信号的选择也有很多学问，我没有仔细研究。

2.7 几个问题

• 没有力传感器的话扭矩不能直接得到，可以用电流估算扭矩吗？
• 用电流估算的话摩擦要怎么处理？
• 实际的加速度测量肯定存在噪声和误差，要怎么处理噪声误差？处理过程会对辨识有什么样的影响？
• 频率小的时候辨识出来的惯量矩阵有负数项？

3. 更进一步

3.1 考虑电机内部模型（电流与扭矩的关系）

3.2 等等

本文结束，以上。

参考文献

• 动力学参数辨识流程 https://zhuanlan.zhihu.com/p/93463996
• 《robot dynamics and control》 作者：Mark W Spong，Seth Hutchinson， and M. Vidyasagar