基于pc+fpga的帶速度觀測器的有限時間穩(wěn)定性控制系統(tǒng)的制作方法
【專利摘要】本發(fā)明公開了一種基于PC+FPGA的帶速度觀測器的有限時間穩(wěn)定性控制系統(tǒng)，包括IPC(1)、FPGA運動控制器(2)、伺服驅(qū)動器(3)、工業(yè)機器人(4)、以太網(wǎng)通訊模塊(5)，F(xiàn)PGA運動控制器(2)包括插補模塊(21)、實時控制模塊(22)、編碼器模塊(23)、D/A輸出模塊(24)，在IPC上給定工業(yè)機器人的慣性參數(shù)和控制參數(shù)，F(xiàn)PGA運動控制器進行有限時間穩(wěn)定性控制算法的實時計算得出控制力矩，并轉(zhuǎn)換為電壓信號，最后通過D/A輸出模塊輸出模擬電壓信號到伺服驅(qū)動器，以力矩模式驅(qū)動工業(yè)機器人完成運動控制。本發(fā)明可同時控制3～6個伺服電機，最小采樣時間為0.01ms，瞬態(tài)響應(yīng)時間為0.5s，能夠?qū)崿F(xiàn)工業(yè)機器人高性能的運動控制要求，可用于高精度機器人的軌跡跟蹤控制。
【專利說明】基于PC+FPGA的帶速度觀測器的有限時間穩(wěn)定性控制系統(tǒng)

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于工業(yè)機器人運動控制系統(tǒng)的【技術(shù)領(lǐng)域】，涉及一種基于PC+FPGA的帶速 度觀測器的有限時間穩(wěn)定性控制系統(tǒng)。

【背景技術(shù)】
[0002] 通常，一臺工業(yè)機器人的運動控制系統(tǒng)由軌跡插補和軌跡跟蹤兩大功能模塊構(gòu) 成，而軌跡跟蹤模塊常采用獨立伺服的PID控制原理。在工業(yè)機器人高速運動的情況下，機 器人的非線性特性愈加明顯，使得傳統(tǒng)獨立伺服PID控制方法存在以下問題：速度越高，跟 蹤精度越低；瞬態(tài)響應(yīng)慢；抗干擾的魯棒性差，最終影響機器人的工作效率和工作質(zhì)量。
[0003] 為了獲得高速情況下滿意的跟蹤性能要求，需要考慮機器人的復(fù)雜動力學(xué)特性， 一些先進的控制方法相繼提出。Yu提出了一種基于終端滑模的有限時間控制方法，Su提出 了一種基于逆動力學(xué)的全局有限時間控制方法，Zhao提出了一種基于反步技術(shù)的魯棒有限 時間控制方法。他們的工作都是在提供一種先進的控制算法，卻很少關(guān)注實時性的算法實 現(xiàn)問題。
[0004] 目前的工業(yè)機器人一般采用工業(yè)級計算機IPC+DSP的串行計算架構(gòu)，其中PC完成 人機交互的功能，DSP用來處理運動控制算法的計算和一些高速I/O信號，如反饋控制等。 基于動力學(xué)的有限時間穩(wěn)定性控制需要實時計算復(fù)雜的動力學(xué)方程和分數(shù)冪函數(shù)，使得計 算量很大，尤其是被控電機較多時，在通常的PC+DSP的架構(gòu)中，在較短的采樣時間內(nèi)（如 lms)，很難完成有限時間穩(wěn)定性伺服控制的計算。而采用多片DSP的系統(tǒng)架構(gòu)，無疑增加了 系統(tǒng)成本，該種硬件架構(gòu)設(shè)計難度也大。
[0005] 另外，目前大多數(shù)機器人通常采用測速發(fā)電機獲取速度信號，這無疑增加了系統(tǒng) 成本，并嚴重影響本體結(jié)構(gòu)的緊湊性。
[0006] 上述論述內(nèi)容目的在于向讀者介紹可能與下面將被描述和/或主張的本發(fā)明的 各個方面相關(guān)的技術(shù)的各個方面，相信該論述內(nèi)容有助于為讀者提供背景信息，以有利于 更好地理解本發(fā)明的各個方面，因此，應(yīng)了解是以這個角度來閱讀這些論述，而不是承認現(xiàn) 有技術(shù)。


