土地调查小型无人机的云台实时控制系统及控制方法与流程

本发明涉及无人机技术及云台控制技术，尤其是涉及一种基于图像角度测算系统。

背景技术：

土地是立国之根本，在国民经济发展和基础设施建设中占有十分重要的地位。随着国民经济的快速发展，我国政府更是迫切需要一种技术手段能够精确高效地完成土地信息的采集和绘制。目前我国城镇土地调查主要使用第二次全国土地调查的GIS系统、GPS-RTK系统与惯导组合和光学全站仪。原有的侦测手段都有灵活机动性不足、精度不够和受地形地貌的影响因素，影响土地调查的效率。

目前军用无人机的研发已经逐渐进入到了实战测试阶段，相比于军用无人机续航里程远、载荷大、电子对抗性强等特点，民用无人机要求低空作业、轻型小型便于操控、价格低廉、故障损毁后造成的损失较小等特点，因此民用无人机应用于商业市场上有着广阔的前景。目前超小型无人机的机载云台系统具有特殊的要求，例如对云台的体积和重量的限制，以及在应对空中复杂气流环境时需要云台保持稳定的角度和灵活的转动；需要任何载荷能够根据遥控指令进行调整或保持一个特定角度，从而保证任务载荷进行有效地观测，即需要一种具有惯性姿态稳定的云台系统。而现有的云台系统现有云台往往利用加速度传感器来检测姿态特征，当携带云台的无人机处于加速飞行过程中，加速度计的输出将包含机体加速度信息，则单纯利用加速度计获得云台姿态角将含有较大的误差。

考虑到土地调查地形地貌和空中管制等因素，选择使用无人机作为云台的搭载设备。随着近年来材料学和微控制技术的发展，无人机技术近年来发展迅速。从飞行方式来看无人机主要有固定翼、扑翼和多旋翼这三种方式。因固定翼体积较大而扑翼抗风能力较弱的原因，多旋翼飞机因其优异的操控性成为目前中短距离无人机的首选。

在无人机飞行的过程中需要实时的对土地信息进行拍照采集，在图像采集的过程中因其受到风力、自身机身抖动和路径变更等影响会改变其飞行姿态，在没有搭载云台的情况下所拍摄的图像会出现严重的对焦模糊的情况对后期的处理造成很大的难度。搭载云台后会解决图像不稳的情况，但是为了保证根据像控点测量土地信息的精确度，必须保证摄像头与土地保持垂直，这样对云台控制技术提出了更高的要求。

技术实现要素：

本发明所要解决的技术问题是针对上述现有技术存在的MEMS陀螺仪漂移和云台控制的不足，而提供一种多控制系统融合的云台实时控制系统及控制方法，解决了小型四旋翼飞机载荷小的情况下对于摄像头的稳态控制。通过将MEMS陀螺仪漂移自整定、模糊PID控制与自适应卡尔曼滤波的结合、在线电子摄像模块对于PID参数的线下修正，提高了摄像的稳定质量，降低了图像后期的处理难度，提高了作业环境的复杂程度。

为解决上述技术问题，本发明的技术方案是：

一种土地调查小型无人机的云台实时控制系统，其特征在于，包含：

云台三轴驱动系统、用于调整无人机的倾斜角度；

摄像机、用于获取地面的图像信息，并根据地面上设置的参考点计算摄像机的倾斜角度；

MEMS传感器、用于获取无人机的倾斜角度；

处理器，包括加权平均模块及PID算法控制模块，所述加权平均模块对所述MEMS传感器获取的无人机的倾斜角度和摄相机得到的摄像机倾斜角度进行加权平均处理得到最终的无人机倾角；所述PID算法控制模块将最终的无人机倾角作为输入，计算出所述云台三轴驱动系统需要调整的角度；

以及核心控制器，根据所述处理器得到的所述云台三轴驱动系统需要调整的角度，控制所述云台三轴驱动系统进行调整。

所述处理器还包括MEMS漂移自整定模块以及自适应卡尔曼滤波算法模块，所述MEMS漂移自整定模块用于消除所述MEMS陀螺仪自身的漂移误差；所述自适应卡尔曼滤波算法模用于对所述MEMS陀螺仪获取的倾斜角度进行滤波去噪处理。

所述处理器还包括电子摄像模块，该电子摄像模块对所述对拍摄到的照片基于相控点进行角度解算的处理。

所述PID算法控制模块为模糊PID算法控制模块。

所述云台三轴驱动系统为三轴电机。

一种土地调查小型无人机的云台实时控制方法，其特征在于，步骤为：

S1、在需要测量的区域标定放置参考点，以此作为摄像机的倾角计算的参考点；

