機器人 全部 環境 14.04 系列 splay p s wid font

註：本篇博文全部源碼下載地址為：Git Repo。

1. 源碼是在 Ubuntu14.04 + Indigo 環境下編寫。

一、轉換矩陣

　　經過上一篇博客介紹，我們已經獲得了Rob一個手臂的D-H參數表，如上表所示，我們要把這些參數轉換成相鄰坐標系的轉換矩陣，D和H兩位老前輩已經推導出通用公式了，通用公式如圖1，其中cθi = cos(θi) ，sθi = sin(θi )。這是一個4x4的矩陣，它表征了相鄰兩個坐標系的位置和姿態兩個維度的轉換關系，具體說明見上一篇博文。

圖1

　　套用圖1中的公式，我們分別得到坐標系{1}相對於坐標系{0}的轉換矩陣T1如下：

　　同理，我們得到坐標系{2}相對於坐標系{1}的轉換矩陣T2

，{3}相對於{2}的轉換矩陣T3，{4}相對於{3}的轉換矩陣T4，{5}相對於{4}的轉換矩陣T5，{6}相對於{5}的轉換矩陣T6，分別如下 ：

至此，我們得到了所有相鄰坐標系的轉換矩陣，由於他們是串聯的，我們可以得到坐標系{6}相對於坐標系{0}的轉換矩陣T2：

式1

其中（s1指θ1，s2指θ2，以此類推）：

　　nx = - cos(s5)*(cos(s4)*(sin(s1)*sin(s3) - cos(s1)*cos(s2)*cos(s3)) + cos(s1)*sin(s2)*sin(s4)) - sin(s5)*(cos(s3)*sin(s1) + cos(s1)*cos(s2)*sin(s3));
　　n

y = cos(s5)*(cos(s4)*(cos(s1)*sin(s3) + cos(s2)*cos(s3)*sin(s1)) - sin(s1)*sin(s2)*sin(s4)) + sin(s5)*(cos(s1)*cos(s3) - cos(s2)*sin(s1)*sin(s3));
　　nz = cos(s5)*(cos(s2)*sin(s4) + cos(s3)*cos(s4)*sin(s2)) - sin(s2)*sin(s3)*sin(s5);
　　ox = sin(s5)*(cos(s4)*(sin(s1)*sin(s3) - cos(s1)*cos(s2)*cos(s3)) + cos(s1)*sin(s2)*sin(s4)) - cos(s5)*(cos(s3)*sin(s1) + cos(s1)*cos(s2)*sin(s3));
　　oy = cos(s5)*(cos(s1)*cos(s3) - cos(s2)*sin(s1)*sin(s3)) - sin(s5)*(cos(s4)*(cos(s1)*sin(s3) + cos(s2)*cos(s3)*sin(s1)) - sin(s1)*sin(s2)*sin(s4));
　　oz = - sin(s5)*(cos(s2)*sin(s4) + cos(s3)*cos(s4)*sin(s2)) - cos(s5)*sin(s2)*sin(s3);
　　ax = sin(s4)*(sin(s1)*sin(s3) - cos(s1)*cos(s2)*cos(s3)) - cos(s1)*cos(s4)*sin(s2);
　　ay = - sin(s4)*(cos(s1)*sin(s3) + cos(s2)*cos(s3)*sin(s1)) - cos(s4)*sin(s1)*sin(s2);
　　az = cos(s2)*cos(s4) - cos(s3)*sin(s2)*sin(s4);
　　Px = 40*cos(s1)*cos(s2) - (764*cos(s1)*sin(s2))/5;

　　Py = 40*cos(s2)*sin(s1) - (764*sin(s1)*sin(s2))/5;

　　Pz = (764*cos(s2))/5 + 40*sin(s2);

由於時間關系，暫時先不把具體過程寫下來，其實源代碼裏面全部都有，需要的請先去文章開頭的代碼倉下載代碼來看。

<-- 未完待續 -->

　

ROS系統MoveIt玩轉雙臂機器人系列（六）--D-H逆運動學求解程序（C++）