python 拟牛顿法 求非线性方程_6R机械臂动力学方程线性化-牛顿-欧拉方式

机械臂动力学方程线性化

给大家分享一下，网上没有找到具体推导公式，不喜欢藏着掖着，这是自己推导的，仅供大家参考。有问题欢迎批评指正。

1.问题来源

进行机械臂惯性参数辨识，需将机械臂的动力学方程线性化，及将动力学方程的形式写成力矩等于矩阵与惯性参数乘积的形式，且矩阵Y中不包含机械臂惯性参数，这样可以利用矩阵的广义逆，求解出惯性参数。本文以6R机械臂为例，假设机械臂运动过程中需要的关节力矩

，机械臂所有连杆的惯性参数组成的集合P，P对应的系数矩阵Y，则线性化动力学方程为式1.1，应用广义逆求解的公式为1.2。将动力学方程化简，求解系数矩阵Y和参数P，就是动力学方程线性化的问题。

但是由于机械臂惯性参数对应的矩阵Y一般非满秩，一般寻求一组最小的惯性参数集Pr，使得最小惯性参数集的系数矩阵Yr满秩，进而使用广义逆求解。定义矩阵C，使得式1.3成立。

则式1.1可化为：

定义

其中

为满秩矩阵，可以直接用矩阵的广义逆求解惯性参数。根据机械臂的惯性参数集P，求解最小惯性参数集

和转换矩阵C，这就是最小惯性参数集求解的问题。

啰嗦一句，matlab求矩阵的逆，即使系数矩阵不满秩，也可以求，具体用机械臂60个参数去辨识还是用最小惯性参数集去辨识，这也是目前个人比较困惑的问题，我们这有人直接60个参数一起辨识，很多文献上都是最小惯性参数辨识，这两种区别，希望有大佬能够解答解答两者的区别。

2.解决思路

常见的机械臂动力学方程建模形式主要有牛顿欧拉动力学方程和拉格朗日动力学方程两种。由于拉格朗日动力学方程中需要动势对关节角求偏导，个人觉得不太好对动力学方程中进行化简，使用matlab符号运算推导，最后求解出的系数矩阵Y表达式很长，因此改用牛顿欧拉方式推导线性化动力学方程。拉格朗日线性化动力学方程推到过程参考霍伟《机器人动力学与控制》。

线性化动力学方程存在两个主要的问题，一是如何将动力学方程中的惯性参数分离出来；二是方程组的化简。为了分离出惯性参数，获得较为简洁的线性化动力学方程，采用牛顿欧拉形式对动力学方程进行推导，推导过程中的根据自己的思路将部分跟惯性参数无关的变量化简，最后得到推导系数矩阵Y的递推公式。

牛顿欧拉方式推导动力学方程，欧拉公式用于描述物体的转动，通用的形式如下：

传统的欧拉公式相对连杆质心C坐标系建立，即欧拉公式中惯性矩阵I是相对于质心的惯性矩阵

。而为了提取出惯性参数，这里欧拉公式是相对于连杆坐标系原点建立欧拉方程，即将惯性矩阵I表示相对于连杆坐标系原点处的惯性矩阵

。方程线性化推导的具体过程，在后面详述。

牛顿欧拉推导线性化动力学方程，求解过程主要分为以下几个步骤：

1）求解相邻连杆坐标系的旋转变换矩阵

和位置关系

。

表示坐标系{i}相对坐标系{i-1}的旋转矩阵。

表示坐标系{i}的原点在坐标系{i-1}中的位置。

2）求连杆角速度

。表示坐标系{i}中连杆i的角速度。

3）求连杆角加速度

，表示坐标系{i}中连杆i的角加速度。

4）求坐标系原点的加速度

，表示坐标系{i}中坐标系{i}原点的加速度。

由于上述1）~4）中的运动学参数与机械臂惯性参数无关，且输入关节角

、关节角速度

、角加速度

后可直接求出，因此认为可将其作为已知量，用于线性化动力学方程的求解。

5）求解每个连杆的合力

，在坐标系{i}中表示，同时分离出惯性参数。

6）从连杆末端到基座递推求连杆i受到连杆i-1的力

和关节力矩

，同时分离出惯性参数。

7）将每个关节力矩