【發(fā)明內(nèi)容】

[0007] 本發(fā)明的目的在于避免現(xiàn)有技術(shù)中的不足而提供一種基于PC+FPGA的帶速度觀 測器的有限時間穩(wěn)定性控制系統(tǒng)，其通過FPGA的并行計算能力和有限時間穩(wěn)定性理論以 解決現(xiàn)有技術(shù)中，獨立伺服PID控制性能不高的問題，同時提高控制系統(tǒng)的實時性，充分發(fā) 揮FPGA實時數(shù)據(jù)處理的能力。
[0008] 本發(fā)明的目的通過以下技術(shù)方案實現(xiàn)：
[0009] 提供一種基于PC+FPGA的帶速度觀測器的有限時間穩(wěn)定性控制系統(tǒng)，包括IPC、 FPGA運動控制器、伺服驅(qū)動器、工業(yè)機器人、以太網(wǎng)通訊模塊，F(xiàn)PGA運動控制器包括插補模 塊、實時控制模塊、編碼器模塊、D/A輸出模塊，工業(yè)機器人的上位機在IPC上給定工業(yè)機器 人的慣性參數(shù)和控制參數(shù)，IPC通過以太網(wǎng)通訊模塊將數(shù)據(jù)傳送到FPGA運動控制器完成 動力學(xué)參數(shù)和控制參數(shù)的設(shè)置，實時控制模塊根據(jù)插補模塊提供的插補數(shù)據(jù)、編碼器模塊 提供的位置反饋信號，進行有限時間收斂速度觀測器算法的實時計算，先獲得速度信號，再 進行有限時間穩(wěn)定性控制算法的實時計算得出控制力矩，并轉(zhuǎn)換為電壓信號，最后通過D/A 輸出模塊輸出模擬電壓信號到伺服驅(qū)動器，以力矩模式驅(qū)動工業(yè)機器人完成運動控制。
[0010] 其中，在IPC上實現(xiàn)的步驟為：
[0011] (1).將插補完成的數(shù)據(jù)存放至FPGA運動控制器的FIFO區(qū)；
[0012] (2). IPC將機器人的動力學(xué)參數(shù)和控制參數(shù)通過工業(yè)以太網(wǎng)接口和集成在FPGA 硬件電路上的以太網(wǎng)接口發(fā)送至FPGA運動控制器，實現(xiàn)對FPGA運動控制器中動力學(xué)參數(shù) 和控制參數(shù)的設(shè)定；
[0013] FPGA運動控制器進行以下步驟：
[0014] (1).將插補完成后的數(shù)據(jù)傳送至FIFO進行緩沖存儲；
[0015] (2).編碼器模塊對工業(yè)機器人各軸實際運動位置光電編碼器反饋信號進行解碼， 并傳送至實時控制模塊；
[0016] (3).實時控制模塊根據(jù)FIFO區(qū)插補數(shù)據(jù)和位置反饋信號數(shù)據(jù)先進行有限時間速 度觀測器的計算，獲得速度信號，再進行有限時間穩(wěn)定性控制算法的實時計算得到控制力 矩，并轉(zhuǎn)換為電壓信號，最后通過D/A輸出模塊將模擬電壓信號實時輸出；
[0017] (4).伺服驅(qū)動器接收來自D/A輸出模塊的模擬電壓信號，采用三環(huán)控制，從里到 外分別為電流環(huán)、速度環(huán)、位置環(huán)，以力矩控制模式控制工業(yè)機器人的機械本體進行協(xié)調(diào)運 動，最終實現(xiàn)工業(yè)機器人末端位置的精確跟蹤。
[0018] 其中，所述有限時間穩(wěn)定性控制算法如下：
[0019]

