基于STM32的機械手夾持力控制系統(tǒng)設計

劉琪

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

0 引言

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

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

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

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

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

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

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

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

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

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

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

增量式PID的表達式：

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

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

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

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

圖4 夾持力控制系統(tǒng)響應曲線

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

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

控制器按PID控制算法處理輸入信號后，需要通過CAN總線把控制指令發(fā)送給電機調速器。這里選用CAN收發(fā)器為TJA1050，它是一款獨立CAN收發(fā)器，在應用方面相當普遍和成熟，可以滿足大部分數(shù)據(jù)通信需求。報文的仲裁場、控制場、數(shù)據(jù)場的數(shù)據(jù)由軟件編程配置TJA1050，報文的幀起始、CRC場、應答場、幀結束由TJA1050自動配置完成。為了保障數(shù)據(jù)正常傳輸，確?？梢越邮盏綗o刷電機調速器的反饋，設置CAN總線通信的比特率為1Mbps。

C610無刷電機調速器采用32位定制電機驅動芯片，高度集成主控、電源和驅動，使用磁場定向控制（FOC）技術，實現(xiàn)對電機轉矩的精確控制。同時，C610電機調速器有很高的耐用度，因為其內(nèi)部設有堵轉、過壓和斷線等多重保護。C610無刷電機調速器功能強大，支持CAN總線指令控制，通過CAN總線獲取轉子位置和轉子轉速等信息。

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

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

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

4 夾持力系統(tǒng)軟件設計

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

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

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

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

首先進行PID參數(shù)的初始化。進行初始化就要定義PID類型結構體，包括模式、輸出限幅、積分限幅、Kp、Ki和Kd。

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

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

■4.3 控制系統(tǒng)的CAN總線通訊設計

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

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

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

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

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

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

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

圖7 測試系統(tǒng)硬件接線圖

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

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

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

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

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

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

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

圖10 串口顯示機械手夾持力

6 結語

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