运动控制算法实验成果解析

本章所有仿真实验均在6.3.4节“上位机软件设计”中构建的可视化界面和评估系统中实现。

1.运动学逆解算法仿真

（1）仿真环境准备 仿真样本为以齐次变换矩阵形式给出的直线段端点：

机械臂初始位姿为

q=[0−π/3π/3000] （6-96）

以齐次变换阵型是给出为

此时的机械臂位形如图6-33所示。

图6-33 q=[0−π/3π/3000]下的机械臂位形

使用等间距直线插补算法，插补点距d=1mm，轨迹模长L为

实际插补点数为

fix（L/d+0.99）=220 （6-99）

仿真路径中各关节运动位置曲线如图6-34所示，机械臂执行目标轨迹T₀-T_t的过程如图6-35所示。

图6-34 仿真路径中各关节运动位置曲线

图6-35 机械臂执行目标轨迹T₀-T_t的过程

仿真实验中通过MATLAB程序计时函数etime和clock进行算法收敛速度和效率的统计。etime和clock函数的使用方式为

但是由于MATLAB的m脚本语言为解释型程序语言，在执行过程中受操作系统资源复用、及代码解释方式的影响，会在算法耗时上受一定的随机因素影响。以下所有仿真实验数据均通过三次独立执行，然后取平均值获得，尽量避免在解释过程中受到的随机性误差影响，以便获得一般性实验结果。

（2）解析法求逆解速度仿真 通过程序计时函数对单次解析法求逆解的过程进行计时，代码实现方式如下：

但是通过该种计时方式得到的结果，矢量tcount中的元素过小，无法辨识，说明解析法求逆解速度过快，超过了程序计时函数etime的计时分辨率。因此对正解求解过程使用另外的计时方式。

该方式通过对220次逆解的总时间进行计时，得到计时值t_c=0.0732s（三次实验数据分别为：t_c1=0.0732s，t_c2=0.0880s，t_c3=0.0750s）。由此可得出，通过解析法求逆解的单次执行平均时间为：

仿真结果表明，对于MATLAB平台，解析法求逆解的算法执行时间在100μs数量级。

（3）牛顿-拉夫逊迭代法效率仿真

1）算法收敛速度实验。对算法执行过程中的迭代次数进行统计得到在对应收敛误差限stol分别取stol=10^-1mm、stol=10^-3mm、stol=10^-6mm时，各个插补点的迭代次数N和单次逆解算法耗时统计表见表6-6。

表6-6 牛顿-拉夫逊迭代法耗时统计表

由表6-6中数据可以得出以下结论：

首先，通过相差两个以上数量级的三组收敛误差限stol的结果对比，算法并没有因为收敛误差限的减小，而使算法复杂度和时间开销线性增加，迭代次数和时间开销与收敛误差限stol的变化基本符合指数函数关系，实现了设计预期的快速收敛。

其次，在迭代结果误差较大时，迭代次数明显增加（表现在从初始位姿到线段始端T₀的逆解求解过程中），而在后续满足1mm插补精度的微变连续点的逆解求结过程中，迭代次数大幅下降，在收敛误差限stol=10^-1mm时，只要一次迭代计算就可以得出满足误差限要求的运动学逆解。说明算法收敛过程满足最小能量变化原则，实现了预期设计目的。

stol分别等于10^-1、10^-3、10^-6时的各插补点逆解耗时如图6-36所示。

2）算法内部耗时评估实验。之后对算法的各个执行部分进行分别计时统计，发现算法中时间资源消耗最大的部分为雅可比矩阵J的广义逆J⁺计算，具体来说，是求解广义逆过程中的SVD分解部分占用了较多算法时间。单次逆解计算中SVD分解执耗时及平均时间占用比例见表6-7。

(www.xing528.com)

图6-36 stol分别等于10^-1、10^-3、10^-6时的各插补点逆解耗时

表6-7 单次逆解计算中SVD分解执耗时及平均时间占用比例

由表6-7可见，SVD分解占用了运动学逆解求结过程中一半以上的时间。牛顿-拉夫逊迭代法与其他基于雅可比广义逆的算法局限性相同，即对雅可比矩阵J的SVD分解占用时间资源过多，算法对硬件系统要求较高。

