用平面三连杆机器人为例贯穿运动学、雅可比、动力学、轨迹规划甚至控制与编程 - 图文

.

一、平面二连杆机器人手臂运动学

平面二连杆机械手臂如图1所示，连杆1长度l1，连杆2长度l2，连杆3长度为l3。建立如图1所示的坐标系，其中，(x0,y0)为基础坐标系，固定在基座上，(x1,y1)、(x2,y2)、

(x3,y3)为连体坐标系，分别固结在连杆1、连杆2、连杆3上并随它们一起运动。关节角顺

时针为负逆时针为正。

?1x3

y3 y0 P

y2 B ?3?2 D x2 2 C x1 1 ?1 y1 A x0

图1平面双连杆机器人示意图 1、用简单的平面几何关系建立运动学方程

连杆2末段与中线交点处一点P在基础坐标系中的位置坐标：

xp?l1cos?1?l2cos(?1??2)+l3cos(?1??2??3)yp?l1sin?1?l2sin(?1??2)+l3sin(?1??2??3)（1）

2、用D-H方法建立运动学方程

假定z0、z1、z2垂直于纸面向外。从(x0,y0,z0)到(x1,y1,z1)的齐次旋转变换矩阵为：

. .

.

??cos?1?sin?100?0cos?100?1T??sin?1??0010?? （2） ?0001??从(x1,y1,z1)到(x2,y2,z2)的齐次旋转变换矩阵为：

??cos?2?sin?20l1?1T?sin?2cos?200?2???0010?? （3） ?0001??从(x2,y2,z2)到(x3,y3,z3)的齐次旋转变换矩阵为：

??cos?3?sin?30l2?1T?sin?3cos?300?2???0010?? （3） ?0001??

从(x0,y0,z0)到(x3,y3,z3)的齐次旋转变换矩阵为：

??cos?1?sin?100??cos?2?sin?20l1??cos?3?sin?30l2?00sin?cos?sin?cos?200?00?3T?1T?121?100?22T?3T???????0010??00????sin?3cos?310?10?? ?0001????0001??00???0001????cos(?1??2??3)?sin(?1??2??3)0l1cos?1?l2cos(?1??2)???sin(?1??2??3)cos(?1??2??3)0l1sin?1?l2sin(?1??2)???0010???0001??那么，连杆2末段与中线交点处一点P在基础坐标系中的位置矢量为：

??cos(?1??2??3)?sin(?1??2??3)0l1cos?1?l2cos(?1??2)??l3?0P?03?sin(?1??2??3)cos(?1??2??3)0l1sin?1?l2sin(?1??2)??0?3T?P??????0010???0??0001????1????l1cos?1?l2cos(?1??2)?l3cos(?1??2??3)??xp???l1sin?1?l2sin(?1??2)?l3sin(?1??2??3)????y??0???p?z?p??1????1??即，

xp?l1cos?1?l2cos(?1??2)+l3cos(?1??2??3)yp?l1sin?1?l2sin(?1??2)+l 3sin(?1??2??3)结论：（6）与用简单的平面几何关系建立运动学方程（1）相同。

. .

4）

（5）（6）（

.

补充：正解用于仿真，逆解用于控制

建立以上运动学方程后，若已知个连杆的关节角?1、?2、?3，就可以用运动学方程求出机械手臂末端位置坐标，这可以用于运动学仿真。 3、平面二连杆机器人手臂逆运动学

二、平面二连杆机器人手臂的速度雅可比矩阵

速度雅可比矩阵的定义：从关节速度向末端操作速度的线性变换。现已二连杆平面机器人为例推导速度雅可比矩阵。

xp?l1cos?1?l2cos(?1??2)+l3cos(?1??2??3)yp?l1sin?1?l2sin(?1??2)+l3sin(?1??2??3)

上面的运动学方程两边对时间求导，得到下面的速度表达式：

dxpdtdypdt??l1sin?1??1?l2sin(?1??2)?(?1??2)?l3sin(?1??2??3)?(?1??2??3)?l1cos?1??1?l2cos(?1??2)?(?1??2)?l2cos(?1??2??3)?(?1??2??3)

