劉琪

（江蘇安科瑞電器制造有限公司，江蘇無(wú)錫, 214400）

0 引言

在日常生活中，機(jī)械手通常用于夾持物體，很多機(jī)械手采用開(kāi)環(huán)控制，這樣就導(dǎo)致當(dāng)機(jī)械手夾持力過(guò)大時(shí)，被夾持物破裂，影響其使用，當(dāng)夾持力太小時(shí)會(huì)導(dǎo)致機(jī)械手夾持松弛，不能穩(wěn)定夾持以進(jìn)行自動(dòng)化作業(yè)。以試管為例，試管屬于材質(zhì)比較脆弱的物品，常用于實(shí)驗(yàn)，需要對(duì)夾持力精準(zhǔn)地控制，不能在夾持過(guò)程產(chǎn)生大的超調(diào)，不然可能會(huì)夾碎試管，所以能很好的測(cè)試系統(tǒng)效果。夾持時(shí)由于試管會(huì)產(chǎn)生形變而導(dǎo)致夾持力變化，所以應(yīng)實(shí)時(shí)檢測(cè)試管所受壓力值，不斷調(diào)整控制輸出以保證穩(wěn)定夾持。而且無(wú)外置傳感器的方式，在實(shí)際應(yīng)用中會(huì)更實(shí)用，更好推廣，不需要在機(jī)械手上安裝復(fù)雜的傳感器，簡(jiǎn)化了安裝。

1 系統(tǒng)整體方案

夾持力系統(tǒng)是靠控制無(wú)刷直流電機(jī)的轉(zhuǎn)速來(lái)控制機(jī)械手的夾持力，控制的夾持力與電機(jī)的轉(zhuǎn)速呈一定比例關(guān)系，所以本系統(tǒng)通過(guò)構(gòu)建一個(gè)速度閉環(huán)來(lái)間接控制機(jī)械手穩(wěn)定夾持試管。

整個(gè)系統(tǒng)主要由機(jī)械手、STM32開(kāi)發(fā)板、電機(jī)調(diào)速器、無(wú)刷直流電機(jī)幾部分組成。首先給定一個(gè)目標(biāo)速度值，機(jī)械手開(kāi)始慢慢夾持試管，電機(jī)調(diào)速器將檢測(cè)到的電機(jī)位置、速度和力矩信息通過(guò)CAN總線(xiàn)反饋到輸入端，這時(shí)給定速度與反饋回來(lái)的速度的差值輸入控制器，控制器按PID控制算法處理輸入信號(hào)并產(chǎn)生輸出，通過(guò)CAN總線(xiàn)發(fā)送控制指令給電機(jī)調(diào)速器，從而控制電機(jī)驅(qū)動(dòng)機(jī)械手夾持試管，產(chǎn)生新的速度值，形成閉環(huán)控制。系統(tǒng)框圖如圖1所示。

圖1 夾持力閉環(huán)控制系統(tǒng)框圖

2 系統(tǒng)數(shù)學(xué)模型及仿真

本設(shè)計(jì)采用的PID控制器，它的傳遞函數(shù)表示為：

根據(jù)直流無(wú)刷電機(jī)的數(shù)學(xué)模型，其傳遞函數(shù)為：

則本夾持力控制系統(tǒng)對(duì)應(yīng)的數(shù)學(xué)模型如圖2所示。

圖2 夾持力控制系統(tǒng)數(shù)學(xué)模型

PID控制主要包含3個(gè)控制參數(shù)：P（比例）、I（積分）、D（微分），根據(jù)搭建的系統(tǒng)，對(duì)輸入信號(hào)處理，實(shí)現(xiàn)無(wú)偏差輸出。本設(shè)計(jì)中選用增量式PID控制策略。由下面公式可知，增量式算法只與最近3次偏差值有關(guān)，工作量小，不僅能夠保證很好的實(shí)時(shí)性和快速性，而且不會(huì)產(chǎn)生很大的累積偏差，對(duì)系統(tǒng)影響較小，出現(xiàn)意外還可以輸出限幅和積分限幅等方式限制本次輸出。

增量式PID的表達(dá)式：

