大学生科技创新竞赛获奖作品：机械机电控制算法设计

1.运动学算法设计与实现

现代工业机器人多采用在线编程的方式，要求在满足必需的精度的前提下实现较快的速度，这就使得作为机器人控制基础的运动学问题受到越来越多的重视。运动学分为正运动学和逆运动学，前者一般采用齐次坐标变换法，此种算法简单可靠，比较成熟。后者由于机器人本体的多变性，算法相对复杂，实现方式也多种多样。

（1）运动学模型建立 进行运动控制学算法设计之前，必须首先对被控对象机械臂进行数学模型建模，对于开链串联结构的多自由度机械臂，一般使用一种名为DH（Denavit-Hartenberg）参数模型的建模方式。DH参数描述了相邻杆件间平移、偏移、旋转和扭转的关系，通过在每个关节处的杆件坐标系建立4×4齐次变换矩阵，表示它与前一杆件坐标系的位姿关系。基于这种描述方式，可以产生多种DH参数建立方式，这里使用修改DH参数模型（modified D-H）。

建立DH参数模型的第一步是建立各个关节的参考坐标系。O₀-x₀y₀z₀是与固定坐标相连（一般为大地坐标系）的固连参考坐标系，称为基坐标系。O_i-x_iy_iz_i与机器人的第i个杆件相固连，坐标原点在第i+1关节的中心点处，即相邻两杆件的交点处。确定和建立每个参考坐标系遵循以下三条规则：

1）z_i-1轴沿着第i关节的运动轴。

2）x_i轴垂直于z_i-1轴及z_i轴并指向离开z_i-1轴的方向。

3）y_i轴按右手坐标系的要求建立。

通过适当的位形调整，尽量保证在坐标系建立过程中各参考坐标系的x_i轴指向相同，这样可以简化DH参数模型的建立。针对“新型机械臂”各关节的参考坐标系如图6-19所示。

图6-19 各关节的参考坐标系

刚性杆件的DH参数表示法取决于连杆的以下四个参数：

1）θ_i：两连杆的夹角，绕z_i轴由X_i-1旋转到X_i到的角度。

2）d_i：连杆关节轴的偏距，沿z_i轴由X_i-1移动到X_i到的距离。

3）a_i：连杆的长度，沿z_i由Z_i-1移动到Z_i的距离。

4）α_i：连杆的扭转角，沿z_i轴由Z_i-1旋转到Z_i到的角度。

关系中，Z_i与X_i表示对应轴所在坐标系。

对于转动关节，θ_i是关节变量，其余为关节参数（常数）；对于移动关节，d_i是关节变量，其余为关节参数。

参考6.3.1小节“自由度配置”中的定义和描述，在各关节处建立DH参数坐标系如图6-19所示，其中因为关节5和关节6坐标系原点建模重合，为了不影响标注，关节6的坐标系用虚线画在实际位置右侧。此外，X_i坐标系为末端执行器的工具坐标系，根据末端执行器参数单独引入，不影响运动学求解，构造的X_t相对于X₆，a₆=50，α₆=π，可得对应齐次变换阵为。

在构建好的各关节DH坐标系基础之上，按照如上所述规则确定基于“新型机械臂”的DH参数见表6-1。

表6-1 “新型机械臂”的DH参数模型

完成DH参数的建立后，即可确定对于“新型机械臂”的六个关节变量θ₁、θ₂、θ₃、θ₄、θ₅、θ₆的绝对角位置零点与角速度方向，并且确定机械臂的零点位形。

运动学正解相对比较容易，只要根据运动学模型，通过各自由度齐次变换矩阵依次连续右乘，就可以求得末端执行器在基坐标下的唯一位姿。而运动学逆解是在已知末端执行器空间位姿的情况下，求解出关节变量，是对机器人进行有效控制所必需的。然而，运动学逆解的求解要比正解求解复杂得多，主要有以下问题：

1）可解性：求解操作臂的运动学方程是一个非线性问题。已知⁰_nT的数值，试图求出θ₁、θ₂…θ_n；通常这种方程为非线性超越方程，特别是当α_i不是0或π/2时，方程非常复杂，很难求解。

2）解的存在性：解是否存在的问题完全取决于操作臂的工作空间。简单地说，工作空间是操作臂末端执行器所能到达的范围。

3）多重解问题：在求解运动学方程时可能遇到的另一个问题就是多重解问题。在实际情况中，经常会出现机械臂可以有几个位形到达指定的位姿，即有几个解，而控制系统只能选择其中的一个解来处理，这里面涉及选择标准问题。

