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

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

1.1?概述

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

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

1.2?系統(tǒng)結構

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

1.3 制作

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

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

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

2.1?硬件板間接口

2.1.1?系統(tǒng)簡介

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

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

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

2.1.2?舵機和PCA9865模塊

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

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

2.1.3?小MU視覺傳感器

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

2.1.4?激光測距模塊VL53L0X

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

2.2?硬件結構

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

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

2.3?軟件設計

2.3.1?設計思想

分階段實施：

（1）可行性驗證

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

（2）開發(fā)階段

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

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

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

（4）項目總結

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

2.3.2?軟件模塊描述

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

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

2.3.2.1?云臺的PID算法

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

說是PID算法，其實在這個項目里面，并沒有用到D功能，在上述計算公式中，可以認為Kd=0。

2.3.2.2?機械臂的定位

機械臂的定位包括：

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

（2）準備位置，即機械爪停留在最靠近云臺的位置，這時機械爪張開。

（3）推進至抓取準備位置，即根據(jù)距離傳感器讀取到的距離，將機械爪向前推進（因為精度原因，推進并非完全水平方向，后續(xù)表格做得更細，會更加接近水平）。

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

（5）抬高機械爪，恢復至機械臂原始位置。

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

2.3.2.3?機械爪的開合

機械爪只有開合動作，相對簡單，因為機械手比較簡陋，沒有壓力傳感器，因此當機械手抓握積木時，我使用最小的閉合角度，通過舵機本身力反饋抓取積木，因為舵機本身是防燒的金屬舵機（9g尺寸，14g力），且驅(qū)動電壓并不高（標稱6V，實測5.3V），不會燒壞。

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

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

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

3.1?軟硬件配合

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

3.2?圖像識別

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

3.3?距離

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

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