其中：k為采樣的序號(hào)，err(k)為第k次的誤差，u(k)為輸出量，Δu(k)為第k次輸出量與第k-1次輸出量的差，在這里，為了簡(jiǎn)便計(jì)算，方便說(shuō)明，令Ki=Kp*T/Ti、Kd=Kp*Td/T，在后文統(tǒng)一用Ki和Kd來(lái)代表積分、微分環(huán)節(jié)的性能參數(shù)。

本課題采用試湊法整定PID參數(shù)，根據(jù)夾持力系統(tǒng)的數(shù)學(xué)模型，把仿真需要的特征參數(shù)帶入，在整定過(guò)程中，根據(jù)系統(tǒng)過(guò)渡過(guò)程調(diào)整PID參數(shù)，邊觀(guān)察過(guò)程曲線(xiàn)特點(diǎn)，邊根據(jù)PID的3個(gè)參數(shù)各自功能修改對(duì)應(yīng)參數(shù)值，直到響應(yīng)曲線(xiàn)滿(mǎn)足速度、靜差等設(shè)計(jì)要求。

建立仿真模型如圖3所示，給定夾持力為3N，經(jīng)過(guò)PID參數(shù)的整定過(guò)后，整個(gè)系統(tǒng)的響應(yīng)曲線(xiàn)如圖4所示。整個(gè)過(guò)程在開(kāi)始經(jīng)歷了少量的超調(diào)后，能夠較快的穩(wěn)定在給定值3N附近。在整定的過(guò)程中，加快響應(yīng)速度往往以提高超調(diào)量為代價(jià)，綜合考量后，采用第一組PID整定參數(shù)，在保證超調(diào)量不高的情況下，保持一定的響應(yīng)速度，提高了系統(tǒng)整體的性能。

圖3 夾持力控制系統(tǒng)仿真建模

圖4 夾持力控制系統(tǒng)響應(yīng)曲線(xiàn)

3 夾持力系統(tǒng)硬件組成

本設(shè)計(jì)采用STM32F103C8T6芯片作為主控芯片，其最小系統(tǒng)包括電源、晶振和復(fù)位三部分組成，電源為開(kāi)發(fā)板上器件供電，晶振為系統(tǒng)提供準(zhǔn)確的時(shí)鐘信號(hào)。結(jié)合控制需求，控制器需要5V和3 3V兩組電壓，通過(guò)帶USB接口的電源接入電路為控制器和其它硬件電路接入5V電源，并采用AMS1117-3 3芯片，將5V電源轉(zhuǎn)換為3 3V供給需要的硬件電路，保證整個(gè)硬件電路系統(tǒng)穩(wěn)定運(yùn)行，不會(huì)出現(xiàn)欠壓或過(guò)流。同時(shí)，設(shè)計(jì)的電源指示燈電路可以清晰地顯示3 3V供電是否正常。

控制器按PID控制算法處理輸入信號(hào)后，需要通過(guò)CAN總線(xiàn)把控制指令發(fā)送給電機(jī)調(diào)速器。這里選用CAN收發(fā)器為T(mén)JA1050，它是一款獨(dú)立CAN收發(fā)器，在應(yīng)用方面相當(dāng)普遍和成熟，可以滿(mǎn)足大部分?jǐn)?shù)據(jù)通信需求。報(bào)文的仲裁場(chǎng)、控制場(chǎng)、數(shù)據(jù)場(chǎng)的數(shù)據(jù)由軟件編程配置TJA1050，報(bào)文的幀起始、CRC場(chǎng)、應(yīng)答場(chǎng)、幀結(jié)束由TJA1050自動(dòng)配置完成。為了保障數(shù)據(jù)正常傳輸，確保可以接收到無(wú)刷電機(jī)調(diào)速器的反饋，設(shè)置CAN總線(xiàn)通信的比特率為1Mbps。

C610無(wú)刷電機(jī)調(diào)速器采用32位定制電機(jī)驅(qū)動(dòng)芯片，高度集成主控、電源和驅(qū)動(dòng)，使用磁場(chǎng)定向控制（FOC）技術(shù)，實(shí)現(xiàn)對(duì)電機(jī)轉(zhuǎn)矩的精確控制。同時(shí)，C610電機(jī)調(diào)速器有很高的耐用度，因?yàn)槠鋬?nèi)部設(shè)有堵轉(zhuǎn)、過(guò)壓和斷線(xiàn)等多重保護(hù)。C610無(wú)刷電機(jī)調(diào)速器功能強(qiáng)大，支持CAN總線(xiàn)指令控制，通過(guò)CAN總線(xiàn)獲取轉(zhuǎn)子位置和轉(zhuǎn)子轉(zhuǎn)速等信息。

