一种基于机载传感器的小型四旋翼飞行器控制系统及方法

1.一种基于机载传感器的小型四旋翼飞行器控制系统的控制方法，包括惯性测量单元
模块、微处理器（4）、电调（5）、超声波传感器（6）、光流传感器（7）、摄像头（8）、无线模块（9）、直流无刷电机（10）；
所述惯性测量单元模块与微处理器（4）的IIC总线接口相连接；电调（5）与微处理器（4）的PWM口相连接；超声波传感器（6）与微处理器（4）的I/O口相连接；光流传感器（7）与微处理器（4）的SPI总线接口相连接；无线模块（9）与微处理器（4）的串口SCI相连接；直流无刷电机（10）与电调（5）的输出相连接；摄像头（8）与微处理器（4）连接；
所述的惯性测量单元模块包括三轴磁力计（1）、三轴加速度计（2）、三轴陀螺仪（3），所述三轴磁力计（1）、三轴加速度计（2）、三轴陀螺仪（3）同时与微处理器（4）的IIC总线接口相连接；
其特征在于，包括以下步骤：
S1. 微处理器（4）读取三轴磁力计（1）、三轴加速度计（2）、三轴陀螺仪（3）的数据，并且将得到的数据进行数据融合解算得到飞行器的姿态信息；微处理器（4）读取超声波传感器（6）和光流传感器（7）的数据，并且将得到的数据转换成对应的位置坐标信息；
S2. 基于估计得到的飞行器姿态和位置坐标信息，采用PID、PD或PI控制算法，计算出对应微处理器（4）需要输出的PWM信号；
S3. 微处理器（4）将计算得到的PWM信号输出给电调（5）；
S4. 电调（5）驱动直流无刷电机（10），通过控制四个电机的转速；
所述的步骤S1中，三轴磁力计（1）、三轴加速度计（2）、三轴陀螺仪（3）估计飞行器的三个姿态角，三个姿态角包括俯仰角 、横滚角 、偏航角 ；
所述的步骤S1中，超声波传感器（6）测得飞行器相对地面的高度z，光流传感器（7）测得飞行器水平方向的速度信息，对速度进行积分得到飞行器水平方向的位移，得到飞行器水平方向位置x和y；
所述的姿态控制频率为400赫兹，位置控制频率为40赫兹，微处理器（4）每2.5毫秒读取三轴磁力计（1）、三轴加速度计（2）、三轴陀螺仪（3）的数据；微处理器（4）每25毫秒读取超声波传感器（6）和光流传感器（7）的数据；
微处理器（4）获得飞行器完整的姿态角度和位置信息后，飞行器的自主飞行控制采用内环和外环的闭环控制结构，其中内环控制飞行器的姿态角，外环控制飞行器的位置，内环的姿态控制采用比例微分PD控制算法；外环的位置控制采用比例积分微分PID控制算法；
控制方法包括飞行器垂直起降和悬停的控制方法、飞行器自主追踪航迹点的控制方
法；
飞行器垂直起降和悬停的控制方法为：将飞行器姿态期望值设为 ，
水平方向位置设为 ，飞行器期望高度 从地面起飞时的高度增加到一个期望
高度，则可实现飞行器的垂直起飞；起飞后期望高度设置为一个常数，则飞行器悬停在期望高度；期望高度从当前高度减小到飞行器起飞是离地面的高度，实现飞行器的垂直降落；
飞行器自主追踪航迹点的控制方法为：飞行器姿态期望值设为 ，飞
行器的期望位置 设为预定的航迹点的位置坐标，在飞行器当前点和预设的航迹点
之间利用插值算法进行插值，生成期望飞行轨迹，飞行器按照期望飞行轨迹飞往预设的航迹点；
其中，( )为飞行器姿态角的期望值，( )为飞行器姿态的估计值，(
)为飞行器的期望位置，( )为测得的飞行器实际位置。

