一、前言

本篇我們構(gòu)建一個(gè)可以跟人一樣寫字的機(jī)器人python軟件。實(shí)現(xiàn)如下功能：打開一個(gè)寫字板，人類在屏幕上寫或畫出任意形狀，機(jī)器人同步在紙面上畫出和人類一樣的形狀，就好像人類在遠(yuǎn)程操控機(jī)械臂，又或是機(jī)械臂是人的另一只手。這個(gè)軟件是可以擴(kuò)展的，如果連上互聯(lián)網(wǎng)，可以實(shí)現(xiàn)人在北京寫了一行字，在上海的機(jī)器臂同時(shí)也在自己的本子上寫了一行字。

二、整體框架

2.1 系統(tǒng)構(gòu)成

如上圖，寫字機(jī)器人硬件部分主要為三軸機(jī)械臂和主機(jī)構(gòu)成，軟件部分為python編寫用于控制機(jī)械臂末端的軌跡。

2.2 硬件介紹

2.2.1主要組成部分

硬件的組成如下圖所示：

2.2.2機(jī)械結(jié)構(gòu)

寫字機(jī)器人的機(jī)械臂部分采用一臺(tái)開源的優(yōu)秀的三軸機(jī)械臂，如圖所示：

其中各轉(zhuǎn)軸齒輪傳動(dòng)部分改成了皮帶傳動(dòng)，夾具采用了自制的夾具，用于按照寫字用筆。

2.2.3驅(qū)動(dòng)及控制主板

機(jī)械臂的控制器可采用如下主板，作為機(jī)械臂三個(gè)軸的驅(qū)動(dòng)和控制板，也可采用其它成熟的控制板。板子內(nèi)安裝了控制電機(jī)的驅(qū)動(dòng)控制代碼，提供通過串口通信規(guī)約（如G代碼）與上位機(jī)進(jìn)行所有必要的通訊，如電機(jī)的旋轉(zhuǎn)角度、真反正轉(zhuǎn)、停止等，此部分軟硬件產(chǎn)品解決方案已經(jīng)非常成熟，不再贅述。

如圖所示，主板裝有電機(jī)驅(qū)動(dòng)模塊，用于對(duì)機(jī)械臂電機(jī)進(jìn)行驅(qū)動(dòng)，本質(zhì)上是控制板通過串口接收，將上位機(jī)程序要求的電機(jī)目標(biāo)旋轉(zhuǎn)角度轉(zhuǎn)化為脈沖信號(hào)，輸入驅(qū)動(dòng)模塊，驅(qū)動(dòng)模塊驅(qū)動(dòng)電機(jī)旋轉(zhuǎn)相應(yīng)角度，就完成了控制。

PS電機(jī)驅(qū)動(dòng)原理簡(jiǎn)介:

兩相四線步進(jìn)電機(jī)（如42步進(jìn)電機(jī)）以8拍方式工作，則每個(gè)脈沖旋轉(zhuǎn)0.9°，即每旋轉(zhuǎn)一圈需要400個(gè)脈沖信號(hào)來勵(lì)磁。步進(jìn)電機(jī)的正、反轉(zhuǎn)由勵(lì)磁脈沖產(chǎn)生的順序來控制，它通過外部周期脈沖信號(hào)來相應(yīng)改變4個(gè)線圈電極的上電數(shù)量和順序，來周期改變定子的磁場(chǎng)，帶動(dòng)永磁轉(zhuǎn)子向指定方向指定速度旋轉(zhuǎn)如圖所示：

2.2.4其余部分

其余部分，包括了電源變送器及連接線部分，不再贅述。

2.3 機(jī)器人python程序框架

我們重點(diǎn)完成寫字機(jī)器人上位系統(tǒng)功能，如下圖所示：

由上圖可見，機(jī)器人上位操作軟件設(shè)計(jì)了幾大模塊，主要包括：

2.3.1通信服務(wù)模塊

通信模塊主要有兩部分：
1. 串口通信部分
此部分主要使用python的串口通信庫（如pyserial）實(shí)現(xiàn)，用于主機(jī)與機(jī)械臂主板的串行雙向通信。以下為一個(gè)簡(jiǎn)單的串口示例代碼：

