本發(fā)明涉及無人機(jī)，尤其涉及一種無人機(jī)及其基于視覺的抓取方法。

背景技術(shù)：

伴隨著機(jī)器人技術(shù)的迅速發(fā)展，機(jī)器人已經(jīng)在各個領(lǐng)域，如工業(yè)制造、軍事探險、以及日益興起的民用服務(wù)行業(yè)等，得到了廣泛的應(yīng)用。無論軍事應(yīng)用還是民用中，無人機(jī)運(yùn)輸有著很重要的作用，是無人機(jī)應(yīng)用的至關(guān)重要的組成部分，它們易操作，結(jié)構(gòu)獨(dú)立完整，可垂直起飛、降落、懸停于指定點(diǎn)。逐漸地，無人機(jī)機(jī)械手將不僅局限于運(yùn)輸方面，無人機(jī)機(jī)械臂系統(tǒng)將成為新一代空中操作機(jī)器人，最終將代替人類完成重復(fù)性的、高危險、復(fù)雜環(huán)境的空中交互操作。

近年來，無人機(jī)在航拍方面已經(jīng)得到了廣泛的應(yīng)用，但是遠(yuǎn)遠(yuǎn)滿足不了人們的需求。人們更需要無人機(jī)(空中機(jī)器人)能夠像人一樣用手臂去抓取環(huán)境中的物體，而無人機(jī)機(jī)械臂系統(tǒng)是一個復(fù)雜的欠驅(qū)動系統(tǒng)，無人機(jī)搭載多自由度機(jī)械臂在控制上具有極大的挑戰(zhàn)，目前世界也處于初步研究之中。而其中另外一個難點(diǎn)就是如何讓無人機(jī)進(jìn)行自主抓取。本發(fā)明正是根據(jù)這種需求提出了一種無人機(jī)機(jī)械臂基于視覺的抓取和標(biāo)定方法。目前國內(nèi)相關(guān)無人機(jī)機(jī)械臂還只有較少的相關(guān)文獻(xiàn)，雖有少部分相關(guān)無人機(jī)機(jī)械臂的發(fā)明專利，但難以找到相關(guān)無人機(jī)視覺抓取和標(biāo)定的描述。而其他相關(guān)機(jī)械臂的視覺伺服大多是基于工業(yè)機(jī)械臂的，和無人機(jī)機(jī)械臂有很大的區(qū)別。

中國發(fā)明專利《一種無人機(jī)機(jī)動目標(biāo)定位跟蹤中的視覺伺服控制方法》(公開號：cn105353772a)。該發(fā)明通過建立大地坐標(biāo)系、機(jī)體坐標(biāo)系、攝像機(jī)坐標(biāo)系、圖像坐標(biāo)系、機(jī)體大地過渡坐標(biāo)系，通過上述建立的坐標(biāo)系之間的關(guān)系，根據(jù)目標(biāo)的成像序列，進(jìn)行目標(biāo)的定位及目標(biāo)跟蹤的姿態(tài)角給定值和航線跟蹤的姿態(tài)角給定值的計(jì)算，完成視覺伺服控制。在專利cn105014687a中，僅僅使用單一固定的攝像頭，優(yōu)點(diǎn)是無需跟蹤云臺和激光測距設(shè)備，有效地降低了載荷的體積和成本，提高了偵察的隱蔽性，但由于其固定攝像頭，攝像頭不能自由活動的局限性，可視的伺服范圍或者精度上都有一定的影響。并且在專利cn105014687a中，只涉及到無人機(jī)的飛行控制，并沒有涉及無人機(jī)機(jī)械臂的伺服抓取。

技術(shù)實(shí)現(xiàn)要素：

為了解決現(xiàn)有技術(shù)中的問題，本發(fā)明提供了一種無人機(jī)及其基于視覺的抓取方法。

