1. 調(diào)整總線舵機(jī)的模式

????? 實(shí)現(xiàn)思路：

? ? ? 機(jī)械臂包括轉(zhuǎn)臺(tái)、大臂、小臂三部分，先設(shè)置好總線舵機(jī)每個(gè)ID的工作模式。下圖是計(jì)劃給舵機(jī)的各部分設(shè)置的ID號(hào)：

接下來(lái)為各部分設(shè)置相應(yīng)的舵機(jī)模式（見(jiàn)下表），并在程序里進(jìn)行編程設(shè)置。

器材準(zhǔn)備：

PC機(jī)或筆記本電腦（windows操作系統(tǒng)、Arduino IDE）、以及下圖所示的其它器材。

1.1修改轉(zhuǎn)臺(tái)的舵機(jī)模式

第一步：按下圖所示連接好電路。

第二步：打開(kāi)文末資料內(nèi)的”總線舵機(jī)ID及模式更改資料\串口調(diào)試助手\sscom5.13.1.exe”（如下圖所示）。

第三步：選擇端口號(hào)、波特率，并打開(kāi)串口。

先在菜單欄：串口設(shè)置---打開(kāi)串口設(shè)置，選擇端口號(hào)、波特率，點(diǎn)擊“ok”即可。

接著點(diǎn)擊“打開(kāi)串口”，并點(diǎn)擊“擴(kuò)展”。

這里我們已經(jīng)把命令設(shè)置好了，只需要依次點(diǎn)擊描紅的命令（如下圖所示），就可以完成轉(zhuǎn)臺(tái)模式的設(shè)置，即可觀察到轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)到指定角度。

1.2 修改大臂的舵機(jī)模式

第一步：按下圖所示把大臂的舵機(jī)線與小模塊進(jìn)行電路連接（注意：請(qǐng)先把小模塊斷電）。

其中，大臂的舵機(jī)線如下圖所示。

第二步：在串口調(diào)試助手里，選擇端口號(hào)、波特率并打開(kāi)串口。

第三步：依次點(diǎn)擊下邊描紅的命令，就可完成大臂模式的設(shè)置，同時(shí)可觀察到大臂轉(zhuǎn)動(dòng)。

1.3 修改小臂的舵機(jī)模式

第一步：按下圖所示把小臂的舵機(jī)線與小模塊進(jìn)行電路連接（注意：請(qǐng)先把小模塊斷電）。

第二步：在串口調(diào)試助手里，選擇端口號(hào)、波特率并打開(kāi)串口。

第三步：依次點(diǎn)擊下邊描紅的命令，就可完成小臂模式的設(shè)置，同時(shí)可觀察到小臂轉(zhuǎn)動(dòng)。

這樣就完成了總線舵機(jī)模式的修改工作。

2. 機(jī)械臂運(yùn)動(dòng)學(xué)控制

? ? ? 控制機(jī)械臂最基本的方法是對(duì)其建立運(yùn)動(dòng)學(xué)模型，對(duì)于3軸的串聯(lián)機(jī)械臂來(lái)說(shuō)，運(yùn)動(dòng)學(xué)模型其本質(zhì)就是給定空間3D坐標(biāo)，利用運(yùn)動(dòng)學(xué)算法進(jìn)行控制。

2.1分析-串聯(lián)的運(yùn)動(dòng)學(xué)

? ? ? 串聯(lián)的運(yùn)動(dòng)學(xué)有兩種：一種是正運(yùn)動(dòng)學(xué)，一種是逆運(yùn)動(dòng)學(xué)。

? ? ? 串聯(lián)的正運(yùn)動(dòng)學(xué)，簡(jiǎn)單來(lái)說(shuō)是指確定每個(gè)關(guān)節(jié)舵機(jī)轉(zhuǎn)動(dòng)的角度，從而確定端點(diǎn)位置。這種方法在調(diào)試時(shí)對(duì)于少量自由度的機(jī)械臂比較實(shí)用，但是當(dāng)自由度增加時(shí)，調(diào)試復(fù)雜程度也會(huì)隨之增加。比如下方這個(gè)3自由度（不含執(zhí)行器），我們只需要確定其3個(gè)關(guān)節(jié)上的舵機(jī)轉(zhuǎn)動(dòng)角度α，θ，β，即可確定執(zhí)行端的位置（暫時(shí)不考慮臂長(zhǎng)的因素）。

