专利名称：一种微型共轴双旋翼飞行器控制装置及方法
技术领域：
本发明属于飞行器控制技术领域，特别适用于一种微型共轴双旋翼飞行器控制装 置及方法。
背景技术：
目前，因自主导航系统一般体型较大，要实现具有自主飞行能力的飞行器的微型 化难度很大，而且系统随着自主化程度的提高以及功能的增多体积也随之增大，研究集尺 寸微型、功能强大于一体的飞行器还是一片空白。
发明内容
为解决已有技术存在的不足，本发明提供一种微型共轴双旋翼飞行器控制装置及 方法，通过控制微型共轴双旋翼飞行器的舵机和电机，以达到精确控制的目的。本发明的技术方案是这样实现的该装置包括地面装置和空中装置两部分；其中 地面装置包括视频无线接收机、地面无线传输模块、地面处理器、显示器和上位机；空中装 置包括有三轴磁航向传感器、三轴陀螺仪、三轴加速度传感器、GPS、气压计、超声波传感器、 ARM处理器、微型无线摄像头、视频解码模块、视频无线传输模块和空中无线传输模块；所述的地面装置的连接是视频无线接收机输出端连接显示器的输入端，地面无 线传输模块与地面处理器通过同步串口端连接；地面处理器与上位机通过异步串口端连 接；所述的空中装置安装在飞行器上，空中装置的连接是三轴磁航向传感器、三轴陀 螺仪、三轴加速度传感器、GPS、气压计和超声波传感器的输出端均连接ARM处理器相应的 输入端，微型无线摄像头的一个输出端通过视频解码模块连接ARM处理器的输入端，微型 无线摄像头的另一个输出端连接视频无线传输模块的输入端，ARM处理器与空中无线传输 模块通过同步串口端连接；其中地面装置和空中装置通过其中地面无线传输模块和空中无线传输模块实现 无线通讯，视频无线接收机与视频无线传输模块实现无线通讯。所述的微型共轴双旋翼飞行器控制装置的控制方法，包括如下步骤步骤一、装置初始化；步骤二、采集各路传感器数据依次采集三轴加速度传感器，三轴磁航向传感器、三轴角速度陀螺仪、超声波传感 器、气压计和GPS的记录数据(1)将三轴加速度传感器三个轴向输出的模拟电压信号经AD转换成数字信号，对 信号进行滤波，并储存飞行器X、Y、ζ轴上的加速度ax、ay、az ；(2)读取三轴磁航向传感器上地磁场在三个轴上的数字磁场分量，并确定飞行器 与地磁线所成夹角δ ；具体如下
6
如果Hy >0，则d = 90-[arctan(/ic///;0]*180/;z·如果Hy <0，则5 = 270-[arctan(/ic//^)]*180/;r如果Hy = 0，Hx < 0 则a = 180如果Hy = 0，Hx > 0 则a = 0(3)将俯仰、翻滚陀螺仪，偏航陀螺仪输出的模拟电压信号经AD转换成数字信号， 对信号进行滤波，并储存飞行器俯仰角速度Ωχ、翻滚角速度Qy、偏航角速度Ωζ;(4)读取数字气压计的气压值P，并确定绝对高度，方法如下式 H=44330*(1-(p/P0)1/5.255)式中ρ0为海平面气压，其值为1013. 25hPa ；H'为绝对高度，单位为米；如果是第一次测量气压值，则将H'赋值给外部变量Hs，计算飞行器飞行的相对高 度 H=44330*(1-(P/P0)1/5.255)-Hs
在低空环境下采用超声测距发射方波，打开接收中断，记录发射与接收的时间间 隔T，如果固定时间内接收到返回接收信号，则计算相对高度H = 344*T/2，并代替公式(6) 计算得到的相对高度，否则说明测量超过超声波量程，关闭接收中断，储存高度值H ；(5)确定飞行器的经度和纬度；读取GPS信号，得到飞行器的经度E和纬度N，并储存，如果是第一次得到飞行器的 经度E和纬度N，则分别赋值给外部变量Ε。N0,作为起飞点坐标；步骤三、确定飞行模式、开始起飞、飞行高度、飞行速度和飞行目标点命令；将步骤二测到的各路传感器数据组合成数据包后通过机载Zigbee无线发送给地 面Zigbee部分，并且接收来自地面控制界面通过地面Zigbee发送过来的控制命令数据包， 解析数据包后得到飞行模式、开始起飞、飞行高度、飞行速度和飞行目标点命令；步骤四、通过相对高度判断飞行器是否起飞，当相对高度为零，飞行器没有起飞； 当相对高度为正值，飞行器已经起飞，如果没有起飞，转入步骤四；如果已经起飞，转入步骤 五；步骤五、空中装置ARM处理器通过地面发过来的控制命令数据包选择飞行器的飞 行模式；把遥控模式、航线设定模式、自主循迹模式三种飞行模式赋予不同的数据，通过与 地面发过来的控制命令数据包中相应的数据进行比较，选择相应的飞行模式；步骤六、确定飞行器控制增量；
飞行器有四个控制通道，分别为上升下降，前进后退，左右平移，左右摆动，飞行器 的姿态的控制可以分解成对这四个通道的控制，各个通道采用下例式计算出控制增量Δ u (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k-1) +E (k_2)]+kn X Δ a (k) +k12 X a (k) +a13 [a (k) -2a (k_l) +a (k_2)]
+k21 X Δ Ω (k) +k22 X Ω (k) +k23 [ Ω (k) -2 Ω (k_l) + Ω (k_2)]式中kp、kn、k21分别为被控量、加速度、角速度的比例系数；‘ k12、k22分别 为被控量、加速度、角速度的积分系数；kd、k13、k23分别为被控量、加速度、角速度的微分 系数；E(k)、a(k)、Ω (k)分别为被控量、加速度、角速度的系统偏差；ΔΕ(10、Aa(k)、 Δ Ω (k)分别为被控量、加速度、角速度的偏差变化率；AE(k) =E(k)-E(k_l)，Aa(k)= a(k)-a(k-l), Δ Ω (k) = Ω (k)-Q (k-1) ；k, k_l，k_2 分别表示第 k 次，第 k_l 次，第 k_2 次；步骤七、控制飞行器的舵机和电机；通过飞行器上的ARM处理器控制PWM寄存器控制飞行器的舵机和电机；其中因舵 机的转角与控制舵机的脉冲宽度成线性关系，脉冲宽度又是由ARM处理器内部PWM寄存器 值来设置的，通过ARM处理器计算改变PWM寄存器值来控制飞行器左右平移和前进后退；飞 行器上两个电机形成转速差时会产生旋转力矩通过控制，该力矩可以控制飞行器左右方向 摆动，当两电机同比增速或减速时则能控制飞行器的上升和下降；(1)舵机控制左右平移、前进后退运动相对悬停状态下两舵机角度变化相互关系为左平移左舵机角度变化量为+ θ左移，右舵机角度变化量为_ θ左移右平移左舵机角度变化量为_ θ右移，右舵机角度变化量为+ θ拂前进左舵机角度变化量为_ θ纖，右舵机角度变化量为_ θ纖后退左舵机角度变化量为+ θ后退，右舵机角度变化量为+ θ后退；(2)电机控制两个电机转动方向相反，当两个电机形成转速差时会产生旋转力矩，该力矩可以 控制飞行器左右方向摆动，当向右方向转动的电机转速大于向左方向转动的电机转速时， 飞机向右方向摆动；当向左方向转动的电机转速大于向右方向转动的电机转速时，飞机向 左方向摆动，两个电机同比增速，可以控制飞行器上升；两个电机同比减速减时，可以控制 飞行器的下降；电机采用增量式PID控制Δ U1 (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k-1) +E (k_2)]式中，Kp, Ki, Kd分别是PID控制中比例，积分，微分控制参数； E(k)-2E(k-l)+E(k-2)为系统量变化率，在采集数据的时间点上，将得到的值作为PWM脉宽 的变化量来控制电机；步骤八、通过GPS信号获取飞机所在位置，与目的地的经纬度进行比较，如果两者 不等，飞机未到达目的地，转入步骤二 ；如果两者相等，飞机到达目的地，给定高度逐渐减 小，飞行器降落。所述的步骤五，空中装置ARM处理器通过地面发过来的控制命令数据包选择飞行 器的飞行模式，按如下步骤进行
飞行模式分为三种遥控模式、航线设定模式、自主循迹模式；(1)遥控模式计算出遥控信号左右舵机和前后电机对应的通道的遥控脉冲宽度，进而处理得到 飞行器左右舵机控制脉冲宽度和前后电机PWM控制信号的周期和占空比；(2)航线设定模式飞行器飞行的方向即与地磁线的夹角为
β — β没=arctan“ N-Nn飞行器偏移设定航线的距离为L 机V +hV+iX)-^+^ +V)
其中Ε、N分别为本次测得的经度和纬度，En、Nn分别为飞行器由步骤三得到的 下一个目标点的经度和纬度，E0, N0分别作为起飞点坐标彳=概-E0)2+ (Nn - N0)2 ,
I2 = yl(E-E0)2+(N-N0)2 J3 = yl(En -Ef + (Nn 一N)2 ,其中 1” I2 和 k3 分别为飞行器偏移
设定航线在X，Y和Z轴的距离；(3)自主循迹模式将微型无线摄像头采集的视频信号提取白布上黑线，在图像上形成的路径的左右 上下边界，计算出飞行器偏移路径的角度和距离，具体步骤如下①采集图像数据采取了降低图像分辨率的方法每行采集至少48个点，每幅图像采集至少40行，图 像分辨率至少为48X40 ；②计算阈值并二值化图像图像采第1场的第1行数据用双峰法计算出该行的阈值，将该图像中大于等于阈 值的点赋值为1，小于阈值的点赋值为0，实现二值化处理，第2行阈值引用第1行的阈值， 并实现二值化处理，将所有值为1的点所对应的数据相加取平均值，将所有值为0的点所对 应的数据相加取平均值，两个平均值再取平均值便得到第2行的阈值，并与上一行的阈值 进行比较，在允许的范围内更新，如此迭代至最后一行，最后一行的阈值则作为下一场第一 行的阈值，如此反复迭代；③判断每行图像二值化情况每行图像从起点到终点进行判断，如果该行仅一次出现值为“0-1-0”变化，则认为 该行图像二值化情况良好；④搜索路径的左右边界、上下边界从第1行起依次搜索该场图像的其他行的图像二值化情况，如果连续3行是二值 化情况良好，则停止搜索，并把该行作为路径搜索起始行ist，以ist作为起始行向上即行标 号增加的方向和向下即行标号减小的方向对各行进行路径边界的搜索；路径左边界向上搜索将路径左边界向上一行延伸，如果上一行该处值为0的点，则向左搜索路径左边 界，如果搜索超出边界还没搜索到则用边界值代替；如果该处值为1的点，则向右搜索路径左边界路径右边界向上搜索将路径右边界向上一行延伸，如果上一行该处值为0的点，则向右搜索路径右边 界；如果该处值为1的点，则向左搜索路径右边界； 如果搜索到的路径边界出现左路径边界值大于右路径边界值，左右边界太宽即为 0点个数大于10个或左右边界值之间有值为0的点，则尝试跳过行，该行的路径边界值用前 两行边界的平均值进行补充，如果向上路径搜索过程中总跳跃次数达到3次或搜索达到顶 端则结束搜索，同时可以得到路径上边界值b，并计算出上边界左右两边界的中心值B ；向下搜索同理可得到路径下边界值a，计算下边界左右两边界的中心值A，如果搜 索到的有效路径长度小于3行时，则认为本场图像路径提取失败，如果连续3场图像路径提 取失败则，则将起飞点坐标作为下一个目标点坐标，转至步骤①；⑤飞行器飞行方向与路径方向夹角大小为e =
b-a飞行器路径在图像上偏离的距离为
1L = -(M-A-B)其中a为路径下边界值，A为下边界左右两边界的中心值，b为路径上边界值，B为 上边界左右两边界的中心值，M为行图像采集的数据个数。其中Zigbee是IEEE802. 15. 4协议的代名词，根据这个协议规定的是一种短距离、 低功耗的无线通信技术。本发明的优点本微型共轴双旋翼飞行器控制系统以微型四通道共轴双旋翼直升 机为载体，该载体体积小、效率高，机载控制系统选用了内核ARM微处理器作为CPU，提高 了资源的利用率，三轴加速度传感器、三轴磁航向传感器、三轴角速度陀螺仪、超声波传感 器、气压计，采用了芯片级的传感器，采用GPS定位模块，微型无线摄像头。无线通讯采用了 Zigbee无线通讯方式，其能够方便的实现机器人群体的组网通讯。该系统能实现航拍和任 务设定自主飞行以及地面寻迹自主飞行。自动手动可方便切换。
图1本发明装置结构框图；图2本发明地面无线传输模块/空中无线传输模块电原理图；图3本发明三轴磁航向传感器电原理图；图4 (a)本发明三轴陀螺仪_俯仰和翻滚陀螺仪电原理图；图4 (b)本发明三轴陀螺仪_偏航陀螺仪电原理图；图5本发明三轴加速度传感器电原理图；图6本发明GPS电原理图；图7本发明气压计电原理图；图8本发明ARM处理器电原理图；图9本发明视频解码模块电原理图10本发明控制方法流程图。
具体实施例方式本发明一种微型共轴双旋翼飞行器控制装置及方法结合施实例加以说明。超声波传感器选用SRF05模块；微型无线摄像头、视频无线传输模块和视频无线接收机选用深圳市联益达科技有 限公司型号为RC211A+208CWAS无线摄像机的部分器件；地面无线传输模块如图1所示；三 轴磁航向传感器如图2所示；三轴陀螺仪如图3所示；三轴加速度传感器如图4(a)和图 4(B)所示；GPS如图5所示；气压计如图6所示；ARM处理器选用型号为STM32F103，如图7 所示；视频解码模块如图8所示；空无线传输模块的电原理结构和地面无线传输模块相同， 空无线传输模块如图1所示；地面处理器包括ARM处理器、MAX3232、串口、串口线，各部件的连接是ARM处理器 的USARTl的RX、TX引脚与相应的MAX3232引脚相连接，串口与相应的MAX3232引脚相连接。该装置包括地面装置和空中装置两部分；其中地面装置包括视频无线接收机、地 面无线传输模块、地面处理器、显示器和上位机；空中装置包括有三轴磁航向传感器、三轴 陀螺仪、三轴加速度传感器、GPS、气压计、超声波传感器、ARM处理器、微型无线摄像头、视频 解码模块、视频无线传输模块和空中无线传输模块；其中地面无线传输模块如图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)和 电感(TLl)的一端相连接，电感(TLl)的另一端接地，电容(C71)的另一端与CC2520(U15) 的RF-N引脚相连接；电感(TL3)的另一端与电容(C73)和电容(C74)的一端相连接，电容 (C74)的另一端接地，电容(C73)的另一端与CC2520(U15)的RF-P引脚相连接；电容(C75) 一端接地，另一端与32M晶振(Y4)和CC2520(U15)的S0SC32M-2引脚相连接；电容(C76) 一端接地，另一端与32M晶振(Y4)和CC2520(U15)的S0SC32M-2引脚相连接。地面装置器件之间的连接地面无线传输模块(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引脚相连接；地面处理器的串口通过串 口线与上位机连接；视频无线接收机输出端连接显示器的输入端。空中装置器件之间的连接三轴磁航向传感器(HMC5843)的I2C接口 SCL、SDA引 脚分别与ARM处理器(STM32F103RC)的PB10、PB11引脚相连接；三轴陀螺仪中的俯仰和翻 滚陀螺仪(LPR530AL)的输入引脚HP与ARM处理器(STM32F103RC)的PC13引脚相连接， 输出引脚4X0UX、4X0UY、Vref三个引脚分别与ARM处理器(STM32F103RC)的PC2、PC3、
11PAO引脚相连接；三轴陀螺仪中的偏航陀螺仪(LY330ALH)输出引脚OUZ、Vref引脚分别 与ARM处理器(STM32F103RC)的PCO、PCl引脚相连接；三轴加速度传感器(MMA7260A)的 输出端 ACC_X、ACC_Y、ACC_Y 分别与 ARM 处理器(STM32F103RC)的 PA1、PA2、PA3 引脚相连 接；GPS(G591BGPS 模块)的 RXA、TXA 和 RXB、TXB 引脚分别与 ARM 处理器(STM32F103RC)的 PA9、PAlO和PC10、PCll引脚相连接，输出引脚IPPS引脚与ARM处理器(STM32F103RC)的 PA12引脚相连接；超声波传感器(SRF05模块)的输入/输出双向引脚TRIGGER INPUT/ECHO OUTPUT与ARM处理器(STM32F103RC)的PB7引脚相连接；气压计的I2C接口 SCL、SDA引脚 分别与ARM处理器(STM32F103RC)的PB10、PBll引脚相连接；空中无线传输模块(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相连接，无线摄像头的另一个输 出端与视频无线传输模块的输入端相连接。
示
所述的微型共轴双旋翼飞行器控制装置的控制方法，包括如下步骤，如图10所
一、装置初始化；
二、采集各路传感器数据
依次采集三轴加速度传感器，三轴磁航向传感器、三轴角速度陀螺仪、超声波传感 器、气压计、GPS的数据(1)将三轴加速度传感器三个轴向输出的模拟电压信号，实验测量为Vx = 1. 1231V，Vy = 1. 7741V, Vz = 1. 6734V，经AD转换成数字信号，对信号进行滤波，并储存飞
行器 X、Y、Z 轴上的加速度 ax、ay、az，实验测量为 ax = -7. 8m/s2、ay = 1. 52m/s2、az = 0. 23m/
2
S ；(2)读取三轴磁航向传感器上地磁场在三个轴上的数字磁场分量，并确定飞行器 与地磁线所成夹角具体如下如果Hy >0，则5 = 90- [arctan(/ic / Hy)] *\%Q/n(1)如果Hy <0，则5 = 270 - [arctan(i/x / Hy)] * 180 / π(2)如果Hy = 0，Hx < 0 则5 = 180(3)如果Hy = 0，Hx > 0 则3 = 0(4)实验数据为3=90-[arctan(l77.35 / 2.61)] * 180 / ττ=41.573 °(3)将俯仰、翻滚陀螺仪，偏航陀螺仪输出的模拟电压信号经AD转换成数字信号， 对信号进行滤波，并储存飞行器俯仰、翻滚、偏航角速度Ωχ、Qy、Ω ζ，实验数据为飞行器俯
12仰角速度0. 107rad/s ；翻滚角速度0. 013rad/s ；偏航角速度0. 361rad/s ；(4)读取数字气压计的气压值P，并确定绝对高度，方法见下式
(丄、
,(η ^1255H =44330χ 1-(5)
UoJ
ν/式中P0为海平面气压，其值为1013. 25hPa ；H'为绝对高度，单位为米，测量结果 为 42m ；如果是第一次测量气压值，则将H'赋值给外部变量Hs，计算飞行器飞行的相对高 度
(丄、// = 44330X 1-丄 -Hs(6)
V/发射方波，打开接收中断，用定时器记录发射与接收的时间间隔T(单位为秒)，如 果固定时间内接收到返回接收信号，则计算相对高度H = 344*Τ/2，并代替公式(6)计算得 到的相对高度，否则说明测量超过超声波量程，关闭接收中断，储存高度值H，测试实际在低 空环境，所以采用超声测距测量高度，测量得到T = 0. 017s，计算得高度为2. 92m ；(5)确定飞行器的经度和纬度；读取GPS信号，得到飞行器的经度E和纬度N，并储存，如果是第一次得到飞行器的 经度E和纬度N，则分别赋值给外部变量Ε。N0分别作为起飞点坐标，本地经纬度测量值为 东经 121° 25，50. 1〃 北纬 31° 13，20.8〃 ；三、确定飞行模式、开始起飞、飞行高度、飞行速度、飞行目标点命令；将步骤二测到的各路传感器数据组合成数据包后通过机载Zigbee无线发送给地 面Zigbee部分，并且接收来地面自控制界面通过地面Zigbee发送过来的控制命令数据包， 解析数据包后得到飞行模式、开始起飞、飞行高度、飞行速度、飞行目标点命令；四、通过相对高度判断飞行器是否起飞，当相对高度为零，飞行器没有起飞；当相 对高度为正值，飞行器已经起飞，如果没有起飞，转入步骤四；如果已经起飞，转入步骤五；五、空中装置ARM处理器通过地面发过来的控制命令数据包选择飞行器的飞行模 式，把三种飞行模式赋予不同的数据，通过与地面发过来的控制命令数据包中相应的数据 进行比较，选择相应的飞行模式；六、计算飞行器控制增量；飞行器的有四个控制通道，分别为上升下降，前进后退，左右平移，左右摆动，飞行 器的姿态的控制可以分解成对这四个通道的控制，各个通道采用下例式计算出控制增量Δ U (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k_l) +E (k_2)]
+kn X Δ a (k) +k12 X a (k) +a13 [a (k) -2a (k_l) +a (k_2)]+k21 X Δ Ω (k) +k22 X Ω (k) +k23 [ Ω (k) -2 Ω (k_l) + Ω (k_2)]式中kp、kn、k21分别为被控量、加速度、角速度的比例系数，实验时得到的值分别 为80、90、30 ；k” k12、k22分别为被控量、加速度、角速度的积分系数，实验时得到的值分别 为40、40、20 ；kd、k13、k23分别为被控量、加速度、角速度的微分系数，实验时得到的值分别为=45,50,30 ；E(k)、a(k)、Ω (k)分别为被控量、加速度、角速度的系统偏差，分别计算得 65、0.29、0.0021 ;AE(k)、Aa(k)、Δ Ω (k)分别为被控量、加速度、角速度的偏差变化率， 三个偏差分别为 35、0· 73、0· 04 ； AE(k) = E(k)-E(k_l)，Aa(k) = a(k)-a(k_l)，Δ Ω (k) =Ω (k)-Q (k-1) ；k, k-1，k-2分别表示第k次，第k-1次，第k-2次，经过计算，控制增量 Au(k)为 2347. 57 ；七、舵机和电机的控制通过飞行器上的ARM7处理器控制PWM寄存器控制飞行器的舵机和电机；其中因舵 机的转角与控制舵机的脉冲宽度成线性关系，脉冲宽度又是由处理器内部PWM寄存器值来 设置的，通过处理器计算改变PWM寄存器值来控制飞行器左右平移和前进后退；两个电机 形成转速差时会产生旋转力矩通过控制，该力矩可以控制飞行器左右方向摆动，当两电机 同比增速或减速时则能控制飞行器的上升和下降；(1)舵机控制左右平移、前进后退运动相对悬停状态下两舵机角度变化相互关系为左平移左舵机角度变化量为+ θ &移，右舵机角度变化量为_ θ左移右平移左舵机角度变化量为_ θ $移，右舵机角度变化量为+ θ拂前进左舵机角度变化量为_ θ胃进，右舵机角度变化量为_ θ纖后退左舵机角度变化量为+ θ g退，右舵机角度变化量为+ θ ^ia假设飞行器需要往X-Y平面的第一象限平移，则飞行器可以分解成前进和右平 移，则最终左右舵机分别旋转的角度为θ左舵=90-θ前进+θ右移θ右舵=90-θ前进-θ右移在采集数据的时间点上，程序给出的处理结果是下螺旋桨前倾12°，分别输出到 两个控制方向的舵机分别是左舵减小6°，右舵增加6°(2)电机控制两个电机转动方向相反，当两个电机形成转速差时会产生旋转力矩，该力矩可以 控制飞行器左右方向摆动，当向右方向转动的电机转速大于向左方向转动的电机转速时， 飞机向右方向摆动；当向左方向转动的电机转速大于向右方向转动的电机转速时，飞机向 左方向摆动，两个电机同比增速，可以控制飞行器上升；两个电机同比减速减时，可以控制 飞行器的下降；电机采用增量式PID控制Δ U1 (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k-1) +E (k-2)]式中，Kp, Ki, Kd分别是PID控制中比例，积分，微分控制参数，控制中分别取值为 80,50,20 ；E(k)为系统偏差；E(k)-2E(k-l)+E(k-2)为系统量变化率，在采集数据的时间点 上，AE(k) =31，E(k) = 46，E(k)-2E(k-l)+E(k-2) = -24，计算得到 Δ U1 (k) = 4300，得 到的值作为PWM脉宽的变化量来控制电机；八、通过GPS信号获取飞机所在位置，与目的地的经纬度进行比较，如果两者不 等，飞机未到达目的地，转入步骤二 ；如果两者相等，飞机到达目的地，给定高度逐渐减小， 飞行器降落，实验中测得的飞行位置为东经121° 25’45.4〃北纬31° 13’17.9〃，与目的 地位置东经121° 25，40〃北纬31° 13，20〃不等，飞机未到达目的地，转入步骤二。
飞行模式分为三种遥控模式、航线设定模式、自主循迹模式；(1)遥控模式定时器输入捕捉方式计算出遥控信号左右舵机和前后电机对应的通道的遥控脉 冲宽度，进而处理得到左右舵机控制脉冲宽度和前后电机PWM控制信号的周期和占空比；(2)航线设定模式飞行器飞行的方向(与地磁线的夹角)为
E-E^ = arctan “
N-Nn飞行器偏移设定航线的距离为L _ MV+h%2+/Λ2)-(// +hA +/7)
一 2/,其中Ε、N分别为本次测得的经度和纬度，En、Nn分别为飞行器由步骤(3)得到 的下一个目标点的经度和纬度，E0, N0分别作为起飞点坐标，Z1 = ^(En-E0)2+(Nn-N0)2， l2=^E-E0f HN-N0Y %^{En_EfHNn_Nf ；飞行器飞行的高度Htl和速度Vtl由步骤三得到；(3)自主循迹模式对摄像头视频信号进行数据采集，提取白布上黑线在图像上形成的路径的左右上 下边界，计算出飞行器偏移路径的角度和距离，具体步骤如下①采集图像数据采取了降低图像分辨率的方法每行采集48个点，每幅图像采集40行，图像分辨率 为 48*40 ；②计算阈值并二值化图像图像采第1场的第1行数据用双峰法计算出该行的阈值，将该图像中大于等于阈 值的点赋值为1，小于阈值的点赋值为0，实现二值化处理，第2行阈值引用第1行的阈值， 并实现二值化处理，将所有值为1的点所对应的数据相加取平均值，将所有值为0的点所对 应的数据相加取平均值，两个平均值再取平均值便得到第2行的阈值，并与上一行的阈值 进行比较，在允许的范围内更新，如此迭代至最后一行，最后一行的阈值则作为下一场第一 行的阈值，如此反复迭代；③判断每行图像二值化情况每行图像从起点到终点进行判断，如果该行仅一次出现值为“0-1-0”变化，则认为 该行图像二值化情况良好；④搜索路径的左右边界、上下边界从第1行起依次搜索该场图像的其他行的图像二值化情况，如果连续3行是二值 化情况良好，则停止搜索，并把该行作为路径搜索起始行ist，以ist作为起始行向上(行标 号增加的方向)和向下(行标号减小的方向)对各行进行路径边界的搜索；路径左边界向上搜索将路径左边界向上一行延伸，如果上一行该处值为0的点，则向左搜索路径左边
界，如果搜索超出边界还没搜索到则用边界值代替；如果该处值为1的点，则向右搜索路径
15左边界路径右边界向上搜索将路径右边界向上一行延伸，如果上一行该处值为0的点，则向右搜索路径右边 界；如果该处值为1的点，则向左搜索路径右边界；如果搜索到的路径边界出现左路径边界值大于右路径边界值，左右边界太宽(为 0点个数大于10个)或左右边界值之间有值为0的点，则尝试跳过行，该行的路径边界值用 前两行边界的平均值进行补充，如果向上路径搜索过程中总跳跃次数达到3次或搜索达到 顶端则结束搜索，同时可以得到路径上边界值b，并计算出上边界左右两边界的中心值B ；向下搜索同理可得到路径下边界值a，计算下边界左右两边界的中心值A，如果搜 索到的有效路径长度小于3行时，则认为本场图像路径提取失败，如果连续3场图像路径提 取失败则，则将起飞点坐标作为下一个目标点坐标，转至步骤①，返回起飞点；⑤飞行器飞行方向与路径方向夹角大小为
ΛB-A(9 = arctan-
b-a飞行器路径在图像上偏离的距离为L = ^(M-A-B)其中a为路径下边界值，A为下边界左右两边界的中心值，b为路径上边界值，B为 上边界左右两边界的中心值，M为行图像采集的数据个数。
1权利要求
一种微型共轴双旋翼飞行器控制装置，其特征在于该装置包括地面装置和空中装置两部分；其中地面装置包括视频无线接收机、地面无线传输模块、地面处理器、显示器和上位机；空中装置包括有三轴磁航向传感器、三轴陀螺仪、三轴加速度传感器、GPS、气压计、超声波传感器、ARM处理器、微型无线摄像头、视频解码模块、视频无线传输模块和空中无线传输模块；所述的地面装置的连接是视频无线接收机输出端连接显示器的输入端，地面无线传输模块与地面处理器通过同步串口端连接；地面处理器与上位机通过异步串口端连接；所述的空中装置安装在飞行器上，空中装置的连接是三轴磁航向传感器、三轴陀螺仪、三轴加速度传感器、GPS、气压计和超声波传感器的输出端均连接ARM处理器相应的输入端，微型无线摄像头的一个输出端通过视频解码模块连接ARM处理器的输入端，微型无线摄像头的另一个输出端连接视频无线传输模块的输入端，ARM处理器与空中无线传输模块通过同步串口端连接；其中地面装置和空中装置通过其中地面无线传输模块和空中无线传输模块实现无线通讯，视频无线接收机与视频无线传输模块实现无线通讯。
2.采用权利要求1所述的微型共轴双旋翼飞行器控制装置的控制方法，其特征在于， 包括如下步骤步骤一、装置初始化；步骤二、采集各路传感器数据；依次采集三轴加速度传感器，三轴磁航向传感器、三轴角速度陀螺仪、超声波传感器、 气压计和GPS的记录数据(1)将三轴加速度传感器三个轴向输出的模拟电压信号经AD转换成数字信号，对信号 进行滤波，并储存飞行器X、Y、Z轴上的加速度ax、ay、az ；(2)读取三轴磁航向传感器上地磁场在三个轴上的数字磁场分量，并确定飞行器与地 磁线所成夹角(3)将俯仰、翻滚陀螺仪，偏航陀螺仪输出的模拟电压信号经AD转换成数字信号，对信 号进行滤波，并储存飞行器俯仰角速度Ωχ、翻滚角速度Qy、偏航角速度Ωζ;(4)读取数字气压计的气压值P，并确定绝对高度，方法如下式( 式中Ρ0为海平面气压，其值为1013.25hPa;H'为绝对高度，单位为米；如果是第一次测量气压值，则将H'赋值给外部变量Hs，计算飞行器飞行的相对高度 发射方波，打开接收中断，记录发射与接收的时间间隔Τ，如果固定时间内接收到返回 接收信号，则计算相对高度Η = 344ΧΤ/2，并代替公式(6)计算得到的相对高度，否则说明 测量超过超声波量程，关闭接收中断，储存高度值H;(5)确定飞行器的经度和纬度；读取GPS信号，得到飞行器的经度E和纬度N，并储存，如果是第一次得到飞行器的经度 E和纬度N，则分别赋值给外部变量&、N0,作为起飞点坐标；步骤三、确定飞行模式、开始起飞、飞行高度、飞行速度和飞行目标点命令； 将步骤二测到的各路传感器数据组合成数据包后通过机载Zigbee无线发送给地面 Zigbee部分，并且接收来自地面控制界面通过地面Zigbee发送过来的控制命令数据包，解 析数据包后得到飞行模式、开始起飞、飞行高度、飞行速度和飞行目标点命令；步骤四、通过相对高度判断飞行器是否起飞，当相对高度为零，飞行器没有起飞；当相 对高度为正值，飞行器已经起飞，如果没有起飞，转入步骤四；如果已经起飞，转入步骤五； 步骤五、空中装置ARM处理器通过地面发过来的控制命令数据包选择飞行器的飞行模式；把遥控模式、航线设定模式、自主循迹模式三种飞行模式赋予不同的数据，通过与地面 发过来的控制命令数据包中相应的数据进行比较，选择相应的飞行模式； 步骤六、确定飞行器控制增量；飞行器有四个控制通道，分别为上升下降，前进后退，左右平移，左右摆动，飞行器的姿 态的控制可以分解成对这四个通道的控制，各个通道采用下例式计算出控制增量 Δ u (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k_l) +E (k_2)] +kn X Δ a (k) +k12 X a (k) +a13 [a (k) -2a (k_l) +a (k_2)] +k21X Δ Ω (k)+k22X Ω (k) +k23 [ Ω (k) -2 Ω (k-D + Ω (k_2)] 式中kp、kn、k21分别为被控量、加速度、角速度的比例系数；k” k12、k22分别为被控量、 加速度、角速度的积分系数；kd、k13、k23分别为被控量、加速度、角速度的微分系数；E(k)、 a(k)、Ω (k)分别为被控量、加速度、角速度的系统偏差；ΔΕ(10、Aa(k)、Δ Ω (k)分别为 被控量、加速度、角速度的偏差变化率，ΔΕΟΟ = E(k)-E(k_l)，Aa(k) = a(k)-a(k_l)， Δ Ω (k) = Ω (10-Ω (k-1) ；k、k-l、k_2 分别表示第 k 次、第 k-Ι 次、第 k-2 次； 步骤七、控制飞行器的舵机和电机；通过飞行器上的ARM处理器控制PWM寄存器控制飞行器的舵机和电机；其中因舵机的 转角与控制舵机的脉冲宽度成线性关系，脉冲宽度又是由ARM处理器内部PWM寄存器值来 设置的，通过ARM处理器计算改变PWM寄存器值来控制飞行器左右平移和前进后退；飞行 器上两个电机形成转速差时会产生旋转力矩通过控制，该力矩可以控制飞行器左右方向摆 动，当两电机同比增速或减速时则能控制飞行器的上升和下降；(1)舵机控制；左右平移、前进后退运动相对悬停状态下两舵机角度变化相互关系为 左平移左舵机角度变化量为+ θ &移，右舵机角度变化量为-θ左移 右平移左舵机角度变化量为_ θ ，右舵机角度变化量为+ θ拂 前进左舵机角度变化量为_ θ胃进，右舵机角度变化量为-θ纖 后退左舵机角度变化量为+ θ β;ι，右舵机角度变化量为+ θ ^ia ；(2)电机控制；电机采用增量式PID控制Δ U1 (k) = kp X Δ E (k) +Iii X E (k) +kd [Ε (k) -2E (k-1) +E (k-2)]式中，Kp, Ki, Kd分别是PID控制中比例、积分、微分控制参数;E(k)-2E(k-l)+E(k-2) 为系统量变化率，在采集数据的时间点上，将得到的值作为PWM脉宽的变化量来控制电机； 步骤八、通过GPS信号获取飞机所在位置，与目的地的经纬度进行比较，如果两者不 等，飞机未到达目的地，转入步骤二 ；如果两者相等，飞机到达目的地，给定高度逐渐减小， 飞行器降落。
3.按照权利要求2所述的微型共轴双旋翼飞行器控制装置的控制方法，其特征在于所 述的步骤五中空中装置ARM处理器通过地面发过来的控制命令数据包选择飞行器的飞行 模式，按如下步骤进行飞行模式分为三种遥控模式、航线设定模式、自主循迹模式；(1)遥控模式计算出遥控信号左右舵机和前后电机对应的通道的遥控脉冲宽度，进而处理得到飞行 器左右舵机控制脉冲宽度和前后电机PWM控制信号的周期和占空比；(2)航线设定模式飞行器飞行的方向即与地磁线的夹角为E-E θ = arctan-—N-N17飞行器偏移设定航线的距离为L _ 扭 hV +h%2+i2%2)-(h4 +12 +C) ~Tix其中Ε、N分别为本次测得的经度和纬度，En、Nn分别为飞行器由步骤三得到的下 一个目标点的经度和纬度，Etl、Ntl分别作为起飞点坐标，纟=^j(En-E0)2+(Nn-N0)2，I2=^I(E-E0)2+(N-N0)2 ,I3=^(En-E)2+(Nn-N)2，其中 1” I2 和 I3 分别为飞行器偏移设定航线在X，Y和Z轴的距离；(3)自主循迹模式；将微型无线摄像头采集的视频信号提取白布上黑线，在图像上形成的路径的左右上下 边界，计算出飞行器偏移路径的角度和距离，具体步骤如下①采集图像数据；采取了降低图像分辨率的方法每行采集至少48个点，每幅图像采集至少40行，图像分 辨率至少为48X40 ；②计算阈值并二值化图像；图像采第1场的第1行数据用双峰法计算出该行的阈值，将该图像中大于等于阈值的 点赋值为1，小于阈值的点赋值为0，实现二值化处理，第2行阈值引用第1行的阈值，并实 现二值化处理，将所有值为1的点所对应的数据相加取平均值，将所有值为0的点所对应的 数据相加取平均值，两个平均值再取平均值便得到第2行的阈值，并与上一行的阈值进行 比较，在允许的范围内更新，如此迭代至最后一行，最后一行的阈值则作为下一场第一行的 阈值，如此反复迭代；③判断每行图像二值化情况；每行图像从起点到终点进行判断，如果该行仅一次出现值为“0-1-0”变化，则认为该行图像二值化情况良好；④搜索路径的左右边界、上下边界；从第1行起依次搜索该场图像的其他行的图像二值化情况，如果连续3行是二值化情 况良好，则停止搜索，并把该行作为路径搜索起始行ist，以ist作为起始行向上即行标号增 加的方向和向下即行标号减小的方向对各行进行路径边界的搜索；路径左边界向上搜索将路径左边界向上一行延伸，如果上一行该处值为0的点，则向左搜索路径左边界，如 果搜索超出边界还没搜索到则用边界值代替；如果该处值为1的点，则向右搜索路径左边 界路径右边界向上搜索将路径右边界向上一行延伸，如果上一行该处值为0的点，则向右搜索路径右边界；如 果该处值为1的点，则向左搜索路径右边界；如果搜索到的路径边界出现左路径边界值大于右路径边界值，左右边界太宽即为0点 个数大于10个或左右边界值之间有值为0的点，则尝试跳过行，该行的路径边界值用前两 行边界的平均值进行补充，如果向上路径搜索过程中总跳跃次数达到3次或搜索达到顶端 则结束搜索，同时可以得到路径上边界值b，并计算出上边界左右两边界的中心值B ；向下搜索同理可得到路径下边界值a，计算下边界左右两边界的中心值A，如果搜索到 的有效路径长度小于3行时，则认为本场图像路径提取失败，如果连续3场图像路径提取失 败，则将起飞点坐标作为下一个目标点坐标，转至步骤①；⑤飞行器飞行方向与路径方向夹角大小为 θ = arctan B-A/b-a飞行器路径在图像上偏离的距离为L = 1/2(M-A-B)其中a为路径下边界值，A为下边界左右两边界的中心值，b为路径上边界值，B为上边 界左右两边界的中心值，M为行图像采集的数据个数。
全文摘要
一种微型共轴双旋翼飞行器控制装置及方法，属于飞行器控制技术领域。该装置包括地面装置和空中装置两部分；其中地面装置包括视频无线接收机、地面无线传输模块、地面处理器、显示器和上位机；空中装置包括有三轴磁航向传感器、三轴陀螺仪、三轴加速度传感器、GPS、气压计、超声波传感器、ARM处理器、微型无线摄像头、视频解码模块、视频无线传输模块和空中无线传输模块。本发明的优点提高了资源的利用率。其能够方便的实现飞行器群体的组网通讯。该系统能实现航拍和任务设定自主飞行以及地面寻迹自主飞行，自动手动可方便切换。
文档编号G05D1/12GK101916115SQ20101023758
公开日2010年12月15日 申请日期2010年7月27日 优先权日2010年7月27日
发明者孟祥飞, 张青春, 李龙, 窦志龙, 罗凯, 蔡衍, 郭江龙, 陈述平, 陈金湖 申请人:东北大学