注册 登录  
 加关注
   显示下一条  |  关闭
温馨提示!由于新浪微博认证机制调整,您的新浪微博帐号绑定已过期,请重新绑定!立即重新绑定新浪微博》  |  关闭

易拉罐的博客

心静自然凉

 
 
 

日志

 
 

转 motoman 机械臂运动学仿真  

2010-03-25 14:23:53|  分类: 机器视觉 |  标签: |举报 |字号 订阅

  下载LOFTER 我的照片书  |

只考虑了从基部到摄像机的三个关节,既没用twist也没用D-H参数,而是直接写各自的转换矩阵,仿真成功。验证了想法的可行性
机器人示意图如下:

转 motoman 机械臂运动学仿真 - 易拉罐bb - 易拉罐的博客

可见这三个坐标变换分别为:

从世界坐标系c0到坐标系c1,仅有绕z轴的转动
g01=rot(3,s1,[0 0 0]);

从c1到c2,包括一个y轴平移和一个绕x轴的转动
g12=rot(1,s2,[0 150 0]);

从c2到c3,包括一个z轴平移和一个绕x轴的转动
g23=rot(1,s3,[0 0 260]);

从c3到c4,包括一个y轴平移
g34=rot(1,0,[0 350 0]);

总的变换为
g04=g01*g12*g23*g34;

简单的仿真验证了正确性

 

欧拉角公式
RL=[cos(Q2)*cos(Q1)   sin(Q2)*cos(Q1)   -sin(Q1) ;
   -sin(Q2)*cos(Q3)+cos(Q2)*sin(Q1)*sin(Q3)    cos(Q2)*cos(Q3)+sin(Q2)*sin(Q1)*sin(Q3)  cos(Q1)*sin(Q3) ;
   sin(Q2)*sin(Q3)+cos(Q2)*sin(Q1)*cos(Q3)    -cos(Q2)*sin(Q3)+sin(Q2)*sin(Q1)*cos(Q3)  cos(Q1)*cos(Q3)];

根据欧拉转动计算四元数的变换如下:
q = qyaw qpitch qroll

其中
qroll = [cos(y/2), (sin(y/2), 0, 0)];
qpitch = [cos(q/2), (0, sin(q/2), 0)];
qyaw = [cos(f/2), (0, 0, sin(f/2)];

其转换代码如下:
void EulerToQuat( float roll, float pitch, float yaw, Quat* quat)
{
 float cr, cp, cy, sr, sp, sy, cpcy, spsy;
 
 //计算求四元数时使用到的所有三角值
 cr = cos(roll / 2);
 cp = cos(pitch / 2);
 cy = cos(yaw / 2);

 sr = sin(roll/2);
 sp = sin(pitch/2);
 sy = sin(yaw/2);
 cpcy = cp * cy;
 spsy = sp * sy;

 //组合这些值,生成四元数的向量和w
 quat->w = cr*cpcy + sr*spsy;
 quat->x = sr*cpcy - cr*spsy;
 quat->y = cr*sp*cy + sr*cp*sy;
 quat->z = cr*cp*sy - sr*sp*cy;
}

/////////////////

R=rot(1,s1)*rot(2,s2)*rot(3,s3); 

R=rot(3,s3)*rot(2,s2)*rot(1,s1);

 

先绕z轴逆时针绕s3,再...
R=rot(3,-s1)*rot(1,-s2)*rot(3,-s3);

 

exp(w^sita)=I + w^ sin(sita) + (1-cos(sita))*(w^)*(w^)

  评论这张
 
阅读(554)| 评论(0)
推荐 转载

历史上的今天

在LOFTER的更多文章

评论

<#--最新日志,群博日志--> <#--推荐日志--> <#--引用记录--> <#--博主推荐--> <#--随机阅读--> <#--首页推荐--> <#--历史上的今天--> <#--被推荐日志--> <#--上一篇,下一篇--> <#-- 热度 --> <#-- 网易新闻广告 --> <#--右边模块结构--> <#--评论模块结构--> <#--引用模块结构--> <#--博主发起的投票-->
 
 
 
 
 
 
 
 
 
 
 
 
 
 

页脚

网易公司版权所有 ©1997-2017