? ?? 串聯(lián)的逆運(yùn)動(dòng)學(xué)，簡(jiǎn)單來(lái)說(shuō)是指確定端點(diǎn)的位置，然后通過(guò)算法計(jì)算出各關(guān)節(jié)需要轉(zhuǎn)動(dòng)的角度，自動(dòng)調(diào)整到合適的位置。結(jié)合本項(xiàng)目中的3軸機(jī)械臂（見(jiàn)下圖），我們將采用逆運(yùn)動(dòng)學(xué)算法進(jìn)行設(shè)計(jì)，控制運(yùn)動(dòng)。

2.2設(shè)計(jì)-逆運(yùn)動(dòng)學(xué)算法

? ? ? 這里給大家介紹一種3自由度（不含執(zhí)行器）逆運(yùn)動(dòng)學(xué)算法。

? ? ? 為方便大家的理解，算法總共分為5步：第一步為建立空間坐標(biāo)系；第二步、第三步為簡(jiǎn)化模型運(yùn)動(dòng)學(xué)計(jì)算；第四步為末端坐標(biāo)變換的補(bǔ)償計(jì)算；第五步為總體總結(jié)。

? ? ? 第一步：將以轉(zhuǎn)臺(tái)旋轉(zhuǎn)中心為原點(diǎn)建立空間坐標(biāo)系如下：

從上圖可知：O點(diǎn)為轉(zhuǎn)臺(tái)旋轉(zhuǎn)軸和大臂旋轉(zhuǎn)軸的交點(diǎn)，A點(diǎn)為大臂的末端，C點(diǎn)為小臂的旋轉(zhuǎn)中心，AC方向?yàn)樾”鄣男D(zhuǎn)軸，B點(diǎn)為小臂的末端。

? ? ? 第二步：為了方便計(jì)算，我們這里將AC段壓縮為0，這樣可以簡(jiǎn)化模型如下：

關(guān)于AC段運(yùn)動(dòng)的計(jì)算，我們?cè)诤竺娴牟襟E中通過(guò)簡(jiǎn)單的坐標(biāo)變化進(jìn)行計(jì)算。

? ? ? 第三步：進(jìn)行簡(jiǎn)化模型的運(yùn)動(dòng)學(xué)計(jì)算。通過(guò)前面的了解，我們知道逆運(yùn)動(dòng)是通過(guò)坐標(biāo)來(lái)計(jì)算出各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的角度。這里我們可以通過(guò)以下步驟建立幾何模型和建立運(yùn)動(dòng)學(xué)算法：

? ? ? ① 將末端B點(diǎn)坐標(biāo)設(shè)置為（x，y，z），作B點(diǎn)在xy平面的投影B1（x，y，0），如下所示：

? ? ? ② 計(jì)算轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度與末端坐標(biāo)的關(guān)系。作O點(diǎn)到B1點(diǎn)的輔助線OB1（下圖中兩點(diǎn)距離為d），則OB1為OA和AB在xy平面的投影。這里我們假設(shè)OX為轉(zhuǎn)臺(tái)的初始角度，則∠XOB（即圖中α）為轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度，如下所示：

則通過(guò)三角函數(shù)可得出以下關(guān)系：

公式1：

d為OB1，可通過(guò)兩點(diǎn)距離公式得出如下關(guān)系：

公式2：

將“公式1”與“公式2”結(jié)合，可得轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度α與末端坐標(biāo)的關(guān)系如下：

公式3：

? ? ? ③ 計(jì)算大臂和小臂夾角與坐標(biāo)之間的關(guān)系。作O點(diǎn)和B點(diǎn)之間的輔助線OB（圖中兩點(diǎn)距離為e），則可知∠OAB（即圖中β）為大臂小臂的夾角，如下所示：

