机器人控制理论基础353

第1章机器人机构与控制概述

0机器人的应用场景1.1

机器人的机构关节的表示符号1.1

机器人的机构ET腕手腕的机构型式三转动手腕1.1

机器人的机构手臂的结构形式手臂与手腕结合后得到的六自由度操作器

一般情况下，在机器人的各关节安装有电位计（Potentialcode），或传感器以测量关节转角的位移。控制装置依据传感器测量数据，确定机器人的关节驱动，然后以适当的输入使指尖尽可能沿着目标轨迹运动。机器人位置控制的简略框图1.2机器人的控制控制系统的一个例子位置/速度反馈伺服1.2机器人的控制示教/再现的机器人系统直接示教:操作者用手抓住安装在机器人末端的手柄（操纵杆、游戏棒等），让机器人按照实际作业顺序进行动作。机器人控制器记忆各关节的位移和速度。间接示教:把作业顺序数值化或符号化，然后依次将作业顺序输入机器人的计算机中。1.2机器人的控制1.3有关的矩阵理论和稳定性理论对任意矩阵A，满足下式矩阵A+至少有一个存在这个A+被称为的广义逆矩阵。

广义逆矩阵假定，并且，则利用矩阵B、C，矩阵D给定：可以证明，A+存在。接着，唯一性。取两个A+即：，然后下式：因此，唯一性得证。1.3有关的矩阵理论和稳定性理论广义逆矩阵性质：若，则若，则若，则矩阵A：矩阵A可以进行奇异值分解1.3有关的矩阵理论和稳定性理论考虑联立1次方程组，当A是方阵且可逆时(n=m)，若n≠m，则引入广义逆A+，k是n维任意矢量。在所有的解中，范数||x||最小的解为：1.3有关的矩阵理论和稳定性理论奇异值分解若给定任意矩阵A。它的特征值按从大到小的顺序依次排列为，然后，定义

所以有这时，矩阵U和矩阵V存在，是正交矩阵，所以，它们满足1.3有关的矩阵理论和稳定性理论奇异值分解而及的第个元素各为，于是,，。有

分解上式，得到中的以外的部分计算方法不变。关于，在求出以后，求以外的部分与的以外的部分的确定方法类似。并且：1.3有关的矩阵理论和稳定性理论Lyapunov的稳定性理论上式在初始条件(t0,x0)下，有唯一解：Lyapunov稳定：系统对于任意选定的实数，都对应存在另一实数，使当成立时，从任意初始状态x0出发的解都下式。稳定状态与不稳定状态包含原点的某一区域Ω定义的标量函数。在x=0时，满足；但时，满足，则认为在Ω中正定（或非负定）。同样，在x=0时，满足；但s时，满足，就说负定（或非正定）。某一函数在Ω中正定，具有连续的导数，按时间取微分，得到：当上式非负定时，就称这个函数为Lyapunov函数。1.3有关的矩阵理论和稳定性理论Lyapunov的稳定性理论1.3有关的矩阵理论和稳定性理论Lyapunov函数条件2'1）在某一原点的附近Ω中，存在Lyapunov函数；2）是负定函数；并且，条件2）也可以叙述为：2‘）对于任意的的系统的解，在时，不恒等于0。1.3有关的矩阵理论和稳定性理论Lyapunov的稳定性理论线性系统的情形，即：矩阵指数函数系统的解平稳点的充分必要条件是：对于任意的正定对称矩阵有满足下的正定对称矩阵存在且矩阵唯一并设：那么，1.3有关的矩阵理论和稳定性理论Bellman-gronwall推论Bellman-gronwall推论：全部是所定义的实连续函数，且，可微。这时，若满足式下式所示的不等式：则：1.4控制中常用的传感器内部传感器：

检测机器人自身位置、速度信息.外部传感器：

感知作业环境的传感器。机器人系统与传感器信息1.4控制中常用的传感器1.4.1外部传感器1）触觉传感器

接触觉:感知对其外部的物体接触

压觉:感知对象物的抓持力

力觉:感知外界所产生的抵抗力

滑觉:感知物体滑动2)接近觉传感器

是用于估计接近物体但没有接触到物体的状态。3）视觉传感器以光作为媒体，测量对象物的位置和速度等1.4控制中常用的传感器1.4.2内部传感器1）位置、角度的传感器位置传感器2）角速度传感器转子码盘1.4控制中常用的传感器1.4.3机器人控制中的传感器力觉传感器力觉传感器中的桥式测量电路力传感器系统基于力控制的配合作业1.4控制中常用的传感器1.4.3机器人控制中的传感器接近觉传感器接近觉传感器焊接中接近觉传感器1.4控制中常用的传感器1.4.3机器人控制中的传感器滑觉传感器滑觉传感器1.4控制中常用的传感器1.4.3机器人控制中的传感器视觉传感器图像面的投影双目视觉第2章机器人操作器的运动学

2.1物体的位置与姿态2.1.1物体坐标系基础坐标系与物体坐标系基础坐标系为，原点，坐标轴物体坐标系为，原点为，坐标轴从到的矢量在中所表示的矢量写作单位矢量的姿态2.1物体的位置与姿态2.1.2旋转矩阵(a)手坐标系(b)与关系手的位置姿态的表示AxB，AyB，AzB是互相正交的单位矢量，满足取为3×3阶单位矩阵时，有下式这里，叙述几个旋转矩阵的性质：这里，表示矩阵的逆矩阵，因此，旋转矩阵是正交矩阵。2.1物体的位置与姿态的关系图与的原点一致时，从所看到的某一矢量，表示为，而同一矢量，从看时，可表示为对于任意的可以得到进一步，考虑与的原点一致时的第3个坐标系，在中的取值用表示，有2.1.2旋转矩阵2.1物体的位置与姿态2.1.3欧拉角(a)(b)θ(c)2.1物体的位置与姿态2.1.3欧拉角(a)(b)θ(c)如果给定任意的时，我们来看如何求对应的欧拉角。应满足：2.1物体的位置与姿态2.1.3欧拉角(a)(b)θ(c)可得到如下关系：进而得到下两式：2.1物体的位置与姿态2.1.3欧拉角atan2(a,b)对任意的标量接着求、，如果若限制θ的范围在，就可以得到唯一的一种欧拉角，即：如果，对应的欧拉角为：2.1物体的位置与姿态2.1.4Roll，Pitch,York(a)(b)(c)Roll，Pitch，York1.首先，绕坐标轴旋转角，而得到坐标系；2.然后，绕轴旋转角度，而得到坐标系；3.最后，绕轴旋转角度，成为。这时，角度称为Roll角，为Pitch角，为York角。2.2坐标变换2.2.1齐次变换坐标系与的关系