【權(quán)利要求】
1. 一種基于PC+FPGA的帶速度觀測器的有限時間穩(wěn)定性控制系統(tǒng)，其特征在于：包括 IPC(1)、FPGA運動控制器（2)、伺服驅(qū)動器（3)、工業(yè)機器人（4)、以太網(wǎng)通訊模塊（5)，F(xiàn)PGA 運動控制器（2)包括插補模塊（21)、實時控制模塊（22)、編碼器模塊（23)、D/A輸出模塊 (24)，工業(yè)機器人的上位機在IPC(l)上給定工業(yè)機器人的慣性參數(shù)和控制參數(shù)，IPC(l)通 過以太網(wǎng)通訊模塊（5)將數(shù)據(jù)傳送到FPGA運動控制器（2)完成動力學(xué)參數(shù)和控制參數(shù)的 設(shè)置，實時控制模塊（22)根據(jù)插補模塊（21)提供的插補數(shù)據(jù)、編碼器模塊（23)提供的位 置反饋信號，進行有限時間收斂速度觀測器算法的實時計算，先獲得速度信號，再進行有限 時間穩(wěn)定性控制算法的實時計算得出控制力矩，并轉(zhuǎn)換為電壓信號，最后通過D/A輸出模 塊（24)輸出模擬電壓信號到伺服驅(qū)動器（3)，以力矩模式驅(qū)動工業(yè)機器人（4)完成運動控 制。
2. 根據(jù)權(quán)利要求1所述的基于PC+FPGA的帶速度觀測器的有限時間穩(wěn)定性控制系統(tǒng)， 其特征在于， 在IPC(l)上實現(xiàn)的步驟為： (1) .將插補完成的數(shù)據(jù)存放至FPGA運動控制器（2)的FIFO區(qū)； (2) . IPC(l)將機器人的動力學(xué)參數(shù)和控制參數(shù)通過工業(yè)以太網(wǎng)接口和集成在FPGA硬 件電路上的以太網(wǎng)接口發(fā)送至FPGA運動控制器（2)，實現(xiàn)對FPGA運動控制器（2)中動力學(xué) 參數(shù)和控制參數(shù)的設(shè)定； FPGA運動控制器（2)進行以下步驟： (1) .將插補完成后的數(shù)據(jù)傳送至FIFO進行緩沖存儲； (2) .編碼器模塊（23)對工業(yè)機器人各軸實際運動位置光電編碼器反饋信號進行解 碼，并傳送至實時控制模塊（22); (3) .實時控制模塊（22)根據(jù)FIFO區(qū)插補數(shù)據(jù)和位置反饋信號數(shù)據(jù)先進行有限時間速 度觀測器的計算，獲得速度信號，再進行有限時間穩(wěn)定性控制算法的實時計算得到控制力 矩，并轉(zhuǎn)換為電壓信號，最后通過D/A輸出模塊（24)將模擬電壓信號實時輸出； (4) .伺服驅(qū)動器（3)接收來自D/A輸出模塊（24)的模擬電壓信號，采用三環(huán)控制，從 里到外分別為電流環(huán)、速度環(huán)、位置環(huán)，以力矩控制模式控制工業(yè)機器人（4)的機械本體進 行協(xié)調(diào)運動，最終實現(xiàn)工業(yè)機器人末端位置的精確跟蹤。
3. 根據(jù)權(quán)利要求1或2所述的基于PC+FPGA的帶速度觀測器的有限時間穩(wěn)定性控制系 統(tǒng)，其特征在于，所述有限時間穩(wěn)定性控制算法如下：
其中
分別為當(dāng)前周期的關(guān)節(jié)位置、關(guān)節(jié)估計速度和期望加速度向量； M0(q) e RnXn為對稱正定的慣性矩陣；
為哥氏力和向心力矩陣；
為重力向量項；TeRn為各關(guān)節(jié)的力矩輸入向量
0 < α < 1，β = 20^0+1)，!^，K2為正定的對角矩陣，此采樣周期內(nèi)的跟蹤位置誤 差為：e = q-qd，跟蹤速度誤差
其中當(dāng)前速度由有限時間收斂的速度觀
測器估計得出，令
_設(shè)
為其估計值，即有
并定義估計誤差為
則有：
其中Y > 〇, U，L2為正定的對角矩陣，從而得出速度估計值； 再根據(jù)伺服驅(qū)動器輸入電壓信號與輸出力矩的線性關(guān)系λ，求出控制器輸出的模擬電 壓信號u = λ τ。
【文檔編號】G05B19/042GK104155909SQ201410443466
【公開日】2014年11月19日 申請日期:2014年9月2日 優(yōu)先權(quán)日:2014年9月2日
【發(fā)明者】劉海濤, 王貴, 俞國燕, 王宇林 申請人:廣東海洋大學(xué)