TWI826820B

TWI826820B - 機械手臂

Info

Publication number: TWI826820B
Application number: TW110131768A
Authority: TW
Inventors: 賴俊文; 高稚然; 呂晏州
Original assignee: 達明機器人股份有限公司
Priority date: 2021-08-25
Filing date: 2021-08-25
Publication date: 2023-12-21
Also published as: TW202308813A; CN115733410A

Abstract

一種機械手臂及其馬達驅動器，馬達驅動器包含一電流控制模組而偵測兩次一偵測電壓，依據兩次的偵測電壓決定一補償訊號。如此，馬達驅動器依據補償訊號輸出一馬達控制電壓，以提升馬達的性能。

Description

機械手臂

本發明有關一種機械手臂及其馬達驅動器，尤其關於提升馬達電流的準確度，而降低馬達的力矩漣波，驅使馬達與機械手臂振動減少的馬達驅動器。

一般馬達由馬達驅動器控制其馬達電流與轉動速度，然而，馬達驅動器易受溫度影響，致使電路間運作產生的數值或訊號產生飄移，如此馬達驅動器會誤判從馬達偵測獲得的馬達電流數值，長久運作下來，馬達性能越來越差，例如振動現象嚴重。

鑒於上述問題，本發明提供一種馬達驅動器，可以及時偵錯與動態補償馬達電流，而提升控制馬達電流的準確度，達到降低馬達的力矩漣波，驅使馬達的振動減少。

本發明之目的提供一種機械手臂及其馬達驅動器，馬達驅動器可以及時偵錯與動態補償馬達電流，而提升控制馬達電流的準確度，達到馬達的振動減少。

為了達到前述發明的目的，本發明馬達驅動器包含一電流控制模組，偵測兩次一偵測電壓，並依據兩次的偵測電壓決定一補償訊號，且依據補償訊號輸出一馬達控制電壓。

再者，於馬達驅動器未執行一伺服控制時，電流控制模組提供數次偵測電壓與提供數次一馬達電壓，並經由運算數次的該些偵測電壓而獲得一平均偵測電壓，及依據數次的該些馬達電壓進行運算而獲得一平均馬達電壓。於馬達驅動器從未執行伺服控制轉變為執行伺服控制時，偵測馬達電流而獲得一馬達電壓，並依據平均馬達電壓調整馬達電壓。爾後，電流控制模組偵測兩次偵測電壓，並依據兩次的偵測電壓分別與平均偵測電壓比較後決定補償訊號，且依據補償訊號再次調整馬達電壓後產生馬達控制電壓。

10:馬達驅動器

20:位置命令

30:位置控制模組

31:第一運算電路

32:位置控制器

40:速度控制模組

41:第二運算電路

42:速度控制器

50:電流控制模組

51:電流運算電路

52:電流控制器

53:偵測電路

54:電流感測電路

541:電流感知器

55:數位類比轉換電路

56:微處理器

60:馬達

70:編碼器

80:速度計算器

a:馬達電流

ADC1:數位類比轉換電路

ADC2:數位類比轉換電路

ADC3:數位類比轉換電路

ADC4:數位類比轉換電路

b:馬達電流

c:馬達電流

GND:接地電壓

Ref V:參考電壓

Ref V1:參考電壓

Ref V2:參考電壓

Ref V3:參考電壓

T:時間

T_Cri:設定時間

V31:第一運算訊號

V32:位置控制訊號

V41:第二運算訊號

V42:速度控制訊號

V51:電流運算訊號

V52:馬達控制電壓

V55:類比訊號

V56:回授訊號

V70:馬達位置訊號

V80:馬達速度訊號

Va:馬達電壓

Va_avg:平均馬達電壓

Va_d:電壓差

Va_real:馬達電壓

Vb:馬達電壓

Vb_avg:平均馬達電壓

Vb_d:電壓差

Vb_real:馬達電壓

Vc:馬達電壓

Vc_avg:平均馬達電壓

Vc_d:電壓差

Vc_real:馬達電壓

Vcomp:補償訊號

Vr:偵測電壓

Vr_d:電壓差