中用所表示的点在中表示为绕轴旋转角后，两坐标系的齐次变换关系为：旋转后将沿方向移动2，沿轴方向移动为12.2坐标变换2.2.2变换的积与逆变换（Ⅰ）相对于仅移动，同时，仅使其旋转，就可以得到。进一步，相对于移动，同时，使其旋转，也可以得到。（Ⅱ）相对于旋转，同时，移动，就可以得到。进一步，相对于，旋转，同时，移动，也可以得到。2.2坐标变换2.2.2变换的积与逆变换2.3关节变量与机器人末端位置2.3.1一般的关系N自由度机器人机构关节变量矢量末端位置矢量与的关系2.3关节变量与机器人末端位置2.3.1一般的关系2自由度操作器运动学的解手指尖位移矢量关节变量的解其中2.3关节变量与机器人末端位置2.3.2连杆参数n自由度操作器的连杆与关节编号关节轴和关节变量表示的连杆参数关节轴i和关节轴i+1之间公共垂线的长度ai关节轴i与i+1方向的正交投影之间的夹角关节轴i上的两个公共垂线的垂足之间的距离垂直于关节轴的平面的两个公共垂线的正交投影之间的夹角Denavit-Hartenberg参数：2.3关节变量与机器人末端位置2.3.3连杆坐标系连杆坐标系可以将连杆参数表达成下列的形式。

=沿轴，从移动到的距离

=绕轴，从旋转到的角度

=沿轴，从移动到的距离

=绕轴，从旋转到的角度所以：对及进行行下列变换，得到：（1）沿轴平动（角标i-1）（2）绕旋转（3）然后，沿（即）移动（4）绕回转后的旋转2.3关节变量与机器人末端位置2.3.3连杆坐标系连杆坐标系（关节1为回转关节时）或者，（关节1为平动关节时）（关节n为回转关节时）或者，（关节n为平动关节时）2.3关节变量与机器人末端位置2.3.3连杆坐标系具有移动关节的连杆坐标系的确定方法关节i与i+1平行时的连杆坐标系的确定方法（1）确定关节轴。（2）确定公共垂线。（3）确定连杆坐标系。（4）关节轴或公共垂线的确定方法中存在任意性时，返回顺序（1）或（2），以使尽可能多的连杆参数为0而进行修正。2.3关节变量与机器人末端位置2.3.3连杆坐标系Stanford操作器i100020-90°3090°0400°050-90°06090°02.3关节变量与机器人末端位置2.3.4正运动学问题的解法坐标系相对于的齐次变换对应于基础坐标系，末端器的位置和姿态表示为2.3关节变量与机器人末端位置2.3.4正运动学问题的解法PUMA型机器人臂PUMA型机器人的连杆坐标系2.3关节变量与机器人末端位置2.3.4正运动学问题的解法PUMA型机器人臂2.3关节变量与机器人末端位置2.3.4正运动学问题的解法PUMA型机器人臂2.3关节变量与机器人末端位置2.3.4正运动学问题的解法PUMA型机器人臂2.4逆运动学问题3自由度操作器连杆坐标系2.4逆运动学问题连杆坐标系若手指末端的位置给定时，又已知，可得正运动学方程2.4逆运动学问题连杆坐标系求时，逆运动学问题的解，于是，考虑下列关系可得：2.4逆运动学问题连杆坐标系

再考虑：得到：

若把变量分开来解，求解就更方便些。比如，首先求再对上述3式的两边，取平方后求和求得：2.4逆运动学问题连杆坐标系剩下，考虑如下关系：所以2.4逆运动学问题用几何法求解同样的逆运动学问题(a)X1轴与[rx,ry]T相同方向时(b)X1轴与[rx,ry]T相反方向时平面上的投影2.4逆运动学问题用几何法求解同样的逆运动学问题(a)图2.28(a)的情形(b)图2.28(b)的情形向平面投影2.4逆运动学问题2.4.1.基于运动学方程变换的方法

上例中，讨论了手指末端采用三维位置矢量表示的情形。下面，考虑手指末端一般位姿，即采用6个自由度操作器的场合，运动方程式基本形式作一变换，可得到一个由12个方程组成的方程组2.4逆运动学问题2.4.2.Piepper方法

实际的机器人中，存在如上图所示的操作器，其手指末端的三个关节均为转动关节，且关节轴交于一点，这时的原点全部与这个点一致。如果手指末端的位置姿态已知，就意味着是固定的，所以，的原点位置也就确定了。一般按照的原点确定。这样做，能够计算出的位置和姿态，及从看到的的姿态。最后，从这个姿态确立。图2-30可运用Pieper法的操作器2.4逆运动学问题图2-30可运用Pieper法的操作器PUMA型机器人臂首先，求得2.4.2.Piepper方法2.4逆运动学问题图2-30可运用Pieper法的操作器PUMA型机器人臂接着求，根据下式，得到：当时，由下式给出，而当时，即有：2.4.2.Piepper方法2.5雅可比矩阵2.5.1物体的运动速度角速度失量原点位置的变化速度可以用原点位置的矢量关于时间t的微分，即来表示姿态的变化速度，可按下面的2个表达方法进行讨论

（1）方法I：考虑由表达姿态的3个变量（如欧拉角等）构成的矢量，用它对时间取微分（2）方法Ⅱ：考虑与基础坐标系的原点保持一致的坐标系，这时，物体的运动成为绕某轴的旋转运动。所以，的旋转速度的主方向沿着这个瞬时旋转轴，旋转速度的大小用表示。称为角速度矢量。2.5雅可比矩阵2.5.1物体的运动速度

与的积分相同的两种运动第一种情况第二种情况中在t=0时，与保持一致，的积分值是：2.5雅可比矩阵2.5.1物体的运动速度

与的积分相同的两种运动第一种情况第二种情况中当t=2时，从看到的，为：情形1情形22.5雅可比矩阵2.5.1物体的运动速度

旋转速度的两种表示2.5雅可比矩阵2.5.2雅可比矩阵的定义

