机器人雅可比矩阵

机器人技术基础第四章机器人雅可比矩阵(ManipulatorJacobian)课程的基本要求:掌握运动和力雅可比矩阵的物理含义及基本的求解方法4.1雅可比矩阵的定义回顾：基本概念刚体位姿描述和齐次变换齐次坐标,欧拉角与RPY角齐次变换和齐次变换矩阵的运算操作臂运动学连杆参数、连杆坐标系连杆变换和运动学方程机器人关节空间与操作空间关节角位置和操作臂末端的直角坐标位置关节空间操作空间运动学正解运动学反解)(qxx)(qxxq关节角速度和操作臂末端的直角坐标速度关节空间操作空间运动学正解运动学反解4.1雅可比矩阵的定义(Jacobianmatrix)操作空间速度与关节空间速度之间的线性变换。jiijqqxqJqqJx)(操作臂的雅可比矩阵，建立了从关节速度向操作速度的映射关系。进行机器人操作臂的速度分析。)(qJ式中，称为末端在操作空间的广义速度，简称为操作速度，为关节速度；是6×n的偏导数矩阵，称为操作臂的雅可比矩阵。它的第i行第j列元素为，i=1,2,…,6;j=1,2,…,n。xq)(qJ操作臂的运动学方程，描述机器人操作臂的位移关系，建立了操作空间与关节空间的映射关系。)(qxxpTpBABA刚体的齐次变换矩阵，描述刚体之间的空间位姿关系。假设矢量yRm为uRn的函数y=y(u)),,,(),,,(),,,()(y)()(21212211m21nmnnuuuyuuuyuuuyyyuuuy相对于u的偏导数定义为uuuuuuuuuuy)J(yR)J()(y)()(212221212111m21nmnmmmnnuyuyuyuyuyuyuyuyuyyynuyuyuyy12111u对于m=1,（标量对矢量的导数）根据上述一般数学定义，对于6关节机器人：设有6个各含6个独立变量的函数，简写为x=f(q)。求微分，注意，如果函数f1(q)到f6(q)是非线性的，则是q的函数，写成，式子两边同除以时间的微分，上式中，66的偏导数矩阵J(q)叫做雅可比矩阵。其中qqfxqfqqJx)(qqJx)(111262212666126(,,,)(,,,)()(,,,)xfqqqxfqqqxfqxfqqqjiijqqxqJ雅可比矩阵机器人关节数*雅可比矩阵的行数取决于机器人的类型雅可比矩阵在机器人中的应用可以把雅可比矩阵看作是关节的速度变换到操作速度V的变换矩阵在任何特定时刻，q具有某一特定值，J(q)就是一个线性变换。在每一新的时刻，q已改变，线性变换也因之改变，所以雅可比矩阵是一个时变的线性变换矩阵。在机器人学领域内，通常谈到的雅可比矩阵是把关节角速度和操作臂末端的直角坐标速度联系在一起的。必须注意到，对于任何给定的操作臂的结构和外形，关节速度是和操作臂末端的直角坐标速度成线性关系，但这只是一个瞬间关系。q(x,y)21xyl1l2例4.11221112211slslyclclx将平面2R机械手的运动学方程两端分别对时间t求导,则得其雅可比矩阵为平面2R机械手的运动学方程为对于关节空间的某些形位q，操作臂的雅可比矩阵的秩减少、这些形位称为操作臂的奇异形位：操作臂的雅可比矩阵的秩减少的形位（数学上）操作臂在操作空间的自由度将减少（物理上）(singularconfiguration)(x,y)21xyl1l2例4.1可利用雅可比矩阵的行列式判别奇异形位当2＝90或2＝0时,机械手的雅可比行列式为0．矩阵的秩为1，因而处于奇异状态。从几何上看机械手完全伸直(2＝0)或完全缩回(2＝180)时，机械手末端丧失了径向自由度．仅能沿切向运动，在奇异形位时，机械手在操作空间的自由度将减少。例4.2如图所示．为了实现平面2R机械手末端沿x0轴以lm/s的速度运动，求相应的关节速度解：由可以看出，只要机械手的雅可比J(q)是满秩的方阵,相应的关节速度即可解出对于平面2R机械手，运动学方程为xqJq)(1T21q平面2R机械手的速度反解例4.2如图所示．为了实现平面2R机械手末端沿x0轴以lm/s的速度运动，求相应的关节速度解：雅可比J(q)为于是得到与末端速度相应的关节速度反解为[1,0]TxT21q逆雅可比可为122111221221112222111)(slslslclclclsllqJ2112221221121;slcslcslc讨论：机械手接近奇异形位时，关节速度将趋于无穷大。当2＝0；2＝180时，机械手在水平位置，2112221221121;slcslcslcxqJq)(1例：物理仿真中的雅可比矩阵约束函数C(x),单位圆上的质点位置约束为一般情况下，采用位姿矢量q聚合表达n个粒子的位置。在3D空间，矢量长度为3n。考虑位置约束C是一个关于位姿矢量q的未知函数，则速度约束矩阵被称作C的雅可比矩阵，记作J。为了进行物理仿真，求微分，根据力学关系，建立微分约束方程，基于物理仿真。1)(xxxCqqCCqC/qqCJJC=0Ċ=0C=0图5.9粒子运动满足约束函数C，并绕圆周运动。C=0合法位置Ċ=0合法速度C=0合法加速度约束力fC：限制为法线方向；与所有合法位移垂直；不做功、没有能量增加或损失；一个自由度：图5.10虚功原理要求约束力只能位于圆周的法线方向qCfC下面通过一个简单例子介绍约束动力学方法。一个2D粒子被强制绕单位圆周运动，设计一个标量行为函数)(qC来表达约束。例如可规定约束为)1(21)(qqqC把作用在所有粒子上的力用一个力矢量表示，记为Q。则控制粒子系统的牛顿方程为WQq其中W是M的逆。对于约束也使用综合的记号法，所有的标量约束函数形成一个单一的矢量函数C(q)＝(C1(q)C2(q)…Cm(q))。如果我们有n个3D粒子，服从于m个约束,那么这个总的约束函数的输出是一个m维矢量，而它的输入是一个3n维矢量。考虑C为一个行为函数,可以按照一般求导方法写出其导数，为qC/。对C求导，得qqCC矩阵qC/被称作C的雅克比矩阵，记作J(q)。一般地，雅克比矩阵是一个多维导数形式的矩阵。设有m个各含n个独立变量的约束函数nmnnqqqfCqqqfCqqqfC,,,,,,,,,21621222111求微分，Ci的微分是qi微分的函数nnmmmmnnnnqqCqqCqqCCqqCqqCqqCCqqCqqCqqCC22112222112212211111qqCC写成C=J(q)q，式子两边同除以时间的微分，上式中，雅可比矩阵J(q)是m×n的偏导数矩阵nmmmnnqCqCqCqCqCqCqCqCqCqJ212221212111)(qqJC)(注意道，如果函数f1(q)到f6(q)是非线性的，则C/q是q的函数。可以把雅可比矩阵看作是q的速度变换到Ċ的变换矩阵，在任何特定时刻，q具有某一特定值，J(q)就是一个线性变换。在每一新的时刻，q已改变，线性变换也因之改变，所以雅可比矩阵是一个时变的线性变换矩阵。C再对时间求导，得到：qJqJCJ是雅克比矩阵对时间的导数，可记为qqJJ/。用系统的运动方程替代q，得到)ˆW(QQJqJC设C为零，有QJqJQJWˆW如果未知量数目大于方程数目，需要引入虚功原理。合法速度（不改变约束C的速度）必须满足Jq＝0。为确保约束力不做功，要求0|0ˆqJqqQTQˆ矢量满足上式要求的充分必要条件，可以表示为下面的形式Qˆ＝JT其中，是一个与C的维数相同的矢量，JT为J的转置矩阵。为了要理解这个表达式的含义，可把矩阵JT视为矢量的集合qCqCqCJm21T其中，每个矢量Ci/q是标量约束函数Ci的梯度矢量。既然我们的基本要求是C＝0，这些梯度是约束超曲面的法线，表达禁止系统运动的形位空间方向。形式为JT的矢量是这些梯度矢量的线性组合，张成被禁止（prohibited）方向组成的子空间。通过把约束力限定在该子空间中，确保它与系统合法位移的点积为零，满足虚功原理的要求。例子2：立体视觉雅可比矩阵两只CCD摄像机任意的安装在机器人手腕上，形成手眼机器人立体视觉系统。{Xc,Yc,Zc}为摄像机坐标系，{x,y}为图像坐标系，CO为摄像机焦距f{Xw,Yw,Zw}为世界坐标系，则根据上述透视投影关系,得到以世界坐标系表示的P点坐标与其投影点p的坐标(x,y)的关系：scenepointopticalcenterimageplaneyxXcYcCOpP(Xc,Yc,Zc)ZcXwYwZwW摄像机成像模型101000000001ccccZYXffyxZwwc对上式两边求导，得：为世界坐标系到图像坐标系的雅可比映射矩阵，它是摄像机内外参数的函数。进一步，经过立体视觉摄像机定标，得到：其中，=，k代表摄像机1，2。上式为手眼机器人跟踪系统的视觉伺服控制方程。如果物体在世界坐标系下的速度已知，根据采样时间步长t，前一帧图像位置x(k)，根据上式可以估计下一帧图像位置x(k+1)，则可通过控制摄像机位姿，可以实现对目标的跟踪。wVJXXVJ)()()()()1(kukJttkxkxV)(kuwX)()()()()1(kukJttkxkxVwX4.2微分运动与广义速度4.2微分运动与广义速度刚体或坐标系的微分运动包含微分移动矢量d和微分转动矢量。前者由沿三个坐标轴的微分移动组成；后者又绕三个坐标轴的微分转动组成，即将两者合并为6维列矢量D，称为刚体或坐标系的微分运动矢量：相应地，刚体或坐标系的广义速度V是由线速度v,组成的6维矢量：T,,zyxddddT,,zyxdDdtvVt1lim0dδ微分运动D和广义速度V是相对于参考坐标系而言的。例如，相对于坐标系{T}而言，用，表示。VTDTTTTdDTTTvVdδdpaapadddpoopodddpnnpnddzTyTxT若相对于基坐标系的微分运动为D，则相对于坐标系{T}的微分运动为DTaonzTyTxT{T}pnoadT注意：D的微分位移和旋转应看作通过基坐标系的原点的矢量。合并写为dpaapadddpoopodddpnnpnddzTyTxTzyxzyxzyxzyxzyxzyxzyx