執行個體9：四足機器人運動學正解平面RR單腿可視化

執行個體9：四足機器人正向運動學單腿可視化

實驗目的

1. 通過動手實踐，搭建mini pupper四足機器人的腿部，掌握機器人單腿結構。
2. 通過理論學習，熟悉幾何法、旋轉矩陣法在運動學正解（FK）中的用處。
3. 通過程式設計實踐，熟悉可視化簡化機器人單腿的方法。

實驗要求

通過Python程式設計，在簡化的mini pupper平面二連杆模型内，讓使用者輸入關節角，根據關節角及關節的基本資訊，計算出每個關節相對于基坐标系的坐标，并将其可視化輸出。

實驗知識

1.什麼是正向運動學 Forward Kinematics ？

在mini pupper的實際運動操作中，需要考慮計算小腿末端相對身體基座标系位置和姿态關系的靜态幾何問題。簡單來說，就是給定一組關節角的值，計算小腿末端坐标系相對于基坐标系的位姿。

正向運動學探究的問題可以看做由關節空間Joint Space （ θ 1 , θ 2 , ⋅ ⋅ ⋅ ⋅ , θ n ） （\theta_1,\theta_2,····,\theta_n） （θ1​,θ2​,⋅⋅⋅⋅,θn​）描述到 笛卡爾空間Cartesian Space （ W o r l d P ） （^{World}P） （WorldP） 描述的位置表示。

探究這種位置的表示關系就是正向運動學，對于機器人來說，就是以操作臂的關節變量為自變量，描述操作臂末端執行器的位姿與基坐标系之間的函數關系。

2.怎麼描述機器人的一條腿？

既然要考慮計算mini pupper實際運動中小腿末端相對身體基座标系位置和姿态關系的計算，就應該有對應的定性和定量的描述及表示來使得計算能夠實作。

在機器人學中，操作臂由連杆link組成，将連杆視作剛體，相鄰連杆由關節joint連接配接，關節可以相對 轉動（revolute） 或者 移動（prismatic），轉動的軸被稱為Axis。

操作臂的這種描述對應到人體上，可以看作簡化的人類的手臂，“大臂”就是一個連杆，通過“肘關節”這個關節和另一個連杆“小臂”相連，mini pupper也是相同的道理。

關節的轉動和移動引出了兩個數學量，對于轉動關節，相鄰杆件的相對角度變化被稱為關節角，而對于移動關節，相鄰杆件之間的相對位移變化被稱為關節偏移量。

對于操作臂來說，自由度的數量是具有獨立的位置變量的數目，常見的工業機器人大都是開式的運動鍊，每個關節位置由唯一變量來定義，是以關節的數目常常等于自由度的數量。

操作臂的末端被稱為末端執行器，比如夾具、焊槍等，通常用設定在末端執行器上的工具坐标系Frame_Tool與操作臂的固定底座的參考基坐标系Frame_Base的相對關系，來描述操作臂的位置與姿态。

3.怎麼描述腿間部件的關系？

為了描述相鄰連杆之間的關系，引入了4個變量對連杆的空間關系進行描述。

名稱	符号	含義	注釋
連杆長度	a i − 1 a_{i-1} ai−1​	兩個相鄰關節軸之間公垂線的長度	連杆長度是沿着 X ^ i − 1 \hat X_{i-1} X^i−1​方向， Z ^ i − 1 \hat Z_{i-1} Z^i−1​和 Z ^ i \hat Z_{i} Z^i​之間公垂線的長度。因為 a i − 1 a_{i-1} ai−1​是距離，是以始終有 a i − 1 ≥ 0 a_{i-1}\geq0 ai−1​≥0。
連杆扭轉角	α i − 1 \alpha_{i-1} αi−1​	沿着 a i − 1 a_{i-1} ai−1​方向的投影平面中兩關節軸線的夾角	連杆扭轉角是沿着 X ^ i − 1 \hat X_{i-1} X^i−1​方向看去所形成的的二維平面内 Z ^ i − 1 \hat Z_{i-1} Z^i−1​和 Z ^ i \hat Z_{i} Z^i​的夾角，按照右手法從 Z ^ i − 1 \hat Z_{i-1} Z^i−1​扭向 Z ^ i \hat Z_{i} Z^i​取正負。
連杆偏距	d i d_{i} di​	沿兩個相鄰連杆公共軸線方向的距離。	沿着 Z ^ i \hat Z_{i} Z^i​方向看去，對于 X ^ i − 1 \hat X_{i-1} X^i−1​與 Z ^ i \hat Z_{i} Z^i​所在直線的相交點和 X ^ i \hat X_{i} X^i​與 Z ^ i \hat Z_{i} Z^i​所在直線的相交點，這兩個相交點的距離。
關節角	θ i \theta_{i} θi​	兩相鄰連杆繞公共軸線旋轉的角度。	沿着 Z ^ i \hat Z_{i} Z^i​方向看，二維平面内 X ^ i − 1 \hat X_{i-1} X^i−1​和 X ^ i \hat X_{i} X^i​的夾角，方向為 X ^ i − 1 \hat X_{i-1} X^i−1​扭向 X ^ i \hat X_{i} X^i​。