從圖示關(guān)系中，利用三角函數(shù)可得以下關(guān)系：

公式4：

上式中注意a，b分別為大臂和小臂的長(zhǎng)度，可通過(guò)實(shí)際的進(jìn)行測(cè)量，為已知量；

其中e可根據(jù)空間兩點(diǎn)距離公式可得：

公式5：

將“公式4”與“公式5”結(jié)合可得大小臂夾角β與末端坐標(biāo)之間的關(guān)系如下：

公式6：

④ 計(jì)算大臂旋轉(zhuǎn)角度與末端坐標(biāo)之間的關(guān)系。從圖中可知∠AOB1（即圖中θ）為大臂旋轉(zhuǎn)角度：

如圖所示可得以下關(guān)系：

公式7：

其中∠AOB在△AOB中利用三角函數(shù)求出：

公式8：

其中e的值可通過(guò)“公式5”得出；

“公式7”的∠AOB1在△OBB1用三角函數(shù)求出：

公式9：

其中z為BB1長(zhǎng)度；

綜上“公式7”、“公式8”、“公式9”可得大臂轉(zhuǎn)動(dòng)角度θ與末端坐標(biāo)關(guān)系：

公式10：

⑤ 接下來(lái)計(jì)算小臂轉(zhuǎn)動(dòng)角度與末端坐標(biāo)之間的關(guān)系。求小臂轉(zhuǎn)動(dòng)角度，我們需要先來(lái)看下小臂的0°所在位置，如下所示：

? ? ? 由上圖可知，復(fù)位時(shí)大臂與小臂夾角θ=90°；而實(shí)際的小臂初始角度為Z軸正方向，則此時(shí)小臂轉(zhuǎn)動(dòng)角度λ=90°。那么進(jìn)行運(yùn)動(dòng)后，可如下圖所示得出小臂轉(zhuǎn)動(dòng)與末端坐標(biāo)之間的關(guān)系：

如圖可知：

其中θ為大臂角度，β關(guān)系滿足

圖中θ為大臂轉(zhuǎn)動(dòng)角度，圖中進(jìn)行了一個(gè)簡(jiǎn)單的幾何關(guān)系變化。

最后得到簡(jiǎn)化模型的運(yùn)動(dòng)算法：

轉(zhuǎn)臺(tái)：

大臂：

小臂：

其中θ為大臂角度，β關(guān)系滿足

? ? ? 注意：a為大臂長(zhǎng)度，b為小臂長(zhǎng)度，可通過(guò)利用配套游標(biāo)卡尺測(cè)量得出；x，y，z為末端需要達(dá)到的實(shí)際坐標(biāo)值。

? ? ? 第四步：接下來(lái)計(jì)算AC段在運(yùn)動(dòng)過(guò)程中對(duì)各關(guān)節(jié)角度的影響，這里采用一種利用坐標(biāo)變化的補(bǔ)償計(jì)算。

這里我們先來(lái)說(shuō)下補(bǔ)償計(jì)算的作用。在第二步中我們說(shuō)到第三步的算法是將AC段壓縮為0進(jìn)行的計(jì)算，那么我們?cè)诘谌街欣谜鎸?shí)的坐標(biāo)點(diǎn)來(lái)計(jì)算各關(guān)節(jié)角度將會(huì)有一定的誤差。補(bǔ)償算法就是將實(shí)際的坐標(biāo)轉(zhuǎn)化為與第三步中所得算法一致的坐標(biāo)，得出變換后的坐標(biāo)之后，在帶入到第三步的算法中進(jìn)行計(jì)算，這樣就可以獲得準(zhǔn)確的各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的角度。接下來(lái)我們?cè)敿?xì)了解一下具體的計(jì)算：

? ? ? ① 我們先在xy平面進(jìn)行投影，如下所示：

圖中0A為大臂，AC為大臂末端與小臂旋轉(zhuǎn)中心的間距，CB為小臂；末端點(diǎn)B坐標(biāo)為（x，y）。

