開源七軸myArm協(xié)作機(jī)械臂正逆運(yùn)動學(xué)技術(shù)講解

引言：
在本文中，我們將深入探討機(jī)器人學(xué)的兩個核心概念：正運(yùn)動學(xué)和逆運(yùn)動學(xué)。這兩個概念是理解和控制機(jī)械臂運(yùn)動的基礎(chǔ)。通過一個具體的7軸機(jī)械臂實(shí)例，我們將詳細(xì)介紹如何計算機(jī)械臂的正運(yùn)動學(xué)和逆運(yùn)動學(xué)。我們首先會解釋正運(yùn)動學(xué)和逆運(yùn)動學(xué)的基本概念和數(shù)學(xué)原理，然后我們將展示如何應(yīng)用這些原理來計算7軸機(jī)械臂的運(yùn)動。我們的目標(biāo)是讓讀者對機(jī)械臂的運(yùn)動控制有一個深入的理解，并了解如何在實(shí)踐中應(yīng)用這些知識。

myArm 300
我們先來簡要介紹我們待會會使用到的機(jī)械臂myArm 300.

myArm 300 Pi是一塊全新的7自由度的中心對稱構(gòu)型協(xié)作機(jī)械臂，myArm搭載著7個高精度的伺服電機(jī)，使得最大工作半徑300mm，末端最大負(fù)載成都能夠達(dá)到200g，重復(fù)定位精度最高達(dá)到了±0.5mm。

myArm 300 Pi的控制主板采用了樹莓派4B 4G，這是一種廣受歡迎的微型計算機(jī)，因其強(qiáng)大的性能和靈活的編程環(huán)境而備受贊譽(yù)。這意味著用戶可以使用多種編程語言來控制myArm 300 Pi，包括Python、C++、Java等，極大地提高了使用者的便利性和靈活性,它還支持ROS1/ROS2 在ROS操作系統(tǒng)中提供了許多功能，例如路徑規(guī)劃，避障，三維感知等。

基本概念
我們要想控制機(jī)械臂，就得有一套關(guān)于機(jī)械臂運(yùn)動控制的算法。正逆運(yùn)動學(xué)是其中的重要組成部分。在介紹正逆運(yùn)動學(xué)之前，我們想先介紹DH模型，這個模型使得正逆運(yùn)動學(xué)的計算變得更加直接和簡單。

DH模型
DH參數(shù)模型是機(jī)器人學(xué)中一種常用的描述機(jī)器人運(yùn)動學(xué)的方法。它通過一組參數(shù)來描述機(jī)器人的關(guān)節(jié)連桿，從而方便地計算機(jī)器人末端執(zhí)行器地位置和姿態(tài)。

當(dāng)使用DH模型用來描述機(jī)械臂時，我們將機(jī)械臂分解為一系列鏈接和關(guān)節(jié)，每個鏈接和關(guān)節(jié)都與一個坐標(biāo)系相關(guān)聯(lián)。我們選擇一個基準(zhǔn)坐標(biāo)系，通常是機(jī)械臂的基座（底座）坐標(biāo)系，然后，為每個鏈接和關(guān)節(jié)定義一個坐標(biāo)系，我們定義兩個坐標(biāo)系：前一個鏈接的坐標(biāo)系和當(dāng)前鏈接的坐標(biāo)系。這兩個坐標(biāo)系之間的變換由四個參數(shù)描述：

連桿長度（a）：它表示相鄰鏈接之間的距離，沿著前一個鏈接的法線測量。它通常指的是鏈接的長度或者鏈接軸線的長度。

連桿的旋轉(zhuǎn)角度（α）：它表示相鄰鏈接之間的旋轉(zhuǎn)角度，圍繞前一個鏈接的法線旋轉(zhuǎn)。它通常指的是鏈接軸線之間的旋轉(zhuǎn)。

關(guān)節(jié)的長度（d）：它表示鏈接的長度或者關(guān)節(jié)的長度，沿著當(dāng)前鏈接的法線測量。它通常指的是關(guān)節(jié)軸線的長度。

