1、机器人逆运动学详述clear;clc;L1 = Link(d, 0, a, 0, alpha, pi/2); %Link 类函数L2 = Link(d, 0, a, 0.5, alpha, 0,offset,pi/2);L3 = Link(d, 0, a, 0, alpha, pi/2,offset,pi/4);L4 = Link(d, 1, a, 0, alpha, -pi/2);L5 = Link(d, 0, a, 0, alpha, pi/2);L6 = Link(d, 1, a, 0, alpha, 0);b=isrevolute(L1); %Link 类函数robot=SerialL
2、ink(L1,L2,L3,L4,L5,L6); %SerialLink 类函数robot.name=带球形腕的拟人臂; %SerialLink 属性值robot.manuf=飘零过客; %SerialLink 属性值robot.display(); %Link 类函数theta=0 0 0 0 0 0;robot.plot(theta); %SerialLink 类函数theta1=pi/4,-pi/3,pi/6,pi/4,-pi/3,pi/6;p0=robot.fkine(theta);p1=robot.fkine(theta1);s=robot.A(4 5 6,theta);cchain=
3、robot.trchain;q=robot.getpos();q2=robot.ikine(p1); %逆运动学j0=robot.jacob0(q2); %雅可比矩阵p0 = -0.7071 -0.0000 0.7071 1.4142 0.0000 -1.0000 -0.0000 -0.0000 0.7071 0.0000 0.7071 1.9142 0 0 0 1.0000p1 = 0.9874 0.1567 0.0206 1.0098 0.0544 -0.4593 0.8866 1.8758 0.1484 -0.8743 -0.4621 0.0467 0 0 0 1.0000 ss = 1
4、 0 0 0 0 1 0 0 0 0 1 2 0 0 0 1cchain =Rz(q1)Rx(90)Rz(q2)Tx(0.5)Rz(q3)Rx(90)Rz(q4)Tz(1)Rx(-90)Rz(q5)Rx(90)Rz(q6)Tz(1)q = 0 0 0 0 0 0q2 = 1.0e+04 * 0.0003 0.0180 -0.0399 1.1370 0.0002 0.0536j0 = -0.1100 0.0707 0.3577 -0.0114 0.5092 0 -0.8329 -0.0448 -0.2267 -0.6224 0.1813 0 -0.0000 0.7623 0.3956 -0.14
5、10 -0.8413 0 -0.0000 0.5354 0.5354 0.3374 -0.0178 -0.8605 0.0000 0.8446 0.8446 -0.2139 -0.9751 0.1275 1.0000 0.0000 0.0000 0.9168 -0.2209 -0.4933作者:fly qq链接:来源:知乎著作权归作者所有,转载请联系作者获得授权。matlab机器人工具箱求串联机器人运动学逆解一般是采用ikine()函数,所以刚打开matlab看了一下源码。(貌似新版本还加了其他求解算法,这边先不说,先解决题主的问题。)我把它的主要步骤和贴出:% 初始化% 定义目标T,迭代次数
6、,误差等;% 初始化当前误差e,while true % 计算误差 Tq = robot.fkine(q); e(1:3) = transl(T - Tq); Rq = t2r(Tq); th,n = tr2angvec(Rq*t2r(T); e(4:6) = th*n; J = jacob0(robot, q); % 计算雅克比 % 根据末端误差求取关节变化 if opt.pinv % 雅克比伪逆法 dq = opt.alpha * pinv( J(m,:) ) * e(m); else % 雅克比转置法 dq = J(m,:) * e(m); dq = opt.alpha * dq; en
7、d % 更新关节值 q = q + dq; % 判断误差是否小于容许误差tolerance nm = norm(e(m); if nm = opt.tol break endend所以,很简单,Matlab工具箱就是采用雅克比矩阵伪逆(或转置)来确定迭代方向,并通过迭代的方法使关节值收敛至目标位置。这应该是串联机器人运动学逆解数值解的最常用方法。再回答为什么这么简单就可以求解逆解?首先,只要给了D-H参数,雅克比矩阵很容易就可以推导出来(如果不会请参考各种机器人学教材,推荐John Craig的Introduction to robotics: mechanics and control)。i
8、mg src= data-rawwidth=575 data-rawheight=315 class=origin_image zh-lightbox-thumb width=575 data-original=它就是关节速度与末端速度的线性关系。img src= data-rawwidth=658 data-rawheight=332 class=origin_image zh-lightbox-thumb width=658 data-original=于是,世界坐标系中的运动可以近似用关节的运动叠加得到;越小,线性关系越准确,迭代求解的轨迹越接近图中直线;但是,我们并不关心求解过程的运动
9、方向,只关心最后求解的误差大小。所以,一般迭代求解会对求解的步长做处理,也即增加一个变量,这样可以加快求解速度。所以,雅克比伪逆的思路是让机械臂末端往目标点方向移动。使用伪逆是为了应对非6-dof情况。当然,看上面的代码,matlab机器人工具箱似乎可以用雅克比的转置代替雅克比伪逆。 % 根据末端误差求取关节变化 if opt.pinv % 雅克比伪逆法 dq = opt.alpha * pinv( J(m,:) ) * e(m); else % 雅克比转置法 dq = J(m,:) * e(m); dq = opt.alpha * dq; end这也很有趣,而且可以从很多角度来解释:1)从力
10、的角度:根据虚功原理,可以有如下关系:img src= data-rawwidth=740 data-rawheight=407 class=origin_image zh-lightbox-thumb width=740 data-original=其中,是每个关节的力(力矩),F是末端受力。所以,雅克比转置的方法是给机械臂每个关节一个朝目标点移动的力矩,这样就相当于一直拉着机械臂末端往目标点拖,机械臂最终将收敛到目标位置。2)从优化角度: l1=Link(0 0 0 pi/2);l2=Link(0 0 0.4318 0);l3=Link(-pi/2 0.1505 0.02032 -pi/2
11、);l4=Link(0 0.4318 0 pi/2);l5=Link(0 0 0 -pi/2);l6=Link(0 0 0 0);pm560=SerialLink(l1 l2 l3 l4 l5 l6);其中Link( d a )里的表示两个连杆的夹角,d表示z方向的长度(z为选转轴),a为连杆的长度即x方向的长度(参考的坐标为杆首段坐标即前一杆尾端坐标。每个杆的两端放置一个笛卡尔坐标,通过坐标变换实现求解。)。为杆两端两个坐标的夹角(主要指两个选择轴z的夹角)。机器人的正解如下:机器人逆解的如下:其中ikine6s与ikine函数解不一样。但是再正解后结果是一样的?。二、下面为逆解过程:即从坐
12、标点各关节角度值的过程。t=0:0.2:2; T2=transl(0.4521,0,0.4318); T=ctraj(T1,T2,length(t); Q=pm560.ikine6s(T); pm560.plot(Q)%显示机器人三维图动画过程。 tranimate(T)%动画演示坐标系自初始点运动到目标点的过程三、演示如何画图: s(1)=subplot(3,2,1);%一共分三行,每行两个,现在画出第一行第一个。 plot(t,Q(:,1)%画出对应时间t的关节角度变化,Q(:,1)表示一个关节角度值。 xlabel(s(1),times)%为图表添加横坐标。 ylabel(s(1),关节1) s(2)=subplot(3,2,2);plot(t,Q(:,2)xlabel(s(2),times)ylabel(s(2),关节2)。最终六个关节角度如下图:
copyright@ 2008-2022 冰豆网网站版权所有
经营许可证编号:鄂ICP备2022015515号-1