操作器的指尖与关节速度的关系2.5雅可比矩阵2.5.2雅可比矩阵的定义

平面3自由度操作器操作器的正运动学方程于是，因此，得到：2.5雅可比矩阵2.5.3操作器的连杆速度之间的关系

考虑的任意矢量表示在中的形式，即，对其按时间取微分，可得这里，若设，则下式成立得到式2.5雅可比矩阵2.5.3操作器的连杆速度之间的关系

坐标系间的移动速度以作为基准，下面关系成立：对时间取微分,得到2.5雅可比矩阵2.5.4雅可比矩阵的一般表达式

考虑这样的变换关系，其中与都固定在第n个连杆上，所以：如果n个关节都为旋转关节的场合，有：这里：。因此，设：

最后，得到：2.5雅可比矩阵2.5.4雅可比矩阵的一般表达式

矢量与其中，R表示回转关节，P表示移动关节2.5雅可比矩阵2.5.4雅可比矩阵的一般表达式

PUMA型机器人臂2.5雅可比矩阵2.5.4雅可比矩阵的一般表达式

PUMA型机器人臂2.5雅可比矩阵2.5.5实现给定的手指尖速度的关节速度

首先假定可逆，另外，时，利用的广义逆矩阵，解为：2.5雅可比矩阵2.5.6奇异位形

2自由度臂的奇异位形如果或无论怎样给定关节速度，手指末端的速度都不能指向坐标原点与手指末端的连线方向，所以这个方向不能使手指末端产生动作。2.5雅可比矩阵2.5.6奇异位形

首先，如果对应个自由度臂的雅可比矩阵为，设：如果满足下式的腕部姿态存在，则把这个定义为奇异姿态。奇异位形存在的条件进一步，用代替，也能够知道是否存在奇异位形。即设：

奇异位形必须满足：奇异位形存在的一个必要条件2.5雅可比矩阵2.5.6奇异位形

PUMA机械臂的奇异位形2.6静力与力雅可比矩阵2.6.1不同直角坐标系中表示的等效力

固定在物体上的坐标系与假定在物体上固定两个坐标系与，当这个物体相对于运动时，它们之间的速度关系进一步，在中用表示，类似地，可定义，利用旋转矩阵，，可得到下面关系：2.6静力与力雅可比矩阵2.6.1不同直角坐标系中表示的等效力

固定在物体上的坐标系与另一方面，作用在的原点的力与力矩等效价于作用在的原点的力与力矩，它们之间应该满足下列关系：2.6静力与力雅可比矩阵2.6.1不同直角坐标系中表示的等效力

与的关系力传感器的测量值为然后，得：可求得：2.6静力与力雅可比矩阵2.6.2末端外力与等效关节驱动力

求与作用在手指坐标系原点的力与力矩等效的关节驱动力当关节i为旋转关节时，

而当关节i为移动关节时，第3章机器人机构动力学

3.1拉格朗日法与牛顿-欧拉法拉格朗日法：直观上难以理解，但物理意义明确动力学方程式形式简洁，便于分析标准动力学方程推导方法牛顿-欧拉方法直观上容易理解便于计算应用3.2力学基础知识1.牛顿-欧拉运动方程式惯性坐标系中的牛顿运动方程：N外力矩，F外力，刚体质量质心矢量s刚体的角速度为，任意微小部分的体积单元为dv，密度为，G到dv位置矢量为r，进而得到刚体角动量E为：

(3-8b)3.2力学基础知识1.牛顿-欧拉运动方程式建立固定在物体上的坐标系，于是可得进一步，得到中表示的欧拉运动方程3.2力学基础知识2.虚功原理k个质点组成的系统，的位置向量为系统的完整约束：不完整的约束：注：使用虚功原理来推导的运动方程适用于非完整约束系统；

而基于变分原理的方法（例如汉密尔顿原理）不适用(3-23)该系统的虚位移，它是一组满足约束条件的无穷小位移3.2力学基础知识2.虚功原理

r1和r2因受到扰动而分别变为

。应有将上述乘积展开，可得考虑由两个质点组成的某个系统，约束为l。r1和r2满足所有虚位移组成的集合任一组虚位移所做功的总和、约束力所做的总功均为零：质点1作用力质点2作用力约束力的虚功3.2力学基础知识3.拉格朗日运动方程自由度为n的质点系的广义坐标为

，惯性系

中的矢量

：质点

的质量为

，作用力为

，由牛顿运动方程式：取

的内积，并求和，则有进一步，可得K是质点系的动能，Qi称为对应于qi的广义力。包含重力部分引入拉格朗日函数L=K-P3.3基于拉格朗日方法的运动方程的推导2自由度的操作器中各符号的含义如下：回转角质量

连杆长度从关节i到连杆i的质量中心的长度连杆1的动能和势能如下：连杆2的动能和势能如下：根据拉格朗日法得到动力学方程：3.3基于拉格朗日方法的运动方程的推导动力学方程改写如下：其中，Mii为等效惯性系数

Mij为相关惯性系数hij为离心加速度系数hijk(j≠k)为哥氏加速度系数gi表示重力载荷是惯性力，

表示离心力和哥氏力，

重力载荷更简洁的形式：1.n自由度操作器3.3基于拉格朗日方法的运动方程的推导从基础坐标系

看到连杆i坐标系

中某个点的位置速度速度的二次方连杆i的动能1.n自由度操作器3.3基于拉格朗日方法的运动方程的推导连杆i的动能广义惯性矩阵(惯性矩)(惯性积)(连杆质量)(质心位置)1.n自由度操作器3.3基于拉格朗日方法的运动方程的推导其中重力加速度为连杆i的势能Pi拉格朗日函数：动力学方程：1.n自由度操作器3.3基于拉格朗日方法的运动方程的推导动力学方程：M(q)的第(i,j)个元素的第i个元素g(q)的第i个元素1.n自由度操作器3.3基于拉格朗日方法的运动方程的推导动力学方程：可以发现：其中：2.并行驱动的2自由度操作器3.3基于拉格朗日方法的运动方程的推导伺服1供给台座与连杆1之间的驱动力矩τ1，伺服2通过皮带、链、齿轮、平行四边形机构等提供给作用在基座与连杆2之间的驱动力矩τ2。增加一个自由度，可作为手臂机构，能够配置在离台座较近的位置。2.并行驱动的2自由度操作器3.3基于拉格朗日方法的运动方程的推导τ2没有作用在连杆1和2之间，而是作用在台座与连杆2之间。θ2是台座与连杆2之间的夹角。注意：区别于串行驱动连杆1的动能和势能如下：连杆2的动能和势能如下：对于连杆2，它的质量中心在