連杆扭轉角和連杆長度定義了同一杆件的兩個關節軸之間的關系，關節角和連杆偏距定義了連杆是如何連接配接的。

上表注釋中提及的 X ^ i \hat X_{i} X^i​、 Y ^ i \hat Y_{i} Y^i​、 Z ^ i \hat Z_{i} Z^i​将在後文中結合連杆坐标系提及，可結合了解。

4.Denavit-Hartenberg表達法

簡稱為DH表達法，版本為Craig version，DH法是用四個連杆參數來描述機構運動關系的規則，兩個參數用于描述連杆本身，另外兩個參數用于描述連杆之間的連接配接關系。

這四個參數即是之前所提到的連杆扭轉角 α i − 1 \alpha_{i-1} αi−1​、連杆長度 a i − 1 a_{i-1} ai−1​、關節角 θ i \theta_{i} θi​、連杆偏距 d i d_{i} di​。

通常來說，對于轉動關節，關節變量為關節角 θ i \theta_{i} θi​，對于移動關節，關節變量為連杆偏距 d i d_{i} di​。

對于這類關節，除了關節變量外的其他的三個連杆參數都是固定不變的硬體屬性參數。

5.連杆坐标系

為了描述連杆間的關系，坐标系被引入，是以需要在每個連杆上建立連杆坐标系，對于連杆 i − 1 {i-1} i−1，其上有連杆坐标系 { i − 1 } \{i-1\} {i−1}，在Craig改良DH法中，連杆坐标系的原點通常被規定在在關節軸 A x i s ( i − 1 ) Axis_{(i-1)} Axis(i−1)​上。

按照這種坐标系的規定，固定連杆坐标系的各正交軸如下：

軸或點	定義
原點	公垂線 a i a_i ai​與關節軸 A x i s i Axis_i Axisi​的交點
Z ^ i \hat Z_i Z^i​	與關節軸 A x i s i Axis_i Axisi​重合
X ^ i \hat X_i X^i​	沿着 公垂線 a i a_i ai​由關節 j o i n t i joint_i jointi​指向關節 j o i n t i + 1 joint_{i+1} jointi+1​方向。當公垂線 a i = 0 a_i=0 ai​=0時， X ^ i \hat X_i X^i​垂直于 Z ^ i \hat Z_i Z^i​與 Z ^ i + 1 \hat Z_{i+1} Z^i+1​形成的平面，可以有兩個方向，根據友善選用
Y ^ i \hat Y_i Y^i​	根據右手定則确定，已知拇指為 X ^ i \hat X_i X^i​，食指為 Y ^ i \hat Y_i Y^i​ ，已知中指為 Z ^ i \hat Z_i Z^i​

連杆坐标系中的特殊存在

首端連杆

首端連杆被稱為地杆，機器人的固定基座上的坐标系 { 0 } \{0\} {0}常被用作參考坐标系。 Z ^ 0 \hat Z_0 Z^0​方向和 Z ^ 1 \hat Z_1 Z^1​相同，并當坐标系 { 1 } \{1\} {1}的關節變量 θ i \theta_{i} θi​或 d i d_{i} di​為0時，使坐标系 { 0 } \{0\} {0}和坐标系 { 1 } \{1\} {1}重合，以簡化計算。

轉動關節Revolute joint α 0 = 0 a 0 = 0 d 1 = 0 θ 1 = ? ( 當為 0 時 F r a m e 重合 ) \text {轉動關節Revolute joint} \quad \alpha_0=0 \quad a_0=0 \quad d_1=0 \quad\theta_1=?(當為0時Frame重合) 轉動關節Revolute jointα0​=0a0​=0d1​=0θ1​=?(當為0時Frame重合)