Vr_d1:第一電壓差

Vr_d2:第二電壓差

Vr_avg:平均偵測電壓

Vr_Cri:門檻電壓

第一圖為本發明馬達驅動器控制馬達之一實施例的電路圖。

第二圖為本發明電流控制模組偵測馬達之一實施例的電路圖。

第三圖為本發明決定補償訊號之一實施例的流程圖。

第四圖為本發明偵測電壓隨馬達驅動器運作時間而上升之一實施例的波形圖。

第五圖為本發明驅使馬達減緩振動現象前後之一實施例的比較圖。

有關本發明為達成上述目的，所採用之技術手段及其功效，茲舉實施例，並配合圖式加以說明如下。

請參閱第一圖，其為本發明馬達驅動器控制馬達之一實施例的電路圖。如圖所示，馬達驅動器10控制馬達60運作，馬達驅動器10包含一電流控制模組50，可以偵測兩次一偵測電壓Vr，而依據兩次的偵測電壓Vr決定一補償訊號Vcomp，及依據補償訊號Vcomp輸出一馬達控制電壓V52，達到動態補償馬達電流，而提升控制馬達電流的準確度，使馬達的振動現象減緩。其中，馬達電流a、b、c請參閱第二圖，其為本發明電流控制模組偵測馬達之一實施例的電路圖。如圖所示，馬達電流a、b、c包含馬達60的三相電流，且馬達驅動器10偵測不同相位的電流為相同技術，因此實施例中以馬達電流a代表三種不同相位的電流作為說明之用。

換言之，於馬達驅動器10執行一伺服控制時，馬達驅動器10的電流控制模組50偵測兩次偵測電壓Vr，而依據兩次的偵測電壓Vr的電壓差決定補償訊號Vcomp，如圖所示補償訊號Vcomp可以由微處理器56處理後產生。復參閱第一圖，電流控制模組50包含一電流運算電路51、一電流控制器52、一偵測電路53、一電流感測電路54、一ADC電路55、一微處理器56。其中，電流運算電路51接收回授訊號V56與一速度控制訊號V42而產生一電流運算訊號V51，此外若偵測三相電流則電流運算電路51接收複數回授訊號V56。電流控制器52耦接電流運算電路51，依據電流運算訊號V51產生馬達控制電壓V52，而控制馬達電流a(或b、c)。

因此，馬達60的運轉狀態可以藉由電流控制模組50的電流感測電路54，感測馬達電流a(或b、c)而產生馬達電壓Va，同理若感測三相電流則電流控制模組50可以包含複數電流感測電路54而產生複數馬達電壓，如第二圖的Va、Vb、Vc。偵測電路53耦接於一參考電壓Ref V以產生偵測電壓Vr，如此，數位類比轉換電路55耦接偵測電路53與電流感測電路54，而轉換偵測電壓Vr與馬達電壓Va為不同的類比訊號V55。所以，微處理器56 耦接ADC電路55而處理多個類比訊號V55後產生回授訊號V56，類比訊號V55與回授訊號V56的數量同樣相關於偵測馬達電流a、b、c的數量。故，藉由電流感測電路54及時偵錯馬達60的馬達電流a(或b、c)，產生補償訊號Vcomp而進行回授控制，可以降低馬達60的力矩漣波，驅使馬達60的振動減少。

復參閱第二圖，偵測電路53耦接於參考電壓Ref V與一接地電壓GND之間，且偵測電路53包含複數阻抗元件，該些阻抗元件相互串聯，並於該些阻抗元件間的節點(即分壓)產生偵測電壓Vr至ADC電路55。再者，電流感測電路54因偵測複數馬達電流a、b、c而於馬達驅動器10設置複數個，且該些電流感測電路54分別包含一電流感知器541與複數阻抗元件，以分別偵測馬達電流a、b、c後，分別產生馬達電壓Va、Vb、Vc至ADC電路55。再者，該些電流感測電路54的電流感知器541分別耦接參考電壓Ref V1、Rer V2、Rer V3以供運作之用，且第二圖所示四個參考電壓Ref V、Ref V1、Rer V2、Rer V3可以互為相同或不同，其為設計之選項。因此，於第二圖實施例中包含複數個ADC電路55，例如ADC1、ADC2、ADC3、ADC4，從第二圖的上至下，分別耦接偵測電路53與三個電流感測電路54。而且ADC1電路、ADC2電路、ADC3電路、ADC4電路設置於微處理器56內，或者可以如第一圖實施例設置於微處理器56外，皆為設計之選項，本發明之實施未加以限制。