一种基于机载传感器的小型四旋翼飞行器控制系统及方法\n技术领域\n[0001] 本发明涉及四旋翼飞行器的技术领域，更具体地，涉及一种基于机载传感器的小型四旋翼飞行器控制系统及方法。\n背景技术\n[0002] 由于小型四旋翼飞行器具有体积小、重量轻、可垂直起飞降落、定点悬停、高机动性等优点，在交通监控、自然灾害监视与救援、环境和污染监测、农林业勘测、应对突发事件等领域具有广阔的应用前景。\n[0003] 四旋翼飞行器是一个多变量、强耦合的复杂非线性系统，由于在三维空间中运动，它的飞行姿态和运动轨迹的控制比地面机器人的控制更为困难。受有限负载能力和电源的限制，小型旋翼飞行器根据机载的轻便传感器系统（例如视觉传感器、小型激光测距仪）感知外部环境。同时，由于机载控制板的运算能力有限，难以在机载控制板上进行复杂的视觉和控制算法的运算。因此，如何根据机载小型传感器系统感知外部环境，如何实时规划和控制其运动轨迹以实现小型旋翼飞行器的自主飞行，是非常具有挑战性的问题。当前在微小型旋翼无人机领域的研究工作主要集中在飞行器的自主稳定飞行、自主避障、基于机载视觉的目标识别和跟踪、三维空间的同步自定位与地图创建及多飞行器的协调控制等。美国宾夕法尼亚大学、瑞士苏黎世联邦理工学院、斯坦福大学等机构在小型旋翼飞行器的自主飞行研究领域处于领先地位。 宾夕法尼亚大学的GRASP实验室和瑞士联邦理工学院的研究团队研究了室内环境中单个小型旋翼飞行器的自主飞行以及多个小型飞行器的编队飞行与协同合作。在飞行过程中，宾夕法尼亚大学和瑞士联邦理工的研究团队利用Vicon高速动作捕捉系统所提供的旋翼无人机的精确室内全局位置信息，实现了旋翼飞行器的高灵敏度与高精度的飞行姿态控制。\n[0004] 在室外环境下，飞行器的位置信息可由全球定位系统系统（GPS）提供实时位置信息。在无GPS信号的环境下，例如在室内环境中，通常由高速、高精度的室内定位系统（如Vicon高速动作捕捉系统、Optitrack运动捕捉系统、基于超宽带技术的无线定位系统等）提供飞行器的实时位置信息，这些高速、高精度室内定位系统的价格非常昂贵，同时，由于飞行器受限于定位系统的有效测量空间，这极大地限制了飞行器自主飞行的性能。实现飞行器自定位的另一种解决方案是使用激光测距仪，但适于小型飞行器的激光测距仪的价格昂贵、更新速率低。光流技术是一种有效的自主定位方案，基于电脑鼠标传感器或CMOS摄像头的光流传感器都已被成功的应用于单个飞行器的自主导航。\n发明内容\n[0005] 本发明为克服上述现有技术所述的至少一种缺陷，提供一种基于机载传感器的小型四旋翼飞行器控制系统，是基于低成本、轻量的机载传感器的小型四旋翼飞行器的自主飞行控制系统。在无GPS信号环境中，基于机载的传感器和机载的微处理器，实现了飞行器6自由度飞行姿态的实时估计与控制，提供了一种精确、可靠、低成本的小型旋翼飞行器的自主飞行控制策略。进一步的，提供一种基于机载传感器的小型四旋翼飞行器控制方法，其方法步骤简洁，可实时估计飞行器的6自由度飞行姿态。\n[0006] 为解决上述技术问题，本发明采用的技术方案是：一种基于机载传感器的小型四旋翼飞行器控制系统，其中，包括惯性测量单元模块、微处理器、电调、超声波传感器、光流传感器、摄像头、无线模块、直流无刷电机；\n[0007] 所述惯性测量单元模块与微处理器的IIC总线接口相连接；电调与微处理器的PWM口相连接；超声波传感器与微处理器的I/O口相连接；光流传感器与微处理器的SPI总线接口相连接；无线模块与微处理器的串口SCI相连接；直流无刷电机与电调的输出相连接；摄像头与微处理器连接。\n[0008] 所述的惯性测量单元模块包括三轴磁力计、三轴加速度计、三轴陀螺仪，所述三轴磁力计、三轴加速度计、三轴陀螺仪同时与微处理器的IIC总线接口相连接。\n[0009] 本发明中，基于机载传感器实时估计飞行器的6自由度飞行姿态，包括三个姿态角（俯仰角 、横滚角 、偏航角 ）以及在惯性坐标系中的( )。融合机载的惯性测量单元模块（包括三轴陀螺仪，三轴加速度计和三轴磁力计）的测量数据，可准确估计飞行器的三个姿态角。\n[0010] 采用基于梯度下降法的数据融合算法(Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan, Estimation of IMU and MARG orientation using a gradient descent algorithm, Proc. of IEEE Conf. Rehabilitation Robotics, pp. \n1-7, Zurich, June 2011)，融合惯性测量单元的数据，可精确解算飞行器的姿态角度。飞行器相对地面的高度由超声波传感器测得；光流传感器测得飞行器水平方向的速度信息，对速度进行积分可得到飞行器水平方向的位移。\n[0011] 飞行器底部安装了彩色摄像头，可获取地面的视频信息，实时视频信息通过图传模块发送给地面工作站，以实现飞行器对地面的实时监控。通过无线模块，飞行器的各状态传输给地面工作站，同时地面工作站通过无线模块给飞行器发送上层控制指令。\n[0012] 本飞行器在无GPS信号环境中，基于机载的传感器和机载的微处理器，实现了飞行器6自由度飞行姿态的实时估计与控制。\n[0013] 进一步的，一种应用所述基于机载传感器的小型四旋翼飞行器控制系统的控制方法，其中，包括以下步骤：\n[0014] S1. 微处理器读取三轴磁力计、三轴加速度计、三轴陀螺仪的数据，并且将得到的数据进行数据融合解算得到飞行器的姿态信息；微处理器读取超声波传感器和光流传感器的数据，并且将得到的数据转换成对应的位置坐标信息；\n[0015] S2. 基于估计得到的飞行器姿态和位置坐标信息，采用PID、PD或PI控制算法，计算出对应微处理器需要输出的PWM信号；\n[0016] S3. 微处理器将计算得到的PWM信号输出给电调；\n[0017] S4. 电调驱动直流无刷电机，通过控制四个电机的转速。\n[0018] 进一步的，所述的步骤S1中，三轴磁力计、三轴加速度计、三轴陀螺仪估计飞行器的三个姿态角，三个姿态角包括俯仰角 、横滚角 、偏航角 。所述的步骤S1中，超声波传感器测得飞行器相对地面的高度z，光流传感器测得飞行器水平方向的速度信息，对速度进行积分得到飞行器水平方向的位移，得到飞行器水平方向位置x和y。\n[0019] 进一步的，所述的姿态控制频率为400赫兹，位置控制频率为40赫兹，微处理器每\n2.5毫秒读取三轴磁力计、三轴加速度计、三轴陀螺仪的数据；微处理器每25毫秒读取超声波传感器和光流传感器的数据。\n[0020] 飞行器处于悬停或者近似悬停状态时，四旋翼飞行器可近似为线性系统，此时，飞行器的姿态角( )和位置( )可解耦为6个独立状态变量分别进行控制。通过机载传感器系统，微处理器获得飞行器完整的姿态角度和位置信息后，飞行器的自主飞行控制采用内环和外环的闭环控制结构，其中内环控制飞行器的姿态角，外环控制飞行器的位置，内环的姿态控制采用比例微分PD控制算法；外环的位置控制采用比例积分微分PID控制算法。\n[0021] 四旋翼飞行器的控制首先是姿态的稳定控制，在姿态稳定控制的基础上实现飞行器的位置控制。因此，内环姿态控制的控制频率远高于外环位置控制的控制频率，以实现飞行器姿态的稳定、快速、准确的控制，进而提高外环位置控制的精度。\n[0022] 具体的，控制方法包括飞行器垂直起降和悬停的控制方法、飞行器自主追踪航迹点的控制方法；\n[0023] 飞行器垂直起降和悬停的控制方法为：将飞行器姿态期望值设为\n，水平方向位置设为 ，飞行器期望高度 从地面起飞时的\n高度增加到一个期望高度，则可实现飞行器的垂直起飞；起飞后期望高度设置为一个常数，则飞行器悬停在期望高度；期望高度从当前高度减小到飞行器起飞是离地面的高度，实现飞行器的垂直降落；\n[0024] 飞行器自主追踪航迹点的控制方法为 ：飞行器姿态期望值设为\n，飞行器的期望位置 设为预定的航迹点的位置坐标，在飞行器\n当前点和预设的航迹点之间利用插值算法进行插值，生成期望飞行轨迹，飞行器按照期望飞行轨迹飞往预设的航迹点；\n[0025] 其中，( )为飞行器姿态角的期望值，( )为飞行器姿态的估计值，()为飞行器的期望位置，( )为测得的飞行器实际位置。\n[0026] 另外，利用光流传感器所提供的水平位置信息，还可基于领队随从策略实现两个飞行器的编队控制。考虑如下情况：领队和随从按照同一期望轨迹自主飞行，随从在飞行中需与领队保持特定的距离。由于光流传感器测量的是飞行器相对于起始点的水平位置，因此初始化时领队与随从的相对距离即为两个飞行器在飞行中需保持的距离。领队在飞行轨迹控制中，分别对其的位置和速度进行控制，位置通过PI控制，而速度通过PD控制。此时，不仅对随从的位置和速度分别进行PI和PD控制，还引入了随从与领队之间相对距离的PD控制。\n[0027] 与现有技术相比，有益效果是：本发明融合低成本、轻量的机载传感器系统的信息实时估计飞行器的6自由度飞行姿态，设计了内环姿态控制和外环位置控制的闭环控制策略，在没有全球定位系统（GPS）和室内定位系统的环境下，利用机载传感器系统和微处理器实现了旋翼飞行器的自主垂直起降、室内精确定位、自主悬停、自主追踪航迹点的飞行轨迹控制以及基于领队跟随策略的飞行器编队控制，为实现旋翼飞行器的自主飞行提供了一种可靠、精确、低成本的控制策略。\n附图说明\n[0028] 图1是本发明的四旋翼飞行器控制系统的结构示意图。\n[0029] 图2是本发明的四旋翼飞行器的控制框图。\n[0030] 图3是本发明的四旋翼飞行器的系统流程图。\n[0031] 图4是本发明的飞行实验中，自主垂直起降和悬停时，飞行器姿态角与位置信息的实时估计值。\n[0032] 图5是本发明的飞行实验中，旋翼飞行器自主跟踪轨迹时的实际飞行轨迹。\n[0033] 图6是本发明的编队飞行实验中，两个飞行器的实际飞行轨迹。\n具体实施方式\n[0034] 附图仅用于示例性说明，不能理解为对本专利的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本专利的限制。\n[0035] 如图1所示，一种基于机载传感器的小型四旋翼飞行器控制系统，其中，包括惯性测量单元模块、微处理器4、电调5、超声波传感器6、光流传感器7、摄像头8、无线模块9、直流无刷电机10；\n[0036] 惯性测量单元模块与微处理器4的IIC总线接口相连接；电调5与微处理器4的PWM口相连接；超声波传感器6与微处理器4的I/O口相连接；光流传感器7与微处理器4的SPI总线接口相连接；无线模块9与微处理器4的串口SCI相连接；直流无刷电机10与电调5的输出相连接；摄像头8与微处理器4连接。\n[0037] 惯性测量单元模块包括三轴磁力计1、三轴加速度计2、三轴陀螺仪3，三轴磁力计\n1、三轴加速度计2、三轴陀螺仪3同时与微处理器4的IIC总线接口相连接。\n[0038] 本实施例中，基于机载传感器实时估计飞行器的6自由度飞行姿态，包括三个姿态角（俯仰角 、横滚角 、偏航角 ）以及在惯性坐标系中的( )。融合机载的惯性测量单元模块（包括三轴陀螺仪3，三轴加速度计2和三轴磁力计1）的测量数据，可准确估计飞行器的三个姿态角。\n[0039] 采用基于梯度下降法的数据融合算法(Sebastian O.H. Madgwick, Andrew J.L. Harrison, Ravi Vaidyanathan, Estimation of IMU and MARG orientation using a gradient descent algorithm, Proc. of IEEE Conf. Rehabilitation Robotics, pp. \n1-7, Zurich, June 2011)，融合惯性测量单元的数据，可精确解算飞行器的姿态角度。飞行器相对地面的高度由超声波传感器6测得；光流传感器7测得飞行器水平方向的速度信息，对速度进行积分可得到飞行器水平方向的位移。\n[0040] 本实施例中，飞行器底部安装了彩色摄像头8，可获取地面的视频信息，实时视频信息通过图传模块发送给地面工作站，以实现飞行器对地面的实时监控。通过无线模块9，飞行器的各状态传输给地面工作站，同时地面工作站通过无线模块9给飞行器发送上层控制指令。本飞行器在无GPS信号环境中，基于机载的传感器和机载的微处理器4，实现了飞行器6自由度飞行姿态的实时估计与控制。\n[0041] 如图2、3所示，一种应用所述基于机载传感器的小型四旋翼飞行器控制系统的控制方法，其中，包括以下步骤：\n[0042] S1. 微处理器4读取三轴磁力计1、三轴加速度计2、三轴陀螺仪3的数据，并且将得到的数据进行数据融合解算得到飞行器的姿态信息；微处理器4读取超声波传感器6和光流传感器7的数据，并且将得到的数据转换成对应的位置坐标信息；\n[0043] S2. 基于估计得到的飞行器姿态和位置坐标信息，采用PID、PD或PI控制算法，计算出对应微处理器4需要输出的PWM信号；\n[0044] S3. 微处理器4将计算得到的PWM信号输出给电调5；\n[0045] S4. 电调5驱动直流无刷电机10，通过控制四个电机的转速。\n[0046] 步骤S1中，三轴磁力计1、三轴加速度计2、三轴陀螺仪3估计飞行器的三个姿态角，三个姿态角包括俯仰角 、横滚角 、偏航角 。步骤S1中，超声波传感器6测得飞行器相对地面的高度z，光流传感器7测得飞行器水平方向的速度信息，对速度进行积分得到飞行器水平方向的位移，得到飞行器水平方向位置x和y。\n[0047] 本实施例中，姿态控制频率为400赫兹，位置控制频率为40赫兹，微处理器每2.5毫秒读取三轴磁力计、三轴加速度计、三轴陀螺仪的数据；微处理器每25毫秒读取超声波传感器和光流传感器的数据。\n[0048] 由此基于机载传感器实现了飞行器的全状态反馈。基于估计得到的飞行器姿态和位置坐标信息，采用PID、PD或PI控制算法，计算出对应微处理器4需要输出的PWM信号，微处理器4将计算得到的PWM信号输出给电调5，电调5驱动无刷电机10，通过控制四个电机的转速，实现单个飞行器的自主垂直起降、自主悬停、轨迹跟踪飞行以及两个飞行器的编队飞行。\n[0049] 如图2中，飞行器处于悬停或者近似悬停状态时，四旋翼飞行器可近似为线性系统，此时，飞行器的姿态角( )和位置( )可解耦为6个独立状态变量分别进行控制。通过机载传感器系统，微处理器获得飞行器完整的姿态角度和位置信息后，飞行器的自主飞行控制采用内环和外环的闭环控制结构，其中内环控制飞行器的姿态角，外环控制飞行器的位置，内环的姿态控制采用比例微分PD控制算法；外环的位置控制采用比例积分微分PID控制算法。\n[0050] U1 为飞行器垂直速度控制量，U2 为飞行器滚转控制量，U3 为飞行器俯仰控制量，U4 为飞行器偏航控制量。电调与微处理器和直流无刷电机相连接。微处理器计算出控制量U1，U2，U3，U4，将对应的力转换成电调所需输入PWM值，加载到电调的输入端，控制电机的转速，从而实现对飞行器的姿态和位置控制。\n[0051] 四旋翼飞行器的控制首先是姿态的稳定控制，在姿态稳定控制的基础上实现飞行器的位置控制。因此，内环姿态控制的控制频率远高于外环位置控制的控制频率，以实现飞行器姿态的稳定、快速、准确的控制，进而提高外环位置控制的精度。\n[0052] 具体的，本实施例中，控制方法包括飞行器垂直起降和悬停的控制方法、飞行器自主追踪航迹点的控制方法；\n[0053] 飞行器垂直起降和悬停的控制方法为：将飞行器姿态期望值设为\n，水平方向位置设为 ，飞行器期望高度 从地面起飞时的\n高度增加到一个期望高度，则可实现飞行器的垂直起飞；起飞后期望高度设置为一个常数，则飞行器悬停在期望高度；期望高度从当前高度减小到飞行器起飞是离地面的高度，实现飞行器的垂直降落；\n[0054] 飞行器自主追踪航迹点的控制方法为 ：飞行器姿态期望值设为\n，飞行器的期望位置 设为预定的航迹点的位置坐标，在飞行器\n当前点和预设的航迹点之间利用插值算法进行插值，生成期望飞行轨迹，飞行器按照期望飞行轨迹飞往预设的航迹点；\n[0055] 其中，( )为飞行器姿态角的期望值，( )为飞行器姿态的估计值，()为飞行器的期望位置，( )为测得的飞行器实际位置。\n[0056] 另外，利用光流传感器所提供的水平位置信息，还可基于领队随从策略实现两个飞行器的编队控制。考虑如下情况：领队和随从按照同一期望轨迹自主飞行，随从在飞行中需与领队保持特定的距离。由于光流传感器测量的是飞行器相对于起始点的水平位置，因此初始化时领队与随从的相对距离即为两个飞行器在飞行中需保持的距离。领队在飞行轨迹控制中，分别对其的位置和速度进行控制，位置通过PI控制，而速度通过PD控制。此时，不仅对随从的位置和速度分别进行PI和PD控制，还引入了随从与领队之间相对距离的PD控制。\n[0057] 如图4中，飞行实验中，自主垂直起降和悬停时，飞行器姿态角与位置信息的实时估计值，上面的图表示飞行器的姿态角度估计值：从上到下分别是横滚角、俯仰角、偏航角；\n下面的图表示飞行器的三维坐标位置测量值：x、y、z。\n[0058] 如图5所示，飞行实验中，旋翼飞行器自主跟踪轨迹时的实际飞行轨迹。虚线1为期望的飞行轨迹；曲线2为光流传感器所测量的飞行器的飞行轨迹；作为定位坐标的参考，虚线3为基于超宽带技术的室内全局定位系统Ubisense所提供的飞行器的飞行轨迹。如图所示，基于所提出的基于机载传感器的飞行器控制系统，飞行器可实现自主垂直起降、自主悬停以及轨迹跟踪飞行。\n[0059] 如图6所示，编队飞行实验中，两个飞行器的实际飞行轨迹，其中领队和随从按同一期望轨迹自主飞行，随从需与领队保持10米的距离。（a）虚线1为期望的飞行轨迹；曲线2为光流传感器所测量的领队飞行器的飞行轨迹；曲线3为光流传感器所测量的随从飞行器的飞行轨迹。\n[0060] (b) 连续三次飞行中，在XY平面上，领队与随从飞行器之间的距离，其中曲线4为期望的10米的距离。曲线1、2、3分别表示第一、第二、第三次飞行。\n[0061] 显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。