本發(fā)明涉及的是一種無人機(jī)控制方法，具體地說是一種無人機(jī)環(huán)繞物體外形輪廓飛行的控制方法。

背景技術(shù)：

無人機(jī)具有使用成本低，無直接人員傷亡風(fēng)險，可搭載多種檢測儀器等特點。無人機(jī)在航拍、搜救、野生動物考察、大氣研究、電力巡線、橋梁檢測等領(lǐng)域發(fā)揮重要作用。

橋梁支座是橋梁易損壞部位，是橋梁檢測的重點檢查對象，無人機(jī)可搭載相機(jī)對支座病害進(jìn)行拍照記錄，人為控制無人機(jī)環(huán)繞橋墩飛行檢查橋梁支座健康狀態(tài)對無人機(jī)操控者操控技術(shù)要求較高，采用無人機(jī)定距環(huán)繞橋墩飛行控制技術(shù)可大幅度降低無人機(jī)操控難度，同時提高橋梁檢測效率。

名稱為“無人機(jī)定點環(huán)繞飛行的控制方法”的專利文件中，通過遙控器或地面站設(shè)定環(huán)繞目標(biāo)點和環(huán)繞半徑，利用gps和機(jī)載傳感器對無人機(jī)進(jìn)行定位，控制無人機(jī)環(huán)繞目標(biāo)點飛行，該方法不能沿物體外形輪廓飛行，在無gps信號情況下該方法不能使用。雙目視覺可構(gòu)建環(huán)境三維立體影像，可以獲得環(huán)境大量信息，在目標(biāo)跟蹤中有廣泛應(yīng)用，雙目視覺可識別環(huán)繞目標(biāo)的方位和無人機(jī)與環(huán)繞目標(biāo)的距離，也可用于環(huán)繞飛行控制。視覺傳感器獲取的環(huán)境的信息量大，特征提取算法復(fù)雜，計算量大，當(dāng)環(huán)境光線較暗或者較強時或環(huán)境特征不明顯時會影響特征提取效果，進(jìn)而影響無人機(jī)環(huán)繞目標(biāo)飛行的控制效果。

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

本發(fā)明的目的在于提供一種計算復(fù)雜度低、易于操控的無人機(jī)環(huán)繞物體外形輪廓飛行的控制方法。

本發(fā)明的目的是這樣實現(xiàn)的：

步驟a，無人機(jī)開機(jī)自檢，檢查系統(tǒng)中各傳感器工作狀態(tài)，當(dāng)有故障時通過提示裝置發(fā)出警報；

步驟b，遙控?zé)o人機(jī)起飛，使環(huán)繞目標(biāo)進(jìn)入目標(biāo)可檢測范圍，開啟目標(biāo)環(huán)繞模式，通過提示裝置發(fā)出環(huán)繞模式開啟成功或開啟失敗的提示信息；

步驟c，利用激光雷達(dá)探測信息解算無人機(jī)機(jī)頭與目標(biāo)中心方向的夾角即轉(zhuǎn)差角及無人機(jī)與目標(biāo)的距離即目標(biāo)距離；

步驟d，將轉(zhuǎn)差角輸入轉(zhuǎn)向pid控制器，轉(zhuǎn)向pid控制器輸出量用于控制無人機(jī)鎖定目標(biāo)；

步驟e，將目標(biāo)距離輸入定距pid控制器，定距pid控制器輸出量用于控制無人機(jī)與目標(biāo)維持在設(shè)定距離，環(huán)繞模式下通過遙控器俯仰通道改變定距距離；

步驟f，控制無人機(jī)環(huán)繞目標(biāo)飛行。

本發(fā)明還可以包括：

1、步驟c具體包括：將激光雷達(dá)探測信息變換到水平面內(nèi)，在目標(biāo)可檢測范圍搜索連續(xù)體，合并可以合并的連續(xù)體，尋找探測點數(shù)最多連續(xù)體，確定連續(xù)體邊界，計算轉(zhuǎn)差角和目標(biāo)距離；