本發(fā)明提供了一種無人機(jī)，包括無人機(jī)機(jī)體、目標(biāo)物、機(jī)械臂、舵機(jī)控制關(guān)節(jié)和攝像頭，所述機(jī)械臂固連于所述無人機(jī)機(jī)體的下方，所述舵機(jī)控制關(guān)節(jié)連接于所述無人機(jī)機(jī)體的上方前部，所述舵機(jī)控制關(guān)節(jié)為單自由度，所述舵機(jī)控制關(guān)節(jié)與所述攝像頭連接，所述無人機(jī)機(jī)體上貼有運(yùn)動捕捉系統(tǒng)的第一定位標(biāo)簽，所述舵機(jī)控制關(guān)節(jié)、攝像頭之間貼有運(yùn)動捕捉系統(tǒng)的第二定位標(biāo)簽，所述目標(biāo)物(視覺標(biāo)靶)貼有運(yùn)動捕捉系統(tǒng)的第三定位標(biāo)簽。

作為本發(fā)明的進(jìn)一步改進(jìn)，所述第一定位標(biāo)簽為至少四個反光球體組成的剛體，所述第二定位標(biāo)簽為至少四個反光球體組成的剛體，所述第三定位標(biāo)簽為至少四個反光球體組成的剛體。

本發(fā)明還提供了一種無人機(jī)基于視覺的抓取方法，包括以下步驟：

s1、標(biāo)定；

假設(shè)機(jī)械臂末端執(zhí)行器相對于無人機(jī)的位置已知，給定機(jī)械臂關(guān)節(jié)位置，機(jī)械臂末端執(zhí)行器位置相對于無人機(jī)已知，攝像機(jī)內(nèi)參、畸變參數(shù)已經(jīng)事先標(biāo)定已知，目標(biāo)物相對于攝像機(jī)坐標(biāo)系的旋轉(zhuǎn)和平移已知，目標(biāo)物相對于攝像機(jī)坐標(biāo)系的旋轉(zhuǎn)和平移為攝像機(jī)外參，在獲得攝像機(jī)外參的過程中使用apriltags及其標(biāo)靶，能夠?qū)崟r的獲得目標(biāo)標(biāo)靶在攝像機(jī)坐標(biāo)系下的旋轉(zhuǎn)和平移矩陣，采用optitrack的運(yùn)動捕捉系統(tǒng)，可以獲得反光材質(zhì)小球組成的標(biāo)簽在運(yùn)動捕捉系統(tǒng)坐標(biāo)系下的位置，在無人機(jī)機(jī)體上貼有運(yùn)動捕捉系統(tǒng)的定位標(biāo)簽tag_body、在舵機(jī)控制關(guān)節(jié)之后貼有運(yùn)動捕捉系統(tǒng)的定位標(biāo)簽tag_link，在目標(biāo)物(視覺標(biāo)靶)上貼有運(yùn)動捕捉系統(tǒng)的定位標(biāo)簽tag_a，標(biāo)定過程中需要保證各個標(biāo)簽不同防止被運(yùn)動捕捉系統(tǒng)誤識別，所以組成的反光材質(zhì)小球的位置和個數(shù)不同，通過運(yùn)動捕捉系統(tǒng)，可以得到已知的參數(shù)有：無人機(jī)機(jī)體在世界坐標(biāo)系下的平移向量和旋轉(zhuǎn)矩陣舵機(jī)控制關(guān)節(jié)在世界坐標(biāo)系下的平移向量和旋轉(zhuǎn)矩陣目標(biāo)物在世界坐標(biāo)系下的平移向量和旋轉(zhuǎn)矩陣通過攝像頭的apriltag捕捉，得到目標(biāo)物在攝像機(jī)坐標(biāo)系下的平移向量和旋轉(zhuǎn)矩陣繼而由以下關(guān)系可得：

