【机器人原理与实践（三）】六轴机械臂正逆解控制
文章目錄
- 3.1 空間轉換矩陣的理解
- 3.1.1平移變換
- 3.1.2旋轉變換
- 3.2 D-H參數法
- 3.3 建立機械臂模型
- 3.3.1 機械臂模型介紹
- 3.3.2 使用Matlab進行示教仿真
- 3.4 機器人運動學
- 3.4.1 機器人正運動學
- 3.4.2機器人逆運動學
- 3.5 機械臂運動過程分析
- 3.6 本章小結
本章針對機械臂的運動學進行建模分析。機械臂運動學模型反映的是個關節的角度與執行器末端位姿之間的映射關系。在工業過程中需要考慮的根據期望位姿推導關節角度（機械臂逆解），以及根據現有的關節角度推導末端執行器位姿（機械臂正解）。
3.1 空間轉換矩陣的理解
3.1.1平移變換
3.1.2旋轉變換
除了位置之外，還需要對剛體的指向，即姿態進行描述。而這也是旋轉矩陣最本質的來源，即來源于 坐標系的旋轉，這一部分理解不好或是理解不透徹，會導致此后面對各種形式的旋轉時出現混亂，所以公式將進行詳細的表述。
描述姿態的方式就是為剛體建立一個固連于剛體的標準正交坐標系，并由 其相對于參考坐標系的 單位向量 在參考坐標系中的描述 來表示。如上圖所示，剛體的局部固連參考系為o-xyz，其局部固連參考系坐標系的單位向量為（x,y,z），而這局部坐標系的單位向量在參考坐標系 O?xyz 中的表示為：
可以進行進一步的整理，即上式等價于：
最后一個矩陣就是兩個坐標系基之間的關系，即兩個坐標系之間的轉換關系，將這個矩陣定義為旋轉矩陣
結合上面兩種變換可以得到三維空間變換的一般形式：
用這個4X4的矩陣就可以描述四個機械臂末端相對于機械臂位置的變換，而當你將機械臂的起始坐標設置為（0,0,0）你就可以直接通過上述矩陣確定機械臂的末端位姿。(px,py,pz)將直接反應機械臂末端所直接對應的坐標。而n,o,a將直接反應機械臂末端坐標系相對于機械臂基座坐標系所發生的的旋轉。
3.2 D-H參數法
D-H參數法是Denavit和Hartenberg在他們1955年提出的一種機器人的建模方法。該方法看似簡單，但是通用性很強，雖然年代久遠，但仍然是推導機器人運動學方程的標準方法。
機械臂通常有多個關節組成。D-H參數法在這些關節處以一定的規則建立參考，然后找到兩兩相鄰的關節坐標系之間的轉換關系，就是說在建立了坐標系的約束下，有限次的平移的旋轉運動可以使用一個齊次矩陣表示這種轉換關系。只要將相應的轉換矩陣相乘，就可以得到任意兩個關節坐標系的位置關系。在實際應用中，僅僅需要第一關節與最后一個關節的轉換關系，就可在首關節確定的情況下確定末端執行器位姿。
以上圖為例從關節n+1到關節n+2：
繞z軸轉θ_(n+1)，使x_n軸和x_(n+1)軸相互平行。
延z軸平移d_(n+1)，使x_n軸和x_(n+1)軸共線。
延x軸平移a_(n+1)，使x_n軸和x_(n+1)軸原點重合。
z_n軸繞x_(n+1)軸旋轉旋轉α_(n+1)角度，使z_n z_(n+1)重合。
相鄰坐標系之間的變換都可以遵循以上步驟：
繞z軸轉θ_(n+1)：
延z軸平移d_(n+1)：
延x軸平移a_(n+1)：
z_n軸繞x_(n+1)軸旋轉旋轉α_(n+1)角度：
總變換矩陣為以上矩陣按順序相乘
這就是任意相鄰坐標系之間轉換矩陣的通式。根據上述描述可以很容易的發現，使用D-H參數法建立機械臂運動學方程，關鍵在于要根據具體的機械臂建立合適點的關節坐標系。建立這樣的一個關節坐標系需要四個關鍵參數，即θ,d,a,α, 這些參數就是機械臂的D-H參數，我們也可以根據這些參數使用Matlab進行相關的仿真。
3.3 建立機械臂模型
3.3.1 機械臂模型介紹
這是一個簡單的六軸機械臂
圖3.5 簡化圖片
經過測量以后得到機械臂的D-H參數表：
表格 3.1 D-H參數表
| 0-1 | θ1 | 58 | 0 | pi/2 |
| 1-2 | θ2+pi/2 | 0 | 55 | 0 |
| 2-3 | θ3 | 0 | 55 | 0 |
| 3-4 | θ4 | 0 | 55 | 0 |
使用Matlab 機器人工具箱構建仿真
L(1) = Link([ 0, d(1), aa(1), alpha(1),0,offset(1) ]);
L(2) = Link([ 0, d(2), aa(2), alpha(2) ,0,offset(2) ]);
L(3) = Link([ 0, d(3), aa(3), alpha(3),0,offset(3) ]);
L(4) = Link([ 0, d(4), aa(4), alpha(4),0,offset(4) ]);
結合表3.1與公式2.4結合Matlab得到A1-A6所有的轉換矩陣，與總的轉換矩陣：
那么該機械臂的正運動學方程即為以上4個矩陣按順序乘積，由于結構太過于復雜這里不直接列出了A04=A01A12A23*A34;
在得到旋轉矩陣之后，根據以下公式就可求出末端執行器的位姿
這里有四個旋轉角，在D-H參數已知的情況下，該運動學方程的計算結果就是機械臂末端相對機械臂底端坐標系的位姿矩陣。
3.3.2 使用Matlab進行示教仿真
3.4 機器人運動學
3.4.1 機器人正運動學
關于機械臂的正運動學問題，可以調用Robotics工具箱中的fkine函數進行求解。函數調用的一般形式為T=R.fkine{Q}，其中R表示機械臂模型，T為前向運動的正解，Q為機械臂四個關節的角度值。
取Q1=[pi/5,-pi/3.pi/2.-pi/4],帶入fkine函數，或者直接帶入公式1計算的矩陣的到一個齊次變換矩陣如下：
T =
可以得到仿真圖像：
根據得到的圖像結合實際可以確定，我們做出的正向運動學仿真大致是正確的。
3.4.2機器人逆運動學
[4]我們已經看到了前向運動學問題。逆運動學問題更有趣，其解決方案更有用。在位置層面，問題表述為：“考慮到機器人手部的理想位置，所有機器人關節的角度必須是什么？”。人類一直在解決這個問題，甚至沒有想到它。當你早上吃麥片時，你只需伸手拿起勺子。你不會想，“我的肩膀需要這樣做，我的肘部需要這樣做，等等。” 下面我們將看看大多數機器人如何解決這個問題。我們將從一個非常簡單的例子開始：
將機械臂的末端指向（-130,0,113）
對于機器人逆運動學的問題同樣可以使用工具箱中的ikine函數解決這個問題。
可以在指定轉化函數p的情況下輸入,得到所有關節的角度向量：
q=bot.ikine(p,‘mask’,[1 1 1 1 0 0])
figure(2);
bot.plot(q)
p =
-1 0 0 -1300 0 -1 00 -1 0 1130 0 0 1q =
0.0000 0.1111 1.0963 0.6225
當然只用MATLAB是沒有用的我們必須使用可以在開發板上運行的語言重新實現這樣的效果。
這樣的逆解問題本質上其實是解方程的問題，我們可以將上面的問題轉換為：
我們需要達到的效果是已知x,y,z(機械臂末端坐標)，我們需要計算出th1,th2,th3,th4
通過三個算式四個未知數顯然是不太可能的，所以可以指定出th4也就是末端角度以后解算下面的方程。
基于這樣的方程解算通過python實現：
def changeto(x,y,z,th4,pwm):th4=th4*3.14/180def solve_function(unsolved_value):th1,th2,th3=unsolved_value[0],unsolved_value[1],unsolved_value[2]return [75*sin(th4)*(cos(th1)*sin(th2)*sin(th3) - cos(th1)*cos(th2)*cos(th3)) - 75*cos(th4)*(cos(th1)*cos(th2)*sin(th3) + cos(th1)*cos(th3)*sin(th2)) - 55*cos(th1)*sin(th2) - 55*cos(th1)*cos(th2)*sin(th3) - 55*cos(th1)*cos(th3)*sin(th2)-x,75*sin(th4)*(sin(th1)*sin(th2)*sin(th3) - cos(th2)*cos(th3)*sin(th1)) - 75*cos(th4)*(cos(th2)*sin(th1)*sin(th3) + cos(th3)*sin(th1)*sin(th2)) - 55*sin(th1)*sin(th2) - 55*cos(th2)*sin(th1)*sin(th3) - 55*cos(th3)*sin(th1)*sin(th2)-y,55*cos(th2) + 55*cos(th2)*cos(th3) - 55*sin(th2)*sin(th3) + 75*cos(th4)*(cos(th2)*cos(th3) - sin(th2)*sin(th3)) - 75*sin(th4)*(cos(th2)*sin(th3) + cos(th3)*sin(th2)) + 58-z,]solved=fsolve(solve_function,[0, 0, 0])solved=solved*180/3.14 print(solved)3.5 機械臂運動過程分析
機械臂的運動過程，也就是舵機驅動關節轉動的過程。過程如下：
（1）通過程序設置初始化各個脫機狀態。
（2）各個舵機轉動一定角度，使得長臂、斷臂和機械臂末端都處于水平位置，整個過程中 號舵機旋轉一定角度控制機械爪處于打開狀態。
（3）2號舵機旋轉一定角度，使長臂處于垂直狀態，同時機械爪關閉。
（4）3號關節旋轉一定角度，使斷臂穿衣水平狀態，通過機械爪打開。
（5）關閉機械爪。
（6）使機械臂回到初始狀態
3.6 本章小結
本章首先進行連桿坐標，使用并分析了 D-H 模型，并使用 MatLab 軟件進行了正反解的驗證，分析了機械臂運動學正逆解，并給出了仿真圖。通過Matlab計算出的算式直接應用到python中，給出了在python中的正逆解算法。
總結
以上是生活随笔為你收集整理的【机器人原理与实践（三）】六轴机械臂正逆解控制的全部內容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 手机开机启动慢是什么原因_手机开机慢,小
- 下一篇： HCNP——路由基础