CN110332932A - 一种室内无人机定位系统 - Google Patents

Abstract

本发明提出了一种室内无人机定位系统，旨在提供一种无人机在室内处于无或者弱GPS定位信号状态下的进行精准定位，且不需要外围辅助设备的方案，该系统包括：数据采集模块，包括惯性传感器、高度计、摄像头；底层飞控模块，利用姿态控制回路和增稳控制回路对飞行器的姿态角分别做角度控制和稳定控制；位置解算模块，采用的是互补滤波，将加速度计和高度计、光流计采集获得的数据融合，得到三维位置和速度，通过三个模块的协同调用与使用多种传感器，本发明可以使得在弱GPS定位状态与多干扰状态仍具有较好的定位效果。

Description

一种室内无人机定位系统

技术领域

本发明涉及无人机领域，涉及一种室内无人机定位系统。

背景技术

当前无人机应用于各行各业，给各行业发展带来许多便利，但目前室内弱 GPS定位信号状态对于无人机在室内定位的影响仍然较大，虽然现有技术有一些 解决的方案，但其在定位方式上仍有一些缺点。例如专利号201510888634.X公 开的室内无人机导航方法及无人机,只使用了超声波传感器与陀螺仪，只是使用 了超声波传感器，误差来源单一，专利号201410350665.5公开的一种微型无人 机室内定位方法，该方案使用图像处理和机载IMU融合计算定位，环境光照度 对其精度影响较大。

发明内容

为了克服上述现有技术的不足，本发明提供了一种室内无人机定位系统， 能够使得无人机在室内没有GPS信号且在同时存在声音与光线干扰的环境下， 按照预定线路进行精确飞行。

本发明所采用的技术方案是：

整体硬件平台设计如图2所示，惯性传感器MPU6050解算出姿态角通过I2C 通信将数据传输给微控制器；气压计MS5611测出气压值通过I2C通信将数据传 输给微控制器用作高度估计；PX4-Flow光流模块测得光流信号通过I2C通信将 数据传输给微控制器；超声波XL-MaxSonar-EZ将测出的高度信息通过PWM波的 形式传输给微控制器；无线传输模块NRF24L01P充当数传，用于连接飞控和地 面站；遥控器发出的2.4GHz信号仅接收机解码后以PWM波形式输送给微控制器。 为保证硬件的稳定性，所有模块均使用的模块化产品。

整体软件设计，如图3

1)光流处理算法

得出光流与实际速度值之间的转换关系，需建立摄像头图像坐标系与大地 坐标系之间的转换模型。这里分两部分，分别是摄像机坐标系与大地坐标系之 间的转换，摄像机坐标系与图像坐标系之间的转换。

(1)摄像机坐标系与大地坐标系之间的转换，如图4

P为地面上一点，摄像头坐标系下坐标为大地坐标系下坐 标为两者之间的关系为：

Tⁿ表示两个坐标系之间的平动，表示两个坐标系之间的旋转矩阵。

对两边求导可得：

摄像机相对于地面运动，P为地面上一点固定不动，所以令得：

展开得：

(2)摄像机坐标系与成像平面坐标系之间的转换，如图5

成像平面的x,y坐标轴与摄像机坐标系的x,y轴平行。P^c在成像平面中投 影坐标为

由投影关系可得：

两边求导得：

将(4)(5)带入(6)得：

很小时(另一方面相对f也比较小)，并且忽略高阶项

根据上述转换，可以得出光流与实际速度值之间的关系，并提供给飞控系统 作为位置解算数据信息。

2)姿态解算部分算法

四元数法和欧拉角法，这两种姿态解算算法中都只需要角速度值就可解算出 姿态，但因为对陀螺仪数据积分会造成累积误差，导致解算出的姿态误差很大。 所以在使用上述两种算法的时候需要使用加速度计对陀螺仪的值进行修正，以 得到相对准确的姿态。下面的互补滤波算法就是融合加速度计、陀螺仪数据的 解算算法。

将无人机看作暂稳态，在这种假设下，加速度计的测量值在NED坐标系下始 终为(经过归一化处理)：

经旋转矩阵转换到机体坐标系b系中(旋转矩阵为上一时刻解算得出)：

在b系中，加速度计的测量值为

现在和acc均表示在b系中的竖直向下的向量，两者之间的误差

也就是说只要陀螺仪解算得出的姿态的XOY平面与加速度计得出的XOY不重 合，上述误差就会一直存在。加速度计可以修正横滚叫和俯仰角但是不能修正 偏航角。一般还需要用磁力计或GPS修正偏航角，因为暂时没有用到这两个传感 器，所以不对偏航角进行修正。

误差经PI控制器得到角速度的补偿值：

ω_PCorr＝K_p·Corr_rp

ω_ICorr＝ω_PCorr+K_I·Corr_rp·dt

