機械臂正運動學-DH參數-Python快速實現

機械臂正運動學-DH參數-Python快速實現

前言：

最近在玩一個非常弱智的機械臂，好多功能都沒有，連個配套的仿真環境都沒， 虛擬邊界和碰撞檢測的功能都非常難用。
沒辦法，我只能自己實現一個簡陋的虛擬邊界功能，這必須要在已知關節角的情況下，提前計算出每個關節的三維座標。
這裏的問題凝結爲輸入輸出就是：
已知： 機械臂的關節長度，關節構型

輸入： 機械臂的關節角度；
輸出： 機械臂的關節座標。
全網好像沒有搜到一個簡單可用、基於DH參數的Python的正運動學代碼（github有一個不能用）。
爲了防止以後忘記，以及方便大家學習借鑑。先拋出來，供大家參考

我爲了實現這個功能 ，來來回回看了三天的資料和課程，但是感覺核心步驟也就幾個公式和對應關係，所以我就把這幾個核心的東西單拎出來了。

大家如果時間充分，可以詳細的看看課程和教材，如果時間不夠，就可以看我這個，如果我有哪些細節沒有描述清楚的話，可以在評論區留言~

整體思路流程：

蒐集機械臂相關配置資料：關節長度、構型、官方設定的座標系；
通過兩個對應關係，找到機械臂的DH參數表；
找到了之後，代入轉換矩陣T中；
連乘所有關節的T；
獲取關節三維座標。

學習資料

要想得到上面的輸出，需要的基礎知識比較多。
有：

1. 剛體的座標變換；
2. DH參數；
3. 基本上就是上面兩個了。

爲了弄明白這個過程， 我請教了幾位大佬，大佬說可以看看b站臺大的機器人學，和《機器人學導論》-斯坦福的那本，自己去網上搜PDF版就好了
然後直接基本概念，大家可以去看看：
臺大機器人學之運動學——林沛羣（含課件+書籍）

核心概念：

我們將機械臂的每一個關節軸，都建立一個座標系，那麼從關節1到關節0的變化，其實就是做了一次剛體的座標變換。
而關節7的末端點，則是串着做了好多次的座標變換。

DH參數的理解。

先掛一個參考鏈接，這裏面的介紹的更詳細：
https://blog.csdn.net/aic1999/article/details/82490615
上節說到本質是座標變換，那麼我們如何根據已知信息，確定好座標變換的基本信息？
這裏面就得用到一個神奇的DH參數（兩位大佬名字的縮寫）
來看看課本里的這張經典圖。

我們需要知道，決定四個軸的相對位置關係，我們可以用四個變量來描述（雖然可能不唯一，但是夠了）。
那麼我們需要知道的第一個對應關係：

DH參數的定義：

沿着軸方向，逆時針爲正。

這裏面我們還差一個東西，如何定義座標系？

建立座標系

其實一般如果是靠譜的機器人，這個座標系應該是給的。

把我這次用的機器人的拿過來，作爲例子，有例子，大家理解起來就方便了。

可以看出來一個很有意思的事情， 01關節是放在一起看了，即12座標系原點重合，這裏是將1杆的長度看作0了。下面去計算DH參數的時候也需要注意的。而且我們計算的時候，是無法計算出1軸的座標。

座標系和參數對應關係來了，我們就能填好DH表了：
如何填寫？
對着座標軸的圖和連桿參數定義，一個一個填。

我們以第一個軸爲例，第一行有四個值：$\alpha$_1-0 , $a$_1-0 , $d$_1-0 $\theta$_1-0
這裏面的$\alpha$_1-0即$\alpha$₀是繞X₀正方向, 從Z₀旋轉到Z₁的角度。從圖中可以看出來，沒旋轉，即爲0.
$a$₀沿着X₀,從Z₀移動到Z₁的距離，因爲兩個Z重合，不存在移動距離，即距離爲0；
d₁的話，不一樣，沿着Z₁軸，從X₀移動到X₁的距離，可以看出來，移動了0.31米，雖然方向不一樣，但是確實是得移動這麼多，才能重合。
$\theta$的話，就是關節轉動的角度了，後面幾個關節，可能初始角度得加一個180°纔行。
這個DH參數值拿到了之後，就得想辦法拿到轉換矩陣了。
好在《機器人學導論》這本書裏直接給了計算公式：

