【机械臂算法】机械臂动力学参数辨识仿真

本文以puma560m机械臂为例子进行动力学参数辨识的讲解，puma560m可以在robotic toolbox中找到，这里以它真实机械臂对他的动力学参数进行辨识。

此外这里还有要说的是，机械臂参数辨识其实是一个系统工程，其中和机械、电子、嵌入式都有着很深的联系，并不是仿真这么简单的。

但是仿真又是很重要的，如果仿真都做不出来，那么实验结果不可能有理论依据，仿真做不出来的，实验也是做不出来的。

本篇文章的思路和我的另一篇是完全一样的，只不过是扩展到了多自由度->【机器人算法】二连杆机器人动力学参数辨识。

仿真环境

因为这里是仿真，所以要有一个仿真环境，这里为了简单直接用matlab中的mdl_p560akb，把他当成真实的机械臂，此外这里再对其进行建模作为真实机械臂的仿真。
下图为“真实”机械臂和仿真结果的对比，其中线是真实机械臂的数据，点是我们仿真出来的数据，可以看出仿真结果完全正确。

线性化

线性化实际上就是将辨识参数分离出来，不考虑关节动力学和不考虑摩擦力，那么一个杆子就有10个待辨识参数，p560就有6*10个待辨识参数。那么动力学公式就可以转化为如下的形式：

tau=K*pi;

其中K就是一个和DH参数和关节位置、速度和加速度有关的矩阵。

一般思路来说我们直接采集实验数据然后，用最小二乘法就可以了：

但是，由于机械臂靠近基座关节的自由度缺失和我们只关心关节轴的力矩的原因（力传感缺失），K.'K是不可逆的。有人说可以用SVD求伪逆云云，但是SVD求得的伪逆是一个没有物理意义的“假”值，用这个值求出的待辨识参数也一定是不真实的，辨识没有意义。
因此我们需要通过转化，使得K.'K可逆，这个过程就是提取最小参数集。

最小惯性参数集

我们不可能辨识出所有的动力学参数，但是我们可以通过提取最小参数集来辨识出动力学参数的线性组合。
提取最小参数集的方式在网上有很多种，例如霍伟老师所著《机器人动力学与控制》中提到的使用DH参数直接推导出来。
本博客使用的是根据观测矩阵推导出最小惯性参数集。这里参考了一篇论文核心思想是通过QR分解K来对惯量矩阵Psi进行重组。通过论文中所提到的方法，我们得出最小惯性参数集为36个，具体如下：

我们随机对关节位置速度加速度进行赋值，进行仿真，仿真结果如下，左侧为真实的最小惯性参数集，右侧为辨识出的最小惯性参数集，可以看出结果有效。

激励轨迹设计

为什么要有激励轨迹设计？可以看出线性化后的动力学方程为tau=K*Psi，其中K是和机器人关节位置速度加速度有关的量，如果我们的轨迹设计不合理，那么K矩阵很容易成为病态矩阵，这十分不利于参数辨识。同时，激励轨迹还需要满足不损坏机器人本体。因此需要一定的考究。
激励轨迹的设计问题实质上是一个优化问题，你可以用各种各样的优化算法比如说图搜索，fmincon、GA等等的，这里直接调用matlab的函数包就可以了。优化的目标就是要满足以上要求，转化为公式的话，如下：

cost function是使得K的条件数最小。

这里我首先建议用一台性能高的电脑去完成，用轻薄本的话注定要跑很久，同时也要注意计算技巧，如果计算方法选不对那么也会耗费很多的时间。

这里给出我是用上述方法计算出来的激励轨迹，共花费三个半小时，如下图：

激励轨迹的执行如上图，因为我这里给的限位都比较大，所以看起来有些夸张。

实验

基于使用franka emika panda，进行参数辨识实验。这里见我的另一篇博文->【机械臂算法】Franka Emika Panda动力学参数辨识。