基于RT-Thread与滑模控制的微型多旋翼定点控制系统研究

 引言

微型飞行器在情报侦察[1]、遥感测绘[2]等领域有着极高的应用价值，而定点悬停是微型飞行器各项动作的基础，研究微型飞行器的定点悬停有着不可小觑的实用价值。

目前，主流的多旋翼定点方法有基于GPS的定点方法、基于视觉系统的定点方法[3]、基于光流的定点方法[4]以及基于UWB的定点方法[5]。本文以微型无人机光流定点控制为研究对象，完成了微型飞行器定点控制系统的设计与实现，提出了基于滑模控制的定点控制方法，并通过实验进行了验证。

1  总体方案与硬件设计

微控制器采用基于Coretex-M3内核的STM32F103C8，内置浮点运算单元等，主频最高72 MHz，非常适合微型飞控开发。无线通信模块为nRF24L01，支持实时双向通信，用于控制指令的接收与控制器参数无线调试。姿态传感器为MPU6050，内置数字运动处理引擎。罗盘为HMC5883L，最大输出频率可达160 Hz。光流模块为PMW3901MB，支持80 mm至无限远的工作距离。ToF测距传感器为VL53L0X，测量频率可达50 Hz。总体方案设计如图1所示。

硬件设计如图2所示，为尽可能减小PCB面积，便于在微型无人机上安装，元器件采用双面布局，完成后PCB为边长不超过4.5 cm的正方形。

1  现场总线技术的特点分析

虽然仅依靠前台系统即可实现飞行控制，但随着飞控中传感器种类的扩增以及飞行任务需求的日益增多，前后台系统已无法胜任日趋复杂的应用场景。引入RTOS后，将有效解决该问题。本设计中，操作系统采用RT-Thread NanoV3.1.5，小巧精悍，拥有完整内核，支持抢占式调度。此外还添加了FinSH[6]组件，用以查看各线程栈的最大使用量并合理分配各线程栈大小，避免线程栈溢出导致硬错误。姿态传感器和罗盘基于同一路I2C与微控制器通信，因此信号量互斥地使用I2C总线。

多线程调度方式、线程优先级、线程栈大小、中断优先级等的设置是本设计中稍难的点，如果安排不合理，则会导致硬实时控制无法实现、线程死锁、硬错误等问题。一般而言，在单核多任务系统中，并发运行的主线程及其他各线程可视为后台，而支持操作系统运行的SysTick、PendSV中断及其他中断则可视为前台。

除操作系统自带的tShell和tidle线程外，其余线程负责完成远程控制指令接收，姿态、高度、光流信息的获取等，具体如表1所示。

设置NVIC分组为两位抢占优先级、两位响应优先级。SysTick中断负责实现RTOS的调度，PendSV中断负责上下文切换，定时器TIM4负责脉冲宽度调制波的发生，定时器TIM1工作于周期中断模式，负责进行4路脉冲宽度调制，完成电机转速控制与姿态控制，USART1实现字符缓冲接收以支持FinSH，具体如表2所示。

3  控制方法

微型飞行器中各传感器方向的定义、坐标系定义如图3所示。

姿态获取不作为本文重点，采用Digital Motion Processing库获取四元数（q0，q1，q2，q3）及三轴角速度（ωx，ωy，ωz），按式（1）可由四元数转换得欧拉角（φ，γ，θ），配合三轴角速度作为姿态控制的输入。

整体控制设计如图4所示，高度和航向控制较为简单，图中不进行说明。姿态控制上选择串级PID，内环控制角速度，以100 Hz运行，外环控制角度，以25 Hz运行，同时将外环角度环输出的期望角速度等份递增地输出至内环角速度控制器以获得平滑的控制效果。

定点过程中往往需要定航向和定高，由罗盘和测距仪可得出航向与高度，经过简单的反馈控制即可实现航向和高度的锁定。对于航向的锁定采用简单的比例反馈控制即可实现良好的效果，对于高度采用比例积分微分控制可得到良好的效果，值得一提的是，经过激光测距仪测得的距离需引入横滚角和俯仰角校正[7]后才能得到高度。

采用式（1）计算所得偏航角存在漂移现象，因此根据磁力计在其x、y方向测得的磁感应强度分量可计算得到偏航角，在计算前需要对磁力计进行校正。

定点控制需要光流传感器的辅助，光流传感器输出连续两帧之间像素变化量Δpx和Δpy。由于光流传感器的水平位移与像素变化量、光流传感器距离地面高度、姿态角存在联系，因此根据光流传感器距离地面高度、像素变化量、姿态角可计算出光流传感器即飞行器的水平位移，计算方法如下：

式中：Sx、Sy为根据光流传感器得到的x、y方向的位移，初值均为0；L为测距传感器测量结果；R为飞行器在距地面1 m时产生1个像素变化量对应的位移；Kx为针对飞行器横滚的校正系数；Ky为针对飞行器俯仰的校正系数；Kx、Ky和R均由实验测得。

光流速度由光流位移差分并经一阶低通滤波器得到，差分间隔不宜过短，否则计算所得光流速度会有很多毛刺，无法反映飞行器实际运动。

以横滚通道为例分析定点控制过程，假定微型多旋翼定高后横滚角γ保持在某非水平值γd＞0，由运动学分析可知多旋翼将向右做加速运动。因定高定点过程中运动速度低，故可忽略风阻。可分析得出：

式中：px为水平位置；g为重力加速度。

尽管采用速度—位置双闭环控制可将无人机水平位移控制在零附近，实现定点控制，但该方法存在参数多、调参复杂的问题，因此提出了基于滑模控制的多旋翼定点控制方法。

式中：pxd为期望的无人机位置，一般取0；s、z1、z2为滑模控制器中间变量；其余的c、k、η为控制器参数。

相比于速度—位置双闭环PID控制，调试参数由6个下降至3个。

4  飞行实验

对所设计的多旋翼定点控制系统进行实验验证。依次调节角速度环PID、角度环PID参数，保证角度控制快速无静差。校正系数Kx和Ky均取480.0像素，R取0.213 cm/（m·像素），滑模控制中c=0.017 2，k=0.017 2，η=5×10-5。开始光流定点后，微型飞行器在其x方向和y方向的位移曲线及位置如图5所示。

可以看到微型飞行器位置保持在（0，0）附近，x、y方向位置均值分别为1.12 cm、-0.86 cm，方差分别为16.39 cm2、32.69 cm2，成功实现了定点控制。

5  结语

本文采用RT-Thread设计并实现了一种微型多旋翼定点控制系统，其软件中多个中断/异常嵌套运行、多个线程并发运行，并采用信号量保护了临界资源；控制上采用DMP和角速度—角度双闭环PID进行姿态控制，应用罗盘和激光测距仪实现偏航角与高度的闭环控制，融合光流传感器、测距仪、姿态传感器数据得到水平速度、位移，并对水平位移采用滑模控制器进行控制，解决了速度—位置双闭环定点方式控制参数多、调参难的问题。最终通过飞行实验验证了本系统的可行性。

审核编辑：刘清