關(guān)節(jié)的旋轉(zhuǎn)角度（θ）：它表示關(guān)節(jié)的旋轉(zhuǎn)角度，圍繞當(dāng)前鏈接的法線旋轉(zhuǎn)。它通常指的是關(guān)節(jié)的角度或者關(guān)節(jié)的轉(zhuǎn)動。

通過組合這些參數(shù)，可以構(gòu)建一個四維的變換矩陣，這個變換矩陣可以表示機(jī)械臂末端執(zhí)行器的位置和姿態(tài)。

以myArm為例子，下圖是myArm 300 的DH模型圖以及SDH參數(shù)表。當(dāng)我們有了一個機(jī)械臂的DH模型圖我們具體能干些什么呢：

正向運(yùn)動學(xué)：通過給定關(guān)節(jié)角度，可以使用DH模型圖計算機(jī)械臂末端執(zhí)行器的位置和姿態(tài)，從而進(jìn)行路徑規(guī)劃和運(yùn)動控制

逆向運(yùn)動學(xué)：反過來，也可以用DH模型圖來解決逆向運(yùn)動學(xué)問題，即給定目位置和姿態(tài)，計算出關(guān)節(jié)角度的變化，從而實(shí)現(xiàn)平滑的運(yùn)動軌跡。

路徑規(guī)劃：給定一個開始位置和一個結(jié)束位置，可以使用DH模型來規(guī)劃機(jī)械臂的運(yùn)動路徑。這可能涉及到解決一系列的逆運(yùn)動學(xué)問題。

除此之外還有很多，比如說碰撞檢測和臂章，動畫和可視化機(jī)械臂，仿真機(jī)械臂等等。

正運(yùn)動學(xué)
正運(yùn)動學(xué)是用來描述機(jī)械臂從基座到末端執(zhí)行器的位置和方向的計算方法。給定每個關(guān)節(jié)的角度，我們可以計算出機(jī)械臂末端的位置和方向。這是一個基于幾何和三角學(xué)的過程。

下面舉一個簡單的例子，在一個平面坐標(biāo)系當(dāng)中，已知連桿的感覺長度為l1和l2，并且已知兩個關(guān)節(jié)的轉(zhuǎn)動角度分別為θ1和θ2，那么其末端執(zhí)行器的位置(x,y)可以根據(jù)三角函數(shù)求出。

x=l1cos(θ1)+l2cos(θ1+θ2)

y=l1sin(θ1)+l2sin(θ1+θ2)

就可以求出x，y的坐標(biāo)了。

在實(shí)際應(yīng)用中，我們通常會直接在三維空間中直接進(jìn)行計算，我們可以使用齊次坐標(biāo)來進(jìn)行轉(zhuǎn)換，在齊次坐標(biāo)中，一個二維點(diǎn)(x, y)可以表示為一個三維點(diǎn)(x, y, 1)，一個三維點(diǎn)(x, y, z)可以表示為一個四維點(diǎn)(x, y, z, 1)。通過這種方式，我們可以使用相同的矩陣乘法操作來描述平移和旋轉(zhuǎn)。

例如，一個二維的平移變換可以表示為以下形式的3x3矩陣：

[[1, 0, dx], [0, 1, dy], [0, 0, 1]]

其中，dx和dy表示在x軸和y軸方向上的平移距離。同樣，一個二維的旋轉(zhuǎn)變換可以表示為以下形式的3x3矩陣：

[[cos(theta), -sin(theta), 0], [sin(theta), cos(theta), 0], [0, 0, 1]]

其中，theta表示旋轉(zhuǎn)角度，也就是DH參數(shù)中的θ。

通過將所有關(guān)節(jié)的變換矩陣相乘，我們就可以得到從機(jī)器人基座到末端執(zhí)行器的總變換。這就是正運(yùn)動學(xué)的基本計算過程。這個過程可以用以下的數(shù)學(xué)形式來表示：

