基于視覺(jué)系統(tǒng)的機(jī)械手-LPC55S69開(kāi)發(fā)板和ESP32-CAM的應(yīng)用

一、系統(tǒng)總體部分

1.1?概述

這是一個(gè)參賽的項(xiàng)目，當(dāng)時(shí)申請(qǐng)板卡理由是希望使用LPC55S69來(lái)進(jìn)行串口數(shù)據(jù)管理，后來(lái)發(fā)現(xiàn)這個(gè)實(shí)在沒(méi)什么挑戰(zhàn)性（更主要的是可視性太差，兒子完全不感興趣），于是任性的換了一個(gè)課題：基于視覺(jué)系統(tǒng)的機(jī)械手。

說(shuō)是基于視覺(jué)系統(tǒng)，實(shí)際上就是簡(jiǎn)單的顏色分類(lèi)，我們使用機(jī)械手拾取不同顏色的積木分類(lèi)存放。

1.2?系統(tǒng)結(jié)構(gòu)

系統(tǒng)構(gòu)造復(fù)雜，將會(huì)導(dǎo)致系統(tǒng)使用復(fù)雜、維護(hù)復(fù)雜，失效率升高，因此盡可能借（chao）鑒（xi）已經(jīng)過(guò)驗(yàn)證使用的代碼是一個(gè)好習(xí)慣；另外，從節(jié)約的角度來(lái)說(shuō)，使用過(guò)往的硬件也是一個(gè)好習(xí)慣。因此我們盡可能的簡(jiǎn)化了設(shè)計(jì)和實(shí)現(xiàn)。
本項(xiàng)目除了挑戰(zhàn)賽主辦方提供的MCU板卡外，主要硬件包括：機(jī)械手一套、攝像頭、距離傳感器、若干彩色積木塊（兒子友情贊助）。完成圖如下：

1.3 制作

購(gòu)買(mǎi)一套4自由度機(jī)械手（木制或亞克力）的大約30RMB（唔，主要是窮，其實(shí)高精度機(jī)械手也不便宜），不過(guò)好丑，配上若干mg90s金屬齒舵機(jī)（用模擬舵機(jī)快坑死了，下次改進(jìn)可以用數(shù)字舵機(jī)，后面會(huì)細(xì)講），大概70元左右，配上利舊的云臺(tái)（云臺(tái)是以前資金寬裕的時(shí)候買(mǎi)的，一個(gè)云臺(tái)就比這個(gè)機(jī)械手貴多了）；視覺(jué)系統(tǒng)只需要能夠進(jìn)行畫(huà)面和顏色識(shí)別的ESP32-CAM即可（我這里是利舊采用了“小MU”，是不是顯得更高大上了呢？）為了精確測(cè)距，選用了激光測(cè)距模塊VL53L0X。

在本項(xiàng)目中，總新增硬件成本大約200元（實(shí)際上即使不包括參賽指定的LPC55S69，總硬件成本也應(yīng)該是超過(guò)了1000元），人力投入大約為80小時(shí)。

二、軟硬件模塊設(shè)計(jì)與實(shí)現(xiàn)

2.1?硬件板間接口

2.1.1?系統(tǒng)簡(jiǎn)介

本設(shè)計(jì)和核心思想是采用了IIC通信，通過(guò)LPC55S69讀取和寫(xiě)入數(shù)據(jù)，來(lái)實(shí)現(xiàn)機(jī)械手對(duì)各種積木塊的認(rèn)識(shí)和抓取。

IIC只用兩根線(xiàn)，一條Serial Clock (SCL)，提供信號(hào)觸發(fā)時(shí)間邊緣，一條Serial Data Line (SDA)?為雙向信號(hào)數(shù)據(jù)傳輸。IIC無(wú)需片選，主控器件通過(guò)地址碼建立多機(jī)通信的機(jī)制，根據(jù)7位“從器件專(zhuān)用地址碼”確定MCU需要操作的設(shè)備，“從器件專(zhuān)用地址碼”高4位為器件類(lèi)型，由生產(chǎn)廠(chǎng)家制定，低3位為器件引腳定義地址，由使用者定義。