如上示例所述，首先，使用了serial庫導(dǎo)入serial模塊以進(jìn)行串口通信。然后，你可以通過調(diào)用serial.Serial()函數(shù)打開串口。在此函數(shù)中，你需要指定串口號(hào)和波特率。根據(jù)實(shí)際情況，修改’COM1’為正確的串口號(hào)（例如linux格式為’/dev/ttyUSB0’，windows格式為‘COM3’）和波特率（例如 9600）。接下來，你可以將要發(fā)送的G代碼賦值給變量gcode（在示例中是"G90 G0 X0 Y0 Z0\r"）。最后，使用ser.write()函數(shù)將G代碼發(fā)送到下位機(jī)，使用.encode()將字符串編碼為字節(jié)數(shù)據(jù)。最后，通過調(diào)用ser.close()關(guān)閉串口。

2.web通信部分
此部分主要是建立B/S的寫字機(jī)器人架構(gòu)，用于支持遠(yuǎn)程寫字操作功能。把寫字板作為寫字機(jī)器人的客戶端，在寫字機(jī)器人的核心模塊中設(shè)置一個(gè)寫字板服務(wù)器端?？梢酝ㄟ^WebSocket協(xié)議進(jìn)行遠(yuǎn)程通信。
Python 支持多個(gè)用于 WebSocket 通信的庫。以下是一些常用的庫：

• websocket：websocket 是一個(gè)純 Python 實(shí)現(xiàn)的 WebSocket 客戶端和服務(wù)器端庫，提供了基本的
  WebSocket 功能。

• websockets：websockets 是一個(gè)支持異步的 WebSocket 客戶端和服務(wù)器端庫，使用起來簡(jiǎn)單方便。

• tornado.websocket：Tornado 是一個(gè)強(qiáng)大的 Python Web 框架，其中包含了用于 WebSocket 通信的模塊。 autobahn：autobahn 是一個(gè)基于 Twisted 的 WebSocket 庫，提供了客戶端和服務(wù)器端的功能，支持高級(jí)特性，如發(fā)布-訂閱和RPC模式。

• socket.io：socket.io 是一個(gè)跨平臺(tái)的實(shí)時(shí)通信引擎，支持 WebSocket 和其他實(shí)時(shí)傳輸協(xié)議，有 Python 的客戶端和服務(wù)器端實(shí)現(xiàn)。

2.3.2消息處理模塊

該模塊用于對(duì)上下通信模塊和應(yīng)用模塊之間產(chǎn)生的數(shù)據(jù)交互進(jìn)行有效處理。有利于模塊化建模。采用多線程消息的發(fā)布訂閱機(jī)制，實(shí)現(xiàn)各功能模塊間的協(xié)同順序工作（關(guān)于消息系統(tǒng)詳見博客相關(guān)章節(jié)）。以下為一個(gè)消息流示意：

2.3.3軌跡解析模塊

軌跡解析模塊作用是將來自寫字板的寫字字跡信息轉(zhuǎn)化為序列的機(jī)械臂末端執(zhí)行坐標(biāo)[（x0、y0、z0）,（x1、y1、z1）,（x2、y2、z2）…（xn、yn、zn）]，并安順序發(fā)布至消息中心。

2.3.4機(jī)械臂逆解模塊

機(jī)械臂逆解模塊是實(shí)現(xiàn)末端坐標(biāo)點(diǎn)（xn、yn、zn）轉(zhuǎn)化為機(jī)械臂各關(guān)節(jié)電機(jī)的角度及電機(jī)控制指令，并發(fā)布至消息中心。本模塊為機(jī)械臂控制的核心模塊之一，涉及機(jī)械臂的建模和逆解。在后面節(jié)中描述。

2.3.5寫字板模塊

寫字板模塊可以是一個(gè)Web前端程序，功能是捕捉人的手寫字軌跡，并對(duì)軌跡進(jìn)行離散化采樣，形成軌跡序列離散點(diǎn)，周期或打包發(fā)送至寫字服務(wù)器。寫字板如下：?

三、機(jī)械臂的建模

3.1 機(jī)械臂機(jī)構(gòu)幾何分析

如下圖所示，寫字機(jī)械臂是一個(gè)有三個(gè)旋轉(zhuǎn)軸的機(jī)械臂，這樣的機(jī)械臂在工業(yè)制造領(lǐng)域有著非常廣泛的應(yīng)用，常用于產(chǎn)品的碼垛、搬運(yùn)等:

下面我們對(duì)寫字機(jī)械臂進(jìn)行幾何解析，首先看一張俯視圖：

3.1.1 俯視——水平面的運(yùn)動(dòng)投影

在vrep環(huán)境里面，所定義的坐標(biāo)系為機(jī)械臂初始位置俯視投影在x軸軸上，機(jī)械臂的左側(cè)為y軸正方向。其它軸不變，只是運(yùn)動(dòng)軸J1，則機(jī)械臂在x-y平面內(nèi)做旋轉(zhuǎn)運(yùn)動(dòng)，如果改變J2、J3的角度，在俯視圖看來，投影的臂長(zhǎng)L或增加或縮短，簡(jiǎn)化如下圖所示：

3.1.2 側(cè)視——垂直平面的運(yùn)動(dòng)投影

這里所說的“側(cè)視”投影面，是需要想象出來的，它是指機(jī)械臂左側(cè)正對(duì)著L-Z平面看到的機(jī)械臂的投影，如下圖：

我們的下面的算法將在這個(gè)面進(jìn)行解析，我們通過繪制機(jī)械臂的主要機(jī)構(gòu)，對(duì)這個(gè)機(jī)械臂的運(yùn)動(dòng)特點(diǎn)進(jìn)行分析，畫出如下稍微簡(jiǎn)單的結(jié)構(gòu)：

上圖可以看到，其實(shí)這個(gè)機(jī)械臂在側(cè)視平面內(nèi)的運(yùn)動(dòng)，是通過改變兩根紅色臂桿L1、Lq的旋轉(zhuǎn)帶動(dòng)末端（或抓具）運(yùn)動(dòng)的，兩根驅(qū)動(dòng)的桿子又分別是兩個(gè)電機(jī)J2、J3來驅(qū)動(dòng)的。如下圖：

上圖把機(jī)械臂主要的部分“扣”了出來，左上角畫出了只轉(zhuǎn)動(dòng)臂桿L1時(shí)出現(xiàn)的狀態(tài)，可見，在一定的驅(qū)動(dòng)桿Lq位置固定下，運(yùn)動(dòng)是有位置限制的（邊界的），我們?cè)趐ython實(shí)現(xiàn)的時(shí)候要注意這一點(diǎn)，以免超出邊界還再給電機(jī)發(fā)送指令導(dǎo)致電機(jī)過載（仿真環(huán)境還好，在實(shí)際機(jī)械臂中容易發(fā)生），運(yùn)動(dòng)L1后，機(jī)械臂的前后運(yùn)動(dòng)為主方向，變化比較大，相對(duì)地，上下運(yùn)動(dòng)為次要方向，變化幅度小。；同樣的，右邊是固定L1時(shí)，改變Lq，可以使機(jī)械臂末端主要進(jìn)行上下運(yùn)動(dòng)，同樣也存在邊界狀態(tài)。
以上是機(jī)械臂的運(yùn)動(dòng)特點(diǎn)，需要注意的是，它還有一個(gè)連桿機(jī)構(gòu)，如下圖（紅圈），其目的是使機(jī)械臂的末端抓具始終垂直于水平面，這是一個(gè)隱含條件：

最后，我們可以將機(jī)械臂側(cè)視投影簡(jiǎn)化為（下圖所示）：由6跟桿組成包括（Lg、L0、L1、L2、L3、L4）、三個(gè)轉(zhuǎn)軸（或關(guān)節(jié)，包括J1、J2、J3）的一個(gè)連桿結(jié)構(gòu)，同時(shí)受到隱形的約束，抓具必須始終保持垂直狀態(tài)。
Ps：此處去掉了對(duì)軸J4的考慮，因?yàn)槲覀冇玫氖俏^，它的角度改變實(shí)際是沒有用的。

3.1.3 機(jī)械臂逆解公式推導(dǎo)

問題描述：
已知目標(biāo)物體中心相對(duì)機(jī)械臂的位置坐標(biāo)（x,y,z），求抓取該物體，所需的機(jī)械臂各旋轉(zhuǎn)關(guān)節(jié)的角度各是多少（J1，J2，J3）？
求解問題得流程如下兩步：