T = A1 * A2 * A3 * ... * An

其中 T 是總變換矩陣，Ai 是第 i 個關(guān)節(jié)的變換矩陣，n 是關(guān)節(jié)的數(shù)量。注意，矩陣乘法不滿足交換律，所以乘法的順序很重要。最終得到的T總變化矩陣算出來的結(jié)果就是機(jī)械臂末端相對于基座的坐標(biāo)。提前了解DH模型，對理解后面的矩陣變換很有幫助。

逆運(yùn)動學(xué)
逆向運(yùn)動學(xué)是指根據(jù)機(jī)械臂末端執(zhí)行器的位置和姿態(tài)，計算機(jī)械臂各個關(guān)節(jié)的角度。剛好跟正運(yùn)動學(xué)反過來，是求取各個關(guān)節(jié)的角度。逆運(yùn)動學(xué)的計算通常要比正運(yùn)動學(xué)復(fù)雜得多，這是一個從效果反推原因的過程，通常需要解決非線性方程組，而且解可能不唯一，或者根本不存在。我們還是舉例說明，以2自由度的平面機(jī)械臂來做一個了解。我們已知（x，y）的值，要求出θ1和θ2的角度。

首先我們可以x,y到原點(diǎn)的位置的距離d，這可以勾股定理得到（sqrt代表平方根）

d = sqrt(x^2 + y^2)

然后，我們可以使用余弦定理來求解第二個關(guān)節(jié)的角度θ2。余弦定理可以描述三個邊長已知的三角形中，任何一個角的余弦值。在這里，我們可以將d，L1和L2看作是三個邊長，然后求解θ2：

cos(theta2) = (L1^2 + L2^2 - d^2) / (2 * L1 * L2)

因為θ2可能有兩個解（順時針和逆時針），所以我們需要根據(jù)實(shí)際情況來選擇合適的解。

最后，我們可以使用正弦定理或余弦定理來求解第一個關(guān)節(jié)的角度θ1。正弦定理可以描述三個邊長和對應(yīng)的角的正弦值之間的關(guān)系。在這里，我們可以將d，L1和θ1看作是三個已知的值，然后求解θ1：

theta1 = atan2(y, x) - atan2(L2 * sin(theta2), L1 + L2 * cos(theta2))

求解完之后就可以得知各個關(guān)節(jié)的角度了，這是在二維的空間中的求解方法，在三維空間中的計算方法也是一樣的道理，在計算的時候得考慮機(jī)械臂關(guān)節(jié)的限制，以及其他的因素來決定最終的角度。

7軸機(jī)械臂實(shí)例
接下會用到python對myArm進(jìn)行編程，用到的是pymycobot庫。

正運(yùn)動學(xué)關(guān)節(jié)控制
正運(yùn)動學(xué)算法通常有一個確定的結(jié)果，正運(yùn)動學(xué)的目標(biāo)是根據(jù)給定的關(guān)節(jié)角度，計算機(jī)械臂執(zhí)行器的位置和姿態(tài)，只存在唯一的結(jié)果。下面是使用角度控制的代碼：

from pymycobot import Myarm import time # create myarm object ma = Myarm('/dev/ttyAMA0',115200) ma.send_angles([degree_list],speed) # send_angles function def send_angles(self, degrees, speed): """Send the degrees of all joints to robot arm. Args: degrees: a list of degree values(List[float]).n for mycobot: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0].n for mypalletizer: [0.0, 0.0, 0.0, 0.0] for mypalletizer 340: [0.0, 0.0, 0.0] for myArm: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0].n speed : (int) 1 ~ 100 """ # self.calibration_parameters(degrees=degrees, speed=speed) degrees = [self._angle2int(degree) for degree in degrees] return self._mesg(ProtocolCode.SEND_ANGLES, degrees, speed)

這就是最直觀的正運(yùn)動學(xué)算法的控制，關(guān)節(jié)角度控制機(jī)械臂運(yùn)動。

