[發(fā)明專利]一種改進卡爾曼濾波坐標分離機械手控制算法

在審

• 申請?zhí)枺?/td>: 201710558646.5
• 申請日: 2017-07-03
• 公開（公告）號: CN107315342A
• 公開（公告）日: 2017-11-03
• 發(fā)明（設計）人: 劉新福;范巖;張丙酉
• 申請（專利權）人: 河北工業(yè)大學
• 主分類號: G05B13/02
• 分類號: G05B13/02;B25J9/16
• 代理公司: 暫無信息
• 代理人: 暫無信息

技術領域

本發(fā)明屬于自動化控制領域，特別是一種基于卡爾曼濾波的機械手控制算法，具體為一種改進卡爾曼濾波坐標分離機械手控制算法。可應用于軍事，勘探，家庭護理，藥品檢測等領域。

背景技術

智能機器人技術涉及計算機應用技術、自動控制、人工智能、傳感器、運動控制等眾多學科。但是，怎樣使機器人更加自主化和智能化已得到更廣泛的應用領域是現(xiàn)代機器人研究面臨的重大難題。應用智能機器人可以幫助老年人和殘疾人來完成他們日常生活中難以做到的一些操作，解決他們生活上的困難。而在工業(yè)生產和軍事領域中，機器人可以完成抓取搬運任務，可以避免在惡劣和危險環(huán)境中人工作業(yè)的危險性。

機械手的靈敏控制是智能機器人實現(xiàn)自動化非常關鍵的環(huán)節(jié)，在工業(yè)和軍事等領域中，為實現(xiàn)精確控制手臂抓取動作，本發(fā)明提出一種改進卡爾曼濾波坐標分離機械手控制算法。所述算法具有很好的實時性、反饋性、安全性，并且提高了抓取目標物體的控制精度

發(fā)明內容

本發(fā)明的目的是提供一種改進卡爾曼濾波坐標分離機械手控制算法，結合卡爾曼濾波算法，迭代算法以及坐標分離的方法提高機械手抓取目標物體的精度，增強控制過程中的可控性，減小累積誤差以及人為誤差。

卡爾曼濾波算法的核心思想是利用k-1時刻采集到的機械手位置信息，來估計k 時刻機械手的位置信息。卡爾曼濾波的算法的優(yōu)勢在于，舍掉冗余的觀測歷史數(shù)據，只采集前一時刻的數(shù)據，累積誤差會大幅度減少。系統(tǒng)中誤差估計和均方差也在時刻改變，具有非常好的時變性。應用到機械手的控制中，系統(tǒng)的運算速度和控制精度會同時得到質的改變。

機械手的k時刻位置信息由角度傳感器和加速度傳感器采集，角度傳感器噪聲偏差為w_k，加速度傳感器噪聲偏差為v_k，機械手末端軌跡s_k的狀態(tài)方程和測量方程y_k表示為：

s(k)＝As(k-1)+BU(k)+w(k)(1)

y(k)＝Hs(k)+v(k)(2)

其中，A與B為系統(tǒng)參數(shù)；H為測量系統(tǒng)參數(shù)；U(k)是k時刻對系統(tǒng)的控制量，若系統(tǒng)沒有控制量，則B＝0。

狀態(tài)預測方程：

s(k/k-1)＝As(k-1/k-1)+BU(k)(3)

式中s(k/k-1)表示由k-1時刻預測k時刻的機械手軌跡，s(k-1/k-1)表示k-1 時刻最優(yōu)估計值。

預測均方差方程：

P(k/k-1)＝AP(k-1/k-1)A^T+Q(4)

式中P(k-1/k-1)表示B(k-1/k-1)對應的協(xié)方差，Q表示過程噪聲協(xié)方差狀態(tài)估計方程：

s(k/k)＝s(k/k-1)+K_s(k)[y_k-Hs(k/k-1)](5)

式中為卡爾曼增益參數(shù)，卡爾曼濾波的最優(yōu)增益方程為

K_s(k)＝P(k/k-1)H^T/(HP(k/k-1)H^T+R)(6)

估計均方差方程：

P(k/k)＝(1-K_s(k)H)P(k/k-1)(7)

經過方程式(3)-(7)的計算得出最優(yōu)的狀態(tài)估計值s(k/k)，然后將s(k/k) 的最優(yōu)狀態(tài)估計值再次作為上一時刻的采集值，即s(k-1/k-1)。利用上述方程式重新代入，計算流程中過程協(xié)方差Q和誤差估計R都會根據新采集的數(shù)據值發(fā)生變化，滿足系統(tǒng)的時變性。

卡爾曼濾波可以應用于線性系統(tǒng)和非線性系統(tǒng)，控制精度非常高，應用廣泛，但是線性卡爾曼濾波器也不是萬能的，它有一個短板就是發(fā)散。為了彌補這個缺點，這里采用迭代的控制算法使運算收斂，進一步提高控制精度。

機械手運動本身是一個連續(xù)性的動作，這里為了分析問題方便將其運動離散化，同時將運動時間分為等長的單位時間作為一個周期，在任意一個周期內機械手運算出現(xiàn)了范圍內發(fā)散，結束本周期運算，立即重新進入下一個周期的動作。迭代運算律

s_k+1(t)＝s_k(t)-L(s_k(t))e_k(t)，k＝0，1，…，t∈[0，T](8)

其中e_k(t)＝y(tǒng)_k(t/t)-y_k(t/t-1)為第k個周期的誤差，L(s_k(t))為線性學習算子。