其中，o代表平移向量，r代表旋轉(zhuǎn)矩陣，角標(biāo)b代表機(jī)體坐標(biāo)系，角標(biāo)c代表攝像機(jī)坐標(biāo)系，角標(biāo)l代表舵機(jī)關(guān)節(jié)坐標(biāo)系，和為機(jī)體坐標(biāo)系到攝像機(jī)坐標(biāo)系的平移向量和旋轉(zhuǎn)矩陣，和為機(jī)體坐標(biāo)系到舵機(jī)關(guān)節(jié)坐標(biāo)系的平移向量和旋轉(zhuǎn)矩陣，和為舵機(jī)關(guān)節(jié)坐標(biāo)系到攝像機(jī)坐標(biāo)系的平移向量和旋轉(zhuǎn)矩陣，每一時刻以及舵機(jī)控制關(guān)節(jié)變化之后，都可以得到一組和的解，通過多組測量，由于為向量，對每個分量直接求平均值，而為旋轉(zhuǎn)矩陣，先將其轉(zhuǎn)換為四元素，然后使用多組四元素的均值算法進(jìn)行四元素求均值，最后轉(zhuǎn)換回旋轉(zhuǎn)矩陣得到最終的均值另外，舵機(jī)控制關(guān)節(jié)的控制角度的準(zhǔn)確性需要通過舵機(jī)的輸入標(biāo)定與運(yùn)動捕捉系統(tǒng)下舵機(jī)關(guān)節(jié)定位標(biāo)簽tag_link的俯仰角度保持一致，后續(xù)在無外部捕捉系統(tǒng)下就可直接通過正運(yùn)動學(xué)求解目標(biāo)在無人機(jī)坐標(biāo)系下的位姿；

s2、視覺伺服抓??；

s21、無人機(jī)的機(jī)械臂系統(tǒng)檢測到任務(wù)，開始執(zhí)行視覺伺服抓取或者投放任務(wù)；

s22、攝像機(jī)識別目標(biāo)物，獲得像素坐標(biāo)；

s23、根據(jù)目標(biāo)物像素坐標(biāo)偏差，進(jìn)行攝像頭和無人機(jī)航向調(diào)節(jié)：

其像素縱軸上偏離攝像機(jī)像素中心的誤差表針的是舵機(jī)控制關(guān)節(jié)的角度跟蹤誤差，用pd控制器進(jìn)行舵機(jī)控制關(guān)節(jié)的控制，使得在俯仰方向上，在無人機(jī)或目標(biāo)運(yùn)動中，攝像機(jī)能夠準(zhǔn)確對準(zhǔn)目標(biāo)物，進(jìn)行俯仰追蹤；

其像素橫軸上偏離攝像機(jī)像素中心的誤差表針的是無人機(jī)航向頭部對目標(biāo)物的偏離，用pd控制器調(diào)整無人機(jī)航向，使得在水平方向上，能夠使得無人機(jī)頭部正對目標(biāo)物；

s24、已知舵機(jī)關(guān)節(jié)姿態(tài)在無人機(jī)坐標(biāo)系下的姿態(tài)、舵機(jī)關(guān)節(jié)到攝像機(jī)坐標(biāo)系下的轉(zhuǎn)換關(guān)系、攝像機(jī)坐標(biāo)系下的目標(biāo)物位姿，通過正運(yùn)動學(xué)求解得到目標(biāo)物在無人機(jī)坐標(biāo)系下的位姿，進(jìn)一步可轉(zhuǎn)換到其他參考坐標(biāo)系；

s25、根據(jù)得到的目標(biāo)物在無人機(jī)坐標(biāo)系下的位置，同步計(jì)算末機(jī)械臂端執(zhí)行器需要的動作，執(zhí)行基于位置伺服的飛行抓取或者投放任務(wù)。

本發(fā)明的有益效果是：實(shí)現(xiàn)了無人機(jī)機(jī)械臂的伺服抓取，具有低成本，高精度的特點(diǎn)，極具應(yīng)用價值。

附圖說明

圖1是本發(fā)明一種無人機(jī)的示意圖。

圖2是本發(fā)明一種無人機(jī)的標(biāo)定示意圖。

