CN108007437B - 一种基于多旋翼飞行器测量农田边界与内部障碍的方法 - Google Patents

Abstract

本发明涉及一种基于多旋翼飞行器的视觉测量农田边界与内部障碍的方法，包括如下步骤：步骤1：对多旋翼飞行器下视摄像头进行标定。步骤2：使用多旋翼飞行器对农田进行视觉测量；步骤3：利用采集的数据求解农田位置；其中，步骤3还包括：3.1、求解特征点位置初值；3.2、求解每帧图像偏航角初值；3.3、求解特征点位置；3.4、求解障碍区域位置。本发明解决了人工测量不便、费时费力的问题，并可以直接求出位置数据以便其他自动设备对农田进行机械化、自动化的植保、施肥和播种等作业。

Description

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

技术领域

本发明涉及一种基于多旋翼飞行器的视觉测量农田边界与内部障碍的方法，该发明属于测量领域。

背景技术

在现代化农业中，农田位置与边界的测绘有着很多的应用。比如在自动化、智能化的农业运用中，飞机播种、喷药均需要知道农田位置的边界、内部的障碍物等信息。而人工测量费时费力、甚至工作环境对测量者可能造成伤害。所以，快速、自动地测量出农田位置的边界是十分重要和有意义的。

多旋翼的视觉测量可以测量出农田某点相对飞机的位置信息，结合多旋翼上GPS测量的位置信息，可以实现对农田边界与内部障碍的测量。

发明内容

本发明给出了一种基于飞行器的自身传感器结合机载视觉针对农田的位置测量方法。它解决了农田测量的智能化和自动化的问题，节省人力。

本发明认定农田为多边形，且处于同一高度，在利用视觉测量过程中，应保证多旋翼稳定悬停时拍照，通过拍摄的农田图片和拍摄时刻的飞机自身传感器的测量数据，测量农田边角点和内部障碍物(如电线杆、建筑物)的位置。在找到其边角点之后，即可通过联线得到整体农田的边界。

由于在实际测量情况中，多旋翼飞行器遇到高压电线、通讯信号塔等产生强磁场干扰的情况时有存在，进而会导致机体传感器测量的机体偏航角测量不准，故本发明中认为机体传感器测量的机体偏航角不可用。本发明中会采用多旋翼飞行器测量的机体位置数据、高度数据、机体俯仰角和滚转角数据。

本发明中采用的视觉测量过程如下所示：

如图1所示，多旋翼上摄像头为下视摄像头，其初始起飞位置为A点，起飞后对农田进行拍摄，其所在位置分别为B、C、D点。农田的角落为H、J、K、L点。令A点为世界坐标系原点，坐标系方向按照北-东-地原则。

如图2所示，在拍摄的图片中，农田角落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做如下说明。

如图3所示，地面坐标系按照右手定则分布。机体坐标系中机头方向为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：对多旋翼飞行器下视摄像头进行标定。

使用一个棋盘格标定物(如图4所示)，棋盘格的边长可以自由定义，但棋盘格的格子数不应低于6*6个，且长和宽的格子数不应相同。令多旋翼摄像头在不同位置，不同角度对棋盘格拍摄多幅图片，将拍摄的图片传入电脑，使用MATLAB R2015b中带有的CameraCalibrator(相机标定工具箱)工具箱进行标定。标定后得到相机的内部参数α_x,α_y,u₀,v₀。

步骤2：使用多旋翼飞行器对农田进行视觉测量

在农田的一个角落令多旋翼起飞，并以最初起飞点作为地面坐标系原点位置，我们称这一点为A点，A点为世界坐标系原点。其有^GP_A＝[0 0 0]^T。向北方向为地面坐标系X轴，向东为地面坐标系Y轴，向下为Z轴方向。首先操纵多旋翼飞行器竖直起飞，并在适当高度，高度范围是30-100m，最大不会超过200m；拍摄包含有农田所有特征点的图片作为初值图像。因为是竖直起飞，所以在这一时刻多旋翼飞行器的偏航角为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₁…X_w Y₁…Y_w Z₁…Z_w ψ₂…ψ_w]^T。