现阶段主流的运动学逆解求解方法主要有以下两种：

1）对于Puma560或类Puma560等符合Pieper准则的机器人，利用齐次变换矩阵中旋转子矩阵正交的特性，通过矢量运算构造方程等方法提高求逆解的速度，实现对逆运动学问题的解析表达式。

2）由于机器人的生产、装配及使用磨损等不可避免地会带来误差，甚至一些机器人在设计时就不符合Pieper准则。针对此时求解困难的问题，通过矢量乘法运算找到14个线性无关的逆运动学方程，利用分离变量消元法将逆运动学问题简化为关节变量半角正切的一元16次方程的求根问题，但此算法得到的运动学解不唯一，需要进一步验证各解正确性并建立取优策略，增加了算法运算量；将局部搜索能力强的模拟退火算法与全局搜索能力强的遗传算法相结合提出了一种逆运动学解法，虽然能达到一定的精度，但程序需要占用较大的存储空间且存在迭代次数多、运算量较大的问题，在嵌入式系统上难以达到较高的实时性。

（2）运动学正解 类Puma560等符合Pieper准则的机器人由于具有解耦的位置与位姿结构，具有良好的解析形式，便于通过解析推导计算获得逆运动学解析解。为了进行解析推导，需要正运动的解析表达式，基于“新型机械臂”的正运动学解析表达式推导与化简过程如下：

根据标准DH参数模型使用的杆件模型可得，一般杆件的齐次变换阵形式为

式中，c₁为cosθ₁的缩写；s₁是sinθ₁的缩写，后同。

代入表6-1中DH参数可得各连杆齐次变换矩阵。

将以上各个连杆变换矩阵连续右乘，就得到了从机械臂基坐标系到末端腕坐标系的变换矩阵

其中：

又因为关节2与关节3的轴线平行，可通过θ₂与θ₃的两角合角公式化简。设θ₂₃=θ₂+θ₃，可得

将c₂₃=c₂c₃-s₂s₃

s₂₃=c₂s₃+s₂c₂

代入上述方程组，得

至此得到简化后便于计算与求解的正运动学表达式。

（3）解析法求运动学逆解 利用前述运动学正解算法的结果式（6-2），进行逆运动学解析推导求解的算法设计过程如下：

1）解θ₁

由运动学正解式（6-3）可得

等式两边同时左乘（⁰₁T）^-1得

式中，

又因为齐次变换阵中左上角的3×3旋转变换阵为正交矩阵可得

代入式（6-5）得

使式（6-5）与式（6-7）的（2，4）元素对应相等得

-s₁p_x+c₁p_y=d₃ （6-8）

通过三角恒等变形得

p_x=ρcosφ （6-9）

p_y=ρsinφ （6-10）

式中，

将式（6-10）和式（6-12）代入式（6-8）得

化为余弦形式避免重根得

2）解θ₃

已解得θ₁，使式（6-5）与式（6-7）中（1，4）元素和（3，4）元素分别对应相等，可得

将式（6-17）和式（6-18）平方和后相加得

设可得

式（6-20）与式（6-13）相同，用同样的三角恒等变换可解得

3）解θ₂

由运动学正解可得

重新整理式（6-5）可得

令式（6-24）与式（6-25）中两矩阵（1，4）元素和（2，4）元素分别对应相等，可得

联立式（6-26）与式（6-27）可解得

式（6-28）与式（6-29）分母为正，可通过四象限反正切求得θ₂与θ₃的合角θ₂₃。

可解得θ₂

θ₂=θ₂₃-θ₃ （6-31）

根据θ₁与θ₃的解的四种组合形式，θ₂解有四种可能形式。

4）解θ₄

式（6-24）完全已知，令矩阵中（1，3）元素和（3，3）元素分别对应相等可得

当sinθ₅≠0时，可解得θ₄。

当θ₅=0时，操作臂处于位形奇异点，此时关节4与关节6轴线重合，发生自由度退化现象（第4自由度与第6自由度退化为一个自由度）。这种情况下，可通过检查式（6-34）中arctan2函数的两个变量是否都趋近于零来判断。如果趋近于零，此时的θ₄可任意选取（一般选取上一次的计算值），之后计算θ₆时，可根据θ₄进行选取。

5）解θ₅

由θ₄求解过程可得

再次改写式（6-5）

令式（6-37）与式（6-38）两矩阵（1，3）元素和（3，3）元素分别对应相等，可得

可解得θ₅。

6）解θ₆。

由θ₅求解过程可得

其中：

其中：

再次改写式（6-5）