在本設(shè)計(jì)中，LPC55S69為主用MCU，提供時(shí)鐘信號(hào)和進(jìn)行設(shè)備信息讀取和寫(xiě)入。帶有三個(gè)從設(shè)備，分別為“小MU視覺(jué)傳感器”、“VL53L0X測(cè)距模塊”和“PCA9865舵機(jī)驅(qū)動(dòng)模塊”。IIC相關(guān)的代碼可以從例程“l(fā)pcxpresso55s69_se_hostlib_se05x_ex_i2cMaster”中大量套用現(xiàn)有代碼，各從設(shè)備模塊的庫(kù)是從這些模塊廠(chǎng)商提供的arduino應(yīng)用中直接取用的，僅需要少量修改。

2.1.2?舵機(jī)和PCA9865模塊

舵機(jī)是用來(lái)控制機(jī)械手位置和抓取動(dòng)作的，本機(jī)械手用的是模擬舵機(jī)，給出特定時(shí)間的高電平，舵機(jī)能轉(zhuǎn)動(dòng)在一定的角度，理論上這時(shí)如果不給出信號(hào)，只保持上電狀態(tài)，舵機(jī)能保持不動(dòng)，但我在LPC55S69安裝一塊arduino擴(kuò)展板，嘗試用GPIO驅(qū)動(dòng)舵機(jī)，也許是功率不足，也許是代碼有問(wèn)題，也可能是不能用arduino的擴(kuò)展板，舵機(jī)倒是可以轉(zhuǎn)動(dòng)，就是抖動(dòng)不停，而且根本抓不牢，推測(cè)如果是數(shù)字電機(jī)應(yīng)該就沒(méi)這個(gè)問(wèn)題了。

因此加了一塊PCA9865模塊，把舵機(jī)驅(qū)動(dòng)這塊，完全交給其控制，只需要對(duì)PCA9865寫(xiě)數(shù)據(jù)就行。我用的是網(wǎng)購(gòu)的PCA9865驅(qū)動(dòng)模塊，支持16路輸出（其實(shí)我只用了4路），采用的默認(rèn)IIC從地址是0x40（可以通過(guò)板上硬件跳線(xiàn)修改），使用“Adafruit_PWMServoDriver.h”頭之后，可以通過(guò)setPWM(n, 0, pulse)指定0~15路輸出的高電平持續(xù)周期，而對(duì)于9g舵機(jī)來(lái)說(shuō)，高電平寬度在20ms內(nèi)通過(guò)控制脈沖寬度范圍0.5ms~2.5ms為有效信號(hào)，直接寫(xiě)入就行。

2.1.3?小MU視覺(jué)傳感器

小MU視覺(jué)傳感器是我之前一個(gè)項(xiàng)目中利舊留下來(lái)的，是一塊ESP32加上攝像頭的工具，支持讀取特定區(qū)域內(nèi)的顏色、形狀，再通過(guò)WLAN、藍(lán)牙、TTL或者IIC傳輸。我這里采用的是其IIC接口。它的驅(qū)動(dòng)為“MuVisionSensor.h”，同樣是從arduino應(yīng)用中套用出來(lái)的，其IIC的從地址是0x60。

2.1.4?激光測(cè)距模塊VL53L0X

激光測(cè)距模塊是本次新購(gòu)，也是首次使用這種高精度模塊（之前超聲波測(cè)試模塊干擾大，精度差）。激光測(cè)距的原理其實(shí)很簡(jiǎn)單，就是通過(guò)讀取反射信號(hào)的往返時(shí)間，再根據(jù)空氣中光速計(jì)算距離，從資料上可以找到VL53L0X最遠(yuǎn)測(cè)距的距離是2m，精度是2mm，就本系統(tǒng)來(lái)說(shuō)，精度是足夠的。VL53L0X通過(guò)I2C讀取，缺省地址是0x29，其采用的頭文件是“VL53L0.h”，因?yàn)槭遣捎媒嚯x模式，因此在程序中調(diào)用vl53l0x_set_mode函數(shù)，將其模式設(shè)置高精度。

2.2?硬件結(jié)構(gòu)

采用外購(gòu)的機(jī)械手，拋棄原配的底座，換上高精度云臺(tái)，同時(shí)云臺(tái)上還安裝了攝像頭和距離傳感器，確保云臺(tái)轉(zhuǎn)動(dòng)時(shí)，機(jī)械臂、攝像頭、距離傳感器的相對(duì)位置不變。機(jī)械臂本身采用了兩個(gè)舵機(jī)來(lái)決定機(jī)械爪所在位置，因平行四邊形原理，可以保證機(jī)械爪基本上是可以保證在水平方向上的，具體的硬件安裝與本賽關(guān)系不大，暫略。

