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

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

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

1 模型搭建

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

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

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

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

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

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

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

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

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

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

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

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

式中：JF為雅可比矩陣；e為伸縮連桿的長(zhǎng)度方向的方向矢量；p為伸縮連桿與機(jī)械臂基座的鉸接點(diǎn)相對(duì)機(jī)械臂基座的位置矢量。

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

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

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

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

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

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

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

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

2 仿真分析

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

圖1 x 軸懸停控制結(jié)果

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

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

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

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

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

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

由圖1～圖6 可知，基于模型預(yù)測(cè)控制的軌跡跟蹤控制器控制效果較好，與真實(shí)值間的誤差較小?？刂屏杂啥任恢米藨B(tài)補(bǔ)償裝置對(duì)機(jī)械臂基座進(jìn)行補(bǔ)償，能夠保證機(jī)械臂基座的穩(wěn)定性，提高了飛行機(jī)器人機(jī)械臂末端的控制精度，可以滿足工程應(yīng)用設(shè)計(jì)要求，具有一定工程意義和研究?jī)r(jià)值。

3 結(jié)語(yǔ)

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