令式（6-44）左右两边（1，1）元素和（3，1）元素分别对应相等，可得

联立式（6-45）和式（6-46）可解得

arctan2（s₆，c₆） （6-47）

其中

7）多解整理。由于在式（6-14）与式（6-21）中出现±，因此目前方程组可能有4组解。另外三个轴线交于一点的腕关节所形成的球形副的“翻转”会产生两种θ₄，θ₅，θ₆的组合，可以得到另外的4组解。球形副翻转形式如下(www.xing528.com)

至此，六自由度串联结构腕解耦型机械臂的所有8组逆运动学的解的形式都已解得，根据关节运动范围和工作空间的约束，其中一些（甚至全部）解要被舍弃，在剩下的解中，通常根据位置或能量最小变化原则，选取与上一位姿最为接近的解。

（4）牛顿-拉夫逊迭代法求运动学逆解 运动学逆解问题中的两个关键问题为解的存在问题和多解问题。对于工作空间边界点等具有奇异性质的问题时，需要一种不需要考虑多解问题的逆解求解形式。

牛顿-拉夫逊迭代法求解运动学逆解的算法基础为：以雅可比矩阵和微分运动为基础，采用基于豪斯霍尔德的SVD分解求雅可比矩阵的伪逆来解决奇异性问题，通过迭代可以得到逆运动学的唯一解，此解等同于按能量最小原则确定的最终解。算法参数和所需存储空间均较少，借助牛顿-拉夫逊迭代法的局部快速收敛性，以较少的迭代次数即可得到所需精度的解。而且，当机械臂结构发生变化，不具备腕解耦条件后，解析法有可能难于求解甚至完全失效。此时，基于牛顿-拉夫逊迭代法的求解方式仍然有效适用。因此，该方法对于研究工业机器人的运动学控制具有重要的意义。

“新型工业焊接机械臂”的牛顿-拉夫逊迭代运动学逆解算法流程如图6-20所示。牛顿-拉夫逊迭代运动学逆解算法是一种以机械臂DH参数、以齐次变换矩阵给出的目标位姿信息T_t和初始关节空间位置q₀为输入量，经过迭代计算，最终给出关节空间位置矢量q的算法。算法主要包含以下求解步骤：

1）求解T_cur。使用q（算法启动时使用q₀）代入运动学正解求解当前位姿T_cur（以齐次变换阵形式给出）

图6-20 牛顿-拉夫逊迭代运动学逆解算法流程

2）求解微分运动矢量D。微分运动矢量D一般可以表示为微分移动矢量d和微分转动矢量δ，分别可以表示为

将两个矢量合并为一个六维矢量D，称其为刚体相对于基坐标系的微分运动矢量。

通过求对于特定变换阵T的变换微分来确定微分运动矢量。

T+dT=Ttrans（d）rot（δ） （6-59）

式中，

其中I₄为4×4的单位阵。用微分算子∇简化表示

求解从T_cur到T_t的变换微分为

联立式（6-63）与式（6-65）可解得

式中，n、o、a、p为齐次变换阵中对应的列向量。

3）求解雅可比矩阵J。机械臂雅可比矩阵J通常是指从关节空间向笛卡尔空间运动速度的广义传动比。即

式中，笛卡尔空间速度矢量，为关节速度矢量。

笛卡尔空间速度用微分运动矢量表示为

联立式（6-67）与式（6-68）可得

①求解相对于工具坐标系的雅可比矩阵J₆。机械臂雅可比变换矩阵可以写成分块矩阵的形式

在建立DH参数时，旋转关节的旋转轴定义为z轴，雅可比矩阵可以利用微分变换的方式求解，对于旋转关节i，连杆i相对于连杆i-1绕坐标轴z_i做微分转动dθ_i，则关节i坐标系的微分运动矢量为

微分运动矢量的等价坐标变换式如下

式（6-73）表示根据当前坐标系微分运动矢量D，可以通过变换得相对于变换阵T坐标系的微分运动矢量^TD。式中

将式（6-72）代入式（6-73）得：

式中，n、o、a、p为变换阵T的相应列向量。因此，式（6-75）中列元素可表示为

式中，n、o、a、p为变换阵ⁿ₆T的相应列向量。

建立求解末端执行器的雅可比矩阵J算法，流程图如图6-21所示。

图6-21 求解雅可比矩阵J算法流程图

通过该算法即可解得相对于工具坐标系的雅可比矩阵J₆。

②求解相对于基坐标系的雅可比矩阵J。已求得相对于工具坐标系的雅可比矩阵J₆。通过雅可比矩阵的参考系变换，可以变换获得相对于基坐标系的雅可比矩阵J。雅可比矩阵的参考系变换公式如下

