【正文】 助推器 (b)兩足步行機(jī)器人倒立擺的研究具有重要的工程背景，其控制方法在軍工、航天、機(jī)器人和一般工業(yè)過(guò)程領(lǐng)域中都有著廣泛的用途，如兩輪電動(dòng)車的平衡控制、機(jī)器人行走過(guò)程中的平衡控制、火箭發(fā)射中的垂直度控制和衛(wèi)星飛行中的姿態(tài)控制等均涉及到倒置問(wèn)題。最常見的噪聲干擾比如:收音機(jī)遠(yuǎn)離發(fā)射臺(tái)時(shí)發(fā)出的雜音，電視屏幕上的雪花點(diǎn)，或是引起模/數(shù)轉(zhuǎn)換器轉(zhuǎn)換錯(cuò)誤的信號(hào)等等。通過(guò)對(duì)一系列帶有觀測(cè)噪聲和干擾信號(hào)的實(shí)際觀測(cè)數(shù)據(jù)的處理，從中得到所需要的各種參量的估計(jì)值，這就是估計(jì)問(wèn)題[3]。由于卡爾曼濾波器采用遞推形式，數(shù)據(jù)量小，易于計(jì)算機(jī)實(shí)現(xiàn)，所以一經(jīng)提出便在各行各業(yè)得到了廣泛應(yīng)用，是當(dāng)今被研究最多的濾波算法之一。當(dāng)隨機(jī)噪聲干擾存在于系統(tǒng)裝置或系統(tǒng)觀測(cè)通道中時(shí)，就不能使用確定性系統(tǒng)情況下的觀測(cè)器進(jìn)行狀態(tài)估計(jì)了，這時(shí)要使用統(tǒng)計(jì)估計(jì)方法實(shí)現(xiàn)系統(tǒng)的狀態(tài)估計(jì)。1912年，對(duì)估計(jì)理論的廣泛研究做出了又一個(gè)重大貢獻(xiàn)，但需要知道觀測(cè)數(shù)據(jù)的概率分布函數(shù)。卡爾曼濾波的實(shí)現(xiàn)是基于信號(hào)和噪聲的狀態(tài)空間模型，利用現(xiàn)時(shí)刻的觀測(cè)值，并結(jié)合前一時(shí)刻的狀態(tài)預(yù)測(cè)值，來(lái)更新當(dāng)前時(shí)刻的狀態(tài)變量的估計(jì)值，得出當(dāng)前時(shí)刻最優(yōu)的狀態(tài)估計(jì)值[8]?？柭鼮V波算法是一種線性狀態(tài)的最優(yōu)估計(jì)，是在系統(tǒng)狀態(tài)方程和量測(cè)方程都是線性方程，系統(tǒng)噪聲和觀測(cè)噪聲均為高斯白噪聲，且統(tǒng)計(jì)特性已知的條件下，得到最優(yōu)濾波結(jié)果。在之后的近半個(gè)世紀(jì)里，許多學(xué)者以Kalman濾波器為基礎(chǔ)進(jìn)行了推廣和改進(jìn)。EKF最初是StanleySchmidth在1967年為了將卡爾曼濾波應(yīng)用在非線性航天器導(dǎo)航問(wèn)題提出的，EKF算法是通過(guò)對(duì)非線性函數(shù)進(jìn)行Taylor展開，對(duì)二階及其高階項(xiàng)進(jìn)行忽略，實(shí)現(xiàn)非線性系統(tǒng)的線性化轉(zhuǎn)換。由于近似非線性函數(shù)不容易，而相比之下近似非線性函數(shù)的概率密度分布更容易，因此人們想到使用采樣的方法來(lái)近似非線性函數(shù)的概率密度分布來(lái)解決非線性系統(tǒng)的濾波問(wèn)題。這種濾波算法是牛津大學(xué)Julier[121,13]等學(xué)者首先提出的。欠驅(qū)動(dòng)機(jī)器人Pendubot是一個(gè)在垂直平面上運(yùn)動(dòng)的兩桿機(jī)器人，它在肩部有一個(gè)驅(qū)動(dòng)，而肘部沒有，因此Pendubot是典型的欠驅(qū)動(dòng)系統(tǒng)，同時(shí)由于Pendubot兩桿之間強(qiáng)的耦合性及加速度約束不可積，因而具有較強(qiáng)的非線性特征。直接運(yùn)用牛頓方程的力學(xué)體系也稱為矢量力學(xué)，拉格朗日動(dòng)力學(xué)則稱為分析力學(xué)。 二級(jí)倒立擺系統(tǒng)實(shí)驗(yàn)軟硬件條件。系統(tǒng)的動(dòng)力學(xué)方程（第二類拉格朗日方程）為 ()由于勢(shì)能不顯含速度項(xiàng)，因此動(dòng)力學(xué)方程也可以寫成 ()那么，對(duì)于欠驅(qū)動(dòng)機(jī)器人倒立擺機(jī)器人系統(tǒng)，其拉格朗日運(yùn)動(dòng)方程則為： () 基于倒立擺機(jī)器人的動(dòng)力學(xué)模型，其中擺臂連桿受到的力矩為，只有擺臂連桿的主動(dòng)關(guān)節(jié)受力。計(jì)算平移動(dòng)能的一般表達(dá)式為，而系統(tǒng)兩個(gè)連桿的角速度為 ()主動(dòng)臂的平移動(dòng)能可以直接表示為 ()勢(shì)能與機(jī)械臂質(zhì)心的高度有關(guān)，高度用y坐標(biāo)表示，于是勢(shì)能可以直接寫成 ()對(duì)于欠驅(qū)動(dòng)臂，先寫出質(zhì)心處笛卡兒坐標(biāo)位置的表達(dá)式，然后求微分，以便得到關(guān)節(jié)角速度。并用基于運(yùn)動(dòng)能量的拉格朗日動(dòng)力學(xué)方法建立Pendubot動(dòng)力學(xué)模型。圖 其中：為主動(dòng)臂長(zhǎng)度，為主動(dòng)臂的質(zhì)心長(zhǎng)度，為欠驅(qū)動(dòng)臂的質(zhì)心長(zhǎng)度，為主動(dòng)臂質(zhì)量，為欠驅(qū)動(dòng)臂質(zhì)量，為主動(dòng)臂質(zhì)心轉(zhuǎn)動(dòng)慣量，為欠驅(qū)動(dòng)臂質(zhì)心轉(zhuǎn)動(dòng)慣量。 能量法辨識(shí)方程建立能量法即通過(guò)能量守恒定律來(lái)建立辨識(shí)方程，是機(jī)械系統(tǒng)最常用的在線或離線辨識(shí)方法之一。令，為倒立擺機(jī)器人系統(tǒng)在時(shí)刻的總能量，為倒立擺機(jī)器人系統(tǒng)在時(shí)刻的總動(dòng)能，為倒立擺機(jī)器人系統(tǒng)在時(shí)刻的總勢(shì)能。 辨識(shí)實(shí)驗(yàn) 辨識(shí)程序?qū)崿F(xiàn)根據(jù)辨識(shí)方程可知，當(dāng)方程組中線性無(wú)關(guān)方程數(shù)大于需辨識(shí)參數(shù)個(gè)數(shù)時(shí)，通過(guò)最小二乘方法即可獲得最優(yōu)解，同時(shí)所需辨識(shí)參數(shù)為正常數(shù)，所以我們最終采用離線的非負(fù)最小二乘辨識(shí)系統(tǒng)參數(shù)。參數(shù)辨識(shí)程序的實(shí)現(xiàn)需要經(jīng)過(guò)兩個(gè)步驟：數(shù)據(jù)采集和參數(shù)辨識(shí)程序。%第一個(gè)采樣周期主動(dòng)臂的速度值dq1 (2)=0。 %第三個(gè)采樣周期欠驅(qū)動(dòng)臂的速度值dq1 (3)=0。dL3=(cos(q2).*(dq1.^2+dq1.*dq2))。for i=1:(length(dL1)10), DL(i,1)=dL1(i+10)dL1(i)。 DL(i,5)=dL5(i+10)dL5(i)。表 實(shí)驗(yàn)參數(shù)設(shè)置激勵(lì)信號(hào)幅值采樣周期編碼器精度5毫秒1250*4 脈沖/圈系統(tǒng)常數(shù)如下：。運(yùn)行程序，保存實(shí)驗(yàn)曲線，將所需數(shù)據(jù)存在Workplace中，運(yùn)行非負(fù)最小二乘辨識(shí)程序()，獲得參數(shù)辨識(shí)結(jié)果。LQR理論是現(xiàn)代控制理論中發(fā)展最早的、也是最為成熟的一種狀態(tài)空間設(shè)計(jì)法。線性二次型問(wèn)題的最優(yōu)解可以寫成統(tǒng)一的解析表達(dá)式和實(shí)現(xiàn)求解過(guò)程的規(guī)范化，并可簡(jiǎn)單地采用狀態(tài)線性反饋控制律構(gòu)成閉環(huán)最優(yōu)控制系統(tǒng)，能夠兼顧多項(xiàng)性能指標(biāo)，因此得到特別的重視，為現(xiàn)代控制理論中發(fā)展較為成熟的一部分。不過(guò)，這里也可以使用Matlab指令來(lái)直接得到值。為設(shè)計(jì)平衡控制器，首先必須將倒立擺機(jī)器人的非線性運(yùn)動(dòng)方程線性化，同時(shí)設(shè)計(jì)一個(gè)具有線性模型的全狀態(tài)反饋控制器。平衡控制器是將倒立擺機(jī)器人控制在不穩(wěn)定平衡點(diǎn)，因此為0，因此我們所要做的是找到偏微分矩陣并求其在平衡點(diǎn)的值。本實(shí)驗(yàn)中的輸出量可以認(rèn)為是，且。操作步驟如下：(1) 打開Easy Control軟件，并連接控制器。保存以上編輯好的程序，并編譯下載至控制器中，打開觀測(cè)相應(yīng)信號(hào)的示波器。 a 下平衡點(diǎn)控制主動(dòng)臂位置q1曲線 b 下平衡點(diǎn)控制欠驅(qū)動(dòng)臂位置q2曲線圖 a 上平衡點(diǎn)控制主動(dòng)臂位置q1曲線 b 上平衡點(diǎn)控制欠驅(qū)動(dòng)臂位置q2曲線圖 實(shí)際實(shí)驗(yàn)效果 下平衡點(diǎn)平衡控制將上面所得到的下平衡點(diǎn)參數(shù)下載至Simulink程序中（）中，主要功能為將編碼器返回的脈沖信號(hào)轉(zhuǎn)化為我們需要的角位移信號(hào)，為接下來(lái)的控制提供所必須的實(shí)時(shí)信號(hào)。觀察Pendubot的平衡情況，然后施加擾動(dòng)，分別記錄下正常運(yùn)行和施加擾動(dòng)時(shí)的波形。觀察Pendubot的平衡情況。若初始狀態(tài)與平衡點(diǎn)偏離較大，控制效果會(huì)比較差，甚至無(wú)法控制。并且系統(tǒng)噪聲序列和觀測(cè)噪聲序列、都與不相關(guān)，即: () ()從方程的形式可以看出，卡爾曼濾波器在一周期內(nèi)的計(jì)算過(guò)程可以分為兩步驟:時(shí)間更新與測(cè)量更新。這樣反復(fù)循環(huán)，從1時(shí)刻到k時(shí)刻，便能充分合理的利用觀測(cè)值Z。重寫上述推導(dǎo)的倒立擺狀態(tài)空間方程如下: ()其中 要設(shè)計(jì)出完整的濾波器，還需要確定幾個(gè)濾波參數(shù)與迭代初值。選用LQR控制器，該控制器的輸出即控制量u作為指令送達(dá)電機(jī)，同時(shí)u也作為卡爾曼濾波器的一個(gè)輸入。圖 Kalman濾波結(jié)構(gòu)圖圖 Kalman濾波算法框圖此外，由于Kalman濾波算法需要一段時(shí)間的訓(xùn)練，因此需要設(shè)計(jì)一個(gè)訓(xùn)練切換開關(guān)，在訓(xùn)練一段事件后，觀測(cè)信號(hào)由帶噪聲的原信號(hào)切換至經(jīng)kalman濾波后的觀測(cè)信號(hào)。圖 濾波前后q1,q2的對(duì)比圖 控制器輸出信號(hào) 小結(jié)在實(shí)際工作條件下，機(jī)械存在著噪聲干擾。在實(shí)際的使用過(guò)程中發(fā)現(xiàn)，控制器模型參數(shù)不精確對(duì)于卡爾曼濾波器的濾波效果的影響非常大；并且卡爾曼濾波器需要一小段時(shí)間進(jìn)行訓(xùn)練，訓(xùn)練時(shí)間與采樣時(shí)間和初始參數(shù)等因素有關(guān)。然后，提出了利用物理學(xué)守恒定律建模的方法，此種方法推導(dǎo)過(guò)程簡(jiǎn)單直觀，且能夠揭示出數(shù)學(xué)模型的物理含義。本文在控制器設(shè)計(jì)中選用基于能量的系統(tǒng)參數(shù)在線測(cè)定法得到的參數(shù)。（4） 基于卡爾曼濾波器對(duì)電位器擺角信號(hào)進(jìn)行檢測(cè)，實(shí)現(xiàn)了對(duì)pendubot系統(tǒng)擺角信號(hào)較為準(zhǔn)確的估計(jì)。（2） 卡爾曼濾波（KF）對(duì)模型精準(zhǔn)度要求較高，一些對(duì)模型要求較低的新算法是進(jìn)一步的研究方向