S2、无人机升空在垂直地面航拍的过程中摄像机根据参考点计算出其摄像头的倾斜角度；同时无人机输出MEMS传感器获取的无人机的倾斜角度；

S3、通过MEMS传感器和摄相机得到的倾斜角度加权平均后作为最终的输出角度输入到PID算法控制模块中；

S4、模糊PID算法控制模块根据设定的模糊规则，计算出最终的云台三轴驱动系统需要调整的角度；云台三轴驱动系统根据接收的需要调整的角度信息进行调整。

步骤S2中，还需对MEMS传感器获取的无人机的倾斜角度进行卡尔曼滤波处理。

所述云台三轴驱动系统为三轴电机，所述需要调整的角度信息以PWM信号的形式驱动所述三轴电机。

核心控制器首先根据前一时刻的角度值算出此时刻角度偏差变换率，然后将角度偏差和角度偏差变化率作为模糊PID的输入参数。模糊PID相较于普通PID控制有更快的响应速度和较小的系统超调，相较于其他复杂的控制算法应用于有限硬件资源的效率较高。模糊PID控制器根据模糊规则库制定的标准实施的改变PID控制器的三个参数K_P、K_I和K_D。但是小型无人机飞行时受到的干扰很大其主要来自于测量干扰和控制干扰，当控制系统引入这些未知噪声后会出现较大的稳态误差，因此需要加入自适应卡尔曼滤波来克服未知噪声带来的扰动。当模型存在摄动时，会引起噪声统计特性的变化，可以通过噪声的统计估值器来对过程噪声和观测噪声的一、二阶矩阵进行估计，从而调整卡尔曼滤波的增益，提高滤波精度。但是云台系统如果本身存在很大的误差其自身形成的闭环系统无法纠正，根据每帧拍摄的图像进行在线的解算，比较每幅图片同一特征点的重合度，建立矢量坐标系解算出帧与帧之间的重合度误差，计算每幅图像的外方元素，根据解算的外方元素求得摄像机与地面垂直偏差的角度。根据角度误差大小实时更改模糊PID和自适应卡尔曼滤波的增益。

有益效果

本发明使得航拍测量无人机在飞行姿态控制和云台稳定控制两方面都能做到理想的精度要求。特别是加入了独特的线上图像角度测算方法，真正根据实际拍摄的图像特征对云台进行控制，使得无人机能适应多重复杂环境；同时本发明将模糊PID控制与自适应卡尔曼滤波相结合，可以获得稳定的图像，从而减轻图像后期处理的难度。

附图说明

图1是本发明控制系统原理框图。

图2为本发明控制方法原理框图。

具体实施方式

下面结合附图，对本发明作详细说明：

如图1所示，本发明一种土地调查小型无人机的云台实时控制系统，包含：

云台三轴驱动系统、用于调整无人机的倾斜角度；

摄像机、用于获取地面的图像信息，并根据地面上设置的参考点计算摄像机的倾斜角度；

MEMS传感器、用于获取无人机的倾斜角度；

处理器，包括加权平均模块、PID算法控制模块、MEMS漂移自整定模块、自适应卡尔曼滤波算法模块以及电子摄像模块，加权平均模块对所述MEMS传感器获取的无人机的倾斜角度和摄相机得到的摄像机倾斜角度进行加权平均处理得到最终的无人机倾角；PID算法控制模块将最终的无人机倾角作为输入，计算出云台三轴驱动系统需要调整的角度；MEMS漂移自整定模块用于消除MEMS陀螺仪自身的漂移误差；自适应卡尔曼滤波算法模用于对MEMS陀螺仪获取的倾斜角度进行滤波去噪处理；电子摄像模块对对拍摄到的照片基于相控点进行角度解算的处理。

以及核心控制器，根据所述处理器得到的云台三轴驱动系统需要调整的角度，控制云台三轴驱动系统进行调整。

PID算法控制模块为模糊PID算法控制模块。

云台三轴驱动系统为三轴电机。

一种土地调查小型无人机的云台实时控制方法，步骤为：

S1、在需要测量的区域标定放置参考点，以此作为摄像机的倾角计算的参考点；

S2、无人机升空在垂直地面航拍的过程中摄像机根据参考点计算出其摄像头的倾斜角度；同时无人机输出MEMS传感器获取的无人机的倾斜角度；

S3、通过MEMS传感器和摄相机得到的倾斜角度加权平均后作为最终的输出角度输入到PID算法控制模块中；

S4、模糊PID算法控制模块根据设定的模糊规则，计算出最终的云台三轴驱动系统需要调整的角度；云台三轴驱动系统根据接收的需要调整的角度信息进行调整。

步骤S2中，还需对MEMS传感器获取的无人机的倾斜角度进行卡尔曼滤波处理。