逆運(yùn)動學(xué)算法控制，坐標(biāo)控制
前面也有提及到，你運(yùn)動學(xué)算法相對來說復(fù)雜很多，可能存在多個解的情況，也可能存在無解的情況。逆運(yùn)動學(xué)算法的控制是通過控制機(jī)械臂末端坐標(biāo)的變化，讓機(jī)械臂前往下一個坐標(biāo)位置。我們用python編寫控制坐標(biāo)運(yùn)動的代碼:

from pymycobot import Myarm import time # create myarm object ma = Myarm('/dev/ttyAMA0',115200) ma.send_coords([coordinates_list],speed,mode) # mode: 0:linear motion,1:nonlinear motion def send_coords(self, coords, speed, mode=None): """Send all coords to robot arm. Args: coords: a list of coords value(List[float]). for mycobot :[x(mm), y, z, rx(angle), ry, rz]n for mypalletizer: [x, y, z, θ] for mypalletizer 340: [x, y, z] speed : (int) 0 ~ 100 mode : (int) 0 - angluar, 1 - linear (mypalletizer 340 does not require this parameter) """ # self.calibration_parameters(coords=coords, speed=speed) coord_list = [] for idx in range(3): coord_list.append(self._coord2int(coords[idx])) for angle in coords[3:]: coord_list.append(self._angle2int(angle)) # print(coord_list) if mode is not None: return self._mesg(ProtocolCode.SEND_COORDS, coord_list, speed, mode) else: return self._mesg(ProtocolCode.SEND_COORDS, coord_list, speed)

7軸機(jī)械臂又被稱之為冗余機(jī)器人，多的一個軸通常用于提供更多的靈活性和運(yùn)動自由度，所以就出現(xiàn)了這么一個現(xiàn)象，機(jī)械臂的末端坐標(biāo)值保持不變的時候，會有許多不同的姿態(tài)。

這就是開頭說到的逆運(yùn)動學(xué)的特點(diǎn)，同一個坐標(biāo)下有許多不同的解。在坐標(biāo)控制中還有一個關(guān)鍵的信息就是線性運(yùn)動和非線性運(yùn)動，主要是指機(jī)械臂末端的運(yùn)動路徑的類型。

線性運(yùn)動：在這種模式下，機(jī)器人末端執(zhí)行器在兩個點(diǎn)之間直線移動。這就意味著，無論機(jī)器人的關(guān)節(jié)如何移動，末端執(zhí)行器都會沿著直線路徑從一點(diǎn)移動到另一點(diǎn)。這種類型的運(yùn)動通常在需要精確位置控制的應(yīng)用中使用，例如在裝配線上的裝配任務(wù)。
非線性運(yùn)動：在這種模式下，機(jī)器人末端執(zhí)行器的路徑不是直線，而是一條曲線。這種類型的運(yùn)動可以使機(jī)器人在復(fù)雜的環(huán)境中更靈活地移動，例如在需要避開障礙物或在特定的路徑上移動的情況下。
總結(jié)
在我們探討了正逆運(yùn)動學(xué)的基本概念以及數(shù)學(xué)原理后，我們可以看到這兩個概念在機(jī)械臂中的控制是特別重要的。然而，正運(yùn)動學(xué)和逆運(yùn)動學(xué)只是機(jī)械臂控制的一部分。在實(shí)際應(yīng)用中，我們還需要考慮到動力學(xué)、控制理論、傳感器反饋、以及實(shí)際硬件的限制。

隨著技術(shù)的發(fā)展，機(jī)械臂將在很多領(lǐng)域發(fā)揮越來越大的作用，包括制造業(yè)、醫(yī)療保健、家庭服務(wù)、搜索和救援等。通過深入理解這些知識，我們可以更加的了解什么是機(jī)械臂，讓它在各種應(yīng)用中發(fā)揮更大的作用。

如果你喜歡這篇文章的話歡迎在下方留言或者點(diǎn)贊，你的支持是我們更新的動力！

審核編輯 黃宇