基于dsp和fpga的无人直升机飞行控制系统及方法
【专利摘要】一种基于DSP和FPGA的无人直升机飞行控制系统及方法，由无人直升机机体、机载舵机组和无线电接收机构成的模型直升机分别连接机载控制平台和地面站监控平台，机载控制平台有机载下位机和分别与机载下位机相连的姿态测量模块、经纬度测量模块和机载上位机，以及与机载下位机相连的气压传感器，地面站监控平台设置有与无线电接收机无线连接的遥控器和与机载上位机相连的地面站PC。方法有用于FPGA的信息采集、命令字接收、舵机驱动和飞行数据上传，用于DSP的姿态、经纬度和高度，转发飞行数据和命令字，实现飞行数据存储，飞行数据的接收和显示，以及向机载平台发布飞行命令。本发明使无人直升机具有了自主飞行能力：自主调节姿态和位置。
【专利说明】基于DSP和FPGA的无人直升机飞行控制系统及方法
【技术领域】
[0001] 本发明涉及一种无人直升机飞行控制系统。特别是涉及一种基于DSP和FPGA的 无人直升机飞行控制系统及方法。
【背景技术】
[0002] 与固定翼飞机相比，直升机具有起飞着陆场地小、可垂直起降、空中悬停、使用灵 活等一系列优点。作为直升机的一种特例，无人直升机还具有无人员伤亡、体积小、造价低、 战场生存能力高等有人驾驶直升机无法比拟的优越性。它的这些独特性，使其在民用和军 事方面均有着广泛的应用。
[0003] 在民用方面，主要的应用领域有：空中交通管制、大地测量、气象观测、城市环境控 制、地质勘测、森林防火、人工降雨、超高压输电线路巡检和通信中继等。
[0004] 在军事方面，主要应用领域有：战场侦察、通信中继、通信情报侦察、军事测绘、探 测战场上大规模杀伤性武器（有害物质）、与其他空中平台协同作战、与卫星或预警机组成 多/双基地雷达预警/侦察系统、信息战、电子干扰及对敌防空兵压制等。
[0005] 无人直升机的应用前提是自主飞行能力。所以，设计一个使无人直升机具有自主 飞行能力的无人直升机飞行控制系统是有意义的。
【发明内容】
[0006] 本发明所要解决的技术问题是，提供一种能够增强系统的可靠性和灵活性的基于 DSP和FPGA的无人直升机飞行控制系统及方法。
[0007] 本发明所采用的技术方案是：一种基于DSP和FPGA的无人直升机飞行控制系统， 包括有由无人直升机机体、机载舵机组和无线电接收机构成的模型直升机，所述的模型直 升机分别连接机载控制平台和地面站监控平台，其中，所述的机载控制平台包括有机载下 位机和分别通过RS232接口与所述的机载下位机相连的姿态测量模块、经纬度测量模块和 机载上位机，以及通过SPI接口与所述的机载下位机相连的气压传感器，所述的地面站监 控平台设置有与所述的无线电接收机无线连接的遥控器和与所述的机载上位机相连的地 面站PC。
[0008] 所述的机载下位机包括有：现场可编程门阵列模块，分别与所述的现场可编程门 阵列模块相连的数字信号处理模块、RS232协议电平转换模块、现场可编程门阵列程序存储 模块、多路三态缓冲模块，其中，所述的现场可编程门阵列模块通过SPI接口连接所述的气 压传感器，所述的RS232协议电平转换模块通过RS232接口分别连接姿态测量模块、经纬度 测量模块和机载上位机，所述的多路三态缓冲模块通过PWM输出通道连接机载舵机组和通 过PWM输入通道连接无线电接收机。
[0009] 一种基于DSP和FPGA的无人直升机飞行控制系统的控制方法，包括有机载下位 机、机载上位机和地面站PC三部分的控制方法，其中，所述的机载下位机控制方法包括有 用于FPGA的信息采集、命令字接收、舵机驱动和飞行数据上传的控制方法，以及用于DSP 的姿态、经纬度和高度的控制方法；所述机载上位机控制方法包括有转发飞行数据和命令 字，实现飞行数据的存储；所述的地面站PC控制方法包括飞行数据的接收和显示，以及向 机载平台发布飞行命令。
[0010] 所述的机载下位机控制方法中的信息采集是指：FPGA通过串口采集MU和GPS测 量的姿态和经纬度，通过SPI接口采集气压传感器测量的大气压和温度，然后发起中断通 知DSP读取采集信息；所述的机载下位机控制方法中的命令字接收是指：FPGA通过串口接 收并处理机载上位机转发的命令字；所述的机载下位机控制方法中的舵机驱动是指：FPGA 根据飞行模式产生驱动舵机组的PWM信号；所述的机载下位机控制方法中的飞行数据上传 是指：FPGA通过串口将飞行数据上传给机载上位机。
[0011] 所述的通过SPI接口采集气压传感器测量的大气压和温度，包括如下步骤：
[0012] 1)复位：FPGA向设置在无人直升机上的气压传感器发送复位命令，延时3ms后进 入下一步骤；
[0013] 2)校正参数读取：FPGA向气压传感器发送校正参数读取命令，然后FPGA读取气压 传感器中的校正参数，并存入内部寄存器供DSP读取；
[0014] 3)主循环：FPGA先向气压传感器发送大气压转换命令，延时9ms后读取气压传感 器中的大气压，并存入内部寄存器，再向气压传感器发送温度转换命令，延时9ms后读取气 压传感器中的温度，并存入内部寄存器，然后，将FPGA大气压中断标志位置1，FPGA发起大 气压中断通知DSP读取大气压测量值和温度测量值；
[0015] 4) FPGA延时Is的采样周期后返回步骤3)。
[0016] 所述的命令字接收包括接收飞行模式切换命令字、姿态给定命令字和位置给定命 令字，具体包括如下步骤：
[0017] 1)接收命令字第一个字节；
[0018] 2)判断所接收字节的高4位是否都为0,若是，说明正在接收的命令字为飞行模式 切换命令字，将所接收字节的低4位赋给飞行模式寄存器，上传飞行模式返回步骤1);若不 是，说明正在接收的命令字为姿态给定命令字或位置给定命令字，进入下一步骤；
[0019] 3)接收命令字后四个字节；
[0020] 4)根据命令字类型将姿态给定值或者位置给定值存入FPGA内特定的寄存器供 DSP读取，然后返回步骤1)。
[0021] 所述的舵机驱动是选用FPGA中5路PWM信号输出通道的前两路PWM信号输出通 道驱动机载舵机组中的总距舵机和油门舵机，后三路PWM信号输出通道驱动横滚舵机、俯 仰舵机和偏航舵机，具体包括如下步骤 ：
[0022] 1)将FPGA中的5个计数寄存器和5个初值寄存器的初始值设定为1500,重装载 寄存器和周期寄存器的初始值设定为〇 ;
[0023] 2)判断是否经过一微秒的计数周期，是则将计数寄存器中非零的寄存器减1，周 期寄存器加1后进入下一步骤，否则直接进入下一步骤；
[0024] 3)判断DSP是否将重装载寄存器设为OxFF或者周期寄存器是否大于28000,是则 说明DSP更新了初值寄存器或者本周期超过28 了毫秒，将计数寄存器中的计数初值装载至 计数寄存器，将重装载寄存器和周期寄存器设为〇,产生下个周期的PWM信号后进入下一 步；否则直接进入下一步；
[0025] 4)由飞行模式寄存器判断飞行模式，飞行模式寄存器等于0为手动模式，进入步 骤5);飞行模式寄存器等于1为姿态模式，进入步骤6);飞行模式寄存器等于2为位置模 式，进入步骤7);
[0026] 5)将与5个计数寄存器相对应的5路PWM信号输入通道接收的信号输出在同样与 5个计数寄存器相对应的5路PWM信号输出通道上后返回步骤2);
[0027] 6)将前两路PWM信号输入通道接收的信号输出在前两路PWM信号输出通道上；5 个计数寄存器中的后三个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄 存器相对应的PWM信号输出通道输出高电平1，否则输出低电平0后返回步骤2);
[0028] 7) 5个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器相对 应的PWM信号输出通道输出高电平1，否则输出低电平〇后返回步骤2)。
[0029] 所述的DSP的姿态控制方法包括如下步骤：
[0030] 1)从FPGA读取姿态给定值、姿态测量值和飞行模式；
[0031] 2)判断当前飞行模式是否为姿态模式或者位置模式，是则进入下一步，否者直接 结束；
[0032] 3)将横滚角给定值、俯仰角给定值和偏航角给定值依次与横滚角测量值、俯仰角 测量值和偏航角测量值相减，得横滚角偏差roll_error、俯仰角偏差pitch_error和偏航 角偏差 yaw_error ;
[0033] 4)计算横滚舵机控制量增量Λ u_rol 1、俯仰舵机控制量增量Λ u_pitch和偏航舵 机控制量增量Au_yaw :
[0034]
【权利要求】
1. 一种基于DSP和FPGA的无人直升机飞行控制系统，包括有由无人直升机机体（11)、 机载舵机组（12)和无线电接收机（13)构成的模型直升机（1)，其特征在于，所述的模型直 升机（1)分别连接机载控制平台（2)和地面站监控平台（3)，其中，所述的机载控制平台 (2)包括有机载下位机（21)和分别通过RS232接口与所述的机载下位机（21)相连的姿态 测量模块（22)、经纬度测量模块（23)和机载上位机（25)，以及通过SPI接口与所述的机载 下位机（21)相连的气压传感器（24)，所述的地面站监控平台（3)设置有与所述的无线电接 收机（13)无线连接的遥控器（31)和与所述的机载上位机（25)相连的地面站PC(32)。
2. 根据权利要求1所述的基于DSP和FPGA的无人直升机飞行控制系统，其特征在于， 所述的机载下位机（21)包括有：现场可编程门阵列模块（211)，分别与所述的现场可编程 门阵列模块（211)相连的数字信号处理模块（212)、RS232协议电平转换模块（213)、现场 可编程门阵列程序存储模块（215)、多路三态缓冲模块（218)，其中，所述的现场可编程门 阵列模块（211)通过SPI接口连接所述的气压传感器（24)，所述的RS232协议电平转换模 块（213)通过RS232接口分别连接姿态测量模块（22)、经纬度测量模块（23)和机载上位机 (25)，所述的多路三态缓冲模块（218)通过PWM输出通道连接机载舵机组（12)和通过PWM 输入通道连接无线电接收机（13)。
3. -种用于权利要求1所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方 法，其特征在于，包括有机载下位机、机载上位机和地面站PC三部分的控制方法，其中，所 述的机载下位机控制方法包括有用于FPGA的信息采集、命令字接收、舵机驱动和飞行数据 上传的控制方法，以及用于DSP的姿态、经纬度和高度的控制方法；所述机载上位机控制方 法包括有转发飞行数据和命令字，实现飞行数据的存储；所述的地面站PC控制方法包括飞 行数据的接收和显示，以及向机载平台发布飞行命令。
4. 根据权利要求3所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法， 其特征在于，所述的机载下位机控制方法中的信息采集是指：FPGA通过串口采集MU和 GPS测量的姿态和经纬度，通过SPI接口采集气压传感器测量的大气压和温度，然后发起中 断通知DSP读取采集信息；所述的机载下位机控制方法中的命令字接收是指：FPGA通过串 口接收并处理机载上位机转发的命令字；所述的机载下位机控制方法中的舵机驱动是指： FPGA根据飞行模式产生驱动舵机组的PWM信号；所述的机载下位机控制方法中的飞行数据 上传是指：FPGA通过串口将飞行数据上传给机载上位机。
5. 根据权利要求4所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，其 特征在于，所述的通过SPI接口采集气压传感器测量的大气压和温度，包括如下步骤： 1) 复位：FPGA向设置在无人直升机上的气压传感器发送复位命令，延时3ms后进入下 一步骤； 2) 校正参数读取：FPGA向气压传感器发送校正参数读取命令，然后FPGA读取气压传感 器中的校正参数，并存入内部寄存器供DSP读取； 3) 主循环：FPGA先向气压传感器发送大气压转换命令，延时9ms后读取气压传感器中 的大气压，并存入内部寄存器，再向气压传感器发送温度转换命令，延时9ms后读取气压传 感器中的温度，并存入内部寄存器，然后，将FPGA大气压中断标志位置1，FPGA发起大气压 中断通知DSP读取大气压测量值和温度测量值； 4. FPGA延时Is的采样周期后返回步骤3)。
6. 根据权利要求3或4所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方 法，其特征在于，所述的命令字接收包括接收飞行模式切换命令字、姿态给定命令字和位置 给定命令字，具体包括如下步骤： 1) 接收命令字第一个字节； 2) 判断所接收字节的高4位是否都为0,若是，说明正在接收的命令字为飞行模式切换 命令字，将所接收字节的低4位赋给飞行模式寄存器，上传飞行模式返回步骤1);若不是， 说明正在接收的命令字为姿态给定命令字或位置给定命令字，进入下一步骤； 3) 接收命令字后四个字节； 4) 根据命令字类型将姿态给定值或者位置给定值存入FPGA内特定的寄存器供DSP读 取，然后返回步骤1)。
7. 根据权利要求3或4所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方 法，其特征在于，所述的舵机驱动是选用FPGA中5路PWM信号输出通道的前两路PWM信号 输出通道驱动机载舵机组中的总距舵机和油门舵机，后三路PWM信号输出通道驱动横滚舵 机、俯仰舵机和偏航舵机，具体包括如下步骤： 1) 将FPGA中的5个计数寄存器和5个初值寄存器的初始值设定为1500,重装载寄存 器和周期寄存器的初始值设定为〇 ; 2) 判断是否经过一微秒的计数周期，是则将计数寄存器中非零的寄存器减1，周期寄 存器加1后进入下一步骤，否则直接进入下一步骤； 3) 判断DSP是否将重装载寄存器设为OxFF或者周期寄存器是否大于28000,是则说明 DSP更新了初值寄存器或者本周期超过28 了毫秒，将计数寄存器中的计数初值装载至计数 寄存器，将重装载寄存器和周期寄存器设为〇,产生下个周期的PWM信号后进入下一步；否 则直接进入下一步； 4) 由飞行模式寄存器判断飞行模式，飞行模式寄存器等于0为手动模式，进入步骤5); 飞行模式寄存器等于1为姿态模式，进入步骤6);飞行模式寄存器等于2为位置模式，进入 步骤7); 5) 将与5个计数寄存器相对应的5路PWM信号输入通道接收的信号输出在同样与5个 计数寄存器相对应的5路PWM信号输出通道上后返回步骤2); 6) 将前两路PWM信号输入通道接收的信号输出在前两路PWM信号输出通道上；5个计 数寄存器中的后三个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器 相对应的PWM信号输出通道输出高电平1，否则输出低电平0后返回步骤2); 7) 5个计数寄存器中若有非零的计数寄存器，则与所述的非零的计数寄存器相对应的 PWM信号输出通道输出高电平1，否则输出低电平〇后返回步骤2)。
8. 根据权利要求3所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，其 特征在于，所述的DSP的姿态控制方法包括如下步骤： 1) 从FPGA读取姿态给定值、姿态测量值和飞行模式； 2) 判断当前飞行模式是否为姿态模式或者位置模式，是则进入下一步，否者直接结 束； 3) 将横滚角给定值、俯仰角给定值和偏航角给定值依次与横滚角测量值、俯仰角测量 值和偏航角测量值相减，得横滚角偏差roll_error、俯仰角偏差pitch_error和偏航角偏 差 yaw_error ; 4) 计算横滚舵机控制量增量Ai^roll、俯仰舵机控制量增量△u+pitch和偏航舵机控 制量增量Au_yaw :
其中roll_error(k)、pitch_error(k)和yaw_error(k)是本次姿态控制周期计算出 的横滚角偏差、俯仰角偏差和偏航角偏差，roll_error(k-l)、pitch_error(k-l)和yaw_ error (k-1)是上次姿态控制周期计算出的横滚角偏差、俯仰角偏差和偏航角偏差，ro 1 l_kp 和roll_ki是横滚角控制的比例增益和积分增益，pitch_kp和pitch_ki是俯仰角控制的 比例增益和积分增益，yaw_kp和yaw_ki是偏航角控制的比例增益和积分增益，它们的取值 通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准； 5) 累加得横滚舵机控制量u_roll、俯仰舵机控制量u_pitch和偏航舵机控制量u_ yaw ：
然后对横滚舵机控制量u_roll、俯仰舵机控制量u_pitch和偏航舵机控制量u_yaw进 行限幅； 6) 将横滚舵机控制量、俯仰舵机控制量和偏航舵机控制量写入FPGA内部寄存器； 7) 将FPGA内部重装载寄存器置为OxFF，使写入FPGA的舵机控制量生效。
9.根据权利要求3所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法，其 特征在于，所述的DSP的经纬度控制方法包括如下步骤： 1) 从FPGA读取经纬度给定值、经纬度测量值和飞行模式； 2) 将表示经纬度测量值的字符串解算成对应的浮点数； 3) 将解算出的浮点数写入FPGA，通知FPGA上传经纬度信息； 4) 判断当前飞行模式是否为位置模式，是则进入下一步，否者直接结束； 5) 采用高斯克吕格投影方法，将经纬度给定值和经纬度测量值换算成两点间南北向距 离偏差NS_error和东西向距离偏差EW_error ; 6) 采用坐标变换，将南北向距离偏差NS_error和东西向距离偏差EW_error换算成机 身纵向距离偏差x_error和横向距离偏差y_error ;坐标变换公式为：
其中Ψ为无人直升机的偏航角； 7) 计算横滚角给定增量APhi_G和俯仰角给定增量ATheta_G:
其中x_error(k)和7_61'1'〇1'(1〇是本次经讳度控制周期计算出的机身纵向距离偏差和 横向距离偏差，x_error(k-l)和7_61'1'〇1'(1^-1)是上次经讳度控制周期计算出的机身纵向 距离偏差和横向距离偏差。x_kp和x_ki是机身纵向距离控制的比例增益和积分增益，y_ kp和y_ki是机身横向距离控制的比例增益和积分增益，它们的取值通过反复试验试凑的 方式得出，以响应速度加快并且无超调为标准； 8)累加得横滚角给定值Phi_G和俯仰角给定值Theta_G : Phi_G = APhi_G+Phi_G Theta_G = Δ Theta_G+Theta_G 然后将Phi_G和Theta_G限幅在-10°和10°之间。
10.根据权利要求3所述的基于DSP和FPGA的无人直升机飞行控制系统的控制方法， 其特征在于，所述的DSP的高度的控制方法包括如下步骤： 1) 从FPGA读取大气压测量值、温度测量值、高度给定值和飞行模式； 2) 根据校正参数和温度对大气压测量值进行温度补偿，得到校正大气压，然后利用大 气压和海拔高度的关系，将校正大气压解算成高度测量值； 3) 将解算出的高度测量值写入FPGA，通知FPGA上传高度信息； 4) 判断当前飞行模式是否为位置模式，是则进入下一步，否则直接结束； 5) 用高度给定值减去高度测量值得到高度偏差z_error ; 6) 计算油门舵机控制量增量Au_pow : Δ u_pow = z_kp X (z_error(k)-z_error(k-1))+z_ki X z_error(k)； 其中z_error(k)是本次高度控制周期计算出的高度偏差，z_error(k-l)是上次高度 控制周期计算出的高度偏差。z_kp和z_ki是高度控制的比例增益和积分增益，它们的取值 通过反复试验试凑的方式得出，以响应速度加快并且无超调为标准。 7) 累加得新油门舵机控制量u_pow = Au_pow+u_pow并对u_pow限幅； 8) 根据油门舵机控制量u_p〇w和总距舵机控制量u_col的对应关系，计算出总距舵机 控制量u_col ; 9) 将油门舵机控制量和总距舵机控制量写入FPGA内部寄存器。
【文档编号】G05B19/042GK104062933SQ201410260776
【公开日】2014年9月24日 申请日期:2014年6月12日 优先权日:2014年6月12日
【发明者】吴爱国, 江涛, 马园, 赵萌, 方星 申请人:天津大学