，2.并行驱动的2自由度操作器3.3基于拉格朗日方法的运动方程的推导连杆2的动能和势能如下：对于连杆2，它的质量中心在

，可求得，动力学方程：3.4基于牛顿-欧拉法的运动方程的推导1.推导的基本过程1.给定各关节的qi2.计算从基础坐标系所观察到连杆i的回转角速度ωi，角加速度

，移动速度

，移动加速度3.计算实现这些运动所需施加在质量中心的力

及回转力矩4.根据指尖的力f4及力矩n4，以指尖到基座的顺序，计算在各关节处施加的力fi与力矩ni5.求各关节驱动力τi2.操作器的连杆加速度之间的关系3.4基于牛顿-欧拉法的运动方程的推导坐标系

之间的速度关系：

求导，并结合，可得

2.操作器的连杆加速度之间的关系3.4基于牛顿-欧拉法的运动方程的推导3.4基于牛顿-欧拉法的运动方程的推导3.

n个自由度的操作器求连杆的角速度关系求连杆的加速度关系3.4基于牛顿-欧拉法的运动方程的推导3.

n自由度操作器定义

为坐标系

原点O0到连杆i的质心Gi的向量，可得施加连杆i上的总外力

及外力矩

连杆i-1施加连杆i上的力及力矩分别为

和，同时考虑可得得到关节驱动力如下3.4基于牛顿-欧拉法的运动方程的推导3.

n自由度操作器惯性张量

变成一个常值时，可以重写如下：3.4基于牛顿-欧拉法的运动方程的推导3.

n自由度操作器考虑重力时考虑摩擦力时摩擦力通常包含库仑摩擦力和粘性摩擦力，于是，上式进一步写为：其中，3.5运动方程的使用方法和需要的计算量逆动力学问题：

目标轨迹已知，计算实现所需的驱动力。正动力学问题：

驱动输入给定时，求物体运动过程。逆动力学问题的两种解法第一种方法是把关节轨迹代入由拉格朗日法所建立的运动方程式另一计算方法是采用由牛顿欧拉法导出的运动方程式计算驱动力1.问题分类3.5运动方程的使用方法和需要的计算量

2.计算量对比方法一（拉格朗日法）：由矩阵A、B计算矩阵C，然后计算Cy。(n3+n2)次乘法运算和(n3-n)次加法运算方法二（牛顿-欧拉法）：先计算By=z，最后再计算Az。2n2次乘法运算与(2n2-2n)次加法运算方法乘法运算量加法运算量拉格朗日法（66.271）（51.548）牛顿欧拉法（852）（738）3.5运动方程的使用方法和需要的计算量3.正动力学求解

时刻t得到

与

时，求解时刻t+Δt时的

和

的值

根据和动力学方程，得到下式假定

在时间区间[t,t+Δt]内是一个常数，即有略去高阶项(Δt)2该方法相当于数值解法中的欧拉法，若想提高精度，可采用龙格库塔法法3.6操作器的参数辨识1.基于拉格朗日方程的辨识方法辨识依据待辨识参数（并非都可辨识）3.6操作器的参数辨识1.基于拉格朗日方程的辨识方法

Kd的各元素是n×nd矩阵，且是

的函数，Ld是nd×10n常数矩阵,Ld的阶数为nd。在N个不同数据点，

的值用

表示。有：Kd*Ld*最小nd的表达式3.6操作器的参数辨识2.手指末端载荷的辨识没有施加手指末端载荷时，参数中ϕd∗值就是ϕd∗a。

施加载荷时的辨识结果是ϕd∗b。接下来介绍依据ϕd∗a和ϕd∗b辨识载荷的质量m1、从Σ2所看质心的位置2s1，以及惯性张量2I1的方法。从

定义有：若

，则：第4章操作器的可操作性

4.1可操作性椭球与可操作度可操作性椭球位移矢量用q表示，末端执行器的位置和姿态用表示速度矢量v与关节速度的关系为：引入欧拉范数，考虑满足的关节速度所能够实现的全部末端速度的集合，就形成了m维欧几里德空间中的椭球体。

这个椭球体最长的主轴半径方向是容易产生最大末端速度的方向，而主轴半径最短的方向对应仅产生最小末端速度的方向。4.1可操作性椭球与可操作度可操作性椭球下式可用于描述可操作性椭球：由矩阵理论知：可将可操作性椭球重写为：对J进行奇异值分解，即可求出可操作性椭球的主轴4.1可操作性椭球与可操作度可操作性椭球对J进行奇异值分解，即可求出可操作性椭球的主轴可操作性椭球的主轴可按σ1u1,σ2u2,⋯,σmum给定。4.1可操作性椭球与可操作度可操作性椭球用椭球体的体积cmw表示操作器的操作能力:可操作度w具有以下的性质：1） 2）当m=n时，即操作器不具有冗余性时， 3）当w≥0且成立，即操作器处于奇异位形时，可操作度可解释为表示离开奇异位形的一种距离。4）当m=n时，4.1可操作性椭球与可操作度可操作性椭球满足上述关系的关节速度构成v的全体集合，可操作椭球成为m维空间内的平行多面体，它的体积由2mw给定。假定，就得到：其中，4.1可操作性椭球与可操作度可操作性椭球当m=n时，可操作度J(q)与正则化后的所计算的可操作度之间有下列关系成立：考虑引入伺服系统及减速器的影响末端器能施加给物体的力/力矩与可操作性的关系4.2各种机器人臂的可操作度分析4.2.12关节连杆机构雅可比矩阵J为：2关节连杆机构可操作度w是：求l1

=

l2

=

1时的可操作性椭球的主轴首先，为：4.2各种机器人臂的可操作度分析4.2.12关节连杆机构2关节连杆机构它的特征值根据可得：然后，可求得奇异值。对应的特征向量是：4.2各种机器人臂的可操作度分析4.2.12关节连杆机构2关节连杆机构可操作性椭球与可操作度操作力椭球

