[發(fā)明專利]一種運動控制方法及裝置

有效

• 申請?zhí)枺?/td>: 201610916139.X
• 申請日: 2016-10-20
• 公開（公告）號: CN106292472B
• 公開（公告）日: 2017-07-07
• 發(fā)明（設計）人: 王常虹;夏紅偉;張寒冰;馬廣程;解偉男;溫奇詠
• 申請（專利權(quán)）人: 哈爾濱工業(yè)大學;哈爾濱工大瑞馳高新技術(shù)有限公司
• 主分類號: G05B19/042
• 分類號: G05B19/042
• 代理公司: 北京君恒知識產(chǎn)權(quán)代理事務所(普通合伙)11466
• 代理人: 黃啟行,張璐

1.一種運動控制方法，其特征在于，所述方法包括：

S1、第一信號轉(zhuǎn)換器將外部編碼器采集的差分數(shù)據(jù)信號轉(zhuǎn)換為單端數(shù)據(jù)信號，并將所述單端數(shù)據(jù)信號發(fā)送至FPGA解碼器；

S2、FPGA解碼器對所述單端數(shù)據(jù)信號進行解碼，并將解碼后的數(shù)據(jù)信號分為兩路：一路數(shù)據(jù)信號發(fā)送至ARM控制器，另一路數(shù)據(jù)信號經(jīng)第二信號轉(zhuǎn)換器轉(zhuǎn)變?yōu)椴罘中盘栃问胶筝敵鲋镣獠吭O備；

S3、ARM控制器通過主定時器模塊讀取所述解碼得到的數(shù)據(jù)信號以及上位機發(fā)送的控制參數(shù)，并根據(jù)預設的復合控制算法獲取控制輸出量，并將所述控制輸出量輸出至DA轉(zhuǎn)換器；所述復合控制算法由PID閉環(huán)控制、前饋控制構(gòu)成；

S4、DA轉(zhuǎn)換器將所述控制輸出量由數(shù)字量形式轉(zhuǎn)變?yōu)槟M量形式，然后將其輸出至電機驅(qū)動器；

其中，所述ARM控制器裝載有UCOSII操作系統(tǒng)；

其中，在步驟S3中，所述ARM控制器通過主定時器模塊讀取所述解碼得到的數(shù)據(jù)信號以及上位機發(fā)送的控制參數(shù)，具體包括：

讀取上位機發(fā)送的控制參數(shù)，并判斷ARM控制器中的控制參數(shù)是否改變；若控制參數(shù)改變，則對ARM控制器重新進行初始化；若控制參數(shù)未改變或者在重新初始化之后，讀取FPGA解碼器解碼得到的數(shù)據(jù)信號、并將其轉(zhuǎn)換為角度數(shù)據(jù)；以及

所述ARM控制器根據(jù)預設的復合控制算法獲取控制輸出量，具體包括：

根據(jù)所述角度數(shù)據(jù)獲取位置反饋數(shù)據(jù)，并對所述位置反饋數(shù)據(jù)進行一階濾波獲取速度反饋數(shù)據(jù)；

根據(jù)電機運行模式調(diào)用相應的軌跡規(guī)劃函數(shù)，計算位置期望數(shù)據(jù)；其中，所述軌跡規(guī)劃函數(shù)為梯形規(guī)劃函數(shù)或三角形規(guī)劃函數(shù)；

根據(jù)所述位置反饋數(shù)據(jù)、位置期望數(shù)據(jù)計算位置環(huán)偏差，并將所述位置環(huán)偏差輸入PID位置閉環(huán)，以獲取位置環(huán)PID控制量；

對所述位置環(huán)PID控制量進行二階濾波，獲取速度期望數(shù)據(jù)；

根據(jù)所述速度反饋數(shù)據(jù)、速度期望數(shù)據(jù)計算速度環(huán)偏差，并將所述速度環(huán)偏差輸入PID速度閉環(huán)，以獲取速度環(huán)PID控制量。

2.如權(quán)利要求1所述的方法，其中，所述一階濾波具體為：

$\mathrm{\Δ}v=\frac{s_4+s_1+3s_3+3s_2}{8};$

式中，Δv為速度反饋數(shù)據(jù)，s₁為當前時刻的角度增量，s₂為與s₁相鄰的前一時刻的角度增量、s₃為與s₂相鄰的前一時刻的角度增量、s₄為與s₃相鄰的前一時刻的角度增量。

3.如權(quán)利要求1所述的方法，其中，所述根據(jù)所述角度數(shù)據(jù)獲取位置反饋數(shù)據(jù)，具體包括：

判斷當前采集的角度數(shù)據(jù)是否正確；若當前采集的角度數(shù)據(jù)正確，則將錯誤計數(shù)清零，并將當前采集的角度數(shù)據(jù)作為當前的位置反饋數(shù)據(jù)；若當前采集的角度數(shù)據(jù)錯誤，則將錯誤計數(shù)加1，并且，在連續(xù)出錯次數(shù)小于100次時，用前一時刻的位置反饋數(shù)據(jù)作為當前的位置反饋數(shù)據(jù)。

4.如權(quán)利要求3所述的方法，其中，在當前采集的角度數(shù)據(jù)錯誤時，所述方法還包括：

若連續(xù)出錯次數(shù)大于等于100次，則令ARM控制器進入緊急停車模式，以使控制輸出量為0。

5.如權(quán)利要求1所述的方法，其中，在獲取速度反饋數(shù)據(jù)之后，且在計算位置期望數(shù)據(jù)之前，所述方法還包括：

根據(jù)所述速度反饋數(shù)據(jù)判斷電機是否超速；若電機未超速，則將超速計數(shù)清零；若電機超速，則將超速計數(shù)加1；并且，在連續(xù)超速次數(shù)大于100次時，令ARM控制器進入緊急停車模式，以使控制輸出量為0。