a.求解J的广义逆J⁺。首先对J进行基于豪斯霍尔德的SVD分解

J=UΣV^* （6-79）

式中，U为酉矩阵，Σ为一个半正定对角阵，V^*为V的共轭转置，为酉矩阵。

通过对SVD分解的形式进行变形，获得J的伪逆J⁺

J⁺=U^*Σ⁺V （6-80）

式中，Σ⁺为将Σ转置，并将其主对角线上每个非零元素都求倒数得到的。

b.求解关节空间微分运动矢量dq

由式（6-70）得到

dq=J⁺（q）D （6-81）

可直接解得关节空间微分运动矢量dq。

c.计算

关节空间运动微分矢量dq的变化误差由dq的方均根dq来评估：

此处的物理意义为多维空间内向量dq的泛数，充分反映了误差的情况。通过设定一个收敛误差限，即一个满足精度的充分小的数stol。当时，退出循环并输出当前关节空间位置q；当循环次数大于m_limit时，循环结束并输出错误信息，其中，m_limit为满足预订迭代次数要求的自定义充分大的整数；当且循环次数m＜m_limit时，令q=q+dq进行迭代计算，重复步骤（1）至（6），直到满足循环退出条件，此时q即为满足条件的机械臂各关节角度。

2.轨迹规划算法设计与实现

（1）轨迹规划算法概述 运动轨迹用以描述机械臂在三维空间内随时间实现期望运动的过程。机械臂的轨迹具体来说，指的是每一个自由度的在同一时间坐标下的位置、速度与加速度。

如逆运动学算法设计过程所示，由于机械臂的运动控制涉及复杂的中间计算过程。因此，通过直接的关节空间命令，往往难以实现准确可靠的轨迹描述。为了使机械臂便于使用，不应当要求用户预估到正运动学的结果，然后通过复杂的关节空间对时间的函数来完成机械臂控制，而应该由用户简单指定一些目标位姿点，由控制系统自动完成具体计算过程。

因此，机械臂的轨迹规划，具体指由用户指定时间坐标和末端执行器的期望位姿信息，通过控制系统来获取达到期望位姿的精确路径、速度和加速度曲线等的过程。针对这一过程除了要建立精确的数学模型和算法之外，还要考虑具体的算法实现方式，以保证轨迹可以在数字系统内正确、实时地进行计算，并能够达到足够控制精度水平的控制频率，典型的操作臂系统中，路径更新率为60～2000Hz。

工业机器人领域通过插补的方法进行机械臂等机器人的轨迹规划。插补是根据给定的数学函数，在理想的轨迹式轮廓上的已知点之间，确定一些中间点的一种方法。针对工业机器人数控加工过程中最常用的直线轨迹规划，进行了简单直线插补和五次样条曲线插补两种轨迹规划算法的设计。

（2）直线插补算法 直线插补通过给定空间直线始末两点的位姿，按照线性函数变化规律求轨迹中间点（插补点）的位姿。使用直线插补时，机械臂的姿态变化按照给定的步长，匀速从初始位姿向终末姿态均匀变化。

进行直线轨迹的定步长插补的算法设计过程为：

1）输入给定沿直线始末点坐标（x₁，y₁，z₁）和（x₂，y₂，z₂），沿直线运动的运动速度v和插补周期T_s（此处以时间为控制量）。

2）计算始末点之间的空间距离L

3）计算运动总时间T

T=L/v （6-84）

4）计算插补点数N

N=fix（T/T_s+0.99） （6-85）

其中，fix()为向零取整函数，根据精度要求选择小数位的进位阈值，本算法精度为0.01mm，因此进位阈值选择0.99。在不考虑伺服机构执行死区大小的情况下，插补次数反映了直线的精度，一般来说插补次数越多，机械臂的轨迹越接近直线，但是随着插补次数的增多，控制系统计算的时间成本也将增加。

5）计算插补增量dx，dy，dz

插补增量dx，dy，dz反应了笛卡尔系下x，y，z三个运动轴位置/时间平面中的轨迹斜率。

6）计算各查补点的绝对位置

式中，n=1，2，3，L，N。

至此，直线插补过程完成。

如果换用插补步长为控制量的插补方式，会略有改变，基本遵循位置伺服的PVT控制原则。计算过程与以时间为控制量的插补方式基本相同。在此给出一种以插补步长为控制量的实时等距插补算法，算法流程图如图6-22所示。

图6-22 以插补步长为控制量的实时等距插补算法流程图