矩陣變換公式：

這玩意兒還得配套幾個公式纔行。

總之拿到了DH參數，就把表裏的值代入到T中， 然後連乘，就能計算出末端位姿了。

最後直接上代碼吧：

from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from matplotlib import cm
import numpy as np
from math import radians, sin, cos


def set_axes_equal(ax):
# 這一段是copy別人的。用處不是很大。
    '''Make axes of 3D plot have equal scale so that spheres appear as spheres,
    cubes as cubes, etc..  This is one possible solution to Matplotlib's
    ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
    Input
      ax: a matplotlib axis, e.g., as output from plt.gca().
    '''

    x_limits = ax.get_xlim3d()
    y_limits = ax.get_ylim3d()
    z_limits = ax.get_zlim3d()

    x_range = abs(x_limits[1] - x_limits[0])
    x_middle = np.mean(x_limits)
    y_range = abs(y_limits[1] - y_limits[0])
    y_middle = np.mean(y_limits)
    z_range = abs(z_limits[1] - z_limits[0])
    z_middle = np.mean(z_limits)

    # The plot bounding box is a sphere in the sense of the infinity
    # norm, hence I call half the max range the plot radius.
    plot_radius = 0.5*max([x_range, y_range, z_range])

    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])

def dh_matrix(alpha, a, d, theta):
# 傳入四個DH參數，根據公式3-6，輸出一個T矩陣。
    alpha = alpha / 180 * np.pi
    theta = theta / 180 * np.pi
    matrix = np.identity(4)
    matrix[0,0] = cos(theta)
    matrix[0,1] = -sin(theta)
    matrix[0,2] = 0
    matrix[0,3] = a
    matrix[1,0] = sin(theta)*cos(alpha)
    matrix[1,1] = cos(theta)*cos(alpha)
    matrix[1,2] = -sin(alpha)
    matrix[1,3] = -sin(alpha)*d
    matrix[2,0] = sin(theta)*sin(alpha)
    matrix[2,1] = cos(theta)*sin(alpha)
    matrix[2,2] = cos(alpha)
    matrix[2,3] = cos(alpha)*d
    matrix[3,0] = 0
    matrix[3,1] = 0
    matrix[3,2] = 0
    matrix[3,3] = 1
    return matrix、
    
joint_num = 7

# --- Robotic Arm construction ---
# DH參數表，分別用一個列表來表示每個關節的東西。
joints_alpha = [0, 90, 90, 90, 90, 90, 90]
joints_a = [0, 0, 0, 0, 0, 0, 0]
joints_d = [0.31, 0.0, 0.4, 0.0, 0.4, 0.0, 0.175]
joints_theta = [0, 180, 180, 180, 180, 180, 180]

#    Joint Angle variables
# joints_angle = [-0.001, -21.0, -0.001, -21.0, 0.0, 0.0, -0.0]
# 選定幾個特定的關節角，看看算出來的值，和真實值是否一致，方向是否反了。
joints_angle = [0, -23.43, 0, 50, 0, 0, 0]
#    DH參數轉轉換矩陣T---------------------
joint_hm = []
for i in range(joint_num):        
    joint_hm.append(dh_matrix(joints_alpha[i], joints_a[i], joints_d[i], joints_theta[i]+joints_angle[i]))

# -----------連乘計算----------------------
for i in range(joint_num-1):
    joint_hm[i+1] = np.dot(joint_hm[i], joint_hm[i+1])    
# Prepare the coordinates for plotting
for i in range(joint_num):
    print(np.round(joint_hm[i][:3, 3], 5))
# 獲取座標值
X = [hm[0, 3] for hm in joint_hm]
Y = [hm[1, 3] for hm in joint_hm]
Z = [hm[2, 3] for hm in joint_hm]
# Plot
ax = plt.axes(projection='3d')
# ax.set_aspect('equal')
ax.plot3D(X, Y, Z)

ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')

set_axes_equal(ax)
plt.show()