這里選用電機(jī)為M2006 P36，它是一種無(wú)刷直流減速電機(jī)，內(nèi)含的減速箱減速比為36:1。M2006 P36電機(jī)采用三相永磁直流無(wú)刷結(jié)構(gòu)，具有控制精度高、輸出轉(zhuǎn)速高、體積小、功率密度高等特點(diǎn)。其內(nèi)部的位置傳感器可高精度地反饋位置量，以FOC矢量控制方式使電機(jī)產(chǎn)生連續(xù)而線(xiàn)性的扭矩，提升控制精密度。

整個(gè)系統(tǒng)的硬件接線(xiàn)圖如圖5所示。

圖5 夾持力控制系統(tǒng)硬件接線(xiàn)圖

4 夾持力系統(tǒng)軟件設(shè)計(jì)

■4.1 控制系統(tǒng)的主程序設(shè)計(jì)

機(jī)械手夾持力控制系統(tǒng)的控制流程：首先給定一目標(biāo)速度，在完成系統(tǒng)初始化后，由STM32開(kāi)發(fā)板通過(guò)CAN總線(xiàn)向C610無(wú)刷電機(jī)調(diào)速器發(fā)送控制指令，電機(jī)調(diào)速器接收控制指令后控制電機(jī)調(diào)速器的電流輸出，驅(qū)動(dòng)電機(jī)運(yùn)轉(zhuǎn)，使機(jī)械手以一定速度慢慢夾持試管，防止速度過(guò)快沖擊力大對(duì)試管造成損壞。同時(shí)，電機(jī)調(diào)速器可以采集電機(jī)的位置、轉(zhuǎn)速、力矩信息，將采集的M2006 P36無(wú)刷直流減速電機(jī)的速度、位置和力矩信息向CAN總線(xiàn)發(fā)送，進(jìn)入CAN中斷后進(jìn)行數(shù)據(jù)解析，MCU獲取電機(jī)速度、位置、力矩信息，并將反饋的電機(jī)速度信息經(jīng)控制器按PID控制算法進(jìn)行運(yùn)算，產(chǎn)生新的輸出并發(fā)送新的控制指令給電機(jī)調(diào)速器，控制電機(jī)調(diào)速器產(chǎn)生新的電流輸出驅(qū)動(dòng)電機(jī)運(yùn)轉(zhuǎn)，機(jī)械手夾持試管形成閉環(huán)控制。當(dāng)檢測(cè)到的電機(jī)的力矩達(dá)到大于2000時(shí)，表明機(jī)械手已經(jīng)穩(wěn)穩(wěn)地夾持住試管，這是把目標(biāo)速度值設(shè)置為0，系統(tǒng)就會(huì)跟隨當(dāng)前夾持力值，動(dòng)態(tài)地保持現(xiàn)在的狀態(tài)附近，保持對(duì)試管的穩(wěn)定夾持。機(jī)械手夾持力控制系統(tǒng)的流程圖如圖6所示。

圖6 夾持力控制系統(tǒng)主程序流程圖

■4.2 控制系統(tǒng)的算法設(shè)計(jì)

首先進(jìn)行PID參數(shù)的初始化。進(jìn)行初始化就要定義PID類(lèi)型結(jié)構(gòu)體，包括模式、輸出限幅、積分限幅、Kp、Ki和Kd。

將之前用試湊法得出的Kp、Ki和Kd值賦給變量，PID模式采用增量式，需要進(jìn)行積分限幅和輸出限幅。有時(shí)PID計(jì)算會(huì)出現(xiàn)錯(cuò)誤，產(chǎn)生一個(gè)過(guò)大的值，發(fā)生誤操作，機(jī)構(gòu)就可能會(huì)因輸出過(guò)高而引發(fā)一系列危險(xiǎn)事故，PID輸出限幅限制了它的最大值，屏蔽了可以出現(xiàn)的誤操作，從而避免了這些事故的發(fā)生。同理，PID積分限幅也是為了防止積分累計(jì)偏差過(guò)大而引起的積分飽和現(xiàn)象。設(shè)置了積分限幅值后，當(dāng)積分項(xiàng)將超過(guò)額定上限值時(shí)，則保持在上限值，避免因積分飽和而失去控制。

