基于偏振光传感器的无人机姿态组合滤波算法

金仁成,刘 忱,黄启鹏,孙庆飞

(大连理工大学辽宁省微纳米技术及系统重点实验室,辽宁大连 116024)

0 引言

随着MEMS技术的不断发展,惯性传感器因体积小、自主性强的特点已被应用到无人车辆[1]、无人机[2]等领域。但由于传感器本身特性:陀螺仪会随着时间产生累计误差、加速度计会产生高频的噪声[3],惯性导航和GPS或磁力计进行组合导航来实现姿态的求解[4-5],但二者受外界影响较大,因此,高可靠性、高精度导航方式的研究对无人机导航系统非常重要。

偏振光导航是一种仿照蜜蜂、沙蚁利用天空偏振光进行导航的一种新型导航方式,具有不易受干扰、无累计误差的优点。针对利用偏振光导航的方法,国内外学者进行了深入的研究。D.Lambrinos等[6]仿照沙蚁导航的思想搭建了偏振光传感器进行了角度测量并用于地面机器人导航,证实利用偏振光导航的可行性。褚金奎等[7]搭建了国内首个天空偏振光测量系统和新型的偏振光导航传感器,并实现了无人车、无人船的自主导航。J.Chanhl等[8]将偏振光传感器搭载在无人机上,实现了航向角的测量。范晨等[9]推导出用于三维空间的偏振光组合导航的模型并进行了仿真,利用卡尔曼滤波算法验证偏振光辅助导航的原理,但其模型中仅考虑了理想的测量噪声,并未考虑其他不良影响。赵成帅等[10]设计了仿生偏振光定向系统,但忽视了无人机对姿态解算的实时性要求,无法满足无人机高动态的要求。

针对传统组合导航系统存在的易受干扰、具有累计误差的问题,本文提出了一种偏振光辅助惯性导航的无人机姿态滤波算法。针对多传感器的性能差异,建立偏振误差矢量和重力加速度误差矢量作为PI互补滤波的输入量,对陀螺仪的输出进行修正;利用粒子滤波,对多传感器信号进一步融合补偿,输出更高精度的姿态信息。最后通过静止、转动实验,验证了本文所提出组合滤波方法能获得高精度的姿态信息。

1 偏振光导航及姿态角求解

定义坐标系如下:选择“北东地”作为导航坐标系,记为n系;选择“右前下”作为载体坐标系,记为b系;偏振光传感器坐标系为m系。

1.1 基于四元数的姿态更新

四元数法[11-12]是一种特别的四参量姿态表示方法,解决了方向余弦法所需计算参数过多的问题,具有计算量小的特点;也解决了欧拉角法中存在90°奇异性问题,具有在全方位完全的姿态解算能力。故本文采用四元数法进行姿态矩阵的实时计算与更新。定义为q=[q0,ρ]T,q0为标量,ρ=[q1,q2,q3]T为三维矢量。

姿态四元数微分方程为

(1)

即:

(2)

(3)

将四元数转化为姿态角:

(4)

θ=-arcsin[2(q1q3-q0q2)]

(5)

(6)

式中:γ为横滚角;θ为俯仰角;Ψ为航向角。

1.2 偏振光导航原理

太阳光遇到大气单粒子单次散射会形成稳定的大气偏振分布模式。在大气偏振分布模式下,使用偏振光传感器可以检测到最大偏振方向E,根据Rayleigh散射模型[13]可知入射光的最大偏振方向E垂直于传感器视线方向OZm和太阳S确定的平面,建立偏振光定向原理图,如图1所示。

图1 偏振光定向原理图

通过偏振光传感器测量输出的偏振角φ,最大偏振矢量可以在m系中表示为

(7)

太阳位置可以由太阳高度角hs和太阳方位角αs表示,太阳矢量OS在n系下的投影表示为

(8)

根据Rayleigh模型可以得到m系下入射光最大偏振矢量的表达式:

(9)

由于通过偏振矢量求解航向角存在180°模糊解[14],可以得到机体航向角Ψ为

Ψ=-(αs-φ±90°)

(10)

2 多源融合滤波算法

由于加速度计、陀螺仪和偏振光传感器具有不同的动静性能,在多传感器导航中进行误差修正和数据融合是非常必要的。本文设计了一种互补滤波与粒子滤波联级解算方法,如图2所示 。第一阶段利用偏振光传感器和加速度计的低频性能对陀螺仪的累计误差进行修正。第二阶段将获得的四元数信息作为系统观测方程的输入信息,利用粒子滤波进行传感器信息的深度融合,获得高精度的无人机姿态。

图2 姿态估计算法流程图

2.1 互补滤波算法

基于互补滤波的航姿估计利用加速度计和偏振光传感器输出建立误差,并将该值作为PI控制器的前馈输入量。利用PI控制器的输出对陀螺仪的漂移进行补偿,最后使用修正后的陀螺仪角速度更新四元数微分方程。

(11)

(12)

将b系下加速度计测得三轴加速度[axayaz]和偏振光传感器输出[pxpypz]做归一化处理:

(13)

(14)

做归一化处理后,获得模长为1的传感器输出,与参考向量叉乘建立系统误差矢量:

(15)

将误差矢量e(ω)输入PI控制器得到误差补偿,对陀螺仪的输出ωg进行修正:

(16)

式中:ω是修正后的角速度;s是拉普拉斯算子;Kp为比例控制系数;Ki为积分控制系数,Kp,Ki选值受传感器参数影响[15]。