需要注意的是安裝時(shí)確保VL53L0X的探測(cè)方向與機(jī)械爪的中心部分在一個(gè)豎直平面內(nèi)。

2.3?軟件設(shè)計(jì)

2.3.1?設(shè)計(jì)思想

分階段實(shí)施：

（1）可行性驗(yàn)證

首先學(xué)習(xí)MCU資料，根據(jù)Demo寫(xiě)一點(diǎn)簡(jiǎn)單代碼，做一些接口信號(hào)的測(cè)試，入手購(gòu)買(mǎi)項(xiàng)目中必要的配套硬件（比如激光測(cè)距模塊和舵機(jī)、舵機(jī)驅(qū)動(dòng)板等）。

（2）開(kāi)發(fā)階段

搭建軟硬件平臺(tái)，測(cè)試完成單個(gè)功能模塊正常工作。

（3）調(diào)測(cè)階段

根據(jù)實(shí)際需求進(jìn)行少量調(diào)整，將代碼拼接起來(lái)，完成功能模塊的功能驗(yàn)證。

（4）項(xiàng)目總結(jié)

打包代碼，形成文檔記錄。

2.3.2?軟件模塊描述

該系統(tǒng)的軟件核心為機(jī)械手的控制，包括決定方向的云臺(tái)、實(shí)現(xiàn)抓取功能的開(kāi)合機(jī)械爪、和調(diào)節(jié)具體位置的機(jī)械臂。其中云臺(tái)為其中最為關(guān)鍵的部分，通過(guò)攝像頭確認(rèn)機(jī)械手能夠?qū)?zhǔn)積木的方向，至于機(jī)械臂，只要相差不太多，機(jī)械爪的開(kāi)合可自動(dòng)調(diào)整積木位置，直至將積木抓取住。因此設(shè)計(jì)了對(duì)云臺(tái)的閉環(huán)自動(dòng)控制技術(shù)。

機(jī)械臂部分，因?yàn)椴捎昧藘蓚€(gè)關(guān)節(jié)的控制環(huán)，且不是完全獨(dú)立，其相互存在的耦合使得機(jī)械爪所在位置在二維平面上的運(yùn)動(dòng)呈現(xiàn)出嚴(yán)重的非線(xiàn)性特征，要得到機(jī)械手控制的精確運(yùn)動(dòng)軌跡比較困難，采用的方式為預(yù)制定位，先建立好兩個(gè)舵機(jī)的對(duì)應(yīng)轉(zhuǎn)角表格，根據(jù)激光測(cè)距得到的積木距離，打開(kāi)機(jī)械爪，查表運(yùn)動(dòng)，使機(jī)械爪由云臺(tái)直接向積木推前，直到使得積木正好在機(jī)械爪所能掌控的范圍內(nèi)，再閉合機(jī)械爪，實(shí)現(xiàn)積木的抓取，再抬高機(jī)械爪，根據(jù)之前讀取的顏色，將積木放置在指定的區(qū)域內(nèi)。稍微的誤差，可以通過(guò)機(jī)械爪推動(dòng)積木進(jìn)行校準(zhǔn)。

2.3.2.1?云臺(tái)的PID算法

如前文所述，云臺(tái)確定了機(jī)械爪可運(yùn)行的二維平面，如何使這個(gè)二維平面能夠穿過(guò)積木的中心位置呢？我們使用了PID算法，在云臺(tái)轉(zhuǎn)動(dòng)時(shí)，讀取積木在攝像頭內(nèi)的位置，并根據(jù)偏差進(jìn)行調(diào)整。調(diào)整時(shí)，先估算一個(gè)大致需要轉(zhuǎn)動(dòng)的角度，驅(qū)動(dòng)舵機(jī)時(shí)云臺(tái)轉(zhuǎn)動(dòng)，因?yàn)槎鏅C(jī)本身的誤差和PCA9865時(shí)鐘的誤差，肯定不可能一次對(duì)準(zhǔn)，根據(jù)誤差，再進(jìn)行下步調(diào)節(jié)。其中轉(zhuǎn)動(dòng)的系數(shù)為Kp，很容易理解，單獨(dú)的P調(diào)節(jié)系統(tǒng)始終會(huì)處于存在誤差的情況。因此我們引入了積分環(huán)節(jié)的輸出系數(shù)Ki，在這個(gè)系統(tǒng)中，因?yàn)榇a原因（主要是我也不知道調(diào)用數(shù)學(xué)函數(shù)），采用的一個(gè)較小的量δ乘以非線(xiàn)性系數(shù)（讀取出來(lái)的誤差），再進(jìn)行相加，取代積分函數(shù)∫，事實(shí)證明是可行的。

