專利名稱：一種氣浮臺(tái)抓捕試驗(yàn)機(jī)械臂控制方法
技術(shù)領(lǐng)域：
本發(fā)明屬于航天器地面物理試驗(yàn)技術(shù)領(lǐng)域，涉及一種氣浮臺(tái)抓捕試驗(yàn)機(jī)械臂控制方法。
背景技術(shù)：
隨著我國航天事業(yè)的發(fā)展，由于燃料耗盡等技術(shù)性原因，空間失效衛(wèi)星增多，軌道資源和新航天任務(wù)需求的矛盾越來越突出，因此空間在軌服務(wù)成為未來航天事業(yè)發(fā)展的重要方向。由于失效衛(wèi)星一般不能提供合作對接姿態(tài)，也沒有合作對接機(jī)構(gòu)，因此基于多自由度機(jī)械臂的空間捕獲和操作控制是很好的選擇。鑒于硬件和軟件算法復(fù)雜程度高，需要在地面開展相關(guān)關(guān)鍵技術(shù)的試驗(yàn)驗(yàn)證。在花崗巖平臺(tái)開展的基于兩個(gè)氣浮臺(tái)的超近距離交會(huì)和機(jī)械臂抓捕試驗(yàn)是驗(yàn)證超近距離相對運(yùn)動(dòng)導(dǎo)航、制導(dǎo)和精確控制技術(shù)的重要手段。然而，花崗巖臺(tái)面的非理想狀態(tài)和推力器的滯環(huán)效應(yīng)，使得抓捕前兩氣浮臺(tái)相對狀態(tài)保持精度不高。其次，機(jī)械臂抓捕運(yùn)動(dòng)過程對氣浮臺(tái)帶來干擾力和干擾力矩，使得兩個(gè)氣浮臺(tái)相對狀態(tài)在抓捕過程中不斷變化，目標(biāo)抓捕部位隨之變化。上述兩點(diǎn)都會(huì)引起兩氣浮臺(tái)相對距離和相對姿態(tài)與標(biāo)稱狀態(tài)相比存在較大偏差，如果采用地面一次規(guī)劃機(jī)械臂軌跡的方法，則不能完成對目標(biāo)氣浮臺(tái)的可靠抓捕，因此對抓捕機(jī)械臂的自適應(yīng)性提出了很高要求。由于氣浮臺(tái)是漂浮基座且抓捕機(jī)構(gòu)末端采用夾持連接方式，機(jī)械臂與目標(biāo)氣浮臺(tái)接觸后，如果徑向不加控制，沒有足夠摩擦力，容易分離。此外，由于雙臂運(yùn)動(dòng)不能完全同步，可能出現(xiàn)單臂與目標(biāo)氣浮臺(tái)接觸的現(xiàn)象，發(fā)生彈離或者反復(fù)碰撞，大大影響抓捕的可靠性。