ω_Corr＝ω_PCorr+ω_ICorr

陀螺仪的测量值为

校正后的角速度值为ω＝gyro+ω_Corr

将校正后的角速度带入

求出角度变化量后，再带入下列式子(对四元数法更新旋转矩阵的微分方程)

采用一阶近似毕卡算法求解获得的式子)

即可解算出四元数，再将解算出得四元数带入下式

即可得欧拉角。

3)姿态控制部分

姿态控制采用双环PID，如图6，期望角度＝遥控器输入/(遥控器最大值-遥 控器最小值)×最大控制角度；

期望角速度＝角度环输出/最大控制角度×最大控制角速度

当前角度为姿态解算得出的角度；

当前角速度为陀螺仪测量值。

4)高度互补滤波部分

高度解算采用的是互补滤波，将加速度计和超声波或是气压计的数据融合， 得到高度。因为噪声和零偏的存在，加速度计测得的加速度值有误差，若直接 积分得到速度和高度将会产生很大的累积误差。现利用超声波和气压计对加速 度的测量进行校正。因为超声波的精度和稳定性远高于气压计，所有在超声波 测量值有效的情况下，就采用超声波的值；超声波测量值无效时，则采用气压 计的值，高度互补解算流程，如图7。

5)高度控制部分

高度控制采用串级PID。当油门位于油门中值附近时，启动定高模式，期望 高度为进入定高模式的那个周期解算的高度值，期望速度为高度误差经PID调节 得到。如果油门一直位于中值附近，则保持这个高度；如果油门偏离中值，则 采用手动控制，定高失效，期望速度为油门值乘以一个比例系数。如图8使用渡 越时间检测法作为超声波传感器的测速方法超声波传感器由强电脉冲信号激 励，向外发射超声波，此时微控制器开始计时，超声波遇到被测物体后形成反 射回波，超声波接收器接收到反射回波，停止计时，在这一过程中获得的时间 即为超声波在媒质(空气)中传播的往返时间，通过这一时间可实现高度测量， 时间与所测高度的关系见下式：

h＝v_ct/2

式中h——距离(m)；

t——超声波往返时间(s)；

v_c——超声波在空气中的传播速度(m/s)。

与现有技术相比，本发明的有益效果是增强了无人机在存在声音与光线混合干扰的情况下，相较于单一的依靠超声波传感器或者摄像头的定位方式，此方式 能够提高无人机的定位稳定性。

附图说明

图1整体控制原理图；

图2为整体硬件平台设计；

性传感器MPU6050解算出姿态角通过I2C通信将数据传输给微控制器；气压 计MS5611测出气压值通过I2C通信将数据传输给微控制器用作高度估计； PX4-Flow光流模块测得光流信号通过I2C通信将数据传输给微控制器；超声波 XL-MaxSonar-EZ将测出的高度信息通过PWM波的形式传输给微控制器；无线传输 模块NRF24L01P充当数传，用于连接飞控和地面站；遥控器发出的2.4GHz信号仅 接收机解码后以PWM波形式输送给微控制器。

图3为整体软件设计

光流模块采集图像信号，对位置进行解算，读取加速度计和陀螺仪的测量 数据使用互补解算的方法对姿态和部分三维位置进行解算，综合上述数据进行 姿态控制，同时读取高度计和超声波传感器的数据进行高度解算，加速度计和 陀螺仪数据也会参与这个高度解算的过程，修正高度误差；姿态控制和高度控 制可以由遥控器参与控制，相应的姿态控制和高度控制信息经由混合器处理后， 转换信号到电机上直接进行驱动调整。

图4为光流处理算法中，摄像机坐标系与大地坐标系之间的转换示例

P为地面上一点，Oc代表摄像头坐标系的原点，相应的Xc、Yc、Zc分别代 表摄像头坐标系的X,Y,Z坐标轴，On代表大地坐标系的原点，相应的Xn、Yn、 Zn分别代表大地坐标系的X,Y,Z坐标轴，摄像头坐标系下坐标为大地坐标系下坐标为两者之间的关系为：

如图5为光流处理算法中，摄像机坐标系与成像平面坐标系之间的转换示例

Oc代表摄像头坐标系的原点，相应的Xc、Yc、Zc分别代表摄像头坐标系的 X,Y,Z坐标轴，成像平面的x,y坐标轴与摄像机坐标系的x,y轴平行。P^c在成像平 面中投影坐标为

如图6为姿态控制采用双环PID的流程

输入期望角度参与角度PID运算，得到期望角速度参与下一级的角速度PID， 相应的运算结果以PWM波形式输出到微控制器中，对无人机进行控制，而这两个 PID的输入过程都会产生误差，而无人机上各传感器测量处理得到的角速度与角 度又参与到两个PID的输入过程，对角度与角速度进行修正。