此外，如第一圖所示，馬達驅動器10更包含一位置控制模組30、一速度控制模組40、一編碼器70與一速度計算器80。位置控制模組30接收一位置命令20與馬達位置訊號V70而產生一位置控制訊號V32。位置控制模組30耦接編碼器70且包含一第一運算電路31與一位置控制器32，第一運算電路31接收位置命令20與馬達位置訊號V70而產生第一運算訊號V31。位置控制器32耦接第一運算電路31，且依據第一運算訊號V31產生位置控制訊號V32。速度控制模組40耦接位置控制模組30與速度計算器80，而接收位置控制訊號V32與馬達速度訊號V80，以產生速度控制訊號V42。速度控制模組40包含一第二運算電路41與一速度控制器42，第二運算電路41耦接位置控制器32與速度計算器80，而接收位置控制訊號V32與馬達速度訊號V80產生一第二運算訊號V41。速度控制器42耦接第二運算電路41而接收第二運算訊號V41，且依據第二運算訊號V41產生速度控制訊號V42至電流控制模組50。編碼器70設置於馬達60而偵測一馬達轉動位置，以產生馬達位置訊號V70至第一運算電路31。速度計算器80耦接編碼器70而接收馬達位置訊號V70，並依據馬達位置訊號V70產生馬達速度訊號V80至第二運算電路41。

所以，在馬達驅動器10依據位置命令20驅動馬達60後，在回授控制中藉由編碼器70獲得馬達60受控後的轉動位置，而於位置控制模組30及時偵錯與補償馬達60的轉動位置。而且，依據馬達60受控後的轉動位置計算出馬達60的轉速，即利用位置控制模組30與速度控制模組40重新調整馬達60的轉動位置與轉速。爾後，電流控制模組50依據馬達電流a、位置控制模組30與速度控制模組40的偵測結果，重新輸出馬達控制電壓V52，以調整馬達60的運作狀態。此外，位置控制模組30與速度控制模組40同樣可以整合設置於微處理器56內，即整合後的微處理器56的運算能力足夠應付位置控制模組30、速度控制模組40與ADC電路55的需求。換言之，單一微處理器56取代位置控制模組30、速度控制模組40與ADC電路55亦是實施的一種方式。

請參閱第三圖，其為本發明決定補償訊號之一實施例的流程圖。如圖所示，若在較要求精準度應用下，例如本發明馬達驅動器10控制機械手臂的馬達減少振動現象，可以先藉由步驟S1至步驟S10獲得電流控制模組50的初始數值，之後再依據步驟S11至步驟S15決定補償訊號Vcomp。反之，若對精準度較不要求時，可以選擇僅運作於步驟S11至步驟S15，而決定補償訊號Vcomp的準位，其兩種方式皆為實施例之一。

承接上述，步驟S1初始校正程序1開始後，先於步驟S2解除馬達驅動器10對馬達60的伺服控制，即馬達驅動器10未執行一伺服控制時，之後偵測電路53與電流感測電路54分別提供數次(例如N次)偵測電壓Vr與數次馬達電壓Va(即三個馬達電壓Va、Vb、Vc)，此處的三個馬達電壓Va、Vb、Vc無關馬達60運作時的三相電流。如此，於步驟S3利用ADC電路55數次轉換偵測電壓Vr與三個馬達電壓Va、Vb、Vc，而於步驟S4中，微處理器56經由運算數次而取得平均偵測電壓Vr_avg=Vr/N與平均馬達電壓Va_avg=Va/N，及同理取得Vb_avg=Vb/N與Vc_avg=Vc/N。於步驟S5紀錄上述電流控制模組50內相關的初始數值，即Vr_avg、Va_avg、Vb_avg與Vc_avg。