各插补点逆解计算中SVD分解执行耗时及平均时间占用比例原始数据如图6-37所示。

由于在执行过程中，计时函数etime和clock也需要占用时间，而针对单次逆解的SVD分解时间消耗计算与迭代次数有关，当stol=10^-6mm时，微变插补点之间仍需要三次迭代才可得到满足收敛误差限的运动学逆解，调用计时函数的次数增加一定程度上造成了计时的不准确性。因此只对于stol=10^-1mm、stol=10^-3mm时的数据进行了分析。这一点从侧面反映了使用MATLAB仿真进行算法效率分析的局限性。

图6-37 各插补点逆解计算中SVD分解执行耗时及平均时间占用比例原始数据

图6-37 各插补点逆解计算中SVD分解执行耗时及平均时间占用比例原始数据（续）

（4）逆运动学仿真结果分析 通过对比可得，使用解析法求逆解，对于“新型工业焊接机械臂”的参数来说，算法的解算速度非常快。使用牛顿-拉夫逊迭代法求逆解，算法的时间开销比较大，尤其对于SVD分解的部分，如果使用的嵌入式系统处理器性能较差，该算法的执行效率将相当低下。

对于本设计中使用的嵌入式控制系统核心STM32F1VET6的Cortex-M3 CPU没有浮点运算单元，使用定点DSP算法优化算法效率，而且机械臂在设计阶段有对自由度配置进行合理设计，符合Pieper准则。而牛顿-拉夫逊迭代法的优点主要体现在对于非Pieper准则机械臂、处理奇异性问题、冗余自由度控制及对于多解的选择等方面。因此对于本系统，优先使用解析法进行运动学逆解求解。

2.轨迹规划算法仿真

（1）简单空间直线插补仿真 对6.3.3节所述空间直线轨迹插补算法进行仿真。以齐次变换阵形式给出的端点仿真样本为

将仿真样本代入空间直线轨迹规划算法，可得直线插补与五次样条曲线插补的结果，如图6-38和图6-39所示。

图6-38 直线插补结果

图6-39 五次样条曲线插补的结果

在图6-38中，轨迹对应的X、Y、Z三个坐标轴对应分量的位置/时间曲线为斜率一定的直线；速度/时间曲线在位置/时间直线的拐点处表现为阶跃，其他部分为匀速状态；加速度时间曲线在位置/时间直线的拐点处表现为窄脉冲，其他部分为0。

在图6-39中，轨迹对应的X、Y、Z三个坐标轴对应分量的位置/时间曲线为“S”形曲线（五次曲线），有平缓的过渡段；速度/时间曲线表现为平滑的钟形曲线（四次曲线）；加速度/时间曲线表现为具有三个单调区间的平滑样条曲线（三次曲线）。由实验结果可知，在伺服控制所需的位置、速度、加速度范围内，五次样条曲线插补算法保证了控制量关于时间连续可导，且导数连续的平滑特性。

（2）连续轨迹插补仿真 接下来对连续刀路进行仿真，以NC代码形式给出的仿真轨迹为如图6-40所示的具有起止点路径的空间正方形。

N80GOX25.000Y25.00025.000

N90G1ZO.000F720.0

N100GIY75.000F2520.0

N110X75.000

N120Y25.000

N130X25.000Y25.000

N140G025.000

N150XO.000YO.000

图6-40 具有Z轴起止点的平面矩形轨迹

分别使用直线插补算法和五次样条曲线插补算法进行插补并进行解析法求逆解的各轴角位置/时间曲线如图6-41和图6-42所示。

图6-41 平面矩形轨迹直线插补关节空间位置/时间曲线

图6-42 平面矩形轨迹五次样条插补关节空间位置/时间曲线

由图6-41中仿真结果可知，笛卡尔绝对坐标系下的直线经过直线插补，并进行运动学逆解后，得到的结果为在关节具有很多不可导的拐点，这些拐点对应笛卡尔绝对坐标系中的拐点。这些拐点体现为关节空间速度矢量的阶跃，不利于控制。经过五次样条曲线插补的结果经过运动学逆解后，得到的关节空间曲线具有与笛卡尔绝对坐标系下对应的平滑性，实现了预期的速度与加速度控制功能。