如图7为高度解算采用互补滤波的流程

加速度计测得相应的加速度，但具有一定的误差，高度计中的超声波传感 器和气压计的测得信号数据则可以在经过换算后，参与确定加速度的过程，进 行修正，降低测量加速度的误差，加速度经过校正好，可以用于预测速度与高 度，得到预测的速度与高度后，高度计测得数据再一次经过算法处理后作为修 正速度与高度的参数，以降低预测得到的速度与高度的误差。最后估计出来的 速度与高度，再参与到高度计的数据处理过程，以降低数据处理过程中的误差。

如图8为高度控制采用串级PID的流程

高度控制采用串级PID。当油门位于油门中值附近时，启动定高模式，期望 高度为进入定高模式的那个周期解算的高度值，期望速度为高度误差经PID调节 得到。如果油门一直位于中值附近，则保持这个高度；如果油门偏离中值，则 采用手动控制，定高失效，期望速度为油门值乘以一个比例系数。

具体实施方式

四旋翼无人机室内定位系统，包含无人机、地面控制站、数据处理系统、 数据采集系统，本实例采用的四旋翼无人机，无人机包括飞行控制系统和数据 传输系统，所述的数据传输系统是指用于将各个传感器接收到的信号传输给飞 行控制系统与地面控制系统的物理组件与程序，飞行控制系统是指能够接受来 自地面控制站的信号或者自身相对环境发生状态所产生的信号，并能够对上述 信号做出及时的反馈以根据操控目的改变状态或者在对抗自身以及环境产生的 不稳定而做出稳定化调整的一种物理组件与程序。

本例将无人机自带的遥控器作为地面控制站，本例中的数据采集系统包括 了惯性传感器MPU6050、气压计MS5611、PX4-Flow光流模块、超声波 XL-MaxSonar-EZ，数据传输系统包含无线传输模块NRF24L01P，用于连接飞控和 地面站；遥控器发出的2.4GHz信号仅接收机解码后以PWM波形式输送给微控制 器。

使用STM32F407作为数据处理系统的微控制器，XL-MaxSonar系列中的 MB1240超声波测距传感器、CCD类型图像传感器、MPU6050作为惯性传感器起到 加速度计和陀螺仪的作用、MEAS传感器公司的MS5611气压计传感器作为高度计。

当整个系统开始工作的时候，所述的数据采集系统中的惯性传感器采集到 了能够携带有无人机运动时的各个方向加速度与角速度信息的相关未处理的信 号，而PX4-Flow光流模块采集到了检测附近环境的图像的光强度变化，经过进 一步处理可以得到无人机的在三维坐标系中的速度情况，气压计和超声波传感 器组合作为的检测高度的高度计，根据实际情况，综合两者的信号进行处理， 得到准确的高度信息，然后惯性传感器和光流模块的信号作为一部份被传输至 微处理器，经过算法处理与坐标系变换后，得到无人机的姿态信息，并通过相 关的反馈控制进行实时调整电机的输出状态，维持姿态的稳定，而气压计与超 声波传感器的信号作为一部分也被传输到微处理器中，经过算法处理后，得到了高度信息，并通过反馈控制，驱动电机，维持高度的稳定。

上述的两个的过程会根据地面站发送的控制信号进行调整，以便得到并维 持预期的三维位置与姿态效果，

所述地面站为可移动电脑与遥控器，用于监视无人机实时状态、发送控制 指令和存储无人机下传的数据。

Claims (5)

1.一种无人机室内定位系统，其特征在于：

数据采集模块，包括惯性传感器(内含陀螺仪与加速度计)、高度计(内含超声波传感器和气压计等可以测量高度的模块)、摄像头；

底层飞控模块，利用姿态控制回路和增稳控制回路对飞行器的姿态角分别做角度控制和稳定控制；

位置解算模块，采用的是互补滤波，将加速度计和高度计、光流计采集获得的数据融合，得到三维位置和速度。

2.根据权利要求1所示的底层飞控模块，其特征在于：还包括姿态控制回路根据飞行器回传的姿态角采用PID控制算法做角度控制,增稳控制回路根据回传的角速度进行稳定控制。

3.根据权利要求1所示的数据采集模块，其特征在于：高度计包括超声波传感器以及气压计由一种或多种能够测量高度的传感器组成的模块。

4.根据权利要求1所示的底层飞控模块，其特征在于：飞控通过接收STM32单片机传输到油门、俯仰、滚转、航向四个通道的PWM信号,根据控制算法输出四路PWM信号传输至电调以实现对飞行器的内环控制。

5.根据权利要求2所示的姿态控制回路，其特征在于：姿态控制器包括PID控制器与输出解算器两部分,根据飞机回传的姿态角采用PID控制算法做角度控制。