的系数矩阵提出，组成整个机械臂的系数矩阵Y。

3.求解方案

3.1求旋转矩阵

和位置

旋转矩阵

的求法在机械臂运动学中大量运用，这里求法不推导，默认已知了。假设求得的6个旋转矩阵为

—

，6个位置向量

—

。

3.2求连杆角速度

由于基座不动，所以基座的角速度

，由基座向末端正向推导各连杆的角速度，基本关系式：

其中

。所以各连杆的角速度：

由各连杆角速度表达式可知，连杆角速度

只与关节角速度

和旋转矩阵

有关，与惯性参数P无关，且输入关节角

、关节角速度

后即可求出，因此视为7个已知量，带入后面的动力学参数的系数矩阵中。

3.3求连杆角加速度

由于基座不动，所以连杆0的角加速度

，同样正向推导，求连杆角加速度的基本公式：

由公式3.3求各连杆的角加速度：

同样由上式知角加速度表达式与惯性参数无关，所以将连杆角加速度的7个量可视为已知量，带入后面的动力学参数的系数矩阵中。

3.4求坐标系原点的加速度

已知基座不动，为将重力g的影响带入动力学方程，需在牛顿欧拉公式中，将基座的加速度设为（这里默认机械臂正装，侧装及倒装对应g的位置可能在x方向或者y方向上，具体跟设立的坐标系有关)：

由基座向机械臂末端正向递推求坐标系连杆的加速度，通用公式如下：

根据式3.6，可以求得每个连杆坐标系的加速度：

为坐标系{i}原点在坐标系{i-1}中的位置矢量，注意并不是质心位置，可视为为已知量。7个加速度矢量同样也可视为已知量，带入后面的动力学方程中。

3.5逆推求连杆所受的合力

设连杆质心处加速度

表示，连杆i质心在本连杆坐标系中的位置

，连杆的质量

已知各运动学参数，可根据连杆坐标系原点处的加速度求质心处的加速度

。

由

可以求出连杆i受到的合力

:

由式3.9知，连杆受到的合力

跟连杆质量

和质量与质心位置乘积

成线性关系，将惯性参数

和

提取出来。已知叉乘可以用叉积矩阵S表示，定义：

（这个式子可以将叉乘写成叉积矩阵的形式，是分离动力学方程很重要的一个式子）

用式3.10改写式3.9：

定义每个连杆的惯性参数：

则整个机械臂所有连杆的惯性参数集P:

将3.11式写成矩阵乘惯性参数的形式：

定义一个新的矩阵

，使得：

将

带入式3.14中，可得到：

其中

与是一个与惯性参数

无关的

的矩阵。

3.6逆推求连杆合力矩

和连杆

给连杆i的力

、力矩

1)求关节处作用力

：

对每个连杆列动力学方程，有：

所以有：

2）连杆合力矩

：

研究连杆i的旋转，设连杆受到的合力矩在坐标系{i}中表示为

，坐标系原点处的惯性矩阵

,对连杆i坐标系的原点用欧拉方程：

由于存在关系式：

定义函数:

（这个式子是第二个线性化动力学方程很重要的式子，将Iw转换成矩阵乘惯性参数的形式）

用式3.20和式3.10将式3.19化简

定义

，则式3.21可简化为：

3）求关节处的力矩

：

对连杆i坐标系原点列力矩平衡方程：

可以得出关节处力矩

：

4）由连杆末端向基座逆推关节处力矩

，定义

：

即当i=6时，连杆受力

将3.24式改写成系数矩阵与整机的惯性参数$P$相乘的形式，

定义

，

表示连杆力fi对应的系数矩阵，所以：

由于

:根据式3.23，有：

当i=5时：

根据式3.23可知：

同理，对连杆4、3、2、1有：

3.7求系数矩阵Y

由于每个关节处力矩

表示x,y,z三个方向力矩组成的向量，取z向的值即为每个关节处电机施加的力矩，所以整机的力矩

有：

所以求得系数矩阵Y:

4.总结

本文将牛顿欧拉公式中的部分跟惯性参数无关的运动学参数作为已知量，去简化动力学方程的推导，最终获得线性化的牛顿欧拉方程。本文简化过程均自我推导，仅供参考。