4.2.2SCARA机器人SCARA型机器人4.2各种机器人臂的可操作度分析雅可比矩阵为：可操作度为：PUMA型机器人4.2各种机器人臂的可操作度分析4.2.3PUMA机器人雅可比矩阵为：可操作度为：求可操作度w最大时的姿态最大化可操作度w，可得到θ3即：PUMA型机器人的最佳手腕姿势4.2各种机器人臂的可操作度分析4.2.3PUMA机器人4.2.44个关节的机器人手4.2各种机器人臂的可操作度分析具有4个关节的末端4.2.44个关节的机器人手4.2各种机器人臂的可操作度分析最优末端姿势可操作度的最大值随la的变化w2是椭球的最小半径与最大半径的比w3是椭球主轴的最小半径w4是椭球体的主轴半径σ1,σ2,⋯,σm的几何平均值4.3各种其它可操作性指标操作器的动力特性即：末端末端器位置矢量r与关节矢量q的关系4.4动力学可操作性4.4.1动力学可操作椭球与动力学可操作度对时间微分有：进而可得：4.4动力学可操作性4.4.1动力学可操作椭球与动力学可操作度引入新的变量和考虑满足的关节驱动力能实现末端尖的加速度的全部集合，即：上式表示m维欧几里德空间内的椭球。把它称为动力学可操作椭球。DME的体积用cmwd表示，即：4.4动力学可操作性4.4.1动力学可操作椭球与动力学可操作度的奇异值分解为这个椭球体的主轴由σd1ud1,σd2ud2,⋯,σdmudm给定4.4动力学可操作性2关节连杆机构4.4.22关节连杆机构连杆2的质量，从关节2到其质心的长度，惯性矩，分别为：4.4动力学可操作性2关节连杆机构4.4.22关节连杆机构考虑到Ta,Tr仅仅是对wd乘以标量。令：可得到：4.4动力学可操作性4.4.22关节连杆机构动力可操作性椭球与可操作度（无重力时）动力可操作性椭球与可操作度（有重力时）动力可操作性椭球与可操作度（有重力lg1=0.4m，lg2=0时）第5章位置控制

5.1目标路径与目标轨迹1.由关节变量确定路径的方法初始时刻0和终止时刻tf的关节位移初始时刻0和终止时刻tf的关节速度、加速度满足以上条件的五次多项式五次多项式系数5.1目标路径与目标轨迹1.由关节变量确定路径的方法令α5=0，ξ(t)将成为四次多项式。四次多项式与直线插补结合起来，就可以比较容易给定以下几种情况下的轨迹。（1）在初始点ξ0处，从静止状态，加速、等速、减速到达终点并在终点ξf处静止。根据加减速时间Δ确定插值中间点ξ02，ξf1。首先取ξ01，ξf2为：ξ01=ξ0，ξf2=

ξf，选择ξ01与ξf2使它们位于连结ξ02与ξf1的直线上。然后，在ξ0与ξ02之间及ξf1与ξf之间，采用四次多项式规划轨迹。在各端点处，速度方向与折线{ξ0，ξ01，ξf2，ξf}的斜率保持一致，端点处加速度取零。ξ02与ξf1之间直线连结，于是0⩽t⩽2Δ为加速区间，2Δ⩽t⩽tf-2Δ为等速区间，tf-2Δ<t⩽tf为减速区间。5.1目标路径与目标轨迹1.由关节变量确定路径的方法取Δ=0.25s第一个关节角θ1：第二个关节角θ2：5.1目标路径与目标轨迹1.由关节变量确定路径的方法（2）在ξ0从静止状态出发，通过中间点ξ1,ξ2，在ξf处停止，且未精确地通过中心点2.按末端位置确定轨迹的方法5.1目标路径与目标轨迹（1）单轴旋转法

坐标系ΣA，ΣB之间的转换关系：

等效旋转轴k=[kx,ky,kz]T

绕旋转轴的旋转角α[k,α]对应的旋转矩阵R(k,α)为：任意矢量r1仅绕k旋转α角就成为r2

2.按末端位置确定轨迹的方法5.1目标路径与目标轨迹（2）双轴旋转法

从r0看到的rf，为(ϕ,θ,φ)r0的姿态为其中

，表示由

转动后的z轴于是，可以根据r0来变换rf与

轨迹是这样得到的：绕r0与rf的末端接近方向矢量垂直轴旋转θ角，接着，在各时刻，在绕手指接近方向矢量转(φ+ϕ)角。2.按末端位置确定轨迹的方法5.1目标路径与目标轨迹中间姿态(0.5s)初始姿态（t=0s）最终姿态（tf=1s）单轴旋转法双轴旋转法

考虑粘性阻尼摩擦的操作器动力学方程5.2线性反馈控制1.线性反馈控制定律的有效性伺服系统的动力学方程式系统的动力学方程式将位移/速度的反馈规律应用于这个系统后闭环系统的动力特性近似为该式所示的是一个各关节相互独立的二阶系统。据此，如果适当地选取Gr与Kp,Kv的比值，系统的响应特性是可以调节的。5.2线性反馈控制2.位置/速度反馈控制规律的稳定性Kp取对称正定矩阵，且增加重力补偿项根据上两式，得到取标量函数：考虑如下关系：可以得到：不能精确地知道重力项g(q)及库仑摩擦时，修正控制率如下：1.基本思想5.3线性化补偿的双闭环控制关于关节变量的线性非干涉系统（线性化补偿控制率）不考虑模型化误差和外界干扰时，上述系统可以跟踪预期轨迹补偿器：误差：令：0<α⩽1,ωc>0可得到1.基本思想5.3线性化补偿的双闭环控制θd=[θd1,θd2]T,uq=[uq1,uq2]T补偿器时的输入命令τ1,τ2n维输出变量非线性状态反馈补偿方法得到关于输出y的线性非干涉系统设计对应上式的伺服补偿器，可得到一种等效的控制系统。2.控制系统构成5.3线性化补偿的双闭环控制

实现上述控制系统，不可能取小采样周期。但如果采样周期较大，控制系统的性能如稳态误差、稳定性和适应性就会变得很差，对此我们将提出以下的对策。第一个对策：(5-37a)(5-37b)(5-38)第二个对策：3.并行计算方式5.3线性化补偿的双闭环控制(5-37a)(5-37b)(5-38)

逆动力学计算过程的子任务分解：3.并行计算方式5.3线性化补偿的双闭环控制(5-37a)(5-37b)(5-38)