移動關節Prismatic joint α 0 = 0 a 0 = 0 θ 1 = 0 d 1 = ？ ( 當為 0 時 F r a m e 重合 ) \text {移動關節Prismatic joint} \quad \alpha_0=0 \quad a_0=0 \quad \theta_1=0 \quad d_1=？(當為0時Frame重合) 移動關節Prismatic jointα0​=0a0​=0θ1​=0d1​=？(當為0時Frame重合)

末端連杆

末端杆沒有下一根杆件，是以要和上一根杆件找關聯。

對于轉動關節， X ^ n \hat X_n X^n​方向和 X ^ n − 1 \hat X_{n-1} X^n−1​相同，再選取設定原點使得 d n = 0 d_n=0 dn​=0

對于移動關節，選取設定 X ^ n \hat X_n X^n​方向使得 θ n = 0 \theta_n=0 θn​=0，當 d n = 0 d_n=0 dn​=0時，原點位于關節軸 n n n和 X ^ n − 1 \hat X_{n-1} X^n−1​交點處

學習完連杆坐标系中，連杆參數可以更簡單地了解:

名稱	符号	含義
連杆扭轉角	α i − 1 \alpha_{i-1} αi−1​	連杆扭轉角是繞 X ^ i − 1 \hat X_{i-1} X^i−1​軸， Z ^ i − 1 \hat Z_{i-1} Z^i−1​旋轉到 Z ^ i \hat Z_{i} Z^i​的夾角
連杆長度	a i − 1 a_{i-1} ai−1​	連杆長度是沿着 X ^ i − 1 \hat X_{i-1} X^i−1​軸， Z ^ i − 1 \hat Z_{i-1} Z^i−1​移動到 Z ^ i \hat Z_{i} Z^i​的公垂線的長度
關節角	θ i \theta_{i} θi​	繞 Z ^ i \hat Z_{i} Z^i​軸， X ^ i − 1 \hat X_{i-1} X^i−1​旋轉到 X ^ i \hat X_{i} X^i​的夾角
連杆偏距	d i d_{i} di​	沿着 Z ^ i \hat Z_{i} Z^i​軸， X ^ i − 1 \hat X_{i-1} X^i−1​移動到 X ^ i \hat X_{i} X^i​的距離

建立連杆坐标系和DH表

1. 找出關節軸

   分析運動狀态，找出關節平移或轉動的軸向（joint axes）

2. 找出 Z ^ i \hat Z_{i} Z^i​

   Z ^ i \hat Z_{i} Z^i​的正方向為轉動或移動的軸axis方向

3. 找出 X ^ i \hat X_{i} X^i​

   X ^ i \hat X_{i} X^i​的正方向是兩個關節軸之間公垂線上，指向下一個關節軸的方向

   （ a i = 0 a_i=0 ai​=0時，即當兩關節軸相交時， X X X和 Z i Z_i Zi​和 Z i + 1 Z_{i+1} Zi+1​的方向垂直，此時方向怎麼友善怎麼來）

4. 确定原點

   原點即為 Z ^ i \hat Z_{i} Z^i​與 X ^ i \hat X_{i} X^i​的交點

5. 找出 Y ^ i \hat Y_{i} Y^i​

   根據右手定則确定，拇指為 X ^ i \hat X_i X^i​，食指為 Y ^ i \hat Y_i Y^i​ ，中指為 Z ^ i \hat Z_i Z^i​

6. 處理首端連杆

   并當 F r a m e 1 Frame_1 Frame1​的關節變量 θ i \theta_{i} θi​或 d i d_{i} di​為0時，使 F r a m e 0 Frame_0 Frame0​和 F r a m e 1 Frame_1 Frame1​重合，以簡化計算。

7. 處理末端連杆

   盡量使得連杆參數為0以簡化計算

根據以上幾步處理的結果，可以擷取DH參數表

DH參數表是Frame之間的關系，每個i代表一對軸之間的關系

表中的1為基坐标系 F r a m e 0 Frame_0 Frame0​與第一個軸坐标系 F r a m e 1 Frame_1 Frame1​的關系

表中的3為末端連杆與最後一個可（轉、平移）的軸的關系

i	α i − 1 \alpha_{i-1} αi−1​	a i − 1 a_{i-1} ai−1​	θ i \theta_i θi​	d i d_i di​
1	θ 1 \theta_1 θ1​
2	L 1 L_1 L1​	θ 2 \theta_2 θ2​
3	L 2 L_2 L2​	θ 3 \theta_3 θ3​

6.連杆坐标系之間的轉化

對于 F r a m e i − 1 Frame_{i-1} Framei−1​，要從 A x i s i − 1 Axis_{i-1} Axisi−1​到 A x i s i Axis_{i} Axisi​。