互补滤波器设计的关键在于Kp、Ki的参数选择。只要误差矢量存在,PI控制就会一直作用,直到误差消失。因此Kp、Ki的参数选择直接影响到互补滤波器的控制效果。本文获取Kp、Ki的参数的方法为:假设Ki不变,根据陀螺仪、加速度计和偏振光传感器的输出值动态调整Kp的大小,直到输出姿态值不动荡,确定当前Kp的值;在已选定的Kp的情况下,动态调整Ki使得系统输出稳定,最终确定PI参数。

将修正后的角速度通过微分方程更新四元数:

(17)

式中:k为采样点;T为采样周期;ωx,ωy,ωz分别是陀螺仪在x,y,z方向的输出。

用于更新微分方程的一阶龙格库塔法在实际中会导致一定的误差发散,该误差会随着四元数的更新进行累计。因此更新四元数后需要对其进行归一化处理来进行修正。互补滤波航姿估计框图如图3所示。

图3 互补滤波算法流程图

2.2 粒子滤波算法

通过互补滤波将陀螺仪、加速度计和偏振光数据进行第一阶段融合,初步去除陀螺仪的累计误差以及偏振光传感器的高频噪声。输出的四元数值作为第二阶段粒子滤波的观测量,由四元数微分方程构建系统状态方程,状态量为待求姿态四元数。通过粒子滤波进行航姿解算,获得更稳定和更高精度的姿态角,系统状态方程和观测方程如下:

(18)

Z(k)=H(k|k-1)X(k)

(19)

式中:X(k)为k时刻系统状态量;X(k-1)为k-1时刻系统状态量;Z(k)为k时刻观测量;Δt为传感器采样间隔时间;H(k|k-1)为观测矩阵,是四阶单位矩阵。

本文采用的粒子滤波器为采样-重要性重采样(SIR),每一步迭代都要进行重新采样,SIR粒子滤波流程为:

(1)初始化。用随机分布生成初始粒子集合来构造初始化四元数X0,第1个采样点由参考航姿获得,这里使用高斯随机分布:

X1=X0+Q1/2×randn(4,N)

(20)

式中:X1为初始值产生的粒子集合,是4×N的矩阵,每一列为一个四元数粒子;N为粒子个数;Q为过程噪声协方差;randn是一组符合标准正态分布的随机数。

(2)重要性采样。从第2个采样点k=2开始进行重要性采样,包含粒子集合构建和粒子权值计算:

(21)

式中:F(k|k-1)为状态转移矩阵,即系统状态方程对系统状态量求导。

(22)

式中R为观测噪声协方差。

对每个粒子进行归一化处理:

(23)

(4)输出姿态四元数。对重采样后的粒子群求均值即获得四元数:

(24)

(5)循环。令k值加1,将粒子集合带入第(2)步进行下一时刻的姿态解算,不断迭代直至完成航姿求解。

3 仿真验证

为了验证本文所提出算法性能,使用惯性测量单元MPU6050和偏振光传感器搭建固定翼无人机实验平台采集传感器数据,如图4所示。运用互补滤波法,粒子滤波法以及本文提出的改进组合滤波方法对无人机姿态角进行解算,分析无人机分别处于静态和动态时,各算法的表现力和滤波效果。

图4 固定翼无人机实验平台

最大偏振矢量的求取与实验时间和实验地点的地理位置有着紧密关系。本次实验地点位于大连理工大学西部校区机械工程学院知方楼南侧露天平台(东经121.528 255°,北纬38.881 419°),实验时刻为2022年1月7日上午11点22分,实验时太阳方位角9.96°,太阳高度角28.10°。

在无人机处于静止状态下,进行传感器数据采集,此时各姿态角应都为0°,分别采用3种算法进行姿态解算,各得到横滚角、俯仰角、航向角分别如图5～图7所示。

图5 静态横滚角解算

图6 静态俯仰角解算

图7 静态航向角解算

统计静态姿态角在不同算法下的角度范围和标准差如表1所示。

表1 静态姿态角误差分析 (°)

根据表1信息,本文提出的PI+PF组合的方法,比单独采用PI、PF得到姿态角范围更小,其结果更接近于静止状态下的角度真实值;标准差也更小,证明该改进算法抗传感器噪声干扰能力更强,静态稳定性更高。故本文提出的改进算法表现出更高的静态航姿解算能力,采用PI+PF算法比单独采用PI和PF法,横滚角分别提升了37.9%、50.5%,航向角分别提升了58.8%、31.8%,俯仰角分别提升了42.0%、20.7%。

为了进一步验证算法的动态特性,让无人机分别沿着x,y,z轴做转动实验,输出姿态参考信息如图8所示。采集加速度计、陀螺仪、偏振光传感器数据,分别采用3种算法处理数据,各得到横滚角、俯仰角、航向角误差如图9～图11所示。

图8 高精度姿态参考信息

图9 动态横滚角误差

图10 动态俯仰角误差

统计动态姿态误差在不同算法下的范围和标准差如表2所示。

表2 动态姿态角误差分析 (°)

由上述结果可以看出,本文提出的改进算法在短时间、大幅度转动情况下仍能跟随姿态角的变化,响应更及时。在超高动态条件下,利用PI+PF方法姿态解算精度可以控制在±5°内,而单独采用互补滤波法、粒子滤波法虽然能够跟随姿态的快速变化,但整体误差范围维持在±8°,波动较大。

4 结束语

针对传统导航方式的多种局限性,本文使用偏振光传感器和惯性元件组成了新型组合导航系统,提高了无人机导航的自主性。同时针对现有的航姿解算算法存在的问题,本文提出了一种新型混合滤波方式,并给出了详细的算法流程和系统参数。通过静态、动态实验,本文所提出的算法能够有效过滤传感器和机体的噪声,同时具有良好的动态跟踪性能,改进滤波算法比单独使用互补滤波法、粒子滤波法的精度有所提高。该方法满足了无人机高动态、高稳定的姿态需求。