基于STM32的機械手夾持力控制系統設計

劉琪

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

0 引言

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

1 系統整體方案

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

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

圖1 夾持力閉環控制系統框圖

2 系統數學模型及仿真

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

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

則本夾持力控制系統對應的數學模型如圖2所示。

圖2 夾持力控制系統數學模型

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

增量式PID的表達式：

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

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

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

圖3 夾持力控制系統仿真建模

圖4 夾持力控制系統響應曲線

3 夾持力系統硬件組成

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

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

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

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

整個系統的硬件接線圖如圖5所示。

圖5 夾持力控制系統硬件接線圖

4 夾持力系統軟件設計

■4.1 控制系統的主程序設計

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

圖6 夾持力控制系統主程序流程圖

■4.2 控制系統的算法設計

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

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

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

■4.3 控制系統的CAN總線通訊設計

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

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

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

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

表1 報文數據域中的電機信息

5 控制系統測試與分析

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

圖7 測試系統硬件接線圖

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

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

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

圖8 讀取壓力傳感器數據流程圖

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

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

圖9 夾持力控制系統測試效果圖

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

6 結語

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