完成系統的初始數值量測後，於步驟S6進入在線補償程序2，於步驟S7在線補償程序2開始，即於步驟S8馬達驅動器10開始對馬達60進行伺服控制，即馬達驅動器10從未執行伺服控制轉變為執行伺服控制時。於步驟S9，偵測電路53偵測該偵測電壓Vr，此時的偵測電壓Vr會因環境溫度(或系統溫度，例如馬達驅動器10運行時的溫度)而產生變化；再者，電流感測電路54偵測馬達60的三相電流而獲得三個馬達電壓Va、Vb、Vc，此處的三個馬達電壓Va、Vb、Vc相關馬達60運作時的三相電流，此是與步驟S3不同之處。所以，ADC電路55同樣轉換偵測到的偵測電壓Vr與三個馬達電壓Va、Vb、Vc。於步驟S10微處理器56計算伺服控制前後平均偵測電壓Vr_avg與偵測電壓Vr的電壓差Vr_d=Vr_avg-Vr，且電壓差Vr_d計算兩次，即獲得第一電壓差Vr_d1與第二電壓差Vr_d2；及微處理器56計算三個馬達電壓Va、Vb、Vc各自於伺服控制前後平均馬達電壓Va_avg與馬達電壓Va的電壓差Va_d=Va_avg-Va，同理藉由運算獲得Vb_d=Vb_avg-Vb與Vc_d=Vc_avg-Vc，換言之伺服控制後調整所量測到的三個馬達電壓Va、Vb、Vc為Va_d、Vb_d與Vc_d。

再者，於步驟S11判斷第一電壓差Vr_d1與第二電壓差Vr_d2的差異是否小於一門檻電壓Vr_Cri；然而，當僅執行步驟S11至步驟S15，而無執行步驟S1至步驟S10時，步驟S11改為判斷偵測兩次的偵測電壓Vr的電壓差是否小於門檻電壓Vr_Cri，即無與平均偵測電壓Vr_avg進行比較運算，所以，當偵測兩次的偵測電壓Vr的電壓差小於門檻電壓Vr_Cri時，第二次偵測的偵測電壓Vr作為補償訊號Vcomp；當偵測兩次的偵測電壓Vr的電壓差大於門檻電壓Vr_Cri時，補償訊號Vcomp的準位等於門檻電壓Vr_Cri加上第二次偵測的偵測電壓Vr。

再者，按照第三圖的實施例，電流控制模組50偵測兩次的偵測電壓Vr與平均偵測電壓Vr_avg分別進行比較運算，獲得第一電壓差Vr_d1與第二電壓差Vr_d2，且當第一電壓差Vr_d1與第二電壓差Vr_d2的差異小於門檻電壓Vr_Cri時，補償訊號Vcomp為第二電壓差Vr_d2的準位，並於步驟S121紀錄補償訊號的準位；當第一電壓差Vr_d1與第二電壓差Vr_d2的差異大於或等於門檻電壓Vr_Cri時，補償訊號Vcomp為第二電壓差Vr_d2的準位加上門檻電壓Vr_Cri的準位，並於步驟S122紀錄補償訊號的準位。即依據第一電壓差Vr_d1與第二電壓差Vr_d2決定補償訊號Vcomp，並依據補償訊號Vcomp再次調整三個馬達電壓Va_d、Vb_d、Vc_d後產生馬達控制電壓V52。換言之，利用補償訊號Vcomp再次校正三個馬達電壓Va_d、Vb_d、Vc_d為Va_real、Vb_real、Vc_real後，於步驟S14微處理器56按照伺服控制演算法運算校正後的三個馬達電壓Va_real、Vb_real、Vc_real，而產生多個馬達控制電壓V52。最後於步驟S15結束在線補償程序2。