② 為方便計(jì)算，作OA延長(zhǎng)線AB1，且作B點(diǎn)在該延長(zhǎng)線的垂線BB1，相交于點(diǎn)B1，設(shè)B1坐標(biāo)為（x1，y1）；

則從圖中可知，轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)角度為γ（這里默認(rèn)OX為轉(zhuǎn)臺(tái)初始位置，因?yàn)槲覀冞@里通過(guò)坐標(biāo)點(diǎn)求角度，所以γ為未知量）；

③ 如下圖所示作輔助線，B1點(diǎn)、B點(diǎn)作x軸平行線，且B1點(diǎn)作與B點(diǎn)平行線的垂線，如下所示：

通過(guò)平行線定理和余角定理可得：

公式11：∠B1=γ；

④ 接下來(lái)作連接OB、及B點(diǎn)在X軸垂線的輔助線，計(jì)算轉(zhuǎn)臺(tái)轉(zhuǎn)動(dòng)角度，如下圖所示：

圖中c為大臂末端和小臂旋轉(zhuǎn)中心間距，可通過(guò)配套游標(biāo)卡尺測(cè)量，為已知量；

從圖中可得：

公式12：

依余弦定理可得：

公式13：

在△BOB1中，依余弦定理可得：

公式14：

其中OB依兩點(diǎn)距離公式可得：

公式15：

合并“公式14”、“公式15”，可得：

公式16：

合并“公式12”、“公式13”、公式16，可計(jì)算出γ角度：

公式17：

⑤ 接下來(lái)如下圖所示，在△HBB1中，利用三角函數(shù)計(jì)算出B1（x1，y1）：

公式18：

公式19：

? ? ? ⑥ 通過(guò)前面的步驟，我們已經(jīng)計(jì)算出第一象限的中的坐標(biāo)變換，但是在第一象限中的轉(zhuǎn)動(dòng)范圍只有0-90°，實(shí)際轉(zhuǎn)動(dòng)范圍是0-180°，所以接下來(lái)我們計(jì)算第二象限的坐標(biāo)變換，同前面的步驟，可做如下圖：

從圖中可知，當(dāng)位于90°-180°之間時(shí)（即圖中第二象限），可得轉(zhuǎn)臺(tái)旋轉(zhuǎn)角度關(guān)系如下：

公式20：

公式21：

通過(guò)上面步驟，不難在第二象限中得出B點(diǎn)經(jīng)過(guò)坐標(biāo)變換后得到的B1坐標(biāo)關(guān)系如下：

公式22：

公式23：

⑦ 這里咱們將補(bǔ)充中需要的公式進(jìn)行總結(jié)，如下：

當(dāng)

當(dāng)

? ? ? 到這里我們完成了坐標(biāo)變化的補(bǔ)償計(jì)算。

? ? ? 第五步：大家已經(jīng)了解了運(yùn)動(dòng)學(xué)計(jì)算和補(bǔ)償計(jì)算，那么這兩個(gè)算法如何搭配使用呢？如何利用算法進(jìn)行機(jī)械臂的逆運(yùn)動(dòng)控制呢？這里我們可以通過(guò)一個(gè)簡(jiǎn)單的流程來(lái)理解。

2.3 實(shí)驗(yàn)操作

? ? 實(shí)現(xiàn)思路：在上位機(jī)上通過(guò)軟件串口監(jiān)視器，鍵盤(pán)輸入機(jī)械臂的目標(biāo)坐標(biāo)位置，將從起始位置轉(zhuǎn)到目標(biāo)位置。

? ? 如從串口輸入坐標(biāo)（60，60，60），從坐標(biāo)點(diǎn)（90，90，90）轉(zhuǎn)動(dòng)到坐標(biāo)點(diǎn)（60，60，60）。程序中設(shè)置起始位置為（90，90，90）。

? 【備注：上面的坐標(biāo)點(diǎn)單位為mm】

? ? 器材準(zhǔn)備：機(jī)械臂本體、主控盒、顯示屏、鍵盤(pán)、鼠標(biāo)（如下圖所示）