步驟c1，利用陀螺儀和加速度計信息計算無人機(jī)的姿態(tài)，包括橫滾角、俯仰角和航向角，激光雷達(dá)與無人機(jī)具有相同的姿態(tài)角；

載體坐標(biāo)系到水平坐標(biāo)系的坐標(biāo)變換矩陣為：

其中α為橫滾角，β為俯仰角，利用坐標(biāo)變換矩陣將激光雷達(dá)探測信息變換到水平面內(nèi)；

步驟c2，所述連續(xù)體是指根據(jù)判定規(guī)則確定的激光雷達(dá)探測點的集合；連續(xù)體均由一個激光雷達(dá)探測點逐步擴(kuò)充得到，連續(xù)體擴(kuò)充判定規(guī)則為：

其中r和γ分別為激光雷達(dá)探測點的探測距離和方位角，下標(biāo)表示探測點編號，min表示小括號中取二者中較小值，ddis為相鄰激光雷達(dá)探測點的探測距離差，arcdis為相鄰探測點以其中最小探測距離為半徑所夾圓弧長度，facd為設(shè)定的判定因數(shù)，使用連續(xù)體擴(kuò)充判定規(guī)則確定連續(xù)體；

步驟c3，使用連續(xù)體擴(kuò)充判定規(guī)則對間隔探測點數(shù)小于a的連續(xù)體邊界探測點進(jìn)行判定，合并可以合并的連續(xù)體，a為正整數(shù)；

步驟c4，根據(jù)連續(xù)體距離在[mindis,maxdis]范圍內(nèi)尋找點數(shù)最多的連續(xù)體即最大連續(xù)體，其中mindis和maxdis為設(shè)定的搜索最大連續(xù)體的最小和最大的環(huán)繞目標(biāo)與無人機(jī)的距離；

對于包含b個探測點的連續(xù)體其連續(xù)體距離odis計算公式為：

odis＝(r1+r2+…+rb)/b

步驟c5，當(dāng)最大連續(xù)體左側(cè)邊界小于等于右側(cè)邊界時，從右側(cè)邊界向內(nèi)搜尋探測點作為新邊界，將新邊界與左側(cè)邊界比較，若新邊界距離小于左側(cè)邊界距離或新邊界與左側(cè)邊界夾角小于χ度，停止繼續(xù)向內(nèi)搜尋，將新邊界作為最大連續(xù)體的新右側(cè)邊界，χ為可選設(shè)定夾角值；

當(dāng)最大連續(xù)體右側(cè)邊界小于左側(cè)邊界時，從左側(cè)邊界向內(nèi)搜尋探測點作為新邊界，將新邊界與右側(cè)邊界比較，若新邊界距離小于右側(cè)邊界距離或新邊界與右側(cè)邊界夾角小于χ度，停止繼續(xù)向內(nèi)搜尋，將新邊界作為最大連續(xù)體的新左側(cè)邊界；

將左右兩側(cè)邊界所確定的連續(xù)體作為環(huán)繞目標(biāo)；

步驟c6，目標(biāo)中心方向為：

γo＝(γl+γr)/2

其中γo為目標(biāo)中心方向，γl為左側(cè)邊界方位角，γr為右側(cè)邊界方位角；

目標(biāo)方向與機(jī)頭方向的角度差簡稱轉(zhuǎn)差角為

其中為轉(zhuǎn)差角，γu為機(jī)頭在激光雷達(dá)探測區(qū)域?qū)?yīng)方位角；

無人機(jī)與環(huán)繞目標(biāo)距離簡稱目標(biāo)距離為

ro＝(r1+…+rm+rm+1+…+r2m)/(2*m)

其中ro為目標(biāo)距離，r1,r2…rm為目標(biāo)中心方向左側(cè)與目標(biāo)中心方向鄰近的連續(xù)m個激光雷達(dá)探測點，rm+1,rm+2…r2m為目標(biāo)中心方向右側(cè)與目標(biāo)中心方向鄰近的連續(xù)m個激光雷達(dá)探測點，m為可選正整數(shù)值。

2、步驟d具體包括：轉(zhuǎn)向控制在每個轉(zhuǎn)差角輸出周期確定機(jī)頭方向設(shè)定值為