另外，於步驟S121與步驟S122，微處理器56判斷補償訊號Vcomp的準位大於等於一補償上限值時，馬達驅動器10會執行一定時回復程序，以重新設定補償訊號的準位。或者，如第三圖實施例，在線補償程序2運作時皆會自動進入定時回復程序，以自動計數次數或時間，當超過一設定次數或一設定時間後，重新設定補償訊號的準位，避免補償訊號的準位過大或過小，此時，亦可以藉機檢測是否硬體存在故障，導致補償訊號的準位異常。因此，於步驟S121與步驟S122後，亦會進入步驟S20定時回復程序，並於步驟S21先清除計數器，再開始計時。於步驟S22判斷計數器計數的時間T是否超過一設定時間T_Cri，若無則重新執行步驟S22。若已超過設定時間T_Cri，則重新設定補償訊號Vcomp，並至初始校正程序1重新獲得馬達驅動器10內相關的初始值，爾後進行在線補償程序2，其餘內容相似不再覆述。另外，在不同的實施例中，時間T可以是補償訊號Vcomp的準位，而設定時間T_Cri可以改為判斷補償訊號Vcomp準位大小的數值。即馬達驅動器10執行定時回復程序，判斷補償訊號Vcomp的準位，當補償訊號Vcomp的準位大於等於補償上限值時，重新設定補償訊號Vcomp的準位。

此外，當馬達驅動器10從步驟S22運行至步驟S1初始校正程序1時，產線會依據馬達驅動器10所發出的訊息而先自行主動停機，之後在工作人員重啟產線，馬達驅動器10才重新獲得馬達驅動器10內相關的初始值。然而，在馬達驅動器10堪用的情況下，第三圖實施例可以修改為先對工作人員提出警示，讓工作人員決定產線停機的時機，換言之步驟S22先運行至警示階段，而非運行至步驟S1的初始校正程序1，其為實施例可變化之態樣，非本發明所限。

請參閱第四圖，其為本發明偵測電壓隨馬達驅動器運作時間而上升之一實施例的波形圖。如圖所示，當馬達驅動器10開始運作時，偵測電壓Vr的準位隨第二圖參考電壓Ref V的準位上升而跟著上升，且偵測電壓Vr的準位會隨著環境溫度上下偏移。換言之，在剛開機時偵測電壓Vr的電壓差Vr_d在0-20之間飄移，而在馬達驅動器10穩定工作後，偵測電壓Vr的電壓差Vr_d約在60上下飄移。因此，馬達驅動器10的電流控制模組50可以利用偵測電壓Vr的變化，補償環境溫度對系統(即馬達驅動器10)的影響，提升馬達驅動器10的控制精準度。

請參閱第五圖，其為本發明驅使馬達減緩振動現象前後之一實施例的比較圖。如圖所示，無利用補償訊號Vcomp的馬達電壓Va(或b、c)振動幅度在約677至657之間，而依據補償訊號Vcomp控制馬達60後，馬達電壓Va(或b、c)振動幅度約在672至664之間。兩者相比約減緩50%的振動現象。

綜上所述，本發明馬達驅動器包含一電流控制模組，偵測兩次一偵測電壓，並依據兩次的偵測電壓決定一補償訊號，且依據補償訊號輸出一馬達控制電壓。

或者，於馬達驅動器未執行一伺服控制時，電流控制模組提供數次偵測電壓與提供數次一馬達電壓，並經由運算數次的該些偵測電壓而獲得一平均偵測電壓，及依據數次的該些馬達電壓進行運算而獲得一平均馬達電壓。於馬達驅動器從未執行伺服控制轉變為執行伺服控制時，偵測馬達電流而獲得一馬達電壓，並依據平均馬達電壓調整馬達電壓。爾後，電流控制模組偵測兩次偵測電壓，並依據兩次的偵測電壓分別與平均偵測電壓比較後決定補償訊號，且依據補償訊號再次調整馬達電壓後產生馬達控制電壓。

上述兩種實施方式皆可以及時偵錯與動態補償馬達電流，而提升控制馬達電流的準確度，達到馬達的振動減少。而且，應用於機械手臂亦可以減少手臂的振動現象。

以上所述者，僅為用以方便說明本發明之實施例，本發明之範圍不限於該等實施例，凡依本發明所做的任何變更，於不脫離本發明之精神下，皆屬本發明申請專利之範圍。