[發明專利]一種基于多旋翼飛行器測量農田邊界與內部障礙的方法

有效

• 申請號: 201711205531.4
• 申請日: 2017-11-27
• 公開（公告）號: CN108007437B
• 公開（公告）日: 2020-05-29
• 發明（設計）人: 全權;傅嘉寧
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G01C11/04
• 分類號: G01C11/04
• 代理公司: 北京慧泉知識產權代理有限公司 11232
• 代理人: 王順榮;唐愛華

1.一種基于多旋翼飛行器測量農田邊界與內部障礙的方法，認定農田為多邊形，且處于同一高度，在利用視覺測量過程中，保證多旋翼穩定懸停時拍照，通過拍攝的農田圖片和拍攝時刻的飛機自身傳感器的測量數據，測量農田邊角點和內部障礙物的位置；在找到其邊角點之后，通過聯線得到整體農田的邊界；

由于在實際測量情況中，多旋翼飛行器遇到高壓電線、通訊信號塔產生強磁場干擾的情況時有存在，進而會導致機體傳感器測量的機體偏航角測量不準，所述基于多旋翼飛行器測量農田邊界與內部障礙的方法中認為機體傳感器測量的機體偏航角不可用；采用多旋翼飛行器測量的機體位置數據、高度數據、機體俯仰角和滾轉角數據；

采用的視覺測量過程如下所示：

多旋翼上攝像頭為下視攝像頭，其初始起飛位置為A點，起飛后對農田進行拍攝，其所在位置分別為B、C、D點；農田的角落為H、J、K、L點；令A點為世界坐標系原點，坐標系方向按照北-東-地原則；

在拍攝的圖片中，農田角落H、J、K、L點的像素坐標分別為(u_H,v_H),(u_J,v_J),(u_K,v_K),(u_L,v_L)，對應的世界坐標系坐標分別為(X_H,Y_H,Z_H),(X_J,Y_J,Z_J),(X_K,Y_K,Z_K),(X_L,Y_L,Z_L)；根據單目視覺原理，對任一點如地面H點，在飛機當前相機位置當前點拍攝照片有

其中為相機成像的尺度因子，M表示相機的參數矩陣，而

其中矩陣為相機內參矩陣，與相機內部特性有關，其內部參數α_x、α_y分別表示圖像上u軸和v軸的焦距參數，而u₀、v₀表示相機對應的光學中心像素坐標，r則表示圖像上u軸和v軸的不垂直因子，相機的u軸與v軸幾乎垂直，r近似為0；由于拍攝相機均為同一機載相機，故對于每幅圖片該矩陣均相同；矩陣為攝像機外部參數矩陣，完全由攝像機相對于世界坐標系的方位決定，故對于每幅照片的各個特征點該矩陣保持不變；R陣為旋轉矩陣，由相機相對于世界坐標系的旋轉決定；T為平移向量，由相機相對于世界坐標系的平移決定；R和T表示了從地面到機體上相機的坐標系變換，其滿足

^CP_H＝R·^GP_H+T

其中^GP_H為H點在地面坐標系的表示，^CP_H為H點在相機坐標系下的表示；為了求解出最終H點在地面坐標系的表示^GP_H，也就是位置信息，需要測量得到的像素坐標u，v、相機的內部參數，并需要求解出R和T；

通過矩陣乘法展開式(1)，得到三個均含有系數s的等式，相除消去系數s，有：

式(2)說明對于地面上的每個點，都滿足兩個方程約束；而對于每個點，需要通過求解M，才能知道其位置；此時，M中含有內部參數已知量和外部參數未知量，也即R和T，為了求解出表示坐標系變換的R和T，繼續對R和T做如下說明；

地面坐標系按照右手定則分布；機體坐標系中機頭方向為Y_b軸，機頭左側為X_b軸，機體豎直向下為Z_b軸；三維相機坐標系則是光軸為Z_c軸，相機指向上方為X_c軸，指向右方為Y_c軸；

定義在飛機上，旋轉矩陣的旋轉順序是機體坐標系X_b軸、Y_b軸、Z_b軸，得到其旋轉矩陣R_B：

其中θ、ψ分別表示滾轉角、俯仰角和偏航角；這里，偏航角是未知量，需要求解；對于無人機來說，俯仰角和滾轉角則近似為0，認為是已知量；

而地面坐標系到相機坐標系的旋轉矩陣則為地面到機體的旋轉、機體自身旋轉和機體到相機的旋轉三個旋轉矩陣的乘積；定義地面坐標系的朝向與機體坐標系一致，而機體坐標系到相機坐標系的旋轉矩陣為：

而地面坐標系與機體坐標系的方向一致，因此，最終的旋轉矩陣為：

另一方面，T＝[t₁ t₂ t₃]^T為平移向量，t₁ t₂ t₃分別表示坐標系三個軸向的平移；因此，對于每幀圖像，則最終需要求解機體自身旋轉的偏航角以及三個軸向平移量，求解方法包括下述步驟；

其特征在于：該方法具體包括如下步驟：

步驟1：對多旋翼飛行器下視攝像頭進行標定；

使用一個棋盤格標定物，棋盤格的邊長可自由定義，但棋盤格的格子數不應低于6*6個，且長和寬的格子數不應相同；令多旋翼攝像頭在不同位置，不同角度對棋盤格拍攝多幅圖片，將拍攝的圖片傳入電腦，用工具箱進行標定；標定后得到相機的內部參數α_x,α_y,u₀,v₀；

步驟2：使用多旋翼飛行器對農田進行視覺測量

