注：本篇博文全部源碼下載地址為：Git Repo。
一、轉換矩陣
經過上一篇博客介紹，我們已經獲得了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));
ny = 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);
由於時間關系，暫時先不把具體過程寫下來，其實源代碼里面全部都有，需要的請先去文章開頭的代碼倉下載代碼來看。
tip：
源碼是一個VS工程，基於VS2010創建。
<-- 未完待續 -->
歡迎留言、私信、郵箱、微信等任何形式的技術交流。
作者信息：
名稱：Shawn
郵箱：zhanggx0102@163.com
微信二維碼：↓