圖3是本發(fā)明一種無人機(jī)基于視覺的抓取方法的視覺伺服抓取的流程圖。

具體實(shí)施方式

下面結(jié)合附圖說明及具體實(shí)施方式對本發(fā)明作進(jìn)一步說明。

如圖1至圖2所示，一種無人機(jī)，包括無人機(jī)機(jī)體1、機(jī)械臂2、舵機(jī)控制關(guān)節(jié)3和攝像頭4，所述攝像頭4為單目攝像頭，所述機(jī)械臂2固連于所述無人機(jī)機(jī)體1的下方，所述舵機(jī)控制關(guān)節(jié)3連接于所述無人機(jī)機(jī)體1的上方前部，所述舵機(jī)控制關(guān)節(jié)3為單自由度，所述舵機(jī)控制關(guān)節(jié)3與所述攝像頭4連接，所述無人機(jī)機(jī)體1上貼有運(yùn)動捕捉系統(tǒng)的第一定位標(biāo)簽5，所述舵機(jī)控制關(guān)節(jié)3、攝像頭4之間貼有運(yùn)動捕捉系統(tǒng)的第二定位標(biāo)簽6，所述目標(biāo)物8(視覺標(biāo)靶apriltag)上貼有運(yùn)動捕捉系統(tǒng)的第三定位標(biāo)簽7。此外，該無人機(jī)還包含有多個微型控制器如飛控、舵機(jī)控制器、視覺伺服控制器(圖中忽略)。

如圖2所示，所述第一定位標(biāo)簽5為至少四個反光球體組成的剛體，所述第二定位標(biāo)簽6為至少四個反光球體組成的剛體，第三定位標(biāo)簽7為至少四個反光球體組成的剛體。

如圖1至圖3所示，一種無人機(jī)基于視覺的抓取方法，包括以下步驟：s1、標(biāo)定；

如圖2所示，通過標(biāo)定需要解決的問題是當(dāng)舵機(jī)控制關(guān)節(jié)改變攝像機(jī)角度的時候，目標(biāo)物體的位置在無人機(jī)坐標(biāo)系下的位置是已知的。在這個環(huán)節(jié)中，需要通過標(biāo)定求解得到的量是舵機(jī)控制關(guān)節(jié)到攝像機(jī)坐標(biāo)系的旋轉(zhuǎn)和平移關(guān)系矩陣

首先機(jī)械臂末端執(zhí)行器的標(biāo)定在世界范圍內(nèi)已有成熟的方法，其次，在計(jì)算機(jī)視覺中，在攝像機(jī)內(nèi)外參的標(biāo)定中已經(jīng)可以較容易和準(zhǔn)確的得到攝像機(jī)內(nèi)參、畸變以及目標(biāo)物的位置(即外參)。