在農田的一個角落令多旋翼起飛，并以最初起飛點作為地面坐標系原點位置，稱這一點為A點，A點為世界坐標系原點；其有^GP_A＝[0 0 0]^T；向北方向為地面坐標系X軸，向東為地面坐標系Y軸，向下為Z軸方向；首先操縱多旋翼飛行器豎直起飛，并在適當高度拍攝包含有農田所有特征點的圖片作為初值圖像；因為是豎直起飛，所以在這一時刻多旋翼飛行器的偏航角為0，之后在不同位置對農田拍攝N張圖片；這些圖片中應盡可能拍攝到整個農田，并使農田在圖片中所占區域較大；第k次拍攝中，通過多旋翼上傳感器測量此時飛行器的俯仰角θ_k、滾轉角φ_k；飛行位置和高度記為^GP_k＝[X_k Y_k Z_k]^T，通過多旋翼傳感器所測量的數據為多旋翼機體上相機在地面坐標系下的坐標；然而偏航角不準，令其未知；

步驟3：利用采集的數據求解農田位置

3.1、求解特征點位置初值

對N張圖片進行處理，提取出W個角點的像素坐標；則第i個角點在k張圖片的像素坐標為(u_k,i,v_k,i),i＝1,2,…,W,k＝1,2,…,N，而其在地面坐標系下對應的坐標為(X_i,Y_i,Z_i)，i＝1,2,…,W；以上參數滿足方程組

其中

M_k表示相機在拍攝第k張圖片時的參數矩陣，M_A為相機的內參數矩陣，只與相機有關，M_B,k表示相機在拍攝第k張圖片時的外參數矩陣，R_k和T_k分別表示相機在拍攝第k張圖片時，相機坐標系相對于地面坐標系的旋轉矩陣與平移向量；而

其中表示在拍攝第k幅圖片處機體自身旋轉產生的旋轉矩陣，為機體坐標系到相機坐標系的旋轉矩陣，則表示平移向量；因為多旋翼上測量到的位置數據是飛機機體相對于地面的位置和高度，根據坐標系的定義，得到相機坐標系原點在地面坐標系的坐標為^GP_k＝[X_k Y_k Z_k]^T，得到：

T_k＝-R_k^GP_k (5)

依據式(3)和(4)，有

其中T_k由式(5)計算得到；對于第一張圖片k＝1來說，設定此時偏航角ψ₁＝0，那么相應地，R₁已知，依據式(6)，有

其中由式(3)可知，s₁近似等于飛行器高度，也即s≈Z₁；而X_i,ini、Y_i,ini、Z_i,ini表示第一張圖片所求解出的第i個角點初始坐標位置；

3.2、求解每幀圖像偏航角初值

由式(6)得到：

將地面待測邊角點位置的初值代入式(8)，得到優化函數：

ψ_k即為待測偏航角，f_k,i(ψ_k)即為構建的優化函數；

進一步將式(9)寫成向量形式如下

其中，是所有偏航角所構成的向量，即

η＝[ψ₂ ψ₃ … ψ_N]^T；

由于每幀圖像有多個特征點，使用最小二乘法進行優化得到比較準確的偏航角，故分別求解當前幀圖像相對第一幀圖像的變化的偏航角，而第一幀圖像定義為偏航角為0，因此得到當前幀圖像對應的偏航角；因此構建優化目標函數如下：

F(η)為優化目標函數，g(η)^T為g(η)向量的轉置；

對式(10)進行優化，得到每幀圖像對應的偏航角ψ_2,ini、ψ_3,ini…ψ_N,ini，η_ini＝[ψ_2,ini ψ_3,ini… ψ_N,ini]^T；

3.3、求解特征點位置

通過式(6)，通過以下關系

計算任意點的反投影像素值，其中M_k(ψ_k)表示第k幀在偏航角ψ_k下對應的相機參數矩陣，u_{k,i,BackProject}和v_{k,i,BackProject}即為反投影的像素值；通過式(4)和(5)，定義為

M_k(ψ_k)＝M_AM_B,k(ψ_k)＝M_A[R_k(ψ_k) -R_k(ψ_k)^GP_k]；

之后構建優化函數

其中u_{k,i,BackProject},v_{k,i,BackProject}由式(11)求得，它們是X_i,Y_i,Z_i,ψ_k的函數；

使用最小二乘法構建優化目標函數

其中，W表示特征點總數，N表示圖像總幀數；待優化量χ中包含各特征點的地面系X、Y、Z坐標和各幀圖像對應的機體偏航角，即

χ＝[X₁…X_W Y₁…Y_W Z₁…Z_W ψ₂…ψ_N]^T；

在求解優化式(12)時，步驟3.1和步驟3.2算出的特征點位置初值X_i,ini,Y_i,ini,Z_i,ini和偏航角初值η_ini，作為最后求解的初始值；

3.4、求解障礙區域位置

認為障礙區域為圓形，通過手動選取圖片中障礙物的中心和半徑來求解近似的障礙物區域；

這里只考慮一個障礙物；對于障礙物中心，認為其為一個特征點，對于障礙物區域的半徑，利用相似三角形的方法進行求解，公式如下：

其中r_k是障礙物區域的半徑，r_k,P是第k幅圖像中的障礙物區域的半徑，f是相機焦距，而h_k是第k幅圖像攝像機距離地面的高度；

由于不同圖片所得到的障礙物區域半徑不同，取其均值得到障礙物區域半徑r，即：

結合障礙物中心位置，就能定位出農田中的障礙物位置和大小。