至此，當(dāng)我們知道目標(biāo)物體得坐標(biāo)時(shí)（x,y,z），我們就可以通過以上的簡(jiǎn)單幾何知識(shí)計(jì)算出控制機(jī)械臂的三個(gè)軸的角度值（J1，J2，J3）從而將機(jī)械臂的抓手送至目標(biāo)位置，實(shí)現(xiàn)準(zhǔn)確的抓取動(dòng)作。

3.2 機(jī)械臂軟件定義

3.2.1 機(jī)械臂結(jié)構(gòu)的軟件表達(dá)

對(duì)應(yīng)上面的幾何連桿結(jié)構(gòu)，我們可以用python的程序表達(dá)如下：

3.2.2 逆解算法的python表達(dá)

（1） 俯視圖根據(jù)目標(biāo)點(diǎn)（x,y）計(jì)算J1，L

如下圖俯視，已知坐標(biāo)（x,y）計(jì)算旋轉(zhuǎn)角度，和投影臂長(zhǎng)L：

對(duì)應(yīng)的python函數(shù)，屬于uarm類的方法，如下：

（2）側(cè)視圖根據(jù)目標(biāo)點(diǎn)（J1,L,z）計(jì)算剩余角度J2，J3

我們根據(jù)3.1節(jié)（2）中的推導(dǎo)公式編寫python程序，作為一個(gè)機(jī)器人類的方法：

（3） 繪制側(cè)視圖

為了直觀的能夠看到程序運(yùn)行是否正確，我們可以簡(jiǎn)單的繪制一下側(cè)面的圖，效果如下：

通過構(gòu)建一個(gè)內(nèi)部方法，實(shí)現(xiàn)根據(jù)（x,y,z）計(jì)算出機(jī)械臂的各個(gè)關(guān)節(jié)的角度，并畫出側(cè)視圖，對(duì)應(yīng)python代碼如下：

四、筆跡規(guī)劃

根據(jù)寫字的流程，可以把寫字劃分為以下過程，如圖所示：

如上圖所示，可以將寫字劃分為以筆劃為最小單位，一個(gè)字由諾干個(gè)筆劃安裝時(shí)間序列組成，寫字只要根據(jù)筆劃序列依次循環(huán)執(zhí)行以上流程：首選是準(zhǔn)備寫字，將機(jī)械臂末端筆尖定位在寫字板的準(zhǔn)備位置，一般是寫字板的中間；其次是根據(jù)一筆的起點(diǎn)坐標(biāo)，將末端移動(dòng)到起點(diǎn)位置；接著，降低筆尖至寫字板面，根據(jù)筆劃的序列采樣點(diǎn)坐標(biāo)進(jìn)行筆尖推送；最后完成一筆后，筆尖抬起。部分python程序如下：

五、寫字web服務(wù)器模塊

此模塊用于開啟一個(gè)web服務(wù)，可接收來自網(wǎng)絡(luò)內(nèi)的客戶端的筆跡信息序列。使用web服務(wù)的好處是可以通過TCP/IP網(wǎng)實(shí)現(xiàn)局域網(wǎng)或廣域網(wǎng)的遠(yuǎn)程控制，可以不受地理限制。本模塊采用python庫websockets實(shí)現(xiàn)。在運(yùn)行寫字機(jī)機(jī)器人主程序時(shí)，開啟websocket服務(wù)，部分代碼如下：

六、寫字板客戶端模塊

由于開啟了websocket服務(wù)，寫字板客戶端可以采用HTML語言進(jìn)行開發(fā)。主要功能實(shí)現(xiàn)如下：

如上所示，實(shí)現(xiàn)了一個(gè)簡(jiǎn)易的畫面客戶端，可以按筆劃進(jìn)行寫字，具有撤銷重做等功能，可以通過客戶端連接至服務(wù)器端，并按發(fā)送鍵將畫板上的文字或圖畫筆跡按順序序列發(fā)送至服務(wù)器端。

八、應(yīng)用演示

Setp1.連接寫字機(jī)器人串口并啟動(dòng)，進(jìn)入寫字模式

Setp2.在寫字服務(wù)器界面中點(diǎn)擊在線執(zhí)行模式

Setp3.在寫字板中點(diǎn)擊連接服務(wù)

Setp4.等待連接完成

Setp5.在寫字板上任意寫字或繪畫，點(diǎn)擊send發(fā)送執(zhí)行

九、示例代碼

本示例采用我們提供的寫字機(jī)器人庫，和錄制好的筆跡文件完成寫字功能，代碼如下：