本系统最终使用图6-22所示的直线插补实现方式。

（3）五次样条曲线插补算法 直线插补虽然在计算上简单易行，但是对于工业机械臂运行的一些关键性参数并没有考虑，主要体现在对于速度轨迹和加速度轨迹并没有进行规划。其结果就是阶跃速度和窄脉冲加速度输入对于伺服机构的实际响应情况影响非常不利，因此在轨迹衔接处无法保证良好的精度（具体实验结果请参见运动算法实验章节）。因此需要一种具有良好数学分析性质的插补算法，使其具有三阶以上的解析结构，保证二阶以上的连续与导数连续性质。为此，引入一种五次样条曲线插补算法，该算法使用一个以时间为变量的五次构造多项式描述笛卡儿坐标空间的末态位置，多项式的系数通过初位置p₀、末位置p_t、初速度dp₀、末速度dp_t，并引入拐点处零冲量假设解得。算法设计过程如下：

1）输入给定直线始末点p_t=[x_t，y_t，z_t]、始末点速度dp₀、dp_t和插补时间T。

2）插补时间归一化t/T。

3）构造五次多项式。

p=At⁵+Bt⁴+Ct³+Dt²+Et+F （6-88）

4）求解多项式系数。

对式（6-88）求导和求二阶导可得

将p₀，p_t，dp₀，dp_t，以及根据拐点零冲量假设得到的代入式（6-87）、式（6-88）和式（6-89）得

解得

针对具体的算法实现，时间T可以通过不同的形式给出，当直接以时间形式给出时，可以直接计算插补点；当以时间向量形式给出时，取出时间向量的最大值进行归一化，归一化后得到有时间量纲的表达式，可以直接通过离散的时间向量对应值获得对应插补点。当以定点插补，以插补点数N给出时，通过归一化后可得到无量纲表达式，对t代入对应点数可得到对应的插补点。至此五次样条曲线插补过程完成。

对应空间直线轨迹的插补算法其输入量p₀，p_t，dp₀，dp_t都是三维向量。分别独立计算互补耦合。因此，对于主动自由度之间不相互耦合的机械臂，该算法同样直接适用于关节空间。直接用六维的q₀，q_t，dq₀，dq_t进行计算，即可得到关节空间下的五次样条曲线插补点，算法本身不需做更改。

3.自适应力矩控制算法设计与实现

自适应力矩控制机构部分的分析如自适应力矩控制结构一节中所述。针对这一机构，分别设计了静力平衡自适应算法和闭环动力控制自适应算法。

（1）静力平衡自适应算法设计 杆件质心位置固定，根据机械结构设计分析，可近似假设质心居于几何中点，末端执行器相对于关节轴5力臂较短，可将末端执行器质量近似等效到关节轴5处，静力平衡简化模型如图6-23所示。

图6-23 静力平衡简化模型

建立以关节轴2为轴心的静力平衡方程：

式中，L₁₁为连杆1前端长度，M₁₁为其质量；L₁₂为连杆1后端长度，M₁₂为其质量；L₂为连杆2长度，M₂为其质量；M₃为末端执行器质量；M_x为平衡锤质量，L_x为平衡锤质心到转轴的长度。

式（6-93）变形整理后，得到以θ₁、θ₂为输入，平衡锤位置L_x为输出的静力控制方程：

由于结构限制和总的转动惯量因素考虑，实际的静力控制很难做到驱动单元零载荷，对于L_x的输出需要加上行程限幅。

（2）闭环动力控制自适应算法设计 静力平衡属于对于重力矩的简单规划，而对于实际工作过程中加减速过程中的惯量矩没有考虑。因此对于实际控制，需要一种能够针对关节驱动单元当前负载实时进行力矩补偿的算法。

通常进行力矩反馈的方式有两种：

1）在驱动关节的输出轴串联应变式力传感器。

2）通过伺服电动机的电流变化，间接反映力矩变化。

对于低成本的小型操作臂，本项目设计的操作臂并没有使用独立的力矩检测机构和传感器。设计使用的驱动单元“Dynamixel RX24F”舵机具有通过电流环进行无量纲的力矩控制的能力。通过反馈比较即可实现动力的闭环控制，具体PID控制算法如图6-24所示。

图6-24 闭环动力PID控制算法

该方式使用PID控制器实现反馈控制，图6-24中PID参数K_p，T_i，T_d分别为比例增益，积分增益和微分增益系数。反馈量纲匹配增益系数为f_g，图中各系数均未经过整定，需要结合实物平台进行参数整定，获得最终结果。