假設(shè)機(jī)械臂末端執(zhí)行器相對于無人機(jī)的位置已知，給定機(jī)械臂關(guān)節(jié)位置，機(jī)械臂末端執(zhí)行器位置相對于無人機(jī)已知，攝像機(jī)內(nèi)參、畸變參數(shù)已經(jīng)事先標(biāo)定已知，目標(biāo)物相對于攝像機(jī)坐標(biāo)系的旋轉(zhuǎn)和平移已知，目標(biāo)物相對于攝像機(jī)坐標(biāo)系的旋轉(zhuǎn)和平移為攝像機(jī)外參，在獲得攝像機(jī)外參的過程中使用apriltags及其標(biāo)靶，能夠?qū)崟r的獲得目標(biāo)標(biāo)靶在攝像機(jī)坐標(biāo)系下的旋轉(zhuǎn)和平移矩陣，采用optitrack的運(yùn)動捕捉系統(tǒng)，可以獲得反光材質(zhì)小球組成的標(biāo)簽在運(yùn)動捕捉系統(tǒng)坐標(biāo)系下的位置，在無人機(jī)機(jī)體上貼有運(yùn)動捕捉系統(tǒng)的定位標(biāo)簽tag_body、在舵機(jī)控制關(guān)節(jié)之后貼有運(yùn)動捕捉系統(tǒng)的定位標(biāo)簽tag_link，在目標(biāo)物(視覺標(biāo)靶)上貼有運(yùn)動捕捉系統(tǒng)的定位標(biāo)簽tag_a，標(biāo)簽位置和結(jié)構(gòu)如圖2中所示，圖中標(biāo)簽的示意結(jié)構(gòu)為陰隱填充所示的四個反光小球，實(shí)際標(biāo)簽也可由五個或者六個組成，每組反光小球的組成視為一個剛體，剛體的幾何中心即為標(biāo)簽的中心，標(biāo)定過程中需要保證各個標(biāo)簽不同防止被運(yùn)動捕捉系統(tǒng)誤識別，所以組成的反光材質(zhì)小球的位置和個數(shù)不同，通過運(yùn)動捕捉系統(tǒng)，可以得到已知的參數(shù)有：無人機(jī)機(jī)體在世界坐標(biāo)系下的平移向量和旋轉(zhuǎn)矩陣舵機(jī)控制關(guān)節(jié)在世界坐標(biāo)系下的平移向量和旋轉(zhuǎn)矩陣目標(biāo)物在世界坐標(biāo)系下的平移向量和旋轉(zhuǎn)矩陣通過攝像頭的apriltag捕捉，得到目標(biāo)物在攝像機(jī)坐標(biāo)系下的平移向量和旋轉(zhuǎn)矩陣繼而由以下關(guān)系可得：

其中，o代表平移向量，r代表旋轉(zhuǎn)矩陣，角標(biāo)b代表機(jī)體坐標(biāo)系，角標(biāo)c代表攝像機(jī)坐標(biāo)系，角標(biāo)l代表舵機(jī)關(guān)節(jié)坐標(biāo)系，和為機(jī)體坐標(biāo)系到攝像機(jī)坐標(biāo)系的平移向量和旋轉(zhuǎn)矩陣，和為機(jī)體坐標(biāo)系到舵機(jī)關(guān)節(jié)坐標(biāo)系的平移向量和旋轉(zhuǎn)矩陣，和為舵機(jī)關(guān)節(jié)坐標(biāo)系到攝像機(jī)坐標(biāo)系的平移向量和旋轉(zhuǎn)矩陣，每一時刻以及舵機(jī)控制關(guān)節(jié)變化之后，都可以得到一組和的解，通過多組測量，由于為向量，對每個分量直接求平均值，而為旋轉(zhuǎn)矩陣，先將其轉(zhuǎn)換為四元素，然后使用多組四元素的均值算法進(jìn)行四元素求均值，最后轉(zhuǎn)換回旋轉(zhuǎn)矩陣得到最終的均值另外，舵機(jī)控制關(guān)節(jié)的控制角度的準(zhǔn)確性需要通過舵機(jī)的輸入標(biāo)定與運(yùn)動捕捉系統(tǒng)下舵機(jī)關(guān)節(jié)定位標(biāo)簽tag_link的俯仰角度保持一致，后續(xù)在無外部捕捉系統(tǒng)下就可直接通過正運(yùn)動學(xué)求解目標(biāo)在無人機(jī)坐標(biāo)系下的位姿；

s2、視覺伺服抓??；