在初始化完成后，發(fā)送控制指令驅(qū)動(dòng)電機(jī)運(yùn)行時(shí)，為了防止電機(jī)啟動(dòng)瞬間電流過(guò)大，影響PID控制器對(duì)當(dāng)前電機(jī)信息參數(shù)產(chǎn)生錯(cuò)誤的計(jì)算，所以要延時(shí)一段時(shí)間，防止干擾。在電機(jī)開(kāi)始運(yùn)轉(zhuǎn)后，以給定速度和反饋速度的偏差為輸入，按上文提到的增量式PID算法表達(dá)式不斷計(jì)算，以當(dāng)前采集的速度值為參數(shù)，代入pid_calc()函數(shù)進(jìn)行計(jì)算，產(chǎn)生新的參數(shù)，發(fā)送給電機(jī)調(diào)速器對(duì)應(yīng)的電流信息，驅(qū)動(dòng)電機(jī)運(yùn)轉(zhuǎn)，機(jī)械手則不斷夾緊，直至達(dá)到給定夾持力。同時(shí)，在PID計(jì)算的過(guò)程中，依上文所述，要考慮因特殊情況而產(chǎn)生的誤操作，故設(shè)置輸出限幅和積分限幅，編制對(duì)應(yīng)的程序，保障系統(tǒng)安全運(yùn)行。

■4.3 控制系統(tǒng)的CAN總線(xiàn)通訊設(shè)計(jì)

CAN總線(xiàn)初始化目的在于通過(guò)參數(shù)配置保證CAN總線(xiàn)正常通訊。確認(rèn)TJA1050當(dāng)前為復(fù)位狀態(tài)，開(kāi)始初始化配置。在完成初始化后，TJA1050進(jìn)入工作模式，就可以正常進(jìn)行CAN總線(xiàn)數(shù)據(jù)通訊。初始化步驟為：（1）配置相關(guān)引腳的復(fù)用功能，使能CAN時(shí)鐘；（2）設(shè)置CAN工作模式及波特率等；（3）設(shè)置濾波器及中斷優(yōu)先級(jí)等。

CAN節(jié)點(diǎn)采用輪詢(xún)式發(fā)送方式，當(dāng)發(fā)現(xiàn)端口有報(bào)文時(shí)，端口會(huì)自動(dòng)發(fā)送。本設(shè)計(jì)使用標(biāo)準(zhǔn)ID的數(shù)據(jù)幀，標(biāo)識(shí)符為0x200，數(shù)據(jù)長(zhǎng)度為8字節(jié)。本系統(tǒng)最多可以同時(shí)控制4個(gè)電機(jī)，通過(guò)字節(jié)偏移量的不同區(qū)分電機(jī)ID，在這里控制ID為2的電機(jī)，將電機(jī)的設(shè)置電流賦值到數(shù)據(jù)域的第2、3字節(jié)，Data[2]存放控制電流值高8位，Data[3]存放控制電流值低8位，STM32中庫(kù)函數(shù)提供了發(fā)送消息的函數(shù)CAN_Transmit，供發(fā)送信息使用。等總線(xiàn)空閑后，則完成發(fā)送，發(fā)送信息儲(chǔ)存在FIFO郵箱中，這時(shí)，電機(jī)調(diào)速器就可以通過(guò)CAN協(xié)議來(lái)接收STM32控制板發(fā)送的控制指令報(bào)文，將其解析以產(chǎn)生新的控制電流，驅(qū)動(dòng)M2006 P36電機(jī)運(yùn)轉(zhuǎn)，控制機(jī)械手夾持試管，接收解析報(bào)文由硬件自動(dòng)完成。