逆动力学计算过程的子任务分解：3.并行计算方式5.3线性化补偿的双闭环控制子任务之间的动作关系并行计算流程1.线性伺服系统理论5.4伺服补偿器的设计与评价直流伺服电机与载荷力矩τm与电机电流i的关系电动机输入端的控制电压vb电动机回路电动机的惯性力矩，粘性阻尼系数分别为Jm,Dm，若取τ1为电动机的负载力矩，得下式负载的旋转角，惯性力矩，粘性阻尼系数分别为θ1,J1,D1，齿轮系统减速比为n，就有：输入v与输出θl之间1.线性伺服系统理论5.4伺服补偿器的设计与评价忽略电感L，得到取适当常数a1,a2,a3，可得到G1(s)=b1G2(s)=b2s闭环系统传递函数：固有频率ωn.及阻尼系数ζ再令

，可得1.线性伺服系统理论5.4伺服补偿器的设计与评价当ζ<1时，初始响应为：

在可实现的范围内，为了取得好的速度顺应性，ωn尽可能取得大一些。1.线性伺服系统理论5.4伺服补偿器的设计与评价G(s)按式三阶系统给定时，闭环传递函数：此时，G1(s)=b1G2(s)=b2s+b3s这时，令可得三维系统的惯性响应

2.稳定余度与灵敏度5.4伺服补偿器的设计与评价带前馈补偿和反馈补偿的伺服系统从输入r到输出y的传递函数当G有一小的变化

时，可得到，

这表明了对于控制对象的特性的变化率，传递函数的变化率由S给定，S称为闭环系统的灵敏度函数，对于任意的ω，如果满足公式：

即使有任意变化，闭环系统仍然保持稳定。所以可以说，|T|-1表示稳定余度1.单关节的动作控制(1)前馈控制给定一个所需的关节轨迹qd(t)，最简单的控制方法是按照下式选择指令速度其中

为预期关节速度。由于实现上式不需要传感器数据，因此将这种控制方式称为前馈或无反馈控制。(2)反馈控制实际上，在前馈控制律（5-74）下，位置误差会随着时间而不断积累。另一种方法是连续测量各个关节的实际位置，并施加一个反馈控制器。5.5速度输入下的位置控制5.5速度输入下的位置控制2.反馈控制1：P控制与一阶误差动力学反馈控制器

时，P控制器下的误差动态可表示为上式的解为：

当时间t趋于无穷大时，上式将收敛至非零值c/Kp。此时的稳态误差为非零值；关节位置滞后于预期位置。5.5速度输入下的位置控制2.反馈控制2：PI控制与二阶误差动力学反馈控制器PI控制器方框图误差动力学方程求导，得到当Ki>0且Kp>0时，PI控制的误差动力方程是稳定的，特征方程的根为特征方程复根与响应过程5.5速度输入下的位置控制3.P控制与PI控制对比P控制的关节和PI控制的关节的运动5.5速度输入下的位置控制4.前馈+反馈控制前馈+反馈控制反馈控制的一个缺点是在关节开始活动之前需要有一个误差。前馈控制可以在没有误差的情况下控制运动，而反馈控制可以限制误差的积累，5.6转矩输入下的位置控制单电机驱动单连杆的结构动力学方程式如下：单关节机器人在重力作用下的旋转旋转摩擦是因粘性摩擦力矩引起的，因此考虑摩擦力矩后，我们的最终模型为简写上式，可得5.6转矩输入下的位置控制1.PID控制与三阶误差动力学PID控制器的方框图PID控制器：PID控制与PD控制的对比设定点的误差动力学方程求导，得到τdist为常数时的特征方程5.6转矩输入下的位置控制1.PID控制与三阶误差动力学为了达到稳定状态，应满足下列条件三个根在Ki从零开始增加时的运动5.6转矩输入下的位置控制2.前馈控制动力学模型给定轨迹发生器中的

，前馈力矩模型误差存在时，前馈控制的结果5.6转矩输入下的位置控制3.前馈控制加反馈线性化误差的动力学方程预期加速度设定为得到

前馈加反馈线性化控制器，又称为逆动力学控制器或计算力矩控制器：计算力矩控制单独的前馈(ff)、单独的反馈(fb)和计算力矩控制(ff+fb)的性能5.6转矩输入下的位置控制4.多关节机器人动作控制动力学方程我们将多关节机器人的控制分为两种类型：分散控制，其中每个关节是单独控制的，关节之间不共享信息；集中控制，可使用多个关节的全部状态信息来计算每个关节的控制。分散多关节控制：要求力学方程可被解耦每个关节上安装一个独立的控制器在无重力的情况下，高速运动的机器人也能实现近似解耦5.6转矩输入下的位置控制4.多关节机器人动作控制·集中式多关节控制计算力矩控制器扩展至多关节机器人仅使用PID控制和重力补偿时的近似如下式在忽略摩擦、完全补偿重力且采用PD控制的情况下可将动力学方程写为定义一个虚拟的“误差能量”若

，可简化为求导，得到重新排列，可得(5-110)

根据Krasovskii-LaSalle不变性原理，总误差能量单调下降，机器人将从任意初始状态收敛至θd(θe=0)处静止。第6章机器人的力控制6.1柔顺控制

阻抗控制：

对作用在指尖或末端器上的外力，以设定的机械阻抗作为控制目标，进行位置和力控制。

导纳控制：

通过测量末端受力，调整关节速度，从而实现对接触力的控制。

柔顺控制：

与阻抗控制类似

混合控制法：

测定各个方向的位置和力的大小，通过反馈，构成追踪目标值的位置控制环及力控制环。6.1柔顺控制有接触力存在时的动力学模型：定义在平衡状态下有下式成立：上式说明，机械手达到位姿控制作用的平衡点时，其特性与操作空间中的广义弹簧单元类似。