云台三轴驱动系统为三轴电机，需要调整的角度信息以PWM信号的形式驱动三轴电机。

实施例

本发明控制系统，在无人机机架上下方合适的位置安装可以控制三个相互垂直方向的无刷电机，以此为摄像机搭设云台的控制平台。还需要在无人机上搭设最小飞控系统pixhawk，pixhawk自带MEMS传感器同时能够在一定的程度上稳定机身的平稳。

在需要测量的区域标定放置参考点，以此作为单目摄像机的倾角计算的参考点。

准备工作完成后，无人机升空在垂直地面航拍的过程中单目摄像机根据参考点计算出其摄像头的倾斜角度，同时无人机安装的pixhawk通过串口输出无人机的三个方向上的加速度、速度和倾斜角度，倾斜角度经过卡尔曼滤波减少系统的白色噪声误差的影响。

以ARMA拟合建立陀螺仪输出角度的一阶模型为基础，系统状态方程为：

x(k)＝Ax(k-1)+ω(k-1)

系统的测量方程为：

y(k)＝Cx(k)+v(k)

滤波估计方程为：

滤波增益方程为：K(k)＝P₁(k)C^T[CP₁(k)C^T+R(k)]^-1

均方预测误差：P₁(k)＝AP(k-1)A^T+Q(k-1)

均方滤波误差：P(k)＝P₁(k)-K(k)CP₁(k)

其中Q(k)＝E[ω(k)ω(k)^T]，R(k)＝E[v(k)v(k)^T]，ω(k),v(k)都是白噪声。

将MEMS陀螺仪输出的角度经过AD采样后输入至处理器中，从而得到系统的测量值y(k)。根据上述卡尔曼滤波方程计算各个参量值，并最终解算出滤波估计方程中的作为实际的MEMS陀螺仪输出的角度值。

通过MEMS传感器和单目摄相机得到的倾斜角度加权平均后作为最终的输出角度。输入到模糊PID控制系统中。

使用单向空间后方交会方法计算出外方元素的三个参数：航向倾角、旁向倾角和像片旋角。算法求解过程如下:

(a)在对准备进行航拍的土地上放置四个控制点地面坐标，在外业测量过程中获取控制点的地面测量坐标，并转化成地面摄影测量坐标：

X_S1,Y_S1,Z_S1,X_S2,Y_S2,Z_S2,X_S3,Y_S3,Z_S3,X_S4,Y_S4,Z_S4。

(b)从摄影资料中查取像片比例尺m，平均航高H，内方位元素摄影机主距f。

(c)测量控制点的像点坐标：在拍摄的图片中根据像素计算出像点坐标x、y。

(d)确定未知数的初始值：在竖直摄影的情况下角元素初始值为线元素初始值Z_S0＝H＝mf，

(e)将上一步得到的初始值带入R矩阵，计算其各个参数

b₁＝cosωsinκ

b₂＝cosωcosκ

b₃＝-sinω

(f)根据上一步计算得到的各个参数带入共线方程中计算得到x₀和y₀。

共线方程为：

$x=-f\frac{a_1(X-X_S)+b_1(Y-Y_S)+c_1(Z-Z_S)}{a_3(X-X_S)+b_3(Y-Y_S)+c_3(Z-Z_S)}$

$y=-f\frac{a_2(X-X_S)+b_2(Y-Y_S)+c_2(Z-Z_S)}{a_3(X-X_S)+b_3(Y-Y_S)+c_3(Z-Z_S)}$

(g)计算A矩阵，其中

$\frac{\∂x}{\∂X_S}=\frac{1}{\stackrel{\‾}{z}}(a_{1}f+a_{3}x),\frac{\∂x}{\∂Y_S}=\frac{1}{\stackrel{\‾}{z}}(b_{1}f+b_{3}x)$

$\frac{\∂x}{\∂\mathrm{\ω}}=-fsin\mathrm{\κ}-\frac{x}{f}(xsin\mathrm{\κ}+ycos\mathrm{\κ}),\frac{\∂x}{\∂\mathrm{\κ}}=y$

$\frac{\∂y}{\∂X_S}=\frac{1}{\stackrel{\‾}{z}}(a_{2}f+a_{3}y),\frac{\∂y}{\∂Y_S}=\frac{1}{\stackrel{\‾}{z}}(b_{2}f+b_{3}y)$

$\frac{\∂y}{\∂\mathrm{\ω}}=-fcos\mathrm{\κ}-\frac{x}{f}(xsin\mathrm{\κ}+ycos\mathrm{\κ}),\frac{\∂x}{\∂\mathrm{\κ}}=-x$