機(jī)械手夾持試管時(shí)，C610電機(jī)調(diào)速器通過(guò)CAN總線(xiàn)向STM32開(kāi)發(fā)板發(fā)送采集到的電機(jī)位置、速度和力矩等信息。CAN節(jié)點(diǎn)采用的接收方式為中斷接收，當(dāng)正確的CAN數(shù)據(jù)到來(lái)后，啟動(dòng)CAN的中斷服務(wù)程序，進(jìn)入U(xiǎn)SB_LP_CAN1_RX0_IRQHandler(void)的回調(diào)函數(shù)中，讀取中斷標(biāo)志位寄存器，識(shí)別幀ID，接收保存電機(jī)調(diào)速器傳送的數(shù)據(jù)，再關(guān)閉CAN中斷，最后進(jìn)行數(shù)據(jù)處理與解析，進(jìn)行下一輪PID計(jì)算。

電機(jī)調(diào)速器向總線(xiàn)上發(fā)送的反饋數(shù)據(jù)。報(bào)文格式采用標(biāo)準(zhǔn)ID的數(shù)據(jù)幀，標(biāo)識(shí)符為：0x200 +電機(jī)調(diào)速器ID，使用的電機(jī)ID為2，則標(biāo)識(shí)符為0x202，數(shù)據(jù)長(zhǎng)度為8字節(jié)。數(shù)據(jù)域里每個(gè)字節(jié)分別存放在對(duì)應(yīng)的電機(jī)信息，具體如表1所示。

表1 報(bào)文數(shù)據(jù)域中的電機(jī)信息

5 控制系統(tǒng)測(cè)試與分析

為了驗(yàn)證該機(jī)械手夾持力控制裝置的控制效果，搭建夾持力測(cè)試系統(tǒng)。首先在機(jī)械手上安裝壓力傳感器，再通過(guò)A/D轉(zhuǎn)換模塊輸入STM32控制器，通過(guò)USB轉(zhuǎn)UART協(xié)議，以串口通信的方式，把傳感器檢測(cè)到的壓力值通過(guò)串口實(shí)時(shí)顯示。測(cè)試系統(tǒng)硬件接線(xiàn)如圖7所示。

圖7 測(cè)試系統(tǒng)硬件接線(xiàn)圖

傳感器這里選用精度較高的5kg電阻應(yīng)變式懸臂梁壓力傳感器。機(jī)械手夾持試管時(shí)，懸臂梁會(huì)產(chǎn)生應(yīng)變，傳感器內(nèi)部的電橋會(huì)將應(yīng)變轉(zhuǎn)換成電壓信號(hào)，后續(xù)再采用Hx711模塊處理得到機(jī)械手夾持試管時(shí)產(chǎn)生的壓力。

24位專(zhuān)用模數(shù)轉(zhuǎn)換器芯片 Hx711不僅起到把傳感器輸出信號(hào)放大的作用，而且也負(fù)責(zé)把壓力傳感器輸出的模擬量信號(hào)轉(zhuǎn)化為數(shù)字量，具有速度快、抗干擾、集成度高和性能穩(wěn)定等特點(diǎn)。該模塊為兩路可選擇差分輸入，在5V的工作電壓下，選擇通道A放大128倍來(lái)把微弱的模擬量信號(hào)轉(zhuǎn)換成數(shù)字量并放大，通過(guò)管腳PD_SCK和DOUT以串口方式輸出數(shù)據(jù)，完成模數(shù)轉(zhuǎn)換，進(jìn)入開(kāi)發(fā)板。

按照芯片的流程圖即可讀取夾持力數(shù)據(jù)，如圖8所示。首先對(duì)Hx711進(jìn)行初始化，只有進(jìn)行初始化后才能根據(jù)流程圖完成壓力信號(hào)的讀取。一開(kāi)始數(shù)據(jù)線(xiàn)管腳Dout電平為高，表明A/D轉(zhuǎn)換器還未準(zhǔn)備好輸出數(shù)據(jù)，這時(shí)拉低串口時(shí)鐘線(xiàn)SCK，進(jìn)入芯片自動(dòng)復(fù)位狀態(tài)，同時(shí)開(kāi)始延時(shí)一段時(shí)間。因?yàn)镠x711芯片復(fù)位后要經(jīng)過(guò)4個(gè)數(shù)據(jù)輸出周期后才能穩(wěn)定地輸出數(shù)據(jù)，進(jìn)入正常工作狀態(tài)。當(dāng)Dout電平由高變低后，SCK應(yīng)輸入25~27個(gè)不等的時(shí)鐘脈沖，因?yàn)檫@里一直選擇的是通道A的128放大倍數(shù)，所以會(huì)輸入25個(gè)脈沖，SCK每跳變一次，Dout輸出一位數(shù)據(jù)。在24個(gè)跳變后輸出了24位數(shù)據(jù)，通過(guò)控制SCK的第25個(gè)脈沖選擇下一次轉(zhuǎn)換放大倍數(shù)和通道，這樣一個(gè)周期的數(shù)據(jù)就采樣完畢。

