[发明专利]一种基于多旋翼飞行器测量农田边界与内部障碍的方法

权利要求书

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，即：

结合障碍物中心位置，就能定位出农田中的障碍物位置和大小。