机器人逆运动学

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=SerialLink([L1,L2,L3,L4,L5,L6]);%SerialLink类函数robot.name='带球形腕的拟人臂';%SerialLink属性值robot.manuf='飘零过客';%SerialLink属性值robot.display();%Link类函数theta=[000000];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([456],theta);cchain=robot.trchain;q=robot.getpos();q2=robot.ikine(p1);%逆运动学j0=robot.jacob0(q2);%雅可比矩阵p0=-0.7071-0.00000.70711.41420.0000-1.0000-0.0000-0.00000.70710.00000.70711.91420001.0000p1=0.98740.15670.02061.00980.0544-0.45930.88661.87580.1484-0.8743-0.46210.04670001.0000ss=1000010000120001cchain=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=000000q2=1.0e+04*0.00030.0180-0.03991.13700.00020.0536j0=-0.11000.07070.3577-0.01140.50920-0.8329-0.0448-0.2267-0.62240.18130-0.00000.76230.3956-0.1410-0.84130-0.00000.53540.53540.3374-0.0178-0.86050.00000.84460.8446-0.2139-0.97510.12751.00000.00000.00000.9168-0.2209-0.4933作者：flyqq链接：来源：知乎著作权归作者所有，转载请联系作者获得授权。matlab机器人工具箱求串联机器人运动学逆解一般是采用ikine()函数，所以刚打开matlab看了一下源码。（貌似新版本还加了其他求解算法，这边先不说，先解决题主的问题。）我把它的主要步骤和贴出：%初始化%定义目标T，迭代次数，误差等；%初始化当前误差e,whiletrue%计算误差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);%计算雅克比%根据末端误差求取关节变化ifopt.pinv%雅克比伪逆法dq=opt.alpha*pinv(J(m,:))*e(m);else%雅克比转置法dq=J(m,:)'*e(m);dq=opt.alpha*dq;end%更新关节值q=q+dq';%判断误差是否小于容许误差tolerancenm=norm(e(m));ifnm=opt.tolbreakendend所以，很简单，Matlab工具箱就是采用雅克比矩阵伪逆（或转置）来确定迭代方向，并通过迭代的方法使关节值收敛至目标位置。这应该是串联机器人运动学逆解数值解的最常用方法。再回答为什么这么简单就可以求解逆解？首先，只要给了D-H参数，雅克比矩阵很容易就可以推导出来（如果不会请参考各种机器人学教材，推荐JohnCraig的Introductiontorobotics:mechanicsandcontrol）。imgsrc==575data-rawheight=315class=origin_imagezh-lightbox-thumbwidth=575data-original=它就是关节速度与末端速度的线性关系。imgsrc==658data-rawheight=332class=origin_imagezh-lightbox-thumbwidth=658data-original=于是，世界坐标系中的运动可以近似用关节的运动叠加得到；越小，线性关系越准确，迭代求解的轨迹越接近图中直线；但是，我们并不关心求解过程的运动方向，只关心最后求解的误差大小。所以，一般迭代求解会对求解的步长做处理，也即增加一个变量，这样可以加快求解速度。所以，雅克比伪逆的思路是让机械臂末端往目标点方向移动。使用伪逆是为了应对非6-dof情况。当然，看上面的代码，matlab机器人工具箱似乎可以用雅克比的转置代替雅克比伪逆。%根据末端误差求取关节变化ifopt.pinv%雅克比伪逆法dq=opt.alpha*pinv(J(m,:))*e(m);else%雅克比转置法dq=J(m,:)'*e(m);dq=opt.alpha*dq;end这也很有趣，而且可以从很多角度来解释：1）从力的角度：根据虚功原理，可以有如下关系：imgsrc==740data-rawheight=407class=origin_imagezh-lightbox-thumbwidth=740data-original=其中，是每个关节的力（力矩），F是末端受力。所以，雅克比转置的方法是给机械臂每个关节一个朝目标点移动的力矩，这样就相当于一直拉着机械臂末端往目标点拖，机械臂最终将收敛到目标位置。2）从优化角度：imgsrc==746data-rawheight=443class=origin_imagezh-lightbox-thumbwidth=746data-original=设定优化目标函数F，逆解求解就成了一个非线性优化问题。利用梯度下降法可以推导得到相同的公式，殊途同归的感觉真棒！所以，雅克比转置的思路是给机械臂末端一个朝着目标点方向的力。当然，两种方法各有优劣：1）伪逆法可能会遇到矩阵奇异等问题；2）转置法收敛比较慢。于MATLAB_robotic_toolbox的机器人逆解教程(2014-08-1522:07:20)转载▼标签：股票机器人工具箱使用大致分为三步：一、构件模型；二、规划轨迹及使用逆解函数；三、图形输出。一、建立机器人数学模型（连杆机构），如下：l1=Link([000pi/2]);l2=Link([000.43180]);l3=Link([-pi/20.15050.02032-pi/2]);l4=Link([00.43180pi/2]);l5=Link([000-pi/2]);l6=Link([0000]);pm560=SerialLink([l1l2l3l4l5l6]);其中Link（[θdaα]）里的θ表示两个连杆的夹角，d表示z方向的长度（z为选转轴），a为连杆的长度即x方向的长度（参考的坐标为杆首段坐标即前一杆尾端坐标。每个杆的两端放置一个笛卡尔坐标，通过坐标变换实现求解。）。α为杆两端两个坐标的夹角（主要指两个选择轴z的夹角）。机器人的正解如下：机器人逆解的如下：其中ikine6s与ikine函数解不一样。但是再正解后结果是一样的？。二、下面为逆解过程：即从坐标点~~~各关节角度值的过程。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')。。。。。。。最终六个关节角度如下图：