說(shuō)是PID算法，其實(shí)在這個(gè)項(xiàng)目里面，并沒(méi)有用到D功能，在上述計(jì)算公式中，可以認(rèn)為Kd=0。

2.3.2.2?機(jī)械臂的定位

機(jī)械臂的定位包括：

（1）原始位置，即抬高機(jī)械爪，由云臺(tái)轉(zhuǎn)動(dòng)，探測(cè)積木所在方向。

（2）準(zhǔn)備位置，即機(jī)械爪停留在最靠近云臺(tái)的位置，這時(shí)機(jī)械爪張開(kāi)。

（3）推進(jìn)至抓取準(zhǔn)備位置，即根據(jù)距離傳感器讀取到的距離，將機(jī)械爪向前推進(jìn)（因?yàn)榫仍?，推進(jìn)并非完全水平方向，后續(xù)表格做得更細(xì)，會(huì)更加接近水平）。

（4）抓取積木塊，當(dāng)機(jī)械爪到達(dá)指定位置后，驅(qū)動(dòng)舵機(jī)，使爪子閉合起來(lái)，這時(shí)機(jī)械爪就能抓取積木。

（5）抬高機(jī)械爪，恢復(fù)至機(jī)械臂原始位置。

（6）由云臺(tái)帶動(dòng)機(jī)械臂，轉(zhuǎn)動(dòng)到某個(gè)指定位置，松開(kāi)機(jī)械爪，使積木塊自行掉落在指定區(qū)域內(nèi)。

2.3.2.3?機(jī)械爪的開(kāi)合

機(jī)械爪只有開(kāi)合動(dòng)作，相對(duì)簡(jiǎn)單，因?yàn)闄C(jī)械手比較簡(jiǎn)陋，沒(méi)有壓力傳感器，因此當(dāng)機(jī)械手抓握積木時(shí)，我使用最小的閉合角度，通過(guò)舵機(jī)本身力反饋?zhàn)ト》e木，因?yàn)槎鏅C(jī)本身是防燒的金屬舵機(jī)（9g尺寸，14g力），且驅(qū)動(dòng)電壓并不高（標(biāo)稱(chēng)6V，實(shí)測(cè)5.3V），不會(huì)燒壞。

2.3.3?代碼聯(lián)調(diào)

這個(gè)系統(tǒng)的代碼調(diào)試相對(duì)簡(jiǎn)單，全代碼已化簡(jiǎn)為iic通信，在fsl_i2c.c中，初始化IIC，再進(jìn)行調(diào)用。只需要根據(jù)IIC三個(gè)從設(shè)備的地址分別讀取或者寫(xiě)入數(shù)據(jù)，就能實(shí)現(xiàn)期望的功能。

三、系統(tǒng)功能驗(yàn)證

3.1?軟硬件配合

舵機(jī)雖然采用了0~180°，但事實(shí)上，需要轉(zhuǎn)動(dòng)的角度遠(yuǎn)小于180°，以機(jī)械爪為例，只需要大約90°左右，機(jī)械爪就能從張開(kāi)到最大值變?yōu)榫o緊閉合?？紤]到舵機(jī)本身特點(diǎn)，我們裝配機(jī)械手時(shí)，先在舵機(jī)上加電，使其歸零，再進(jìn)行裝配。

3.2?圖像識(shí)別

小MU內(nèi)部集成了一塊ESP32，ESP32作為MCU，可以協(xié)助計(jì)算出目標(biāo)積木塊的顏色。同時(shí)，

3.3?距離

通過(guò)距離傳感器讀取積木塊到傳感器之間的距離，當(dāng)距離小于或大于機(jī)械爪正常行進(jìn)位置，則認(rèn)為該距離不適合，直接丟棄（后期可以考慮增加聲光告警）

大致內(nèi)容如上描述，后續(xù)再補(bǔ)充，另在論壇上直接重新編輯，不讓新增圖片了，也許是bug或網(wǎng)絡(luò)問(wèn)題導(dǎo)致的。