首先，完成從 Z ^ i − 1 \hat Z_{i-1} Z^i−1​到 Z ^ i \hat Z_{i} Z^i​的旋轉 α i − 1 \alpha_{i-1} αi−1​和平移 a i − 1 a_{i-1} ai−1​距離。

然後，将旋轉和平移後的 A x i s i − 1 Axis_{i-1} Axisi−1​旋轉 θ i \theta_{i} θi​，并沿着 Z ^ i \hat Z_{i} Z^i​方向平移 d i d_{i} di​距離。

總計進行四次操作，最終可得

i − 1 P = i i − 1 T i P ^{i-1}P=^{i-1}_iT^{i}P i−1P=ii−1​TiP

i i − 1 T ^{i-1}_iT ii−1​T中合并了這四次操作，得出該變換矩陣：

i i − 1 T = [ C o s θ i − S i n θ i 0 a i − 1 S i n θ i C o s α i − 1 C o s θ i C o s α i − 1 − S i n α i − 1 − S i n α i − 1 d i S i n θ i S i n α i − 1 C o s θ i S i n α i − 1 C o s α i − 1 C o s α i − 1 d i   0 0 0 1 ] ^{i-1}_iT= \left[ \begin{matrix} Cos\theta_i & -Sin\theta_i & 0 & a_{i-1} \\ Sin\theta_iCos\alpha_{i-1} & Cos\theta_iCos\alpha_{i-1} & -Sin\alpha_{i-1} &-Sin\alpha_{i-1}d_i\\ Sin\theta_iSin\alpha_{i-1} & Cos\theta_iSin\alpha_{i-1} & Cos\alpha_{i-1} &Cos\alpha_{i-1}d_i\ \\ 0&0&0&1 \end{matrix} \right] ii−1​T=

​Cosθi​Sinθi​Cosαi−1​Sinθi​Sinαi−1​0​−Sinθi​Cosθi​Cosαi−1​Cosθi​Sinαi−1​0​0−Sinαi−1​Cosαi−1​0​ai−1​−Sinαi−1​di​Cosαi−1​di​ 1​

​

多次使用該變換矩陣可以得出不相鄰的關節之間的轉化關系

7.mini pupper的簡化運動學結構

mini pupper的單條腿部可簡化視作典型的三連杆非平面操作臂，三自由度的設計使得它理論上能夠到達空間限位範圍内的任意位置。

當我們進一步簡化，不考慮橫向髋關節運動時，mini pupper的腿部可以簡化為平面二連杆機構RR模型，使用幾何解法即可在已知 θ 1 \theta_1 θ1​和 θ 2 \theta_2 θ2​的情況下求得 0 P ^0P 0P的坐标，完成運動學正解。

對于大臂與小臂相交處 Q Q Q：

x q = l 1 C o s θ 1 x_q=l_1Cos\theta_1 xq​=l1​Cosθ1​

y q = l 1 S i n θ 1 y_q=l_1Sin\theta_1 yq​=l1​Sinθ1​

對于小臂末端點：

x p = l 1 C o s θ 1 + l 2 C o s ( θ 1 + θ 2 ) x_p=l_1Cos\theta_1+l_2Cos(\theta_1+\theta_2) xp​=l1​Cosθ1​+l2​Cos(θ1​+θ2​)

y p = l 1 S i n θ 1 + l 2 S i n ( θ 1 + θ 2 ) y_p=l_1Sin\theta_1+l_2Sin(\theta_1+\theta_2) yp​=l1​Sinθ1​+l2​Sin(θ1​+θ2​)

圖檔：平面二連杆機構RR模型

實驗步驟

1. 編寫Python代碼 rr_FK.py

#!/usr/bin/python
# coding:utf-8
# rr_FK.py
# mini pupper的簡化單腿，可視作同一平面的RR類機械臂，可視化該機械臂，由給定角度計算末端點位置
import matplotlib.pyplot as plt  # 引入matplotlib
import numpy as np  # 引入numpy
from math import degrees, radians, sin, cos


# 幾何法：關節角轉端點坐标
def theta_2_position_rr(l1, l2, theta1, theta2):
    """
    運動學正解 将輸入的關節角轉化為對應的端點坐标
    :param l1: 大臂長
    :param l2: 小臂長
    :param theta1: 大臂關節角
    :param theta2: 小臂關節角
    :return: 端點1坐标 端點2坐标
    """
    point_1 = [l1*cos(radians(theta1)), l1*sin(radians(theta1))]
    point_2 = [l1*cos(radians(theta1))+l2*cos(radians(theta1+theta2)),
               l1*sin(radians(theta1))+l2*sin(radians(theta1+theta2))]
    print(point_1)
    print(point_2)
    return point_1, point_2