若机械手的柔性是依靠控制实现的，则将这种柔性称为主动柔顺（activecompliance），而被动柔顺（passivecompliance）则是指具有普遍弹性类型动力学特征的机械系统。6.1柔顺控制6.1.1被动柔顺考虑两个弹性联结的刚体R、S和两个参考坐标系，满足下式：为令位移与R和S之间弹簧变形相一致，刚体S所受弹簧弹性力为矩阵K表示刚度矩阵（stiffnessmatrix），矩阵和分别表示平移刚度（translationalstiffness）和旋转刚度（rotationalstiffness）。矩阵表示耦合刚度（couplingstiffness）。柔顺矩阵（compliancematrix）C可按下式进行分解6.1柔顺控制6.1.2主动柔顺令分别表示末端执行器坐标系和期望坐标系，相应的齐次变换矩阵：末端执行器坐标系的位置与方向相对于期望坐标系的偏移：操作空间误差向量：求导，可得:6.1柔顺控制6.1.2主动柔顺向量可表示为以下形式：其中，为末端执行器的线速度与角速度向量。因此按上式定义的操作空间误差，可得重力补偿PD控制表达式为当机器人与环境不存在交互作用时，平衡姿态的渐进稳定性可由下式所示的李亚普诺夫函数证明

当存在与环境的交互作用时，在平衡状态有：6.1柔顺控制6.1.2主动柔顺假设雅可比矩阵满秩，可以得到以下方程：以元位移形式对误差重新列写：用元位移形式重新列写第一式，得到：其中表示主动柔顺。6.2阻抗控制6.2.1被动柔顺(a)构造(b)施加水平力的场合(c)施加回转力矩的场合RCC手的概念6.2阻抗控制6.2.1被动柔顺RCC手（三维空间用）施加于棒的外力与棒的微小位移之间的静力学关系：6.2阻抗控制6.2.2主动阻抗法-单自由度情形主动阻抗法运动方程式为：希望的阻抗为：根据反馈控制法，则驱动力为：当外力可以测量时，6.2阻抗控制6.2.2主动阻抗法-单自由度情形主动阻抗法为了取得好的输出响应，应尽可能地增大固有频率即：

而阻尼系数即：例如，阻尼系数取0.6-1.0，限定全部为正，则系统稳定，可以保证其位置偏差和速度偏差为0。6.2阻抗控制6.2.2主动阻抗法-单自由度情形与某一固定物体的接触在受外力的作用的场合，它们在接触中的相互作用如下：结合希望的阻抗，可得：这时的固有频率及阻尼系数为：6.2阻抗控制6.2.2主动阻抗法-单自由度情形与某一固定物体的接触相互作用模型：它的系数的范围是：在接触过程中，系统固有频率保持在30-50Hz之间，阻尼系数保持在0.5以上，并且尽可能在左右取值，也尽可能地取小值，所希望阻抗为:6.2阻抗控制6.2.2主动阻抗法-单自由度情形与某一固定物体的接触所希望阻抗为:实现它的控制律为：接着，通过控制目标轨迹，可以实现所希望的阻抗。例如当及已知时，实现阻抗的目标轨迹可按下式选择：上述的任何一种方法，都需要经过长时间的调节才会达到稳定状态，即应施加的力与目标值一致。6.2阻抗控制6.2.3主动阻抗法-一般情形主动阻抗法末端器的期望阻抗:6维作业向量的偏差：基于期望阻抗，6个自由操作器的运动方程式对时间取微分得：与操作器末端外力等价的关节力:用附加的外力修正操作器的运动方程式:6.2阻抗控制6.2.3主动阻抗法-一般情形主动阻抗法其中控制法则如下：若所希望的机械阻抗：控制法则如下：6.2阻抗控制6.2.3主动阻抗法-一般情形2自由度操作器希望机械阻抗为：控制法则：其中：低速情况下，简化控制率，得到：6.3混合控制法6.3.1基于反馈补偿的控制基于反馈补偿的混合控制定义力误差和位置误差:转换到关节空间:位置控制法则和力控制法则如下：混合控制率如下：6.3混合控制法6.3.1基于反馈补偿的控制转动曲轴的作业自然约束为：人工约束为：混合控制系统原理框图:6.3混合控制法6.3.2动力学混合控制·末端约束表达方法末端约束由在6维欧拉空间的部分区域内，相互独立的超曲面表示。对时间变变量进行微分：再次进行微分：令可得：当矩阵按下式定义时可得，6.3混合控制法6.3.2动力学混合控制·末端约束表达方法球面约束图约束超曲面：另一方面，若取，则：与有如下的关系：6.3混合控制法6.3.2动力学混合控制·末端约束时，机器人机构的运动与约束力末端位置r与关节变量q的关系为：

因此，可以得到：手臂的动力学特性，按下列形式给出：沿约束面的法向方向的分量:与对应的在各关节处的反力是：与作为关节驱动力施加在各关节处：若由上两式求与，则：6.3混合控制法6.3.2动力学混合控制·计算关节驱动力矩将下面的非线性反馈控制率应用于操作器，然后，可以得到线性化系统：6.3混合控制法6.3.2动力学混合控制·计算关节驱动力矩的列矢量所表示的单位力矢量力控制变量，有与的目标轨迹记为：实现目标轨迹所需的关节驱动力矩下式给定：6.3混合控制法6.3.2动力学混合控制·动力学混合控制系统曲线约束2自由度操作器切线矢量及法线矢量：可得下式：线性化后：伺服补偿器为：6.4约束运动6.4.1刚性环境约束方程在关节空间的表达为：计算上式的导数，得到：末端执行器力在关节上产生的反力矩，可用虚功原理计算虚位移满足如下方程

进而得到相应地施加于末端执行器的力为：注意对给定的Sf

，约束雅可比矩阵：重新列写约束方程的导数，可得：6.4约束运动6.4.1刚性环境重新考虑可得，被控速度子空间的维数是6-m，而被控力子空间的维数是m；而且可定义6×(6-m)维矩阵Sv，使其满足下式：矩阵Sv也可用雅可比矩阵的形式来解释。与环境接触的机械手位姿可用独立坐标的(6-m)×1向量r的形式进行局部描述定义逆变换为如下等式成立矩阵Sv可按下式计算6.4约束运动6.4.2柔性环境

假设接触无摩擦，末端执行器在刚体S上施加的相互作用力属于被控力子空间R(Sf)，这样环境形变计算如下：末端执行器相对于平衡位姿的位移可分解为：被控速度子空间的位移分量：环境形变的分量：在末端执行器位移的两侧均左乘SfT考虑和，得到应用

和，可：

其中，

为柔顺矩阵第7章冗余度操作器的控制

7.1冗余度操作器7个以上自由度的操作器就可以被称为冗余度操作器。优点与人手同样方便、灵活便于实现避奇异位形、避障碍物、避机构的关节极限优于普通操作器的操作能力缺点机构复杂，本体质量增大控制方法复杂7.2控制问题的公式化与解法给定作业任务的分解思想：1.把复杂作业，按照任务的优先顺序分解为几个子任务。2.尽可能实现优先排级高的子任务，3.然后，逐步实现优先级低的子任务。例如，在焊接作业中，优先考虑手指端的位置控制；