視覺伺服抓取依賴于事先的視覺標(biāo)定以及其他的機(jī)械臂等關(guān)節(jié)的運(yùn)動學(xué)標(biāo)定，需要準(zhǔn)確的獲得從機(jī)體坐標(biāo)系(圖2中tag_body定位標(biāo)簽所在的坐標(biāo)系)到舵機(jī)控制關(guān)節(jié)坐標(biāo)系(圖2中tag_link定位標(biāo)簽所在的坐標(biāo)系)的轉(zhuǎn)換關(guān)系(關(guān)節(jié)、舵機(jī)標(biāo)定并由dh參數(shù)獲得)、舵機(jī)控制關(guān)節(jié)坐標(biāo)系到攝像頭坐標(biāo)系的轉(zhuǎn)換關(guān)系(視覺標(biāo)定獲得)、機(jī)體坐標(biāo)系到機(jī)械臂末端執(zhí)行器的轉(zhuǎn)換關(guān)系(機(jī)械臂關(guān)節(jié)標(biāo)定并由dh參數(shù)獲得)、攝像機(jī)坐標(biāo)系下目標(biāo)物的位姿(攝像機(jī)標(biāo)定及目標(biāo)捕獲，本發(fā)明使用apriltags進(jìn)行標(biāo)靶捕獲和位置計(jì)算)。

如圖3所示，視覺伺服抓取具體包括以下流程：

s21、無人機(jī)的機(jī)械臂系統(tǒng)檢測到任務(wù)，開始執(zhí)行視覺伺服抓取或者投放任務(wù)；

s22、攝像機(jī)識別目標(biāo)物，獲得像素坐標(biāo)；

s23、根據(jù)目標(biāo)物像素坐標(biāo)偏差，進(jìn)行攝像頭和無人機(jī)航向調(diào)節(jié)：

其像素縱軸上偏離攝像機(jī)像素中心的誤差表針的是舵機(jī)控制關(guān)節(jié)的角度跟蹤誤差，用pd控制器進(jìn)行舵機(jī)控制關(guān)節(jié)的控制，使得在俯仰方向上，在無人機(jī)或目標(biāo)運(yùn)動中，攝像機(jī)能夠準(zhǔn)確對準(zhǔn)目標(biāo)物，進(jìn)行俯仰追蹤；

其像素橫軸上偏離攝像機(jī)像素中心的誤差表針的是無人機(jī)航向頭部對目標(biāo)物的偏離，用pd控制器調(diào)整無人機(jī)航向，使得在水平方向上，能夠使得無人機(jī)頭部正對目標(biāo)物；

s24、已知舵機(jī)關(guān)節(jié)姿態(tài)在無人機(jī)坐標(biāo)系下的姿態(tài)、舵機(jī)關(guān)節(jié)到攝像機(jī)坐標(biāo)系下的轉(zhuǎn)換關(guān)系、攝像機(jī)坐標(biāo)系下的目標(biāo)物位姿，通過正運(yùn)動學(xué)求解得到目標(biāo)物在無人機(jī)坐標(biāo)系下的位姿，進(jìn)一步可轉(zhuǎn)換到其他參考坐標(biāo)系；

s25、根據(jù)得到的目標(biāo)物在無人機(jī)坐標(biāo)系下的位置，同步計(jì)算末機(jī)械臂端執(zhí)行器需要的動作，執(zhí)行基于位置伺服的飛行抓取或者投放任務(wù)。

本發(fā)明提供的一種無人機(jī)及其基于視覺的抓取方法，通過利用外部準(zhǔn)確的測量設(shè)備對無人機(jī)關(guān)節(jié)和攝像機(jī)坐標(biāo)系進(jìn)行測量，提出了一種視覺標(biāo)定方法，在標(biāo)定基礎(chǔ)上，提出了一種基于單目攝像機(jī)的視覺伺服抓取系統(tǒng)。該發(fā)明提出的系統(tǒng)具有低成本，高精度的特點(diǎn)，極具應(yīng)用價值。

以上內(nèi)容是結(jié)合具體的優(yōu)選實(shí)施方式對本發(fā)明所作的進(jìn)一步詳細(xì)說明，不能認(rèn)定本發(fā)明的具體實(shí)施只局限于這些說明。對于本發(fā)明所屬技術(shù)領(lǐng)域的普通技術(shù)人員來說，在不脫離本發(fā)明構(gòu)思的前提下，還可以做出若干簡單推演或替換，都應(yīng)當(dāng)視為屬于本發(fā)明的保護(hù)范圍。