技术领域
[0001] 本
发明属于飞行器控制技术领域，特别适用于一种微型共轴双旋翼飞行器控制装置及方法。
背景技术
[0002] 目前，因自主
导航系统一般体型较大，要实现具有自主飞行能
力的飞行器的微型化难度很大，而且系统随着自主化程度的提高以及功能的增多体积也随之增大，研究集尺寸微型、功能强大于一体的飞行器还是一片空白。
发明内容
[0003] 为解决已有技术存在的不足，本发明提供一种微型共轴双旋翼飞行器控制装置及方法，通过控制微型共轴双旋翼飞行器的
舵机和
电机，以达到精确控制的目的。
[0004] 本发明的技术方案是这样实现的：该装置包括地面装置和空中装置两部分；其中地面装置包括视频无线接收机、地面无线传输模
块、地面处理器、显示器和上位机；空中装置包括有三轴磁航向
传感器、三轴
陀螺仪、三轴
加速度传感器、GPS、气压计、
超声波传感器、ARM处理器、微型无线摄像头、视频解码模块、视频无线传输模块和空中无线传输模块；
[0005] 所述的地面装置的连接是：视频无线接收机输出端连接显示器的输入端，地面无线传输模块与地面处理器通过同步串口端连接；地面处理器与上位机通过异步串口端连接；
[0006] 所述的空中装置安装在飞行器上，空中装置的连接是：三轴磁航向传感器、三轴陀螺仪、三轴加速度传感器、GPS、气压计和
超声波传感器的输出端均连接ARM处理器相应的输入端，微型无线摄像头的一个输出端通过视频解码模块连接ARM处理器的输入端，微型无线摄像头的另一个输出端连接视频无线传输模块的输入端，ARM处理器与空中无线传输模块通过同步串口端连接；
[0007] 其中地面装置和空中装置通过其中地面无线传输模块和空中无线传输模块实现无线通讯，视频无线接收机与视频无线传输模块实现无线通讯。
[0008] 所述的微型共轴双旋翼飞行器控制装置的控制方法，包括如下步骤：
[0009] 步骤一、装置初始化；
[0011] 依次采集三轴加速度传感器，三轴磁航向传感器、三轴
角速度陀螺仪、
超声波传感器、气压计和GPS的记录数据：
[0012] (1)将三轴加速度传感器三个轴向输出的模拟
电压信号经AD转换成
数字信号，对信号进行滤波，并储存飞行器X、Y、Z轴上的加速度ax、ay、az；
[0013] (2)读取三轴磁航向传感器上地
磁场在三个轴上的数字磁场分量，并确定飞行器与地磁线所成夹角
[0014] 具体如下：
[0015] 如果Hy＞0，则：
[0016]
[0017] 如果Hy＜0，则：
[0018]
[0019] 如果Hy＝0，Hx＜0则：
[0020]
[0021] 如果Hy＝0，Hx＞0则：
[0022]
[0023] (3)将
俯仰、翻滚陀螺仪，
偏航陀螺仪输出的模拟电压信号经AD转换成数字信号，对信号进行滤波，并储存飞行器俯仰角速度Ωx、翻滚角速度Ωy、偏航角速度Ωz；
[0024] (4)读取数字气压计的气压值p，并确定绝对高度，方法如下式：
[0025]
[0026] 式中：p0为海平面气压，其值为1013.25hPa；H′为绝对高度，单位为米；
[0027] 如果是第一次测量气压值，则将H′赋值给外部变量Hs，计算飞行器飞行的相对高度：
[0028]
[0029] 在低空环境下采用超声测距：发射方波，打开接收中断，记录发射与接收的时间间隔T，如果固定时间内接收到返回接收信号，则计算相对高度H＝344*T/2，并代替公式(6)计算得到的相对高度，否则说明测量超过超声波量程，关闭接收中断，储存高度值H；
[0030] (5)确定飞行器的经度和纬度；
[0031] 读取GPS信号，得到飞行器的经度E和纬度N，并储存，如果是第一次得到飞行器的经度E和纬度N，则分别赋值给外部变量E0、N0，作为
起飞点坐标；
[0032] 步骤三、确定飞行模式、开始起飞、飞行高度、飞行速度和飞行目标点命令；
[0033] 将步骤二测到的各路传感器数据组合成数据包后通过机载Zigbee无线发送给地面Zigbee部分，并且接收来自地面控制界面通过地面Zigbee发送过来的控制命令数据包，解析数据包后得到飞行模式、开始起飞、飞行高度、飞行速度和飞行目标点命令；
[0034] 步骤四、通过相对高度判断飞行器是否起飞，当相对高度为零，飞行器没有起飞；当相对高度为正值，飞行器已经起飞，如果没有起飞，转入步骤四；如果已经起飞，转入步骤五；
[0035] 步骤五、空中装置ARM处理器通过地面发过来的控制命令数据包选择飞行器的飞行模式；
[0036] 把遥控模式、航线设定模式、自主循迹模式三种飞行模式赋予不同的数据，通过与地面发过来的控制命令数据包中相应的数据进行比较，选择相应的飞行模式；
[0037] 步骤六、确定飞行器控制增量；
[0038] 飞行器有四个控制通道，分别为上升下降，前进后退，左右平移，左右摆动，飞行器的
姿态的控制可以分解成对这四个通道的控制，各个通道采用下例式计算出控制增量：
[0039] Δu(k)＝kp×ΔE(k)+ki×E(k)+kd[E(k)-2E(k-1)+E(k-2)]
[0040] +k11×Δa(k)+k12×a(k)+a13[a(k)-2a(k-1)+a(k-2)]
[0041] +k21×ΔΩ(k)+k22×Ω(k)+k23[Ω(k)-2Ω(k-1)+Ω(k-2)]
[0042] 式中：kp、k11、k21分别为被控量、加速度、角速度的比例系数；ki、k12、k22分别为被控量、加速度、角速度的积分系数；kd、k13、k23分别为被控量、加速度、角速度的微分系数；E(k)、a(k)、Ω(k)分别为被控量、加速度、角速度的系统偏差；ΔE(k)、Δa(k)、ΔΩ(k)分别为被控量、加速度、角速度的偏差变化率；ΔE(k)＝E(k)-E(k-1)，Δa(k)＝a(k)-a(k-1)，ΔΩ(k)＝Ω(k)-Ω(k-1)；k，k-1，k-2分别表示第k次，第k-1次，第k-2次；
[0043] 步骤七、控制飞行器的舵机和电机；
[0044] 通过飞行器上的ARM处理器控制PWM寄存器控制飞行器的舵机和电机；其中因舵机的转角与控制舵机的脉冲宽度成线性关系，脉冲宽度又是由ARM处理器内部PWM寄存器值来设置的，通过ARM处理器计算改变PWM寄存器值来控制飞行器左右平移和前进后退；飞行器上两个电机形成转速差时会产生旋转力矩通过控制，该力矩可以控制飞行器左右方向摆动，当两电机同比增速或减速时则能控制飞行器的上升和下降；
[0045] (1)舵机控制
[0046] 左右平移、前进后退运动相对
悬停状态下两舵机角度变化相互关系为：
[0047] 左平移：左舵机角度变化量为+θ左移，右舵机角度变化量为-θ左移[0048] 右平移：左舵机角度变化量为-θ右移，右舵机角度变化量为+θ右移[0049] 前进：左舵机角度变化量为-θ前进，右舵机角度变化量为-θ前进
[0050] 后退：左舵机角度变化量为+θ后退，右舵机角度变化量为+θ后退；
[0051] (2)电机控制
[0052] 两个电机转动方向相反，当两个电机形成转速差时会产生旋转力矩，该力矩可以控制飞行器左右方向摆动，当向右方向转动的电机转速大于向左方向转动的电机转速时，飞机向右方向摆动；当向左方向转动的电机转速大于向右方向转动的电机转速时，飞机向左方向摆动，两个电机同比增速，可以控制飞行器上升；两个电机同比减速减时，可以控制飞行器的下降；
[0053] 电机采用增量式PID控制：
[0054] Δu1(k)＝kp×ΔE(k)+ki×E(k)+kd[E(k)-2E(k-1)+E(k-2)]
[0055] 式 中，Kp，Ki，Kd分 别 是PID 控 制 中 比 例，积 分，微 分 控 制 参 数；E(k)-2E(k-1)+E(k-2)为系统量变化率，在采集数据的时间点上，将得到的值作为PWM脉宽的变化量来控制电机；
[0056] 步骤八、通过GPS信号获取飞机所在
位置，与目的地的经纬度进行比较，如果两者不等，飞机未到达目的地，转入步骤二；如果两者相等，飞机到达目的地，给定高度逐渐减小，飞行器降落。
[0057] 所述的步骤五，空中装置ARM处理器通过地面发过来的控制命令数据包选择飞行器的飞行模式，按如下步骤进行：
[0058] 飞行模式分为三种：遥控模式、航线设定模式、自主循迹模式；
[0059] (1)遥控模式
[0060] 计算出遥控信号左右舵机和前后电机对应的通道的遥控脉冲宽度，进而处理得到飞行器左右舵机控制脉冲宽度和前后电机PWM
控制信号的周期和占空比；
[0061] (2)航线设定模式
[0062] 飞行器飞行的方向即与地磁线的夹角为：
[0063]
[0064] 飞行器偏移设定航线的距离为：
[0065]
[0066] 其中E、N分别为本次测得的经度和纬度，En、Nn分别为飞行器由步骤三得到的下一个目标点的经度和纬度，E0、N0分别作为起飞点坐标，其中l1、l2和k3分别为飞行器偏移
设定航线在X，Y和Z轴的距离；
[0067] (3)自主循迹模式
[0068] 将微型无线摄像头采集的
视频信号提取白布上黑线，在图像上形成的路径的左右上下边界，计算出飞行器偏移路径的角度和距离，具体步骤如下：
[0069] ①采集图像数据
[0070] 采取了降低图像
分辨率的方法每行采集至少48个点，每幅
图像采集至少40行，图像分辨率至少为48×40；
[0072] 图像采第1场的第1行数据用双峰法计算出该行的阈值，将该图像中大于等于阈值的点赋值为1，小于阈值的点赋值为0，实现二值化处理，第2行阈值引用第1行的阈值，并实现二值化处理，将所有值为1的点所对应的数据相加取平均值，将所有值为0的点所对应的数据相加取平均值，两个平均值再取平均值便得到第2行的阈值，并与上一行的阈值进行比较，在允许的范围内更新，如此
迭代至最后一行，最后一行的阈值则作为下一场第一行的阈值，如此反复迭代；
[0073] ③判断每行图像二值化情况
[0074] 每行图像从起点到终点进行判断，如果该行仅一次出现值为“0-1-0”变化，则认为该行图像二值化情况良好；
[0075] ④搜索路径的左右边界、上下边界
[0076] 从第1行起依次搜索该场图像的其他行的图像二值化情况，如果连续3行是二值化情况良好，则停止搜索，并把该行作为路径搜索起始行ist，以ist作为起始行向上即行标号增加的方向和向下即行标号减小的方向对各行进行路径边界的搜索；
[0077] 路径左边界向上搜索：
[0078] 将路径左边界向上一行延伸，如果上一行该处值为0的点，则向左搜索路径左边界，如果搜索超出边界还没搜索到则用边界值代替；如果该处值为1的点，则向右搜索路径左边界：
[0079] 路径右边界向上搜索：
[0080] 将路径右边界向上一行延伸，如果上一行该处值为0的点，则向右搜索路径右边界；如果该处值为1的点，则向左搜索路径右边界；
[0081] 如果搜索到的路径边界出现左路径边界值大于右路径边界值，左右边界太宽即为0点个数大于10个或左右边界值之间有值为0的点，则尝试跳过行，该行的路径边界值用前两行边界的平均值进行补充，如果向上路径搜索过程中总跳跃次数达到3次或搜索达到顶端则结束搜索，同时可以得到路径上边界值b，并计算出上边界左右两边界的中心值B；
[0082] 向下搜索同理可得到路径下边界值a，计算下边界左右两边界的中心值A，如果搜索到的有效路径长度小于3行时，则认为本场图像路径提取失败，如果连续3场图像路径提取失败则，则将起飞点坐标作为下一个目标点坐标，转至步骤①；
[0083] ⑤飞行器飞行方向与路径方向夹角大小为：
[0084]
[0085] 飞行器路径在图像上偏离的距离为：
[0086]
[0087] 其中a为路径下边界值，A为下边界左右两边界的中心值，b为路径上边界值，B为上边界左右两边界的中心值，M为行图像采集的数据个数。
[0088] 其中Zigbee是IEEE802.15.4协议的代名词，根据这个协议规定的是一种短距离、低功耗的无线通信技术。
[0089] 本发明的优点：本微型共轴双旋翼飞行器控制系统以微型四通道共轴双旋翼
直升机为载体，该载体体积小、效率高，机载控制系统选用了
内核ARM
微处理器作为CPU，提高了资源的利用率，三轴加速度传感器、三轴磁航向传感器、三轴角速度陀螺仪、超声波传感器、气压计，采用了芯片级的传感器，采用GPS
定位模块，微型无线摄像头。无线通讯采用了Zigbee无线通讯方式，其能够方便的实现
机器人群体的组网通讯。该系统能实现航拍和任务设定自主飞行以及地面寻迹自主飞行。自动手动可方便切换。
附图说明：
[0091] 图2本发明地面无线传输模块/空中无线传输模块电原理图；
[0092] 图3本发明三轴磁航向传感器电原理图；
[0093] 图4(a)本发明三轴陀螺仪-俯仰和翻滚陀螺仪电原理图；
[0094] 图4(b)本发明三轴陀螺仪-偏航陀螺仪电原理图；
[0095] 图5本发明三轴加速度传感器电原理图；
[0096] 图6本发明GPS电原理图；
[0097] 图7本发明气压计电原理图；
[0098] 图8本发明ARM处理器电原理图；
[0099] 图9本发明视频解码模块电原理图；
具体实施方式
[0101] 本发明一种微型共轴双旋翼飞行器控制装置及方法结合施实例加以说明。
[0102] 超声波传感器选用SRF05模块；
[0103] 微型无线摄像头、视频无线传输模块和视频无线接收机选用深圳市联益达科技有限公司型号为RC211A+208CWAS无线摄像机的部分器件；地面无线传输模块如图1所示；三轴磁航向传感器如图2所示；三轴陀螺仪如图3所示；三轴加速度传感器如图4(a)和图4(B)所示；GPS如图5所示；气压计如图6所示；ARM处理器选用型号为STM32F103，如图7所示；视频解码模块如图8所示；空无线传输模块的电原理结构和地面无线传输模块相同，空无线传输模块如图1所示；
[0104] 地面处理器包括ARM处理器、MAX3232、串口、串口线，各部件的连接是：ARM处理器的USART1的RX、TX引脚与相应的MAX3232引脚相连接，串口与相应的MAX3232引脚相连接。
[0105] 该装置包括地面装置和空中装置两部分；其中地面装置包括视频无线接收机、地面无线传输模块、地面处理器、显示器和上位机；空中装置包括有三轴磁航向传感器、三轴陀螺仪、三轴加速度传感器、GPS、气压计、超声波传感器、ARM处理器、微型无线摄像头、视频解码模块、视频无线传输模块和空中无线传输模块；
[0106] 其中地面无线传输模块如图1所示，包括CC2520(U15)、电容(C66、C69、C70、C71、C72、C73、C74、C75、C76)、
电阻(R43)、电感(TL1、TL2、TL3)、天线(T2)和晶振(Y4)，各部件的连接是：电容(C66)一端接地，另一端与CC2520(U15)的DCOUPL引脚相连接；电阻(R43)一端接地，一端与CC2520(U15)的RBIAS引脚相连接；天线(T2)与电容(C70)和电感(TL2)的一端相连接，电容(C70)的另一端接地，电感(TL2)的另一端与电容(C69)、电容(C72)和电感(TL3)的一端相连接，电容(C69)的另一端接地，电容(C72)的另一端与电容(C71)和电感(TL1)的一端相连接，电感(TL1)的另一端接地，电容(C71)的另一端与CC2520(U15)的RF-N引脚相连接；电感(TL3)的另一端与电容(C73)和电容(C74)的一端相连接，电容(C74)的另一端接地，电容(C73)的另一端与CC2520(U15)的RF-P引脚相连接；电容(C75)一端接地，另一端与32M晶振(Y4)和CC2520(U15)的SOSC32M-2引脚相连接；电容(C76)一端接地，另一端与32M晶振(Y4)和CC2520(U15)的SOSC32M-2引脚相连接。
[0107] 地面装置器件之间的连接：地面无线传输模块(C2520)的输入引脚RESETn、VREG_EN引脚分别与地面处理器中的ARM处理器(STM32F103RC)的PC4、PB6引脚相连接，SPI
接口的SCLK、SO、SI、CSn引脚分别与ARM处理器(STM32F103RC)的PA5、PA6、PA7、PA4引脚相连接，输入/输出双向引脚GPI00、GPI01、GPI02、GPI03、GPI04、GPI05分别与ARM处理器(STM32F103RC)的PB0、PC5、PB15、PB14、PB13、PB12引脚相连接；地面处理器的串口通过串口线与上位机连接；视频无线接收机输出端连接显示器的输入端。
[0108] 空中装置器件之间的连接：三轴磁航向传感器(HMC5843)的I2C接口SCL、SDA引脚分别与ARM处理器(STM32F103RC)的PB10、PB11引脚相连接；三轴陀螺仪中的俯仰和翻滚陀螺仪(LPR530AL)的输入引脚HP与ARM处理器(STM32F103RC)的PC13引脚相连接，输出引脚4×OUX、4×OUY、Vref三个引脚分别与ARM处理器(STM32F103RC)的PC2、PC3、PA0引脚相连接；三轴陀螺仪中的偏航陀螺仪(LY330ALH)输出引脚OUZ、Vref引脚分别与ARM处理器(STM32F103RC)的PC0、PC1引脚相连接；三轴加速度传感器(MMA7260A)的输出端ACC_X、ACC_Y、ACC_Y分别与ARM处理器(STM32F103RC)的PA1、PA2、PA3引脚相连接；GPS(G591BGPS模块)的RXA、TXA和RXB、TXB引脚分别与ARM处理器(STM32F103RC)的PA9、PA10和PC10、PC11引脚相连接，输出引脚1PPS引脚与ARM处理器(STM32F103RC)的PA12引脚相连接；超声波传感器(SRF05模块)的输入/输出双向引脚TRIGGER INPUT/ECHO2
OUTPUT与ARM处理器(STM32F103RC)的PB7引脚相连接；气压计的IC接口SCL、SDA引脚分别与ARM处理器(STM32F103RC)的PB10、PB11引脚相连接；空中无线传输模块(C2520)的输入引脚RESETn、VREG_EN引脚分别与ARM处理器(STM32F103RC)的PC4、PA11引脚相连接，SPI接口的SCLK、SO、SI、CSn引脚分别与ARM处理器(STM32F103RC)的PA5、PA6、PA7、PA4引脚相连接，输入/输出双向引脚GPI00、GPI01、GPI02、GPI03、GPI04、GPI05分别与ARM处理器(STM32F103RC)的PB0、PC5、PB15、PB14、PB13、PB12引脚相连接；微型无线摄像头的一个输出端与视频解码模块(LMH1981)的输入端相连接；视频解码模块(LMH1981)的输出端HSOUT、OEOUTA与ARM处理器(STM32F103RC)的PB6、PB5相连接，无线摄像头的另一个输出端与视频无线传输模块的输入端相连接。
[0109] 所述的微型共轴双旋翼飞行器控制装置的控制方法，包括如下步骤，如图10所示：
[0110] 一、装置初始化；
[0111] 二、采集各路传感器数据
[0112] 依次采集三轴加速度传感器，三轴磁航向传感器、三轴角速度陀螺仪、超声波传感器、气压计、GPS的数据：
[0113] (1)将三轴加速度传感器三个轴向输出的模拟电压信号，实验测量为Vx＝1.1231V，Vy＝1.7741V，Vz＝1.6734V，经AD转换成数字信号，对信号进行滤波，并储存飞
2 2
行器X、Y、Z轴上的加速度ax、ay、az，实验测量为ax＝-7.8m/s、ay＝1.52m/s、az＝0.23m/
2
s ；
[0114] (2)读取三轴磁航向传感器上
地磁场在三个轴上的数字磁场分量，并确定飞行器与地磁线所成夹角
[0115] 具体如下：
[0116] 如果Hy＞0，则：
[0117]
[0118] 如果Hy＜0，则：
[0119]
[0120] 如果Hy＝0，Hx＜0则：
[0121]
[0122] 如果Hy＝0，Hx＞0则：
[0123]
[0124] 实验数据为
[0125] (3)将俯仰、翻滚陀螺仪，偏航陀螺仪输出的模拟电压信号经AD转换成数字信号，对信号进行滤波，并储存飞行器俯仰、翻滚、偏航角速度Ωx、Ωy、Ωz，实验数据为飞行器俯仰角速度：0.107rad/s；翻滚角速度：0.013rad/s；偏航角速度：0.361rad/s；
[0126] (4)读取数字气压计的气压值p，并确定绝对高度，方法见下式：
[0127]
[0128] 式中：P0为海平面气压，其值为1013.25hPa；H′为绝对高度，单位为米，测量结果为42m；
[0129] 如果是第一次测量气压值，则将H′赋值给外部变量Hs，计算飞行器飞行的相对高度：
[0130]
[0131] 发射方波，打开接收中断，用
定时器记录发射与接收的时间间隔T(单位为秒)，如果固定时间内接收到返回接收信号，则计算相对高度H＝344*T/2，并代替公式(6)计算得到的相对高度，否则说明测量超过超声波量程，关闭接收中断，储存高度值H，测试实际在低空环境，所以采用超声测距测量高度，测量得到T＝0.017s，计算得高度为2.92m；
[0132] (5)确定飞行器的经度和纬度；
[0133] 读取GPS信号，得到飞行器的经度E和纬度N，并储存，如果是第一次得到飞行器的经度E和纬度N，则分别赋值给外部变量E0、N0分别作为起飞点坐标，本地经纬度测量值为东经121°25’50.1″北纬31°13’20.8″；
[0134] 三、确定飞行模式、开始起飞、飞行高度、飞行速度、飞行目标点命令；
[0135] 将步骤二测到的各路传感器数据组合成数据包后通过机载Zigbee无线发送给地面Zigbee部分，并且接收来地面自控制界面通过地面Zigbee发送过来的控制命令数据包，解析数据包后得到飞行模式、开始起飞、飞行高度、飞行速度、飞行目标点命令；
[0136] 四、通过相对高度判断飞行器是否起飞，当相对高度为零，飞行器没有起飞；当相对高度为正值，飞行器已经起飞，如果没有起飞，转入步骤四；如果已经起飞，转入步骤五；
[0137] 五、空中装置ARM处理器通过地面发过来的控制命令数据包选择飞行器的飞行模式，把三种飞行模式赋予不同的数据，通过与地面发过来的控制命令数据包中相应的数据进行比较，选择相应的飞行模式；
[0138] 六、计算飞行器控制增量；
[0139] 飞行器的有四个控制通道，分别为上升下降，前进后退，左右平移，左右摆动，飞行器的姿态的控制可以分解成对这四个通道的控制，各个通道采用下例式计算出控制增量：
[0140] Δu(k)＝kp×ΔE(k)+ki×E(k)+kd[E(k)-2E(k-1)+E(k-2)]
[0141] +k11×Δa(k)+k12×a(k)+a13[a(k)-2a(k-1)+a(k-2)]
[0142] +k21×ΔΩ(k)+k22×Ω(k)+k23[Ω(k)-2Ω(k-1)+Ω(k-2)]
[0143] 式中：kp、k11、k21分别为被控量、加速度、角速度的比例系数，实验时得到的值分别为：80、90、30；ki、k12、k22分别为被控量、加速度、角速度的积分系数，实验时得到的值分别为：40、40、20；kd、k13、k23分别为被控量、加速度、角速度的微分系数，实验时得到的值分别为：45、50、30；E(k)、a(k)、Ω(k)分别为被控量、加速度、角速度的系统偏差，分别计算得：65、0.29、0.0021；ΔE(k)、Δa(k)、ΔΩ(k)分别为被控量、加速度、角速度的偏差变化率，三个偏差分别为35、0.73、0.04；ΔE(k)＝E(k)-E(k-1)，Δa(k)＝a(k)-a(k-1)，ΔΩ(k)＝Ω(k)-Ω(k-1)；k，k-1，k-2分别表示第k次，第k-1次，第k-2次，经过计算，控制增量Δu(k)为2347.57；
[0144] 七、舵机和电机的控制
[0145] 通过飞行器上的ARM7处理器控制PWM寄存器控制飞行器的舵机和电机；其中因舵机的转角与控制舵机的脉冲宽度成线性关系，脉冲宽度又是由处理器内部PWM寄存器值来设置的，通过处理器计算改变PWM寄存器值来控制飞行器左右平移和前进后退；两个电机形成转速差时会产生旋转力矩通过控制，该力矩可以控制飞行器左右方向摆动，当两电机同比增速或减速时则能控制飞行器的上升和下降；
[0146] (1)舵机控制
[0147] 左右平移、前进后退运动相对悬停状态下两舵机角度变化相互关系为：
[0148] 左平移：左舵机角度变化量为+θ左移，右舵机角度变化量为-θ左移[0149] 右平移：左舵机角度变化量为-θ右移，右舵机角度变化量为+θ右移[0150] 前进：左舵机角度变化量为-θ前进，右舵机角度变化量为-θ前进
[0151] 后退：左舵机角度变化量为+θ后退，右舵机角度变化量为+θ后退
[0152] 假设飞行器需要往X-Y平面的第一象限平移，则飞行器可以分解成前进和右平移，则最终左右舵机分别旋转的角度为：
[0153] θ左舵＝90-θ前进+θ右移
[0154] θ右舵＝90-θ前进-θ右移
[0155] 在采集数据的时间点上，程序给出的处理结果是下螺旋桨前倾12°，分别输出到两个控制方向的舵机分别是左舵减小6°，右舵增加6°
[0156] (2)电机控制
[0157] 两个电机转动方向相反，当两个电机形成转速差时会产生旋转力矩，该力矩可以控制飞行器左右方向摆动，当向右方向转动的电机转速大于向左方向转动的电机转速时，飞机向右方向摆动；当向左方向转动的电机转速大于向右方向转动的电机转速时，飞机向左方向摆动，两个电机同比增速，可以控制飞行器上升；两个电机同比减速减时，可以控制飞行器的下降；
[0158] 电机采用增量式PID控制：
[0159] Δu1(k)＝kp×ΔE(k)+ki×E(k)+kd[E(k)-2E(k-1)+E(k-2)]
[0160] 式中，Kp，Ki，Kd分别是PID控制中比例，积分，微分控制参数，控制中分别取值为80、50、20；E(k)为系统偏差；E(k)-2E(k-1)+E(k-2)为系统量变化率，在采集数据的时间点上，ΔE(k)＝31，E(k)＝46，E(k)-2E(k-1)+E(k-2)＝-24，计算得到Δu1(k)＝4300，得到的值作为PWM脉宽的变化量来控制电机；
[0161] 八、通过GPS信号获取飞机所在位置，与目的地的经纬度进行比较，如果两者不等，飞机未到达目的地，转入步骤二；如果两者相等，飞机到达目的地，给定高度逐渐减小，飞行器降落，实验中测得的飞行位置为东经121°25’45.4″北纬31°13’17.9″，与目的地位置东经121°25’40″北纬31°13’20″不等，飞机未到达目的地，转入步骤二。
[0162] 飞行模式分为三种：遥控模式、航线设定模式、自主循迹模式；
[0163] (1)遥控模式
[0164] 定时器输入捕捉方式计算出遥控信号左右舵机和前后电机对应的通道的遥控脉冲宽度，进而处理得到左右舵机控制脉冲宽度和前后电机PWM控制信号的周期和占空比；
[0165] (2)航线设定模式
[0166] 飞行器飞行的方向(与地磁线的夹角)为：
[0167]
[0168] 飞行器偏移设定航线的距离为：
[0169]
[0170] 其中E、N分别为本次测得的经度和纬度，En、Nn分别为飞行器由步骤(3)得到的下一个目标点的经度和纬度，E0、N0分别作为起飞点坐标，
[0171] 飞行器飞行的高度H0和速度V0由步骤三得到；
[0172] (3)自主循迹模式
[0173] 对摄像头视频信号进行
数据采集，提取白布上黑线在图像上形成的路径的左右上下边界，计算出飞行器偏移路径的角度和距离，具体步骤如下：
[0174] ①采集图像数据
[0175] 采取了降低图像分辨率的方法每行采集48个点，每幅图像采集40行，图像分辨率为48*40；
[0176] ②计算阈值并二值化图像
[0177] 图像采第1场的第1行数据用双峰法计算出该行的阈值，将该图像中大于等于阈值的点赋值为1，小于阈值的点赋值为0，实现二值化处理，第2行阈值引用第1行的阈值，并实现二值化处理，将所有值为1的点所对应的数据相加取平均值，将所有值为0的点所对应的数据相加取平均值，两个平均值再取平均值便得到第2行的阈值，并与上一行的阈值进行比较，在允许的范围内更新，如此迭代至最后一行，最后一行的阈值则作为下一场第一行的阈值，如此反复迭代；
[0178] ③判断每行图像二值化情况
[0179] 每行图像从起点到终点进行判断，如果该行仅一次出现值为“0-1-0”变化，则认为该行图像二值化情况良好；
[0180] ④搜索路径的左右边界、上下边界
[0181] 从第1行起依次搜索该场图像的其他行的图像二值化情况，如果连续3行是二值化情况良好，则停止搜索，并把该行作为路径搜索起始行ist，以ist作为起始行向上(行标号增加的方向)和向下(行标号减小的方向)对各行进行路径边界的搜索；
[0182] 路径左边界向上搜索：
[0183] 将路径左边界向上一行延伸，如果上一行该处值为0的点，则向左搜索路径左边界，如果搜索超出边界还没搜索到则用边界值代替；如果该处值为1的点，则向右搜索路径左边界：
[0184] 路径右边界向上搜索：
[0185] 将路径右边界向上一行延伸，如果上一行该处值为0的点，则向右搜索路径右边界；如果该处值为1的点，则向左搜索路径右边界；
[0186] 如果搜索到的路径边界出现左路径边界值大于右路径边界值，左右边界太宽(为0点个数大于10个)或左右边界值之间有值为0的点，则尝试跳过行，该行的路径边界值用前两行边界的平均值进行补充，如果向上路径搜索过程中总跳跃次数达到3次或搜索达到顶端则结束搜索，同时可以得到路径上边界值b，并计算出上边界左右两边界的中心值B；
[0187] 向下搜索同理可得到路径下边界值a，计算下边界左右两边界的中心值A，如果搜索到的有效路径长度小于3行时，则认为本场图像路径提取失败，如果连续3场图像路径提取失败则，则将起飞点坐标作为下一个目标点坐标，转至步骤①，返回起飞点；
[0188] ⑤飞行器飞行方向与路径方向夹角大小为：
[0189]
[0190] 飞行器路径在图像上偏离的距离为：
[0191]
[0192] 其中a为路径下边界值，A为下边界左右两边界的中心值，b为路径上边界值，B为上边界左右两边界的中心值，M为行图像采集的数据个数。