避障作业中，优先考虑障碍物的回避。1.任务作业分解7.2控制问题的公式化与解法2.问题的公式化和基本方程式

臂的姿态矢量第1个子任务：m1维作业变量r1的目标轨道为r1d(t)(0⩽t⩽tf

)，r1满足第2个子任务：m2维作业变量r2的目标轨道为r2d(t)，r2满足

7.2控制问题的公式化与解法3.第2个子任务按目标轨道（情形1）

臂的姿态矢量令

，可得

令

，可得下式

其中，

k2是n维任意常数矢量。可得到下式：

当第2个子任务由手臂姿态的目标轨迹给定时，可得

进而得到

7.2控制问题的公式化与解法4.第2个子任务按评价函数给定（情形2）

臂的姿态矢量第二个子任务的评价函数定义为p，且p尽可能大

此时k1按下式更新

7.3应用于避障碍物及奇异位形1.障碍物的回避3自由度的操作器，让其手指尖一边跟踪目标轨迹，一边回避障碍物。

7.3应用于避障碍物及奇异位形1.障碍物的回避第1个子任务：手指尖追踪目标轨迹第一个子任务的作业变量r1第一个子任务目标轨迹雅可比矩阵7.3应用于避障碍物及奇异位形1.障碍物的回避第2个子任务：避开障碍物作业变量r2的目标值由示教者给定为：目标关节速度H2=0.2I2H2=07.3应用于避障碍物及奇异位形2.避奇异位形第2个子任务的评价函数取可操作度，控制操作器使可操作性增大，利用机械臂的冗度性来避开奇异位形。假定目标轨迹是从初始状态

，初始位置

到7.3应用于避障碍物及奇异位形2.避奇异位形

(a)表示在式（7-15）中kp=0，即不考虑避开奇异位形时，手臂姿态的变化规律。(b)中给出了kp=20，即回避奇异位形时的情形。(c)表示了

(a)、(b)中的操作度随时间变化的情况。可以看出，利用冗余度可以有效地避开奇异位形。(c)可操作度变化情况7.4关节目标速度的数值计算法广义逆矩阵数值计算方法可转换为下式当矩阵

是正则矩阵时，当矩阵

不是正则矩阵时，ks被定义为，在奇异位形的附近为正数，在其它的地方为0的一个标量函数。例如，可操作度

可用下式确定：这里k0是合适的正的常数，这样就可以把w0在奇异位形附近的值与其它区域的值分开来。第8章机器人的学习控制

8.1学习控制的前提条件学习的前提条件：一次运动在短时间内（t>0）结束；有限时间区间t∈[0,T]内的理想运动轨迹事先根据经验给定；初始化常常是一定的，从初始化以后所试行的回数称为第k回，这时候的初始状态xk(0)在运动开始时，常常按照下面的情况进行初始化：在重复练习中，对象系统的动力学特性保持不变；输出轨迹yd(t)是可测定的。所以，任意第k回试行的误差通常可以按式(8-2)计算下个时间段内的伺服器输入uk+1(t)在记忆中尽可能以简单的递归形式表达，即：8.1学习控制的前提条件D型学习控制法则P型学习控制法则每一步迭代的约束条件：8.1学习控制的前提条件针对工业机器人：3'初始化误差在容许的范围内，即存在ε1>0，满足下面的条件：4'允许对象系统的动力学稍稍有些波动，即对某一ηk(t)，存在ε2>0，使下式成立：5'测量误差ξk：这时，若存在ε3>0，测量误差ξk满足式(8-11)所示的条件：8.2D型学习控制（线性系统）电压控制型直流伺服马达满足：电压控制型直流伺服马达直流伺服马达的速度控制问题闭环系统的动力学:考虑下式:可以得到:一般解:8.2D型学习控制（线性系统）电压控制型直流伺服马达直流伺服马达的速度控制问题给定理想的角速度yd(t)，若给定第k次试行的控制输入vk(t)，输出的角速度yk(t)应该为：第k+1次的试行的控制输入：可以得到：然后有：8.2D型学习控制（线性系统）直流伺服马达的速度控制问题引入下面的函数范数：两边同乘以eλt，并取最大值，得取合适的γ，就有：

这意味着：D型学习控制的收敛性8.2D型学习控制（线性系统）多输入/输出系统：理想轨道的初始条件为：学习的过递推程为：一般的D型学习控制法则8.2D型学习控制（线性系统）以1个自由度的线性系统作为例子来说明学习控制的收敛性。这时，系统：a1=a2=1.0,b=1.0,Γ=1.0

对应一个自由度系统D型

学习控制方式的收敛性对应一个自由度系统D型

学习控制方式的收敛情形

a1=a2=1.0,b=1.0,Γ=0.58.2D型学习控制（线性系统）Γ=1.0时，收敛条件如下Γ=0.5时，有下式成立，也满足收敛条件，只是这时的收敛速度比Γ=1.0时的收敛速度慢一些。a1=a2=1.0,b=1.0,Γ=1.0

对应一个自由度系统D型

学习控制方式的收敛性对应一个自由度系统D型

学习控制方式的收敛情形

a1=a2=1.0,b=1.0,Γ=0.58.3机器人臂中的D型学习控制PD反馈控制下的机器人动力学方程

采用D型学习控制方法，其法则的递归形式机器人D型学习控制系统初始条件总是一定的常数，并满足下式和收敛性定理：[D型学习控制的收敛性定理（机器人臂）]8.3机器人臂中的D型学习控制证明上述的定理，设：在区间[0,T]内，n维矢量函数e(t)的λ范数为：第k+1与第k次运动的差：假定

在各自的

的附近。这时下式成立运动的差乘以，并取范数e-λtλ将

代入上式的右边，得到下式8.3机器人臂中的D型学习控制采用状态变量

可得写成简洁形式，得

局部地满足Lyapunov条件，非线性函数h也局部地满足Lyapunov条件，即：然后，可得：对时间取积分，并且乘以e-λt，有：取范数，可以得到：应用Bellman-Gronwall推论，就有：8.3机器人臂中的D型学习控制下式成立R(qk)满足全局Lyapun