其中為機(jī)頭設(shè)定方向，為采樣時刻機(jī)頭方向，為轉(zhuǎn)差角；

機(jī)頭方向通過與無人機(jī)相對固定的陀螺儀和加速度傳感器數(shù)據(jù)解算得到，或者通過與無人機(jī)相對固定的磁力計傳感器數(shù)據(jù)解算得到；

在每個轉(zhuǎn)向控制周期，轉(zhuǎn)向pid控制器輸出為：

其中cr為轉(zhuǎn)向pid控制器的輸出量用于轉(zhuǎn)向控制，pr、ir和dr為轉(zhuǎn)向pid控制器參數(shù)，eintr為誤差積分。

3、定距pid控制器為：

其中cd為定距pid控制器的輸出量用于定距控制，pd、id和dd為定距pid控制器參數(shù)，eintd為誤差積分，rt為采樣時刻無人機(jī)與環(huán)繞目標(biāo)的距離，rs為設(shè)定的定矩距離，rs取為固定的距離值rsd或進(jìn)入環(huán)繞模式時采樣到的目標(biāo)距離rso。

4、所述通過遙控器俯仰通道改變定距距離是指：設(shè)定距離通過俯仰通道調(diào)整為

其中rsa為調(diào)整后的設(shè)定距離，rp為遙控器俯仰通道值，rpmin、rpmid、rpmax分別為遙控器俯仰通道最小值、中間值和最大值，rsmin為允許的最小設(shè)定距離，rsmax為允許的最大設(shè)定距離。

5、所述控制無人機(jī)環(huán)繞目標(biāo)飛行包括通過手動方式或設(shè)定環(huán)繞速度方式，

所述通過手動方式是指將遙控器的橫滾信號直接作為無人機(jī)環(huán)繞目標(biāo)飛行的環(huán)繞角速率的控制量；

所述設(shè)定環(huán)繞速度方式是指通過遙控器橫滾通道信號量設(shè)定無人機(jī)環(huán)繞目標(biāo)飛行的角速率，設(shè)定的環(huán)繞角速率為：

ωs＝(rr-rrmid)/(rrmax-rrmin)*ωmax

其中ωs為設(shè)定環(huán)繞角速率，rr為遙控器俯仰通道值，rrmin、rrmid、rrmax分別為遙控器橫滾通道最小值、中間值和最大值，ωmax為允許的最大環(huán)繞角速率；

環(huán)繞角速率pid控制器為：

其中cω為環(huán)繞角速率pid控制器的輸出量，用于控制無人機(jī)環(huán)繞目標(biāo)飛行的環(huán)繞角速率，pω、iω和dω為環(huán)繞角速率pid控制器參數(shù)，eintω為誤差積分，ωt為無人機(jī)在水平坐標(biāo)系內(nèi)z軸方向旋轉(zhuǎn)角速率，ωs為設(shè)定環(huán)繞角速率。

所述無人機(jī)開機(jī)自檢是指無人機(jī)系統(tǒng)啟動后，系統(tǒng)檢測陀螺儀傳感器、加速度計傳感器，激光雷達(dá)等通信和傳感器輸出數(shù)據(jù)是否正常，當(dāng)有異常時通過提示裝置發(fā)出警報。

所述提示裝置至少包括以下方式中的一種：

無人機(jī)上的led燈；

具有顯示頁面的控制終端；

能發(fā)出異常提示音或語音的可穿戴設(shè)備。

所述警報方式至少包括以下方式中的一種：

控制所述無人機(jī)上的指示燈閃爍；

控制所述控制終端顯示異常信息；

控制所述可穿戴式設(shè)備發(fā)出異常提示音或語音。

所述目標(biāo)進(jìn)入目標(biāo)可檢測范圍是指在激光雷達(dá)探測角度范圍內(nèi)設(shè)定的機(jī)頭兩邊正負(fù)ψ角度范圍及激光雷達(dá)有效探測距離所限定的扇形區(qū)域。

