用平面二连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程分析
机器人运动轨迹规划
第2章 工业机器人运动学和动力学
第5讲 讲 机器人运动轨迹规划 机器人运动轨迹规划
第2章 工业机器人运动学和动力学 在规划中,不仅要规定机器人的起始点和终止点, 而且要 给出中间点(路径点)的位姿及路径点之间的时间分配, 即给出 两个路径点之间的运动时间。 轨迹规划既可在关节空间中进行, 即将所有的关节变量表 示为时间的函数,用其一阶、二阶导数描述机器人的预期动作, 也可在直角坐标空间中进行,即将手部位姿参数表示为时间的 函数, 而相应的关节位置、 速度和加速度由手部信息导出。
θ (t ) = c0 + c1t + c2t 2 + c3t 3
(3.67)
第2章 工业机器人运动学和动力学 这里初始和末端条件是:
θ (ti ) = θ i θ (t ) = θ f f & θ (ti ) = 0 θ (t ) = 0 & f
对式(3.67)求一阶导数得到:
第2章 工业机器人运动学和动力学
二自由度机器人关节空间的归一化运动
第2章 工业机器人运动学和动力学 如果希望机器人的手部可以沿AB这条直线运动, 最简单的 方法是将该直线等分为几部分(图3.21中分成5份), 然后计算出各 个点所需的形位角α和β的值, 这一过程称为两点间的插值。 可 以看出,这时路径是一条直线, 而形位角变化并不均匀。很显然, 如果路径点过少, 将不能保证机器人在每一小段内的严格直线轨 迹, 因此,为获得良好的沿循精度, 应对路径进行更加细致的分割。 由于对机器人轨迹的所有运动段的计算均基于直角坐标系, 因此 该法属直角坐标空间的轨迹规划。
空间二连杆机器人的动力学建模及其动态过程仿真
空间二连杆机器人的动力学建模及其动态过程仿真作者：td一引言1.机器人机械臂的运动学与动力学分析方法目录空间二连杆机器人的动力学建模 (1)及其动态过程仿真 (1)作者：td (1)一引言 (1)1.1用户界面模块（ADAMS/View） (4)1.2求解器模块（ADAMS/Solver） (5)1.3后处理模块（ADAMS/PostProcessor） (6)二．空间二连杆机器人adams建模仿真 (6)2.1空间二连杆串联机器人 (6)在ADAMS中用长方形连杆模拟机械臂，对两自由度的机械臂分别进行运动学分析动力学分析。
(6)2.1.1运动学分析 (6)2.1.2运动学分析 (9)机器人的运动学和动力学既包含有一般机械的运动学、动力学内容，又反映了机器人的独特内容。
工业机器人的运动学主要讨论了运动学的正问题和逆问题。
假设一个构型已知的机器人，即它的所有连杆长度和关节角度()1q t ,()2q t ,()3q t …()n q t ，…都是已知的，其中n 为自由度数，那么计算机器人末端执行器相对于参考坐标系的位姿就称为运动学的正问题分析。
换言之，如果已知机器人所有的关节变量，用正运动学方程就能计算任一瞬间机器人的位姿。
然而，如果希望机器人的末端执行器到达一个期望的位姿，就必须要知道机器人操作臂每一个连杆的几何参数和所有关节的角矢量()12,,T n q q q q =⋅⋅⋅利用操作臂连杆几何参数和末端执行器期望的位姿来求解关节角矢量是运动学逆问题。
运动学正问题可以利用齐次变换法来求解。
设i 杆坐标系相对于基座坐标系的位姿齐次变换矩阵是b i T ，则1231b i n n T A A A A A -=⋅⋅⋅⋅⋅⋅ ()11-式中i A 为i 杆坐标系相对于1i -杆坐标系的坐标变换矩阵。
相对于正运动学方程，机器人逆运动学方程显得更为重要。
由于按给定末端执行器的位姿求解关节变量是在关节空间中进行非线性方程的求解，其中涉及多值性和奇异现象，因此，这一逆问题成为机器人运动学中的一个重要内容。
(完整版)用平面二连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程
一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l 。
建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。
关节角顺时针为负逆时针为正。
图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P 在基础坐标系中的位置坐标：)sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p （1）2、用D-H 方法建立运动学方程假定0z 、1z 、2z 垂直于纸面向里。
从),,(000z y x 到),,(111z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 00sin cos 111101θθθθT （2） 从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 0sin cos 2212212θθθθl T （3） 从),,(000z y x 到),,(222z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡+++-+=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-⋅⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=⋅=10000100sin 0)cos()sin(cos 0)sin()cos(1000010000cos sin 0sin cos 1000010000cos sin 00sin cos 112121112121221221111120102θθθθθθθθθθθθθθθθθθl l l T T T （4）那么，连杆2末段与中线交点处一点P 在基础坐标系中的位置矢量为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡++++=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⋅⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡+++-+=⋅=110)sin(sin )cos(cos 10010000100sin 0)cos()sin(cos 0)sin()cos(212112121121121211121212020p p p z y x l l l l l l l P T P θθθθθθθθθθθθθθθθ （5）即，)sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p （6）与用简单的平面几何关系建立运动学方程（1）相同。
用平面三连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程
精心整理一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l ，连杆3长度为3l 。
建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 、33(,)x y 为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。
关节角顺时针为负逆时针为正。
1θ图11112123123p p x y 2、用D-H 方法建立运动学方程假定0z 、1z 、2z 垂直于纸面向外。
从),,(000z y x 到),,(111z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 00sin cos 111101θθθθT （2）从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 0sin cos 2212212θθθθl T （3） 从222(,,)x y z 到333(,,)x y z 的齐次旋转变换矩阵为：33212cos sin 0l T θθ-⎡⎤⎢⎥=从(003T =003P =结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。
补充：正解用于仿真，逆解用于控制建立以上运动学方程后，若已知个连杆的关节角123θθθ、、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。
3、平面二连杆机器人手臂逆运动学二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。
现已二连杆平面机器人为例推导速度雅可比矩阵。
上面的运动学方程两边对时间求导，得到下面的速度表达式：111212123123123111212122123123sin sin()()sin()()cos cos()()cos()()p p dx l l l dt dy l l l dtθθθθθθθθθθθθθθθθθθθθθθθθ=-⋅-+⋅+-++⋅++=⋅++⋅++++⋅++（17）把上式写成如下的矩阵形式：⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡++++-+--=⎥⎦⎤⎢⎣⎡212122121121221211)cos()cos(cos )sin()sin(sin θθθθθθθθθθθθ l l l l l l y x p p （18） 令上式中的末端位置速度矢量Xx p =⎥⎤⎢⎡， 矩阵11l l ⎢⎣⎡-(1θJ1(J θ212222211211θ∂J J J J 由此可知雅可比矩阵的定义：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡∂∂∂∂∂∂=⎥⎦⎤⎢⎣⎡=21212221121121),(θθθθθθp p p pJ y y x x J J JJ （21） 三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。
用平面三连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程
一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l ，连杆3长度为3l 。
建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 、33(,)x y 为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。
关节角顺时针为负逆时针为正。
1θ图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P 在基础坐标系中的位置坐标：112123123112123123cos cos()+cos()sin sin()+sin()p p x l l l y l l l θθθθθθθθθθθθ=++++=++++（1）2、用D-H 方法建立运动学方程假定0z 、1z 、2z 垂直于纸面向外。
从),,(000z y x 到),,(111z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 00sin cos 111101θθθθT （2） 从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 0sin cos 2212212θθθθl T （3） 从222(,,)x y z 到333(,,)x y z 的齐次旋转变换矩阵为：3323312cos sin 0sin cos 000010001l T θθθθ-⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦（3）从),,(000z y x 到333(,,)x y z 的齐次旋转变换矩阵为：11221332112233001231231231231121cos sin 00cos sin 0cos sin 0sin cos 00sin cos 00sin cos 00001000100010000100010001cos()sin()0cos cos(l l T T T T l l θθθθθθθθθθθθθθθθθθθθ---⎡⎤⎡⎤⎡⎤⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥=⋅⋅=⋅⋅⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦⎣⎦++-+++=212312311212)sin()cos()0sin sin()00100001l l θθθθθθθθθθ+⎡⎤⎢⎥++++++⎢⎥⎢⎥⎢⎥⎣⎦（4）那么，连杆2末段与中线交点处一点P 在基础坐标系中的位置矢量为：12312311212312312311212003311212312311212cos()sin()0cos cos()sin()cos()0sin sin()00010000011cos cos()cos()sin sin()l l l l l P T P l l l l l θθθθθθθθθθθθθθθθθθθθθθθθθθθ++-++++⎡⎤⎡⎤⎢⎥⎢⎥++++++⎢⎥⎢⎥=⋅=⋅⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦+++++++=3123sin()011p p p x l y z θθθ⎡⎤⎡⎤⎢⎥⎢⎥+++⎢⎥⎢⎥=⎢⎥⎢⎥⎢⎥⎢⎥⎣⎦⎣⎦（5） 即，112123123112123123cos cos()+cos()sin sin()+sin()p p x l l l y l l l θθθθθθθθθθθθ=++++=++++ （6）结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。
3.4机器人运动学雅可比矩阵
r f ( )
一般情况：
r f ( )
T m1 n1
r r1 , r2 , , rm R
1 , 2 , , n R
rj f j (1,2 ,,n )
j 1,2,, m
若n>m，手爪位置的关节变量有无限 个解，通常工业用机器人有3个位置变量 和3个姿态变量，共6个自由度（变量）。
f1 n m n R f m n
2、与平移速度有关的雅可比矩阵
相对于指尖坐标系的平移速度，是通过把坐标 原点固定在指尖上，指尖坐标系相对于基准坐 标系的平移速度来描述
O0 x0 y0 z0 Oe xe ye ze
：基准坐标系
：指尖坐标系
机器人雅可比矩阵机器人运动学机器人逆运动学雅可比矩阵matlab雅可比矩阵机器人正逆运动学雅克比矩阵机器人雅可比迭代矩阵家可比矩阵安堂机器人
3.4
机器人的雅可比矩阵
微分运动与速度
1、
微分运动指机构的微小运动，可用来推导不 同部件之间的速度关系。 机器人每个关节坐标系的微分运动，导致机 器人手部坐标系的微分运动，包括微分平移与微 分旋转运动。将讨论指尖运动速度与各关节运动 速度的关系。 前面介绍过机器人运动学正问题
J J1 J2
nm6
r f ( )
对位置方程进行求微分得：
dr J d r J dt dt
两边乘以dt，可得到微小位移之间的关系式
dr Jd
J 表示了手爪的速度与关节速度之间关系， 称之为雅克比矩阵。
f1 1 f J T f m 1
ze
z0
P e
Oe
xe
平面两自由度关节机器人算法
平面两自由度关节机器人算法平面两自由度关节机器人是工业制造中常见的一种机器人，它具有两个旋转关节，可以在平面内进行自由运动。
这种机器人在自动化生产线上扮演着重要角色，能够完成各种复杂的操作任务。
在设计和控制平面两自由度关节机器人时，算法起着至关重要的作用。
其中，运动规划算法是其中的重要一环。
通过合理的运动规划，可以使机器人在空间内快速、精准地完成各种任务。
在平面两自由度关节机器人中，常用的运动规划算法包括插补算法和路径规划算法。
插补算法是指在机器人运动过程中，通过对两个关节的角度进行插值计算，从而实现平滑的运动轨迹。
常用的插补算法有线性插补、圆弧插补和样条插补等。
这些算法可以根据机器人的速度、加速度等参数，合理地计算出每个时间点的关节位置，从而实现平滑、高效的运动。
另一个重要的算法是路径规划算法。
路径规划算法是指在给定起始点和目标点的情况下，寻找一条最优路径，使机器人能够在空间内避开障碍物，快速到达目标点。
常用的路径规划算法有最短路径算法、A*算法和D*算法等。
这些算法可以根据地图信息和机器人的动态参数，快速地找到一条最优路径，帮助机器人实现高效的运动。
除了运动规划算法外，碰撞检测算法也是平面两自由度关节机器人中不可或缺的一部分。
碰撞检测算法可以通过对机器人和周围环境的建模，实时地检测机器人是否会与障碍物相撞。
一旦发现潜在碰撞危险，算法可以及时做出调整，避免机器人发生碰撞，确保生产线的安全运行。
总的来说，平面两自由度关节机器人算法是机器人控制领域中的重要研究方向。
通过不断优化算法，可以使机器人在自动化生产中发挥更大的作用，提高生产效率，降低劳动成本。
期待未来，算法将继续发展，为平面两自由度关节机器人的智能化和自主化提供更多可能。
机器人动力学 雅克比-概念解析以及定义
机器人动力学雅克比-概述说明以及解释1.引言1.1 概述机器人动力学是研究机器人运动过程中的力学和动力学特性的学科，主要涉及机器人的姿态、速度、加速度、力和力矩等相关物理量。
机器人动力学一直以来都是机器人领域的关键问题之一，对于机器人的运动控制和路径规划具有重要的指导意义。
雅克比矩阵是机器人动力学中一项关键的工具，用于描述机器人多自由度系统中各关节之间的运动传递关系。
通过雅克比矩阵，我们可以计算出机器人末端执行器在给定关节角速度下的线速度和角速度，从而实现对机器人运动的精确控制。
机器人动力学的研究在实际应用中有着广泛的意义。
首先，深入理解机器人的动力学特性可以帮助我们设计出更加高效、灵活的机器人控制算法，从而提升机器人的运动精度和速度。
其次，机器人动力学的研究还可以为机器人路径规划、障碍物避障等问题提供重要的理论支持和指导。
此外，随着机器人应用领域的拓展，如医疗、教育、家庭服务等，机器人动力学的研究也将在未来发挥更加重要的作用。
总结起来，机器人动力学是研究机器人运动特性的学科，雅克比矩阵则是机器人动力学中的重要工具。
通过研究和应用机器人动力学，我们可以实现对机器人运动的精确控制，提升机器人的运动效率和准确性，并且为机器人的应用和发展打下坚实的基础。
未来，机器人动力学的研究将随着机器人技术的不断发展而不断探索新的方向，并为更广泛的机器人应用提供理论支持和指导。
1.2 文章结构文章结构部分的内容应当包括对整篇文章的组织和章节安排进行介绍。
可以按照以下方式编写文章结构的内容：2. 文章结构本文共分为以下几个部分：引言、正文和结论。
2.1 引言部分将对机器人动力学的概念进行概述，介绍机器人动力学的背景和意义。
在此部分还将阐述本文的目的和结构。
2.2 正文部分将重点讨论雅克比矩阵的概念和应用。
首先，将介绍雅克比矩阵的定义和性质，以及其在机器人动力学中的重要作用。
接着，将探讨雅克比矩阵在路径规划、运动控制和力学分析等方面的应用。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
一、平面二连杆机器人手臂运动学平面二连杆机械手臂如图1所示，连杆1长度1l ，连杆2长度2l 。
建立如图1所示的坐标系，其中，),(00y x 为基础坐标系，固定在基座上，),(11y x 、),(22y x 为连体坐标系，分别固结在连杆1和连杆2上并随它们一起运动。
关节角顺时针为负逆时针为正。
图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程连杆2末段与中线交点处一点P 在基础坐标系中的位置坐标：)sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p （1）2、用D-H 方法建立运动学方程假定0z 、1z 、2z 垂直于纸面向里。
从),,(000z y x 到),,(111z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 00sin cos 111101θθθθT （2） 从),,(111z y x 到),,(222z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=100010000cos sin 0sin cos 2212212θθθθl T （3） 从),,(000z y x 到),,(222z y x 的齐次旋转变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡+++-+=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-⋅⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=⋅=10000100sin 0)cos()sin(cos 0)sin()cos(1000010000cos sin 0sin cos 1000010000cos sin 00sin cos 112121112121221221111120102θθθθθθθθθθθθθθθθθθl l l T T T （4）那么，连杆2末段与中线交点处一点P 在基础坐标系中的位置矢量为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡++++=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡⋅⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡+++-+=⋅=110)sin(sin )cos(cos 10010000100sin 0)cos()sin(cos 0)sin()cos(212112121121121211121212020p p p z y x l l l l l l l P T P θθθθθθθθθθθθθθθθ （5）即，)sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p （6）与用简单的平面几何关系建立运动学方程（1）相同。
建立以上运动学方程后，若已知个连杆的关节角21θθ、，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。
3、平面二连杆机器人手臂逆运动学建立以上运动学方程后，若已知个机械臂的末端位置，可以用运动学方程求出机械手臂二连杆的关节角21θθ、，这叫机械臂的逆运动学。
逆运动学可以用于对机械臂关节角和末端位置的控制。
对于本例中平面二连杆机械臂，其逆运动学方程的建立就是已知末端位置),(p p y x 求相应关节角21θθ、的过程。
推倒如下。
（1）问题)sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p已知末端位置坐标),(p p y x ，求关节角21θθ、。
（2）求1θ由（6）式得到：22211211)sin ()cos (l l y l x p p =-+-θθ （7） 整理得到：)sin cos (2111222122θθp p p p y x l l l y x +=-++ （8）令pp p pp tg y x θθθcos sin == （9）由（8）式得到：)sin sin cos (cos cos 2111222122p p p p p p x l l l y x θθθθθ+=-++)cos(cos 211222122p pp p p x l l l y x θθθ-=-++ （10）由此可解出1θ。
p p p p p p x y arctg x l l l y x +⎥⎥⎦⎤⎢⎢⎣⎡-++=θθcos 2arccos 12221221 （11）（3）求2θ 由（6）式得到：2122122212)]sin([)]cos([l l y l x p p =+-++-θθθθ （12）整理得到：)]sin()cos([221212212222θθθθ+++=-++p p p p y x l l l y x （13） 令pp p pp tg y x θθθcos sin == （14）由（14）式得到：)cos(cos 2]sin )sin(cos )[cos(cos 221221212212222p pp p p p p p p x l x l l l y x θθθθθθθθθθθ-+=+++=-++ （15）由此可解出2θ。
122122222cos 2arccos θθθ-+⎥⎥⎦⎤⎢⎢⎣⎡-++=p p p p p p x y arctg x l l l y x （16）二、平面二连杆机器人手臂的速度雅可比矩阵速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。
现已二连杆平面机器人为例推导速度雅可比矩阵。
)sin(sin )cos(cos 2121121211θθθθθθ++=++=l l y l l x p p上面的运动学方程两边对时间求导，得到下面的速度表达式：)()cos(cos )()sin(sin 2121211121212111θθθθθθθθθθθθ +⋅++⋅=+⋅+-⋅-=l l dtdy l l dt dx p p （17）把上式写成如下的矩阵形式：⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡++++-+--=⎥⎦⎤⎢⎣⎡212122121121221211)cos()cos(cos )sin()sin(sin θθθθθθθθθθθθ l l l l l l y x p p （18） 令上式中的末端位置速度矢量Xy x p p =⎥⎦⎤⎢⎣⎡， 关节角速度矢量Θ=⎥⎦⎤⎢⎣⎡ 21θθ， 矩阵),()cos()cos(cos )sin()sin(sin 212122121121221211θθθθθθθθθθθθJ l l l l l l =⎥⎦⎤⎢⎣⎡++++-+--),(21θθJ 就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。
（18）式可以写成：Θ⋅= ),(21θθJ X 速度雅可比矩阵可以进一步写成：⎥⎦⎤⎢⎣⎡=⎥⎦⎤⎢⎣⎡++++-+--=22211211212212112122121121)cos()cos(cos )sin()sin(sin ),(J J Jl l l l l l J J θθθθθθθθθθθθ （19）其中，)cos()cos(cos )sin()sin(sin 2122222121112121221221211111θθθθθθθθθθθθθθ+=∂∂=++=∂∂=+-=∂∂=+--=∂∂=l y J l l y J l x J l l x J p p p p （20）由此可知雅可比矩阵的定义：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡∂∂∂∂∂∂=⎥⎦⎤⎢⎣⎡=21212221121121),(θθθθθθp p p pJ y y x x J J J J （21） 三、平面二连杆机器人手臂的动力学方程推倒动力学方程的方法很多，各有优缺点。
拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。
下面推导图1所示的平面双连杆机器人的动力学方程。
图1中所示连杆均为均质杆，其转动惯量分别是1I 和2I 。
1、求两连杆的拉格朗日函数 （1）求系统总动能连杆1的动能为：212112121121161)31(2121θθθ l m l m I K A ===（21）求连杆2质心D 处的线速度：对连杆2质心位置求导得到其线速度。
连杆2质心位置为：)sin(21sin )cos(21cos 2121121211θθθθθθ++=++=l l y l l x D D （22）连杆2质心速度为：)()cos(21cos )()sin(21sin 2121211121212111θθθθθθθθθθθθ +⋅++⋅=+⋅+-⋅-=l l Y l l x D D （23）21221222222212212221222)cos 21(41)cos 41(θθθθθθ l l l l l l l l y x V D D D +++++=+=（24）连杆2的动能：21221222222222122122212212212222222122122212221222222212)cos 32(2161)cos 31(21])cos 21(41)cos 41[(21))(121(2121)(21θθθθθθθθθθθθθθθθ l l l m l m l l l l m l l l l l l l l m l m V m I K D D +++++=+++++++=++=(25) 系统总动能：212212222222222122122222112122122122222222212212221221)cos 2131(61)cos 21616121()cos 32(2161)cos 31(21θθθθθθθθθθθθ l l m l m l m l l m l m l m l m l l l m l m l l l l m K K K ++++++=+++++=+= （26） （2）求系统总势能 系统总势能为：))sin(21sin (sin 21212112111θθθθ+++=l l g m gl m P （27）（3）求拉格朗日函数)]sin(21sin [sin 21)cos 2131(61)cos 21616121(21211211121221222222222212212222211212θθθθθθθθθθ++--++++++=-=l l g m gl m l l m l m l m l l m l m l m l m PK L （28） （4）列写动力学方程按照拉格朗日方程，对应关节1、2的驱动力矩分别为：222111θθτθθτ∂∂-∂∂∂∂=∂∂-∂∂∂∂=L L t LL t （29）22212222122122222112121)cos 2131()cos 3131(θθθθθ l l m l m l l m l m l m l m L +++++=∂∂22221221221222212222122122222112121sin 21sin )cos 2131()cos 3131(θθθθθθθθθθ l l m l l m l l m l m l l m l m l m l m L t --+++++=∂∂∂∂ )cos(21cos )21(212211211θθθθ+-+-=∂∂gl m gl m m L )cos(21cos )21(sin 21sin )cos 2131()cos 3131(2122112122221221221222212222122122222112121θθθθθθθθθθθθτ++++--+++++=gl m gl m m l l m l l m l l m l m l l m l m l m l m （30）同理：1221222222222)cos 2131(31θθθθ l l m l m l m L ++=∂∂ 2122122222122122222sin 2131)cos 2131(θθθθθθθ l l m l m l l m l m L t -++=∂∂∂∂ )cos(21sin 21sin 2121222122122122122θθθθθθθθ+---=∂∂gl m l l m l l m L )cos(21sin 2131)cos 2131(21222122122222122122222θθθθθθθτ+++++=gl m l l m l m l l m l m （31）联合（30）、（31）式，将动力学方程写成如下矩阵形式：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+++++⎥⎦⎤⎢⎣⎡⎥⎦⎤⎢⎣⎡-+⎥⎦⎤⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-+⎥⎦⎤⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+++++=⎥⎦⎤⎢⎣⎡)cos(21)cos(21cos )21(000sin 0sin 21sin 21031cos 2131cos 2131cos 3131212221221121212122122221221222122122222122222212222221222221121221θθθθθθθθθθθθθθθθθθθττgl m gl m gl m m l l m l l m l l m l m l l m l m l l m l m l l m l m l m l m （32）四、平面二连杆机器人手臂的轨迹规划轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。