盧利中 丁 偉 劉 旭 關(guān)瀟卓 王景平

（國網(wǎng)吉林省電力有限公司吉林供電公司，吉林 吉林 132012）

目前，作業(yè)型飛行機器人一般由多旋翼飛行機器人、多關(guān)節(jié)機械臂和末端執(zhí)行機構(gòu)組成，具有結(jié)構(gòu)緊湊、工作空間大以及運動靈活等特點[1-2]。飛行機器人完成抓取作業(yè)的作業(yè)流程可以分為4 個階段，即巡航飛行、目標(biāo)接近、懸停作業(yè)和負載返航[3]。在完成懸停作業(yè)的過程中，由于存在多種不確定性的影響因素（例如多旋翼飛行機器人本身動力學(xué)模型具有高度非線性、時變性和不確定性，機械臂運動過程中存在慣性參數(shù)變化（例如重心變化）、風(fēng)場擾動等因素[4]），因此會導(dǎo)致多旋翼飛行機器人的位姿穩(wěn)定性較差，使機械臂的基座位姿不穩(wěn)定，從而降低機械臂末端位姿的控制精度，容易導(dǎo)致抓取作業(yè)失敗[5-6]。針對該問題，該文設(shè)計了一種飛行機器人控制方法，可以提高機械臂末端的位姿控制精度。

1 模型搭建

該飛行機器人控制方法包括以下5 個步驟：1）控制飛行機器人飛行至參考懸停點，并啟動機械臂完成抓取作業(yè)，以抓取空中物體。2）在完成抓取的作業(yè)過程中，獲取多旋翼飛行平臺的第一運動學(xué)信息，并獲取機械臂的第二運動學(xué)信息。3）根據(jù)第一運動學(xué)信息和第二運動學(xué)信息計算機械臂基座受到的擾動力和擾動力矩。4）根據(jù)擾動力和擾動力矩計算六自由度位姿補償裝置須付出的補償力。5）控制六自由度位姿補償裝置輸出補償力，以補償機械臂基座的位姿擾動。

其中，第一個步驟又包括以下內(nèi)容：控制飛行機器人進行巡航飛行，以搜索空中物體。在搜索到空中物體后，控制飛行機器人接近空中物體。當(dāng)空中物體進入機械臂的作業(yè)范圍內(nèi)時，進入懸停作業(yè)狀態(tài)并啟動機械臂完成抓取作業(yè)，以抓取空中物體。通過分階段飛行控制，可以根據(jù)不同階段的特點采用不同的導(dǎo)航方式，有利于提高抓取空中物體的效率和成功率。當(dāng)空中物體進入機械臂的作業(yè)范圍內(nèi)時，進入懸停作業(yè)狀態(tài)，在開始進行位姿穩(wěn)定控制的同時進行抓取作業(yè)，有利于提高機械臂成功抓取空中物體的可靠性。

第三個步驟包括以下內(nèi)容：根據(jù)第一運動學(xué)信息計算多旋翼飛行平臺引起的第一擾動力和第一擾動力矩，根據(jù)第二運動學(xué)信息計算機械臂運動引起的第二擾動力和第二擾動力矩。第一運動學(xué)信息包括多旋翼飛行平臺的第一位姿加速度，第一位姿加速度包括多旋翼飛行平臺3 個軸向的平移加速度和3 個軸向的角加速度。

根據(jù)第一運動學(xué)信息計算多旋翼飛行平臺引起的第一擾動力和第一擾動力矩[7]，如公式（1）所示。

式中：F1為第一擾動力；M1為第一擾動力矩；muav為多旋翼飛行平臺的質(zhì)量；Iuav為多旋翼飛行平臺的慣性張量；auav為多旋翼飛行平臺的平移加速度；ζx、ζy和ζz分別為多旋翼飛行平臺的3 個軸向的平移加速度；αuav為多旋翼飛行平臺的角加速度；ζφ、ζθ和ζφ分別為多旋翼飛行平臺的3 個軸向的角加速度。

第二運動學(xué)信息包括機械臂各關(guān)節(jié)的轉(zhuǎn)動角度，根據(jù)第二運動學(xué)信息計算機械臂運動引起的第二擾動力和第二擾動力矩。首先，根據(jù)機械臂各關(guān)節(jié)的轉(zhuǎn)動角度計算機械臂各關(guān)節(jié)相對機械臂基座的質(zhì)心位置向量。根據(jù)公式（2）計算機械臂相對機械臂基座的總質(zhì)心位置矢量[8]。

其次，根據(jù)公式（3）計算第二擾動力和第二擾動力矩[9]。

六自由度位姿補償裝置包括與多旋翼飛行平臺固定連接的固定臺和6 根連接在固定臺和機械臂基座之間的伸縮連桿，伸縮連桿的兩端分別通過萬向鉸鏈與固定臺和機械臂基座連接；第二運動學(xué)信息包括機械臂基座的第二位姿加速度。

第四個步驟包括以下內(nèi)容：獲取各伸縮連桿的桿長；獲取機械臂基座受到的廣義重力；獲取機械臂基座相對多旋翼飛行平臺的相對角速度；根據(jù)第二位姿加速度、桿長、第一擾動力、第一擾動力矩、第二擾動力、第二擾動力矩、廣義重力和相對角速度，采用基于Newton-Euler 方程的動力學(xué)模型計算各伸縮連桿的驅(qū)動力[10]。

通過該方式控制各伸縮連桿的驅(qū)動力，可以實現(xiàn)對機械臂基座位姿的動態(tài)補償，從而有利于降低機械臂末端跟蹤誤差，達到穩(wěn)定作業(yè)的目的，能夠提高空中機械臂的作業(yè)準(zhǔn)確性、穩(wěn)定性和環(huán)境適應(yīng)性。