環(huán)繞飛行模式開關(guān)開啟后，無人機(jī)系統(tǒng)根據(jù)傳感器檢測信息判定是否符合進(jìn)入環(huán)繞飛行模式條件，并能通過提示裝置提示環(huán)繞模式開啟失敗或成功的消息。

環(huán)繞飛行模式開關(guān)開啟后，當(dāng)轉(zhuǎn)差角過大、目標(biāo)距離過小或過大等因素，使無人機(jī)不能穩(wěn)定繼續(xù)環(huán)繞物體輪廓飛行，應(yīng)退出環(huán)繞飛行模式并進(jìn)入其他飛行模式如手動控制模式，并通過所述提示裝置發(fā)出警報。

本發(fā)明提供了一種無人機(jī)環(huán)繞目標(biāo)外形輪廓飛行的控制方法，使用激光雷達(dá)確定目標(biāo)方位及無人機(jī)與目標(biāo)外形輪廓的距離，本發(fā)明的有益效果為：

1.使用該方法可控制無人機(jī)環(huán)繞目標(biāo)外形輪廓定距飛行，降低無人機(jī)操控難度；

2.使用機(jī)載傳感器即可完成環(huán)繞飛行控制，不依賴于衛(wèi)星導(dǎo)航等外界信息；

3.該方法計算量小，易于在低性能的處理器上實現(xiàn)；

4.激光雷達(dá)為主動發(fā)光原件，可在暗光環(huán)境下工作；

5.設(shè)置了提示裝置向操控者提示系統(tǒng)工作狀態(tài)。

附圖說明

圖1是本發(fā)明實施例的無人機(jī)環(huán)繞物體外形輪廓飛行控制方法的流程示意圖。

圖2是本發(fā)明實施例的連續(xù)體擴(kuò)充判定規(guī)則示意圖。

圖3是本發(fā)明實施例的無人機(jī)環(huán)繞柱狀物體外形輪廓定距飛行示意圖。

具體實施方式

下面結(jié)合附圖舉例對本發(fā)明做詳細(xì)描述，本發(fā)明的實施例是對本發(fā)明的的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實施例僅是本發(fā)明一部分實施例，而不是全部的實施例?；诒景l(fā)明中的實施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動前提下所獲得的所有其他實施例，都屬于本發(fā)明保護(hù)的范圍。

圖1是本發(fā)明實施例的一種無人機(jī)環(huán)繞物體外形輪廓飛行控制方法的流程示意圖。應(yīng)說明的是，本發(fā)明實施例的所述方法并不限于圖所示的流程圖中的步驟及順序。根據(jù)不同的實施例，圖所示的流程圖中的步驟可以增加、移除、或者改變順序。在本實施方式中，無人機(jī)環(huán)繞物體外形輪廓飛行控制包括如下步驟：

步驟s101，無人機(jī)開機(jī)自檢，發(fā)出故障警告。

步驟s102，遙控?zé)o人機(jī)使環(huán)繞目標(biāo)進(jìn)入目標(biāo)可檢測范圍，開啟目標(biāo)環(huán)繞模式。

步驟s103，利用激光雷達(dá)探測信息解算轉(zhuǎn)差角和目標(biāo)距離。

步驟s104，將轉(zhuǎn)差角輸入轉(zhuǎn)向pid控制器，控制器輸出量用于控制無人機(jī)機(jī)頭鎖定環(huán)繞目標(biāo)。

步驟s105，將目標(biāo)距離輸入定距pid控制器，控制器輸出量用于控制無人機(jī)與環(huán)繞目標(biāo)距離維持在定距設(shè)定值。

步驟s106，手動方式或設(shè)定環(huán)繞速度方式控制無人機(jī)環(huán)繞目標(biāo)飛行。

在步驟s101中，無人機(jī)系統(tǒng)啟動后，系統(tǒng)檢測陀螺儀傳感器、加速度計傳感器，激光雷達(dá)是否可由控制器完成初始化設(shè)置，檢測傳感器輸出數(shù)據(jù)是否正常，當(dāng)有異常時通過無人機(jī)操控人員端的可穿戴設(shè)備發(fā)出語音警報。

