机器人逆运动学.docx
《机器人逆运动学.docx》由会员分享，可在线阅读，更多相关《机器人逆运动学.docx（11页珍藏版）》请在冰点文库上搜索。
机器人逆运动学
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.4142
0.0000-1.0000-0.0000-0.0000
0.70710.00000.70711.9142
0001.0000
p1=
0.98740.15670.02061.0098
0.0544-0.45930.88661.8758
0.1484-0.8743-0.46210.0467
0001.0000
>>s
s=
1000
0100
0012
0001
cchain=
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=
000000
q2=
1.0e+04*
0.00030.0180-0.03991.13700.00020.0536
j0=
-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.8605
0.00000.84460.8446-0.2139-0.97510.1275
1.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';
%判断误差是否小于容许误差tolerance
nm=norm（e（m））;
ifnm<=opt.tol
break
end
end
所以，很简单，Matlab工具箱就是采用雅克比矩阵伪逆（或转置）来确定迭代方向，并通过迭代的方法使关节值收敛至目标位置。
这应该是串联机器人运动学逆解数值解的最常用方法。
∙再回答为什么这么简单就可以求解逆解？
首先，只要给了D-H参数，雅克比矩阵很容易就可以推导出来（如果不会请参考各种机器人学教材，推荐JohnCraig的Introductiontorobotics:
mechanicsandcontrol）。
它就是关节速度与末端速度的线性关系。
于是，世界坐标系中的运动
可以近似用关节的运动
叠加得到；
越小，线性关系越准确，迭代求解的轨迹越接近图中直线；
但是，我们并不关心求解过程的运动方向，只关心最后求解的误差大小。
所以，一般迭代求解会对求解的步长做处理，也即增加一个变量
，这样可以加快求解速度。
所以，雅克比伪逆的思路是让机械臂末端往目标点方向移动。
使用伪逆是为了应对非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）从力的角度：
根据虚功原理，可以有如下关系：
其中，
是每个关节的力（力矩），F是末端受力。
所以，雅克比转置的方法是给机械臂每个关节一个朝目标点移动的力矩，这样就相当于一直拉着机械臂末端往目标点拖，机械臂最终将收敛到目标位置。
2）从优化角度：
设定优化目标函数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'）
。
。
。
。
。
。
。
最终六个关节角度如下图：
升级会员 