在求解优化问题(12)时，3.1，3.2节算出的特征点位置初值X_i,ini,Y_i,ini,Z_i,ini和偏航角初值η_ini，可以作为最后求解的初始值。

3.4、求解障碍区域位置

在农田中，障碍区域会存在各种形状，如果要针对形状进行测量，可能会增加大量数据及算法去匹配各种不同形状的障碍区域。本方法采用简化的方法，认为障碍区域为圆形，通过手动选取图片中障碍物的中心和半径来求解近似的障碍物区域。

为了简单起见，我们这里只考虑一个障碍物。对于障碍物中心，可认为其为一个特征点，使用前文所述的方法进行求解。对于障碍物区域的半径，可以利用相似三角形的方法进行求解(图5)，公式如下：

其中r_k是障碍物区域的半径，r_k,P是第k幅图像中的障碍物区域的半径，f是相机焦距，而h_k是第k幅图像摄像机距离地面的高度。

由于不同图片所得到的障碍物区域半径不同，可以取其均值得到障碍物区域半径r，即：

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

优点及功效：本发明给出一种多旋翼上基于视觉测量农田边界、内部障碍物的区域的方法。该方法的优点是：解决了人工测量不便、费时费力的问题，并可以直接求出位置数据以便其他自动设备对农田进行机械化、自动化的植保、施肥和播种等作业。

附图说明

图1：多旋翼飞行器农田测量示意图。

图2：多旋翼飞行器农田拍摄图片示意图。

图3：坐标系定义示意图。

图4：棋盘格标定物示意图。

图5：测量区域原理示意图。

图6：室内实验示意图。

图7：农田测量数据管理系统。

图8：本发明流程框图。

图中符号说明如下：

图1：多旋翼初始起飞位置为A点。起飞后对农田进行拍摄，其所在位置分别为B、C、D点。农田的角落为H、J、K、L点。

图2：在拍摄的图片中，农田角落的像素坐标分别为[u_H v_H][u_J v_J][u_K v_K][u_L v_L]。

图3：地面坐标系O_g-X_gY_gZ_g按照右手定则分布。机体坐标系中机头方向为Y_b轴，机头左侧为X_b轴，机体数值向下为Z_b轴。三维相机坐标系则是光轴为Z_c轴，相机指向上方为X_c轴，指向右方为为Y_c轴。

图4：棋盘格标定物为10×7的黑白相间的格子。其每个格子边长为3.8cm。

图5：1表示地面，2表示相机镜头，3表示相机的成像平面。由相似三角形原理即可求解。

图6：A、B、C、D、E五点为待测点。X_gY_gZ_g则分别表示地面坐标系的三条坐标轴。其中Z_g轴方向是垂直向上，亦即垂直图片平面指向阅读者。

图7：农田测量数据管理系统是MATLAB下的图形界面程序，其使用方法可参见下文。

具体实施方式

本发明提供了多旋翼飞行器利用视觉测量农田位置与边界的方法。

仿真与计算过程是在主频3.60Ghz，内存8.0GB的计算机上，Win7 SP1操作系统下的Matlab R2015b上进行。模拟室内实验对象采用的是小米max手机，其包含有陀螺仪、磁力计、GPS、摄像头等传感器，可以类比为多旋翼飞行器。

(1)本发明利用小米max手机进行视觉测量农田位置与边界，该方法具体包含以下步骤：

步骤一：手机上相机标定

使用手机在不同角度与位置拍摄如图4所示的棋盘格标定物，得到标定物的多张图片(至少应有10张)。利用Matlab中的Camera Calibrator工具箱对获取的图片进行标定，并得到多旋翼摄像机的内部参数α_x,α_y,u₀,v₀。