def preprocess_drawing_data(points):
    """
    處理點坐标資料轉化為matplotlib适應的繪圖格式
    :param points: 點資料
    :return: 繪圖資料x坐标list和對應的y坐标list
    """
    xs = [0]
    ys = [0]
    xs.append(points[0][0])
    xs.append(points[1][0])
    ys.append(points[0][1])
    ys.append(points[1][1])
    return xs, ys


def annotate_angle(x0, y0, rad1, rad2, name, inverse=False):
    """
    為兩條直線繪制角度
    :param x0: 圓心x坐标
    :param y0: 圓心x坐标
    :param rad1: 起始角
    :param rad2: 終止角
    :param name: 角名
    :param inverse: 用于解決點1的重疊問題
    :return: 無
    """
    theta = np.linspace(rad1, rad2, 100)  # 0~rad
    r = 0.3  # circle radius
    x1 = r * np.cos(theta) + x0
    y1 = r * np.sin(theta) + y0
    plt.plot(x1, y1, color='red')
    plt.scatter(x0, y0, color='blue')
    degree = degrees((rad2 - rad1))
    if inverse:
        plt.annotate("%s=%.1f°" % (name, degree), [x0, y0], [x0 - r / 1.5, y0 - r / 1.5])
    else:
        plt.annotate("%s=%.1f°" % (name, degree), [x0, y0], [x0 + r / 1.5, y0 + r / 1.5])


# 關節資訊
# 大臂長度：5 cm 小臂長度：6 cm
link_length = [5, 6]  # in cm

# 關節角初始化
joints_angle_origin = [-150, 90]
joints_angle = [0, 0]
print("關節角初始狀态 theta1=%d°， theta2=%d°" % (joints_angle_origin[0], joints_angle_origin[1]))

# 輸入連杆參數：各關節角
for i in range(1, 3):
    joints_angle[i-1] = int(input("請輸入腿部舵機[%d]要轉動的的角度：" % i))
    print("腿部舵機[{0}]将轉動{1}°".format(i, joints_angle[i-1]))

# 計算并預處理繪圖資料
points_origin = theta_2_position_rr(link_length[0], link_length[1], joints_angle_origin[0], joints_angle_origin[1])
points_after = theta_2_position_rr(link_length[0], link_length[1], joints_angle[0], joints_angle[1])
data_origin = preprocess_drawing_data(points_origin)
data_after = preprocess_drawing_data(points_after)

# 繪圖
fig, ax = plt.subplots()  # 建立圖像
plt.plot(data_origin[0], data_origin[1], color='black', label='original')
plt.scatter(data_origin[0], data_origin[1], color='black')
plt.plot(data_after[0], data_after[1], color='red', label='after')
plt.scatter(data_after[0], data_after[1], color='blue')
ax.set(xlabel='X', ylabel='Y', title='mini pupper FK RR model')
ax.grid()
plt.axis("equal")
plt.legend(loc=2)

# 标注
annotate_angle(data_origin[0][0], data_origin[1][0],
               0, radians(joints_angle_origin[0]), "theta1_original", inverse=True)
annotate_angle(data_origin[0][1], data_origin[1][1], radians(joints_angle_origin[0]),
               radians(joints_angle_origin[0]+joints_angle_origin[1]), "theta2_original", inverse=True)
annotate_angle(data_after[0][0], data_after[1][0],
               0, radians(joints_angle[0]), "theta1_after")
annotate_angle(data_after[0][1], data_after[1][1], radians(joints_angle[0]),
               radians(joints_angle[0]+joints_angle[1]), "theta2_after")
plt.show()

           

2. 運作程式，觀察效果

在rr_FK.py的目錄下執行以下指令：

sudo python rr_FK.py
           

此時應觀察到根據關節角計算出的可視化坐标。

實驗總結

經過本知識點的學習和實驗操作，你應該能達到以下水準：

知識點	内容	了解	熟悉	掌握
硬體	mini pupper單腿結構的搭建	✔
運動學	平面RR類型機械臂的運動學正解	✔
運動學	非平面RRR類型機械臂	✔
運動學	幾何法完成運動學正解	✔
運動學	運動學正解基礎知識	✔
可視化	Python可視化運動學	✔

版權資訊：教材尚未完善，此處預留版權資訊處理方式

mini pupper相關内容可通路：https://github.com/mangdangroboticsclub