圖8 讀取壓力傳感器數(shù)據(jù)流程圖

機(jī)械手夾持試管時(shí)產(chǎn)生壓力值用下列公式計(jì)算可得。其中Yx為目前壓力，單位 kg；Dx為直接讀取到的Hx711采樣數(shù)據(jù)；D0表示未施加壓力時(shí)Hx711采樣數(shù)據(jù)；Ymax表示壓力傳感器最大量程，單位kg；Vc表示Hx711芯片的模數(shù)轉(zhuǎn)換器參考電壓，單位V；VG表示壓力傳感器的激勵(lì)電壓，單位V；S表示壓力傳感器的靈敏度，單位mv/v；A表示Hx711所采用通道的對(duì)應(yīng)放大倍數(shù)。

啟動(dòng)夾持力控制系統(tǒng)，機(jī)械手開(kāi)始慢慢夾持目標(biāo)試管，實(shí)驗(yàn)效果如圖9所示。同時(shí)選擇對(duì)應(yīng)串口，配置波特率、停止位、數(shù)據(jù)位和奇偶校驗(yàn)，完成配置后，點(diǎn)擊打開(kāi)串口，串口開(kāi)啟實(shí)時(shí)顯示當(dāng)前夾持力。經(jīng)檢測(cè)，在給定夾持力為3N的情況下，機(jī)械手夾持力穩(wěn)定在2 95N附近，機(jī)械手可以完成對(duì)試管的穩(wěn)定夾持，且未對(duì)被夾持物造成夾痕。機(jī)械手夾持力值如圖10所示。

圖9 夾持力控制系統(tǒng)測(cè)試效果圖

圖10 串口顯示機(jī)械手夾持力

6 結(jié)語(yǔ)

在市場(chǎng)上眾多機(jī)械手需要安裝復(fù)雜的傳感器來(lái)保證控制效果，筆者研究的機(jī)械手夾持力控制系統(tǒng)結(jié)合實(shí)際需求，創(chuàng)新地提出一種無(wú)外置傳感器的恒夾持力機(jī)械手系統(tǒng)，通過(guò)C610無(wú)刷電機(jī)調(diào)速器獲取電機(jī)轉(zhuǎn)子位置、轉(zhuǎn)速、輸出轉(zhuǎn)矩等信息通過(guò)CAN總線(xiàn)發(fā)送給STM32開(kāi)發(fā)板，將給定速度與反饋速度的偏差輸入控制器按PID控制算法處理，再通過(guò)CAN總線(xiàn)發(fā)送控制指令給電機(jī)調(diào)速器驅(qū)動(dòng)電機(jī)運(yùn)轉(zhuǎn)，形成閉環(huán)控制。完成系統(tǒng)后，在機(jī)械手上安裝壓力傳感器，建立壓力檢測(cè)系統(tǒng)，通過(guò)串口的方式實(shí)時(shí)顯示機(jī)械手在夾持過(guò)程中的夾持力，經(jīng)檢驗(yàn)，這種無(wú)外置壓力傳感器的恒壓力機(jī)械手系統(tǒng)能穩(wěn)定地夾持目標(biāo)物體，保護(hù)被夾持物的完好。這種以控制速度間接控制夾持力的方式比直接控制壓力更具安全性，直接控制壓力方案若在夾持過(guò)程中發(fā)生意外，試管滑落未放在夾持位置，此時(shí)力反饋就會(huì)突然變成0，造成極大的偏差量，電機(jī)會(huì)驅(qū)動(dòng)機(jī)械手以很大的速度去夾，直接會(huì)損壞機(jī)械手結(jié)構(gòu)，造成事故。所以本方案更具有實(shí)用性。