步骤二：手机捕捉目标图片

使用三脚架稳定架设手机，在世界坐标系原点位置垂直俯视拍摄目标区域，并记录此时手机内部传感器测量的传感器数据，之后任意摆放三脚架，拍摄各特征点图片(如图6所示)，并在拍摄图片时记录手机内部传感器测量的传感器数据。此过程可类比实际测量中多旋翼飞行和拍摄的过程。

具体来说，将拍摄的图片中的待测特征点通过人工识别方法得到其像素坐标(u_k,i,v_k,i)，k＝1,…,N,i＝1,…,W。并得到手机相机在惯性系下的位置^GP_k＝[X_k Y_k Z_k]^T，手机的实时欧拉角φ_k,θ_k,ψ_k。以备之后计算使用。

步骤三：求取特征点位置

如图7所示，使用农田测量数据管理系统程序，分别输入农田的角点个数、图片个数后点击开始按钮，在右侧出现图片区域后(如图7所示)按照提示分别选择出特征点和障碍物区域，选择完成后，点击下一幅图片按钮，并重复上述操作。在出现“已处理全部图片，请进行计算”的提示后点击计算按钮，经过计算后，各特征点位置和障碍物区域中心位置、半径大小均显示在位置数据表中。

如图8所示，求取特征点位置方法主要分为3个部分。首先依据(4)式第一幅初始图像求取特征点位置的初值。在求得特征点位置初值后，利用(7-9)式优化求解出每帧对应的机体偏航角，从而求得每帧图像对应的旋转矩阵R和平移向量T。再按(10、11)式所示的方法，先令未知量为除原点外各特征点的地面坐标进行优化，再令未知量为各特征点的地面坐标和机体偏航角共同进行优化，将两次优化的地面特征点坐标值求平均值得到最终特征点坐标的位置。

(2)室内实验结果分析

通过该方法利用小米MAX手机在室内的实验结果如下。

首先对其后置摄像头进行标定，标定结果在附录中如下：α_x＝3457.9α_y＝3448.9u₀＝1722.5v₀＝2294.7。

使用手机后置摄像头拍摄地面特征点，其图片类似图6所示，地面坐标系也一并在图中给出。其实际的欧拉角、相机不同的位置参数均附在附录中。

最后附上计算结果与特征点实际位置的比较。

	实际位置(mm)	计算位置(mm)
			A点	[0 0 0]
B点	[4651900]	[476.55181.68 0]
			C点	[165 6800]	[178.43 687.18 0]
D点	[-600 600 0]	[-594.72 601.65 0]
			E点	[-600 80 0]	[-599.62 75.26 0]

实际拍摄位置所在高度处在1070mm-1530mm高度范围内，而最大距离相对误差(测量值与实际值二维误差)均小于15mm，距离误差相对拍摄高度误差率均小于1.5％。多数点的距离误差相对拍摄高度误差率小于1％。

可以看到误差均较小，证明本发明是可行的。

Claims (6)

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

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

2.根据权利要求1所述的一种基于多旋翼飞行器测量农田边界与内部障碍的方法，其特征在于：该工具箱为MATLAB R2015b中的相机标定工具箱Camera Calibrator。

3.根据权利要求1所述的一种基于多旋翼飞行器测量农田边界与内部障碍的方法，其特征在于：适当的高度范围是30-100m。

4.根据权利要求1所述的一种基于多旋翼飞行器测量农田边界与内部障碍的方法，其特征在于：适当的高度最大不会超过200m。

5.根据权利要求1所述的一种基于多旋翼飞行器测量农田边界与内部障碍的方法，其特征在于：利用小米max手机进行视觉测量农田位置与边界。

6.根据权利要求1所述的一种基于多旋翼飞行器测量农田边界与内部障碍的方法，其特征在于：拍摄的图片至少应有10张。

Images