發(fā)明內(nèi)容
本發(fā)明所要解決的技術(shù)問題是提供一種氣浮臺(tái)抓捕試驗(yàn)機(jī)械臂控制方法，在兩氣浮臺(tái)相對距離和相對姿態(tài)與標(biāo)稱狀態(tài)相比存在較大偏差的條件下仍然能夠保證對選定目標(biāo)特征部位的可靠抓捕，并且能夠保證不發(fā)生機(jī)械臂與目標(biāo)氣浮臺(tái)結(jié)構(gòu)的脫離。本發(fā)明包括如下技術(shù)方案一種氣浮臺(tái)抓捕試驗(yàn)機(jī)械臂控制方法，所述機(jī)械臂為兩個(gè)多自由度機(jī)械臂，步驟如下(I)根據(jù)相對測量敏感器輸出的相對距離確定是否進(jìn)行預(yù)抓捕，當(dāng)追蹤氣浮臺(tái)與目標(biāo)氣浮臺(tái)之間的相對距離小于預(yù)抓捕設(shè)定距離時(shí)，兩個(gè)機(jī)械臂進(jìn)行預(yù)抓捕，將機(jī)械臂伸展到目標(biāo)氣浮臺(tái)大包絡(luò)范圍；(2)采集相對測量敏感器輸出的相對距離和相對姿態(tài)信息，如果相對距離和相對姿態(tài)在機(jī)械臂抓捕有效作用范圍內(nèi)，則根據(jù)相對測量敏感器輸出的相對距離和相對姿態(tài)信息計(jì)算每個(gè)機(jī)械臂目標(biāo)特征抓捕部位坐標(biāo)；根據(jù)每個(gè)機(jī)械臂目標(biāo)特征抓捕部位坐標(biāo)和運(yùn)動(dòng)學(xué)逆解算法解算每個(gè)機(jī)械臂各關(guān)節(jié)目標(biāo)轉(zhuǎn)角；根據(jù)各關(guān)節(jié)目標(biāo)轉(zhuǎn)角與當(dāng)前關(guān)節(jié)轉(zhuǎn)角確定各個(gè)關(guān)節(jié)期望運(yùn)動(dòng)軌跡；然后開始進(jìn)行第一時(shí)間間隔的計(jì)時(shí)；
(3)對于每個(gè)機(jī)械臂，通過機(jī)械臂關(guān)節(jié)位置和速度閉環(huán)控制各關(guān)節(jié)沿期望運(yùn)動(dòng)軌跡運(yùn)動(dòng)；(4)對于每個(gè)機(jī)械臂，根據(jù)安裝在機(jī)械臂作用末端的觸碰傳感器輸出的信號(hào)判斷機(jī)械臂是否與目標(biāo)氣浮臺(tái)接觸，如果機(jī)械臂接觸目標(biāo)氣浮臺(tái)，則轉(zhuǎn)入步驟(5)，否則判斷是否到達(dá)設(shè)定的第一時(shí)間間隔，如果到達(dá)第一時(shí)間間隔則轉(zhuǎn)入步驟(2)，如果沒有到達(dá)第一時(shí)間間隔則轉(zhuǎn)入步驟(3)；(5)根據(jù)機(jī)械臂作用末端安裝的壓力傳感器輸出的信號(hào)進(jìn)行力閉環(huán)控制，使得機(jī)械臂目標(biāo)特征抓捕部位與目標(biāo)氣浮臺(tái)的接觸力恒定。所述多自由度機(jī)械臂為三自由度機(jī)械臂，該三自由度機(jī)械臂包括三個(gè)連桿和三個(gè)關(guān)節(jié)，其中第一連桿、第二連桿的長度為11，12 ;第三個(gè)連桿長度為O ;所述步驟(2)中的運(yùn)
動(dòng)學(xué)逆解算法如下
權(quán)利要求
1.一種氣浮臺(tái)抓捕試驗(yàn)機(jī)械臂控制方法，所述機(jī)械臂為兩個(gè)多自由度機(jī)械臂，其特征在于，步驟如下 (1)根據(jù)相對測量敏感器輸出的相對距離確定是否進(jìn)行預(yù)抓捕，當(dāng)追蹤氣浮臺(tái)與目標(biāo)氣浮臺(tái)之間的相對距離小于預(yù)抓捕設(shè)定距離時(shí)，兩個(gè)機(jī)械臂進(jìn)行預(yù)抓捕，將機(jī)械臂伸展到目標(biāo)氣浮臺(tái)大包絡(luò)范圍； (2)采集相對測量敏感器輸出的相對距離和相對姿態(tài)信息，如果相對距離和相對姿態(tài)在機(jī)械臂抓捕有效作用范圍內(nèi)，則根據(jù)相對測量敏感器輸出的相對距離和相對姿態(tài)信息計(jì)算每個(gè)機(jī)械臂目標(biāo)特征抓捕部位坐標(biāo)；根據(jù)每個(gè)機(jī)械臂目標(biāo)特征抓捕部位坐標(biāo)和運(yùn)動(dòng)學(xué)逆解算法解算每個(gè)機(jī)械臂各關(guān)節(jié)目標(biāo)轉(zhuǎn)角；根據(jù)各關(guān)節(jié)目標(biāo)轉(zhuǎn)角與當(dāng)前關(guān)節(jié)轉(zhuǎn)角確定各個(gè)關(guān)節(jié)期望運(yùn)動(dòng)軌跡；然后開始進(jìn)行第一時(shí)間間隔的計(jì)時(shí)； (3)對于每個(gè)機(jī)械臂，通過機(jī)械臂關(guān)節(jié)位置和速度閉環(huán)控制各關(guān)節(jié)沿期望運(yùn)動(dòng)軌跡運(yùn)動(dòng)； (4)對于每個(gè)機(jī)械臂，根據(jù)安裝在機(jī)械臂作用末端的觸碰傳感器輸出的信號(hào)判斷機(jī)械臂是否與目標(biāo)氣浮臺(tái)接觸，如果機(jī)械臂接觸目標(biāo)氣浮臺(tái)，則轉(zhuǎn)入步驟(5)，否則判斷是否到達(dá)設(shè)定的第一時(shí)間間隔，如果到達(dá)第一時(shí)間間隔則轉(zhuǎn)入步驟(2)，如果沒有到達(dá)第一時(shí)間間隔則轉(zhuǎn)入步驟(3)； (5)根據(jù)機(jī)械臂作用末端安裝的壓力傳感器輸出的信號(hào)進(jìn)行力閉環(huán)控制，使得機(jī)械臂目標(biāo)特征抓捕部位與目標(biāo)氣浮臺(tái)的接觸力恒定。
2.根據(jù)權(quán)利要求1所述的控制方法，其特征在于，所述多自由度機(jī)械臂為三自由度機(jī)械臂，該三自由度機(jī)械臂包括三個(gè)連桿和三個(gè)關(guān)節(jié)，其中第一連桿、第二連桿的長度為I1,I2 ;第三個(gè)連桿長度為O ;所述步驟(2)中的運(yùn)動(dòng)學(xué)逆解算法如下
3.根據(jù)權(quán)利要求1所述的控制方法，其特征在于，所述步驟(3)的力閉環(huán)控制方法如下 首先由設(shè)定的抓捕末端與目標(biāo)氣浮臺(tái)期望壓力與力傳感器輸出壓力值做差，通過兩次積分及運(yùn)動(dòng)學(xué)逆解轉(zhuǎn)換得到期望的關(guān)節(jié)轉(zhuǎn)角； 然后由期望的關(guān)節(jié)轉(zhuǎn)角與碼盤測角裝置獲得的實(shí)際關(guān)節(jié)轉(zhuǎn)角做差，并通過第一 PID控制器濾波，得到期望的關(guān)節(jié)角速度； 最后對碼盤測角裝置獲得的實(shí)際關(guān)節(jié)轉(zhuǎn)角進(jìn)行差分獲得實(shí)際關(guān)節(jié)角速度，對期望關(guān)節(jié)角速度與實(shí)際關(guān)節(jié)角速度做差，通過第二 PID控制器調(diào)整得到機(jī)械臂各關(guān)節(jié)電機(jī)的驅(qū)動(dòng)指令；最后通過各關(guān)節(jié)電機(jī)驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。
全文摘要
本發(fā)明公開了一種氣浮臺(tái)抓捕試驗(yàn)機(jī)械臂控制方法，包括基于運(yùn)動(dòng)學(xué)逆解迭代規(guī)劃的機(jī)械臂抓捕和力閉環(huán)抓捕控制過程，基于運(yùn)動(dòng)學(xué)逆解迭代規(guī)劃的機(jī)械臂抓捕過程根據(jù)不斷更新的兩氣浮臺(tái)相對距離和相對姿態(tài)變化，迭代更新兩個(gè)多自由度機(jī)械臂各關(guān)節(jié)目標(biāo)轉(zhuǎn)角，在線規(guī)劃軌跡直到可靠連接，從而提高了機(jī)械臂抓捕的自適應(yīng)能力，使得目標(biāo)特征部位抓捕冗余范圍擴(kuò)大；在觸碰傳感器檢測到有效信號(hào)后，即由關(guān)節(jié)位置和速度閉環(huán)回路切換為機(jī)械臂力閉環(huán)控制過程，避免了機(jī)械臂對氣浮臺(tái)的硬觸碰，引起彈離。本發(fā)明的控制方法能夠保證機(jī)械臂抓捕連接的可靠性。
文檔編號(hào)B64G4/00GK103010491SQ20121050443
公開日2013年4月3日 申請日期2012年11月30日 優(yōu)先權(quán)日2012年11月30日
發(fā)明者朱志斌, 何英姿, 魏春嶺, 唐強(qiáng), 張勇智 申請人:北京控制工程研究所