操作步驟：

① 下載文末資料內(nèi)的參考程序color_experiment_ws\src\InputPoit_By_SerialPort\InputPoit_By_SerialPort.ino：

② 打開(kāi)串口，選擇好波特率和結(jié)束符（如下圖所示）。

輸入機(jī)械臂的目標(biāo)坐標(biāo)點(diǎn)，查看返回的結(jié)果值?！咎崾荆鹤鴺?biāo)點(diǎn)的范圍為0-220】

③ 現(xiàn)在希望機(jī)械臂轉(zhuǎn)動(dòng)到空間位置點(diǎn)（60，60，60），具體的操步驟如下（注意坐標(biāo)間隔用英文字符“,”，最后一個(gè)坐標(biāo)沒(méi)有字符）：

點(diǎn)擊“發(fā)送”后，可以觀察到機(jī)械臂從原位置轉(zhuǎn)動(dòng)到目標(biāo)位置點(diǎn)，同時(shí)串口顯示目標(biāo)位置，如下圖所示：

當(dāng)然大家也可以嘗試讓機(jī)械臂轉(zhuǎn)動(dòng)到（80，80，80），看看效果。

3. 機(jī)械臂氣動(dòng)搬運(yùn)

實(shí)現(xiàn)思路：利用串聯(lián)機(jī)械臂的逆運(yùn)動(dòng)學(xué)算法，實(shí)現(xiàn)機(jī)械臂的氣動(dòng)搬運(yùn)。實(shí)驗(yàn)場(chǎng)景如下圖所示，其中兩個(gè)物塊代表所需要搬運(yùn)的物品。

操作步驟：

① 下載文末資料內(nèi)的參考程序color_experiment_ws\src\Pneumatic_manipulator_handling\Pneumatic_manipulator_handling.ino：

? ? ? ② 程序下載完成后，觀察氣動(dòng)搬運(yùn)效果。正常情況下機(jī)械臂會(huì)先搬運(yùn)上邊的物塊，放置在一側(cè)；接著再搬運(yùn)下邊的物塊，并把其放置在剛才的第一個(gè)物塊上。

4. 機(jī)械臂手爪搬運(yùn)

? ? ? 實(shí)現(xiàn)思路：利用串聯(lián)機(jī)械臂的逆運(yùn)動(dòng)學(xué)算法，實(shí)現(xiàn)機(jī)械手爪搬運(yùn)兩個(gè)物塊。實(shí)驗(yàn)場(chǎng)景如下圖所示，其中兩個(gè)物塊代表所需要搬運(yùn)的物品。

操作步驟：

① 下載文末資料內(nèi)的參考程序color_experiment_ws\src\Manipulator_handling\Manipulator_handling.ino：

② 程序下載完成后，觀察搬運(yùn)效果。

5. 機(jī)械臂位置反饋

實(shí)現(xiàn)思路：讓機(jī)械臂做3組動(dòng)作，然后返回其相應(yīng)的舵機(jī)的pwm值。其中機(jī)械臂包括轉(zhuǎn)臺(tái)、大臂、小臂三部分，實(shí)驗(yàn)中將分別返回控制其三部分的舵機(jī)Pwm值。

每個(gè)總線舵機(jī)的pwm范圍為：500～2500，每個(gè)舵機(jī)模式為：180度。

所以舵機(jī)的實(shí)際角度與pwm映射關(guān)系為：angle＝map（要輸入的pwm值：500,2500,0,180）。明白了此原理后，接下來(lái)就可以實(shí)際操作了。

操作步驟：

① 下載文末資料內(nèi)的參考程序color_experiment_ws\src\Get_BusServo_Current_Angle\Get_BusServo_Current_Angle.ino：

② 打開(kāi)串口，查看返回的結(jié)果值（如下圖所示）。

6. 資料下載

資料內(nèi)容：

①程序源代碼

②總線舵機(jī)ID及模式更改資料

資料下載地址：桌面級(jí)機(jī)械臂-運(yùn)動(dòng)控制 https://www.robotway.com/h-col-259.html