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

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

1.1?概述

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

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

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

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

1.3 制作

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

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

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

2.1?硬件板間接口

2.1.1?系統(tǒng)簡介

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

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

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

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

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

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

2.1.3?小MU視覺傳感器

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

2.1.4?激光測距模塊VL53L0X

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

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

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

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

2.3?軟件設(shè)計

2.3.1?設(shè)計思想

分階段實(shí)施：

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

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

（2）開發(fā)階段

搭建軟硬件平臺，測試完成單個功能模塊正常工作。

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

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

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

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

2.3.2?軟件模塊描述

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

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

2.3.2.1?云臺的PID算法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

3.1?軟硬件配合

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

3.2?圖像識別

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

3.3?距離

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

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