$\stackrel{\‾}{z}=a_3(X-X_S)+b_3(Y-Y_S)+c_3(Z-Z_S)$

(h)根据平差最小二乘法计算和上一步计算出的A矩阵，计算出近似值的改正数矩阵：

X_S＝X₀+dX_S

Y_S＝Y₀+dY_S

Z_S＝Z₀+dZ_S

ω＝ω₀+dω

κ＝κ₀+dκ

(i)判断矩阵A中的参数是否满足要求，如果满足要求则结束，如果不满足要求则根据上面计算得到的X_S,Y_S,Z_S,ω,κ，带入到R矩阵中各个参数重复(d)-(i)步骤。

模糊PID控制系统根据之前制定的模糊规则，计算出最终3个无刷电机需要调整的角度，以PWM信号的形式驱动云台上的三轴电机。

根据摄影测量和卡尔曼滤波计算得到的两个角度值，计算两个角度的平均值作为模糊PID的控制输入。

给定角度减去测量角度得到误差绝对值|E|和误差变化绝对值|Ec|，|Ec|的模糊集合作为输入语言变量，将Kp，Ki，Kd的模糊集合作为输出语言变量。输入、输出语言变量的语言值分为“大(L)”、“中(M)”、“小(S)”三个等级：

(1)当误差绝对值|E|较大时，为加快系统响应速度，应取较大的Kp；同时为了避免系统在初始时，由于误差的瞬时增大可能出现的微分饱和而使控制作用超出允许的范围，此时，应取较小的Kd。同时，为了防止系统响应出现较大的超调，产生积分饱和，对积分作用应加以限制，所以，此时应取Ki＝0。

(2)当误差|E|适中时，为使系统具有较小的超调，应取稍小的Kp；此时Kd的取值对系统的影响较大，所以Kd的值要大小适中，以保证系统的响应速度。同时，可增加一些积分对控制的作用，Ki太大，易造成积分饱和，太小不能加快系统响应速度，所以Ki取值要适当。

(3)当误差|E|较小时，为使系统具有良好的稳态性能，应取较大的Kp、Ki；同时为避免系统在设定值附近产生振荡，Kd值的选择非常重要。一般情况下：当|E|较小时，Kd可取大些，当|E|变大时，Kd可取小些。

下表为基于以上分析和语言变量的定义总结出的Kp、Ki、Kd的自调整规则：

表1

表2

表3

系统某时刻误差、误差变化率的绝对值|E|、|Ec|已知，现在根据|E|、|Ec|要求PID调节器的Kp、Ki、Kd。以求Kp为例说明推理方法。

根据表1，可将每条Kp调整规则写出来，如第一条可写为：

R₁:if|E|＝L and|EC|＝L then K_p＝M

该规则隶属度的计算方法为：

μ_kPi(c_p)＝μ_LE(|E|)∧μ_LEC(|EC|)

同理，可求出关于Kp的其他所有规则的隶属度μK_pi(c_p)(i＝1，2，...，n)，其中，n为有关Kp的所有规则的条数，为第i条规则中Kp所取模糊集合的中心值。假定系统在某时刻误差、误差变化率的绝对值|E|、|EC|已知，Kp的计算公式为：

$K_P=\frac{\underset{i=1}{\overset{n}{\Σ}}\left({\mathrm{\μ}}_{K_{Pi}}\right(c_p)\×c_{pi})}{\underset{i=1}{\overset{n}{\Σ}}{\mathrm{\μ}}_{K_{Pi}}\left(c_p\right)}$

同理，可得到Ki、Kd计算公式，如下(其中，m、l分别为有关Ki、Kd的所有规则的条数。)：

$K_I=\frac{\underset{i=1}{\overset{m}{\Σ}}\left({\mathrm{\μ}}_{K_{Ii}}\right(d_I)\×d_I)}{\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\μ}}_{K_{Ii}}\left(d_I\right)}$

$K_D=\frac{\underset{i=1}{\overset{l}{\Σ}}\left({\mathrm{\μ}}_{K_{Di}}\right(g_D)\×g_{Di})}{\underset{i=1}{\overset{l}{\Σ}}{\mathrm{\μ}}_{K_{Di}}\left(g_D\right)}$

从上面两个公式中可以看出，Kp、Ki、Kd与误差|E|、|EC|之间建立了一种函数关系，满足了系统在不同的E、EC状态下对PID控制参数的不同要求。

在云台不断调整姿态的过程中其输出角度不断变化，因为现有的MEMS陀螺仪有自身的漂移误差造成测量并不准确。通过航拍摄像和对MEMS陀螺仪卡尔曼滤波减小漂移误差对系统的影响，最后将角度平均值输入模糊PID控制器对云台进行控制。