（17）

把上式写成如下的矩阵形式：

???p???l1sin?1?l2sin(?1??2)?l2sin(?1??2)????x1???? （18） ?y????lcos??lcos(???)lcos(???)?1212212??2??p??1?p??x?， 令上式中的末端位置速度矢量???X?p??y?????， 关节角速度矢量?1??????2? 矩阵???l1sin?1?l2sin(?1??2)?l2sin(?1??2)??J(?1,?2) ??l1cos?1?l2cos(?1??2)l2cos(?1??2)?J(?1,?2)就是速度雅可比矩阵，实现从关节角速度向末端位置速度的转变。（18）式可

以写成：

??J(?,?)??? X12速度雅可比矩阵可以进一步写成：

??l1sin?1?l2sin(?1??2)?l2sin(?1??2)?J(?1,?2)???1??2)l2cos(?1??2)??l1cos?1?l2cos(? （19）

J12??J??11??J21J22?其中，

. .

.

J11?J12?J21?J22??xp??1?xp??2?yp??1?yp??2??l1sin?1?l2sin(?1??2)??l2sin(?1??2) （20）

?l1cos?1?l2cos(?1??2)?l2cos(?1??2)由此可知雅可比矩阵的定义：

?J11J(?1,?2)???J21??xpJ12???1???J22???yp????1?xp???2? （21） ?yp???2??三、平面二连杆机器人手臂的动力学方程

推倒动力学方程的方法很多，各有优缺点。拉格朗日方法思路清晰、不考虑连杆之间的内力，是推倒动力学方程的常用方法。下面推导图1所示的平面双连杆机器人的动力学方程。图1中所示连杆均为均质杆，其转动惯量分别是I1和I2。 1、求两连杆的拉格朗日函数 （1）求系统总动能

连杆1的动能为：

K1?1?2IA?12 （21）

111?2?ml2??2?(m1l12)?1111236求连杆2质心D处的线速度：对连杆2质心位置求导得到其线速度。连杆2质心位置为：

1xD?l1cos?1?l2cos(?1??2)2 （22）

1yD?l1sin?1?l2sin(?1??2)2连杆2质心速度为：

??1lsin(???)?(?????)?D??l1sin?1??x1212122 （23）

1??lcos(????)??lcos???Y?1??2)?(?D111212212222?2?1l2??2?(1l2?llcos?)?????D?DVD?x?y?(l12?l2?l1l2cos?2)?122212212 442 （24）

. .

.

连杆2的动能：

1????)2?1mV2ID(?122D22112????)2?1m[(l2?1l2?llcos?)??2?1l2??2?(1l2?llcos?)?????(m2l2)(?12212122122212212]2122442112?2?1ml2??2?1m(2l2?llcos?)?????m2(l12?l2?l1l2cos?2)?1222221221223623K2? (25) 系统总动能：

K?K1?K2112?2?1ml2??2?1m(2l2?llcos?)????m2(l12?l2?l1l2cos?2)?122222122122362311112?2?1ml2??2?(1ml2?1mllcos?)?????(m2l12?m1l12?m2l2?m2l1l2cos?2)?1222222122122662632? （26） （2）求系统总势能 系统总势能为： P?11m1gl1sin?1?m2g(l1sin?1?l2sin(?1??2)) （27） 22（3）求拉格朗日函数

L?K?P11112?2?1ml2??2?(1ml2?1mllcos?)?????(m2l12?m1l12?m2l2?m2l1l2cos?2)?122222212212266263211?m1gl1sin?1?m2g[l1sin?1?l2sin(?1??2)]22 （28） （4）列写动力学方程

按照拉格朗日方程，对应关节1、2的驱动力矩分别为：

?1?

??L?L???t??1??1??L?L????2?t??2 （29）

?2?

?L112??(1ml2?1mllcos?)?? ?(m2l12?m1l12?m2l2?m2l1l2cos?2)?12221222?3332??1. .

.

??L112???(1ml2?1mllcos?)????(m2l12?m1l12?m2l2?m2l1l2cos?2)?12221222??t??33321????1mllsin???2?m2l1l2sin?2?12212222?L11??(m?m)glcos??m2gl2cos(?1??2) 1211??122???(ml2?mllcos?)????1?(m2l12?m1l12?m2l22?m2l1l2cos?2)?12221222????1mllsin???2?(1m?m)glcos??1mglcos(?m2l1l2sin?2??1??2)1221222121122222 （30）

同理：

13131312?L1112?? ?m2l2?2?(m2l22?m2l1l2cos?2)?1?32??23??L112???1ml2???1mllsin???? ?(m2l2?m2l1l2cos?2)?1222?21221?2??t??23232?L1?2?1mllsin??????1mglcos(???) ??m2l1l2sin?2?12122122212??2222???ml2????2?mglcos(?2?(m2l22?m2l1l2cos?2)?m2l1l2sin?2??1??2)1222?1221312131212 （31）

联合（30）、（31）式，将动力学方程写成如下矩阵形式：

1111??2222ml?ml?ml?mllcos?ml?mllcos????2122222122?????1??21311322132??????????11122??2????2?m2l2?m2l1l2cos?2m2l2323??1??0?mllsin??2?2122????12 ?????2?1?m2l1l2sin?2???2?0?2?1?1?(m?m)glcos??mglcos(???)??12112212???m2l1l2sin?20???1?2??22?????????100??????12???m2gl2cos(?1??2)2?? （32）

四、平面二连杆机器人手臂的轨迹规划

. .

.

轨迹规划就是已知起点和终点的位置速度加速度等参数确定中间点的相应参数的过程。轨迹规划是机器人完成规定任务所必需的。它分为关节空间的轨迹规划和直角坐标空间的轨迹规划、以及基于动力学的轨迹规划等几种类型。

关节空间的轨迹规划就是已知某连杆起点和终点的角位置角速度角加速度等参数确定中间点的相应参数的过程。如图所示，一两自由度机械手，已知两连杆起点和终点的关节角，确定中间位置的关节角。（1）非归一化和归一化问题（2）末端位置的轨迹、关节空间轨迹规划的缺点。

三次多项式轨迹规划

举例：要求一个５轴机器人的第一关节在５秒之内从初始角３０度运动到终端角７５度，用三次多项式计算在第１、２、３、４秒时的关节角。

. .

.

五次多项式轨迹规划

. .

.

抛物线过渡的线性运动轨迹规划（略）

具有中间点以及用抛物线过渡的线性运动轨迹规划（略） 高次多项式运动轨迹规划（略）

直角坐标空间的轨迹规划（1）所有用于关节空间的轨迹规划方法都可以用于直角坐标空间轨迹规划；（2）直角坐标轨迹规划必须不断进行逆运动学运算，以便及时得到关节角。这个过程可以归纳为以下计算循环：

（a）将时间增加一个增量；

（b）利用所选择的轨迹函数计算出手的位姿； （c）利用逆运动学方程计算相应的关节变量； （d）将关节变量信息送给控制器； （e）返回到循环的开始。

. .

.

. .

.

. .

.

五、二连杆机器人的控制

欢迎您的光临，Word文档下载后可修改编辑双击可删除页眉页脚谢谢！希望您提出您宝贵的意见，你的意见是我进步的动力。赠语； 1、如果我们做与不做都会有人笑，如果做不好与做得好还会有人笑，那么我们索性就做得更好，来给人笑吧！ 2、现在你不玩命的学，以后命玩你。、我不知道年少轻狂，我只知道胜者为王。、不要做金钱、权利的奴隶；应学会做“金钱、权利”的主人。、什么时候离光明最近？那就是你觉得黑暗太黑的时候。、最值得欣赏的风景，是自己奋斗的足迹。、压力不是有人比你努力，而是那些比你牛×几倍的人依然比你努力。. .

本文来源：https://www.bwwdw.com/article/o9ex.html