根據(jù)第二位姿加速度、桿長、第一擾動力、第一擾動力矩、第二擾動力、第二擾動力矩、廣義重力和相對角速度，采用基于Newton-Euler 方程的動力學(xué)模型計算各伸縮連桿的驅(qū)動力。

根據(jù)各伸縮連桿的桿長計算各伸縮連桿在長度方向的方向矢量，根據(jù)公式（4）計算雅可比矩陣。

式中：JF為雅可比矩陣；e為伸縮連桿的長度方向的方向矢量；p為伸縮連桿與機械臂基座的鉸接點相對機械臂基座的位置矢量。

根據(jù)公式（5）計算各伸縮連桿的驅(qū)動力。

式中：Fs為驅(qū)動力矩陣；Ib為機械臂基座的慣性張量；q為機械臂基座的第二位姿加速度；Gb為機械臂基座受到的廣義重力；ωb為相對角速度；I3為3×3 階的單位矩陣；mb為機械臂基座本身的質(zhì)量。

對六自由度位姿補償裝置的第i個伸縮連桿來說，其上、下2 個鉸接點在機械臂基座坐標(biāo)系下的位置分別為Bpi1、Bpi2。由于下鉸接點在機械臂基座上的位置是固定且已知的，因此Bpi2是已知的，上鉸接點在多旋翼飛行平臺上的位置是固定且已知的，可以根據(jù)各伸縮連桿的長度計算多旋翼飛行平臺和機械臂基座坐標(biāo)系之間的位姿轉(zhuǎn)換矩陣，從而根據(jù)該位姿轉(zhuǎn)換矩陣和上鉸接點在多旋翼飛行平臺上的位置計算得到Bpi1，進而可以根據(jù)公式（6）計算各伸縮連桿的長度方向的方向矢量。

機械臂基座的Newton-Euler 方程如公式（7）所示。

式中：ab為機械臂基座的平移加速度，包括世界坐標(biāo)系在3 個軸向的平移加速度；Mb為機械臂基座受到的力矩；αb為機械臂基座相對多旋翼飛行平臺的角加速度；Fbi為第i個伸縮連桿的驅(qū)動力。

機械臂基座的動力學(xué)模型如公式（8）所示。

式中：為機械臂基座的位姿變化速度（可以通過機械臂基座上的傳感器測得）；Cb為機械臂基座的向心力和科氏力的系數(shù)矩陣。

在實際應(yīng)用中，由于多旋翼飛行平臺的轉(zhuǎn)動角速度較小，因此機械臂基座的向心力和科氏力可以忽略不計，從而公式（8）可以簡化為公式（9）。

2 仿真分析

當(dāng)該文設(shè)計的基于模型預(yù)測控制的軌跡跟蹤控制器控制機械臂完成抓取作業(yè)時，控制其飛行至參考懸停點，啟動機械臂進行抓取作業(yè)，根據(jù)不同階段的特點采用不同的導(dǎo)航方式，有利于提高抓取空中物體的效率和成功率。為了驗證基于模型預(yù)測控制的軌跡跟蹤控制器的跟蹤控制效果，搭建機械臂大機動條件下旋翼平臺懸停控制的仿真試驗場景：總仿真時間為50 s，在整個模擬期間，機械臂關(guān)節(jié)以0.05 rad/s2的恒定加速度旋轉(zhuǎn)，并且旋翼平臺的預(yù)期軌跡是保持懸停。x軸懸?？刂平Y(jié)果、y軸懸停控制結(jié)果和z軸懸?？刂平Y(jié)果分別如圖1～圖3所示。

圖1 x 軸懸?？刂平Y(jié)果

圖2 y 軸懸?？刂平Y(jié)果

圖3 z 軸懸停控制結(jié)果

設(shè)計和模擬末端執(zhí)行器受到顯著外力時旋翼平臺的軌跡跟蹤控制。試驗場景如下：總模擬時間為50 s，在模擬時間段，1 N 的外力在x方向上施加到末端器，同時y方向受到摩擦系數(shù)為0.25 的壁面反作用摩擦力，并期望帶臂無人機系統(tǒng)在末端器受力時沿y方向均勻移動。受力模式下無人機軌跡跟蹤x軸結(jié)果、受力模式下無人機軌跡跟蹤y軸結(jié)果和受力模式下無人機軌跡跟蹤z軸結(jié)果分別如圖4～圖6所示。

圖4 受力模式無人機軌跡跟蹤x 軸結(jié)果

圖5 受力模式無人機軌跡跟蹤y 軸結(jié)果

圖6 受力模式無人機軌跡跟蹤z 軸結(jié)果

由圖1～圖6 可知，基于模型預(yù)測控制的軌跡跟蹤控制器控制效果較好，與真實值間的誤差較小。控制六自由度位置姿態(tài)補償裝置對機械臂基座進行補償，能夠保證機械臂基座的穩(wěn)定性，提高了飛行機器人機械臂末端的控制精度，可以滿足工程應(yīng)用設(shè)計要求，具有一定工程意義和研究價值。

3 結(jié)語

該飛行機器人控制方法通過估算機械臂基座受到的擾動力和擾動力矩來計算對該擾動力和擾動力矩進行補償時六自由度位姿補償裝置須付出的補償力，然后控制六自由度位姿補償裝置對機械臂基座的位姿進行補償，從而保證機械臂基座位姿的穩(wěn)定性，在該基礎(chǔ)上控制機械臂進行抓取作業(yè)，可以提高機械臂末端的位姿控制精度。