在步驟s102中，由無人機(jī)操控人員控制無人機(jī)起飛，使目標(biāo)進(jìn)入目標(biāo)可檢測范圍，如使被環(huán)繞目標(biāo)在無人機(jī)機(jī)頭兩邊正負(fù)60度范圍內(nèi)可被檢測到且無人機(jī)與目標(biāo)外形輪廓在1～3米之間。進(jìn)入上述區(qū)域通過無人機(jī)操控人員手中遙控器開啟環(huán)繞飛行控制開關(guān)，無人機(jī)可成功進(jìn)入環(huán)繞飛行模式，若在上述區(qū)域之外將不能開啟環(huán)繞飛行模式。當(dāng)環(huán)繞飛行模式控制開關(guān)開啟后，無人機(jī)系統(tǒng)可以通過無人機(jī)操控人員端的可穿戴設(shè)備發(fā)出環(huán)繞飛行模式開啟成功或失敗的提示語音。

步驟s103利用激光雷達(dá)信息解算轉(zhuǎn)差角和目標(biāo)距離步驟包括：將激光雷達(dá)探測信息變換到水平面內(nèi)，在目標(biāo)可檢測范圍搜索連續(xù)體，合并可以合并的連續(xù)體，尋找探測點數(shù)最多連續(xù)體，確定連續(xù)體邊界，計算轉(zhuǎn)差角和目標(biāo)距離。

(1)利用陀螺儀和加速度計信息計算無人機(jī)的姿態(tài)包括橫滾角、俯仰角和航向角，由于激光雷達(dá)與無人機(jī)相對固定的，二者具有相同的姿態(tài)角。

使用歐拉角法由載體坐標(biāo)系到水平坐標(biāo)系的坐標(biāo)變換矩陣為：

其中α為橫滾角，β為俯仰角，利用坐標(biāo)變換矩陣將激光雷達(dá)探測信息變換到水平面內(nèi)。

(2)所述連續(xù)體是指根據(jù)一定判定規(guī)則確定的激光雷達(dá)探測點的集合。

連續(xù)體均由一個激光雷達(dá)探測點逐步擴(kuò)充得到，連續(xù)體擴(kuò)充判定規(guī)則示意圖如圖2所示，連續(xù)體擴(kuò)充判定規(guī)則為：

其中r和γ分別為激光雷達(dá)探測點的探測距離和方位角，下標(biāo)表示探測點編號，min表示小括號中取二者中較小值，ddis為相鄰激光雷達(dá)探測點的探測距離差，arcdis為相鄰探測點以其中最小探測距離為半徑所夾圓弧，facd為人為設(shè)定的判定因數(shù)。

使用連續(xù)體擴(kuò)充判定規(guī)則確定連續(xù)體。

(3)使用連續(xù)體擴(kuò)充判定規(guī)則對間隔探測點數(shù)小于3點的連續(xù)體的邊界探測點進(jìn)行判定，合并可以合并的鄰近連續(xù)體。

(4)根據(jù)連續(xù)體距離在0.5～4米范圍內(nèi)尋找點數(shù)最多的連續(xù)體即最大連續(xù)體，對于包含b個探測點的連續(xù)體其連續(xù)體距離odis計算公式為：

odis＝(r1+r2+…+rb)/b

(5)以無人機(jī)環(huán)繞柱狀物體為例來說明轉(zhuǎn)差角和無人機(jī)與目標(biāo)距離計算方法，圖3為無人機(jī)環(huán)繞柱狀物體外形輪廓定距飛行示意圖。

連續(xù)體左側(cè)邊界是指在機(jī)頭方向從無人機(jī)上方看去，逆時針方向一側(cè)的連續(xù)體邊緣探測點，連續(xù)體左側(cè)邊界是指在機(jī)頭方向從無人機(jī)上方看去，順時針方向一側(cè)的連續(xù)體邊緣探測點。

在圖3中，左側(cè)邊界小于右側(cè)邊界，從右側(cè)邊界向連續(xù)體內(nèi)部尋找新邊界，向內(nèi)尋找的第一個激光雷達(dá)探測點，探測距離小于左側(cè)邊界距離，滿足新右側(cè)邊界的條件，可以將該探測點作為新右側(cè)邊界，將左側(cè)邊界和新右側(cè)邊界所確定的連續(xù)體作為環(huán)繞目標(biāo)。

(6)目標(biāo)中心方向為：

γo＝(γl+γr)/2

其中γo為目標(biāo)中心方向，γl為左側(cè)邊界方向，γr為右側(cè)邊界方向。

目標(biāo)方向與機(jī)頭方向的角度差即轉(zhuǎn)差角為：

其中為轉(zhuǎn)差角，γu為機(jī)頭在激光雷達(dá)探測區(qū)域?qū)?yīng)方向。

取m＝3，求取無人機(jī)與環(huán)繞目標(biāo)距離的2m個激光雷達(dá)探測點如圖3所示。

ro＝(r1+r2+r3+r4+r5+r6)/6

其中ro為目標(biāo)距離，r1,r2,r3為目標(biāo)中心方向左側(cè)與目標(biāo)中心方向鄰近的連續(xù)3個激光雷達(dá)探測點，r4,r5,r6為目標(biāo)中心方向右側(cè)與目標(biāo)中心方向鄰近的連續(xù)3個激光雷達(dá)探測點。

步驟s104，所述轉(zhuǎn)向控制在每個轉(zhuǎn)差角輸出周期確定機(jī)頭設(shè)定轉(zhuǎn)向

其中為機(jī)頭設(shè)定方向，為采樣時刻機(jī)頭方向。

其中機(jī)頭方向通過與無人機(jī)相對固定的陀螺儀和加速度傳感器數(shù)據(jù)解算得到。

在每個轉(zhuǎn)向控制周期，所述轉(zhuǎn)向pid控制器輸出為：

其中cr為pid控制器的輸出量用于轉(zhuǎn)向控制，pr、ir和dr為轉(zhuǎn)向控制pid參數(shù)，eintr為誤差積分，gyroz為有陀螺儀數(shù)據(jù)計算得到的無人機(jī)在水平坐標(biāo)系內(nèi)z軸方向旋轉(zhuǎn)角速度。

步驟s105，所述定距pid控制器為：

其中cd為定距pid控制器的輸出量用于定距控制，pd、id和dd為定距控制pid參數(shù)，eintd為誤差積分，rs為設(shè)定的定矩距離，rt為采樣時刻測得的目標(biāo)距離。

在成功開啟環(huán)繞模式后采樣的目標(biāo)距離作為設(shè)定的定距距離，可通過遙控器俯仰通道改變定距距離，設(shè)定距離通過俯仰通道調(diào)整為

其中rsa為調(diào)整后的設(shè)定距離，rp為遙控器俯仰通道值，rpmin、rpmid、rpmax分別為遙控器俯仰通道最小值、中間值和最大值，rsmin為允許的最小設(shè)定距離，rsmax為允許的最大設(shè)定距離。

步驟s106，無人機(jī)操控者通過遙控器的俯仰通道，直接控制無人機(jī)橫滾角大小，間接控制無人機(jī)沿環(huán)繞目標(biāo)外形輪廓環(huán)繞速度。

當(dāng)環(huán)繞飛行模式開關(guān)開啟后，環(huán)繞飛行模式開關(guān)開啟后，當(dāng)轉(zhuǎn)差角過大、目標(biāo)距離過小或過大等因素，使無人機(jī)不能穩(wěn)定繼續(xù)環(huán)繞物體輪廓飛行，應(yīng)退出環(huán)繞飛行模式并進(jìn)入其他飛行模式如手動控制模式，并通過所述提示裝置發(fā)出警報。

最后應(yīng)說明的是，以上實施例僅用以說明本發(fā)明的技術(shù)方案而非限制，盡管參照較佳實施例對本發(fā)明進(jìn)行了詳細(xì)說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，可以對本發(fā)明的技術(shù)方案進(jìn)行修改或等同替換，而不脫離本發(fā)明技術(shù)方案的精神和范圍。