专利名称：一种用于低成本小型无人机的姿态航向参考系统的制作方法
技术领域：
本发明涉及一种姿态航向參考系统，特别涉及ー种基于传感器融合技术的用于低成本小型无人机的姿态航向參考系统。
背景技术：
姿态航向參考系统(AttitudeHeading Reference System, AHRS),简称航姿系统，用于确定运动载体在空间的方位。由姿态航向參考系统提供的姿态信息可以被广泛应用于导航、制导与控制。对于自主飞行的无人机，航姿系统尤为重要，它为控制系统的内回路提供必要的姿态反馈。通常，姿态信息的确定依赖于高精度的角速率陀螺，然而高精度陀螺存在着成本高、重量大、结构复杂等缺陷，显然不能适用于低成本小型无人机系统。因此， 在低成本传感器的制約条件下，为小型无人机系统提供一套计算量小、可靠性高的姿态航向參考系统一直都是国内外相关领域广泛研究的问题。通常情况下，运动载体的姿态是用欧拉角(f，q，y)描述的。根据载体运动的角速率同欧拉角变化率之间的关系，可以将角速率陀螺的测量值进行积分，从而得到载体的姿态信息，但这种方法对于陀螺精度的要求较高(输出误差小于O. 1° /h)。低成本小型无人机通常采用MEMS (Micro Flectrical Mechanical System)传感器,虽然具备成本低、体积小等特点，此类传感器的精度较差，输出误差在10° 100° /h之间，积分陀螺输出值时会产生较大漂移，因而不能直接用于姿态估计。姿态信息也可以不依赖于角速率陀螺，而采用观测矢量确定。采用观测矢量确定姿态的基本原理是用分别在两个坐标系下两个不共线的观测矢量，通过三次旋转使两个坐标系重合，再计算得到欧拉角。ー些文献针对这种无陀螺系统(Gyro-Free Systems)进行了研究。例如，使用加速度计、磁强计和GPS (Global Positioning System)作为传感器，通过求解Wahba问题获得姿态信息；也可以通过多天线GPS，采用GPS载波相位观测量进行姿态估计。然而，在很多实际情况下，这种无陀螺航姿系统不能単独使用。首先，无陀螺航姿系统不能提供高带宽的的姿态信息；其次，当载体做特定机动时，使用该方法估计的姿态信息会存在较大误差。例如当无人机倾斜转弯时，加速度计会敏感到除重力加速度以外的的向心加速度，导致计算姿态不准确；最后，无陀螺航姿系统给出的姿态信息包含较大的噪声。基于上述原因，可以使用传感器融合(Sensor Fusion)技术将角速率陀螺同加速度计、磁强计及GPS等组成的无陀螺系统进行融合，使它们互为补充，取长补短。由于角速率陀螺能够提供高带宽有偏的姿态信息，可用来“平滑”无陀螺系统；而无陀螺系统则用于校正角速率陀螺产生的漂移，因此也成为校正系统(Aiding System)。这样，利用适当的滤波器将二者结合后，就可获得高带宽且无偏的姿态信息解决方案。传感器融合技术的核心是用于融合不同传感器的滤波器，使用滤波器融合角速率陀螺和无陀螺系统进行测姿的方法数十年来被广泛应用于卫星的姿态确定问题中。对于低成本小型无人机，虽然传感器融合技术已经被广泛应用于姿态确定，但是仍存在一些问题首先，小型无人机系统中用于姿态估计的传感器一般为角速率陀螺、加速度计、磁航向计(用于测量航向角)，而磁航向计的造价较高，不适合于低成本无人机系统；其次，虽然ー些小型无人机姿态估计系统采用GPS代替磁航向计，降低了成本，但是由于GPS输出的是角度，此时的姿态估计算法是建立在欧拉角法的基础之上，这种方法需要在计算中实时求解三角函数，某些时候还会出现奇点，使得问题无解，因而不适用于计算能力较差的低成本飞控计算机。目前尚未见到能够较好地解决当无人机出现机动过载时姿态估计的解决方案。
发明内容
本发明的目的是针对现有技术的缺点，提供一套姿态航向參考系统方案，仅依靠包括角速率陀螺、GPS、加速度计在内的低成本传感器，实现小型无人机姿态信息估计。本发明提供了一种用于低成本小型无人机的姿态航向參考系统，包括角速率陀螺、加速度计、GPS、角速率陀螺运算�？椤⑿Ｕ？楹涂ǘ瞬ㄆ鳎渲�
角速率陀螺测量飞机的滚转角速率、俯仰角速率和偏航角速率；加速度计测量重力在机体坐标轴系下的分量；GPS测量飞机的航迹方位角；角速率陀螺运算�？橥ü撬俾释勇莶饬康墓鲎撬俾�、俯仰角速率和偏航角速率，得到带偏移的滚转角も、俯仰角Q1和偏航角Y1 ;校正模块通过加速度计测量的重力加速度的体轴分量得到校正�？楣兰频墓鲎莊2、俯仰角q2，并将GPS測量得到的航迹方位角作为估计的偏航角y2 ；卡尔曼滤波器将角速率陀螺运算�？楹托Ｕ？椴氖萁腥诤希玫阶罱K的姿态角[f q y]T。有益效果本发明的优点在于通过使用低成本传感器，降低了小型无人机系统的成本；减小了传统姿态航向參考系统的复杂程度；依靠传感器融合技术，提高了系统航姿估计的精度和可靠性；解决了无人机出现过载时姿态估计不准确的问题。
图I为姿态航向參考系统的结构图；图2为静态条件下滚转角解算；图3为静态条件下俯仰角解算；图4为角速率陀螺漂移估计；图5为滚转角误差及其标准差；图6为滚转角速率陀螺漂移误差及其标准差；图7为估计滚转角/同真实滚转角f之间的误差对比；图8为基于飞行试验的滚转角估计，其中(a)为时间窗ロ法估计滚转角，(b)为常规滤波及陀螺积分估计滚转角；图9为基于飞行试验的俯仰角估计，其中(a)为时间窗ロ法估计俯仰角，(b)为常规滤波及陀螺积分估计俯仰角；
图10为滤波器开关标志量；图11为滤波器开关触发量及门限值。
具体实施例方式下面结合附图，具体说明本发明的优选实施方式。本实施方式实现了一种用于低成本小型无人机的姿态航向參考系统，该系统由角速率陀螺子系统、校正子系统和卡尔曼滤波器三个部分組成。图I显示了姿态航向 參考系统的结构图。本实施方式采用误差四元数法建立模型，缓解了机载处理器的运算负荷。I)角速率陀螺运算模块三维空间内刚体的姿态通常是用三个欧拉角描述的，它们分别为滚转角f、俯仰角q和偏航角y。姿态角描述了两个不同坐标系之间的相对关系。在导航、制导与控制中，对于近地运动的飞行器，通常将机体坐标轴Sb-Oxyz系选为动坐标系，导航坐标轴系sn-0xnynzn选为參考坐标系，定义为北东地(North-East-Down, NED)参考系统，Xn为北向，yn为东向，Zn指向地心。定义任意矢量U，其在导航坐标轴系和机体坐标轴系分别表示为Un和ub。矢量u在上述两个不同坐标轴系下的投影转换是通过方向余弦矩阵实现的uh = ChnUn(I)式中为方向余弦矩阵(Direction Cosine Matrix,DCM),也称为姿态矩阵,通常以欧拉角的形式定义。在实际应用中，使用欧拉角表示姿态存在着若干问题。首先，在某些特殊情况下，个别姿态角成为不确定的，运动学方程出现奇异性，例如当俯仰角q= ±90°时，偏航角y为不确定，且方程dy/dt有奇异性。其次，在实际解算中，欧拉角法需要求解大量三角函数，必然给处理器带来计算负担。如果使用姿态四元数法，一方面能避免运动方程的奇异性，另一方面无需计算三角函数，节省了处理器资源，提高了计算效率。为了克服上述缺点，本实施方式使用四元数法建摸。定义姿态四元数q:q= qr⑵
Vq _其中％称为四元数q的标量部分， = & q2 g3f称为四元数q的矢量部分。用
四元数表示的方向余弦阵定义如下
1-2( 2+ 2) 2(q'q2+q3q0) 2(q'q3-q2q0)Cbn(q)= Iiq^2-q^) 1-2(%2+ 2) 2(q2q3+q0q')(3)
2(^1 + ) 2( -q^) I-2(qf +q22)_四元数微分方程由下式确定O -P -q ~rTq0 ^■q = \Pq \ [ ~qp qq (4)
r q -p O」L%_其中，p为滚转角速率，q俯仰角速率，r为偏航角速率。由四元数微分方程式(4)可知，通过角速率陀螺的测量值可实时更新四元数，然后根据姿态四元数表示的方向余弦矩阵(3)得到载体姿态角一 q = arcsin 2 (_qiq3+q2Qo)(5) [隱]一!^2)校正�？榫」芡ü撬俾释勇莶饬恐祷值玫降淖颂谴嬖谧盼藿缥蟛睿捎诮撬俾释勇菘梢蕴峁└叽淼氖涑觯蚨谴蠖嗍鼳HRS系统不可或缺的。由陀螺积分产生的漂移可以通过校正系统进行抑制，因为组成校正系统的传感器具备输出误差有界的特性。校正系统可以周期性地“重置”经角速率陀螺提供的姿态信息以达到校正的目的。本实施方式选取加速度计和GPS共同组成校正系统。通过加速度计对重力矢量在机体坐标轴系下的投影进行观测可得到姿态角，根据加速度计的原理，假设其三轴的输出矢量为fb，可以表示fb=4-gb(6)其中，<为载体相对于惯性系的运动加速度在机体坐标轴系下的分量，gb为重力加速度矢量在机体坐标轴系下的分量
-gsmqgb = Cbn gn = g sin/ cos^W)
gcosj cosq其中，为重力加速度矢量在导航坐标轴系下的投影，可以表示*gn= [O O g]T，g为重力加速度常数。在< =O吋，即载体静止或做匀速直线运动吋，fb变为
Λ 「fb = fy =~gb = -g-sin/cos^(8)
_/」 L_gcos, cos《_此时，俯仰角q和滚转角f可以通过上式确定
Lq = arcsin— (9)
S'
-f/ = arcsin-—
gxosg式(8)不包含偏航角I,因而仅通过加速度计只能确定俯仰角q和滚转角f。为了确定偏航角y，还需引入GPS提供的航向信号。需要指出的是，GPS只能提供航迹方位角·^，但是在侧风较小的情况下可以用来替代偏航角I。3)卡尔曼滤波器本实施方式选择卡尔曼滤波器作为将角速率陀螺和校正系统结合在一起的传感器融合算法。滤波器工作步骤为在确定初始条件后，就可以利用角速率陀螺的測量数据积分，进行姿态估计，这一歩骤称为“时间更新”，提供了高带宽的姿态信息。在时间更新阶段，由于陀螺数据存在噪声，使得估计姿态的误差随时间累积，因而不能靠单纯的陀螺数据积分确定姿态。为了抑制陀螺误差，需要引入校正系统对陀螺数据进行周期性的修正，这ー步骤称为“量测更新”。状态误差的协方差矩阵以及陀螺漂移估计也在该步骤校正。接下来，进行新ー轮的时间更新，如此周期性重复。真实姿态四元数q可以表示为估计四元数#和误差四元数qe相乘的形式，即认为 误差四元数qe是估计四元数#到真实四元数q的旋转。由于角速率陀螺的误差，误差四元数是非零的小量。j和q的关系由下式确定q =な qe(10)误差四元数可近似表示为Qe= r(11)
VcIe _对式(11)求导，并利用四元数微分方程式(4)，经过线性化可得四元数误差微分
方程
^ = -[ωχ]^-^( -ω)(12)其中，表示角速率陀螺测量值与真实值之差，[ふX]由下式确定
O r -q\ω χ] = -r O p(13)
_ q -p ο校正子系统的观測量取为误差角，可由如下方法获得首先利用加速度计和GPS的測量信息根据式(9)计算出载体姿态角，然后将此姿态角同式(5)计算得到的姿态角估计值相減，即可得到角度误差
Sf fAS [,:]Sq = qAS - q(14)
Sy yAS ノ式中，[5f 5q Sy]T为误差角，[fAS qAS yAS]T为通过校正系统得到的姿态角，/ q ノT为利用四元数法计算出的姿态角。角误差同姿态四元数之间的关系由下式确定
权利要求
1.一种用于低成本小型无人机的姿态航向參考系统，其特征在于，包括角速率陀螺、カロ速度计、GPS、角速率陀螺运算�？�、校正�？楹涂ǘ瞬ㄆ鳎渲� 角速率陀螺测量飞机的滚转角速率、俯仰角速率和偏航角速率； 加速度计测量重力在机体坐标轴系下的分量； GPS测量飞机的航迹方位角； 角速率陀螺运算�？橥ü撬俾释勇莶饬康墓鲎撬俾省⒏┭鼋撬俾屎推浇撬俾剩玫酱频墓鲎恰⒏┭鼋荙1和偏航角Y1 ; 校正�？橥ü铀俣燃撇饬康闹亓υ诨遄曛嵯迪碌姆至康玫叫Ｕ？楣兰频墓鲎莊2、俯仰角q2，并将GPS測量得到的航迹方位角作为估计的偏航角y2 ； 卡尔曼滤波器将角速率陀螺运算�？楹托Ｕ？椴氖萁腥诤希玫阶罱K的姿态角[f q y]T。
2.根据权利要求I所述的姿态航向參考系统，其特征在于，所述角速率陀螺运算�？槭褂盟脑ń紫榷ㄒ遄颂脑齫
3.根据权利要求I或2所述的姿态航向參考系统，其特征在于，所述校正�？橥ü韵鹿饺范ü兰频墓鲎扦梁透┭鼋莙2
4.根据权利要求I或2所述的姿态航向參考系统，其特征在于，所述卡尔曼滤波器进行数据融合的具体方法为 第一歩利用角速率陀螺测量数据计算姿态四元数
5.根据权利要求I或2所述的姿态航向參考系统，其特征在干，当飞机进行倾斜转弯时，所述校正�？橥ü韵鹿饺范ü兰频墓鲎莊2和俯仰角q2
6.根据权利要求5所述的姿态航向参考系统，其特征在干，当飞机进行倾斜转弯时，选择角速率的范数〃⑴=| 41作为触发卡尔曼滤波器的标志，时间窗ロ的开闭由下式确定
全文摘要
本发明涉及一种用于低成本小型无人机的姿态航向参考系统，包括角速率陀螺、加速度计、GPS、角速率陀螺运算�？�、校正�？楹涂ǘ瞬ㄆ鳎渲薪撬俾释勇莶饬糠苫墓鲎撬俾省⒏┭鼋撬俾屎推浇撬俾剩患铀俣燃撇饬恐亓υ诨遄曛嵯迪碌姆至浚籊PS测量飞机的航迹方位角；角速率陀螺运算模块得到带偏移的滚转角f1、俯仰角q1和偏航角y1；校正�？榈玫焦兰频墓鲎莊2、俯仰角q2，并将GPS测量得到的航迹方位角作为估计的偏航角y2；卡尔曼滤波器将角速率陀螺运算�？楹托Ｕ？椴氖萁腥诤希玫阶钪盏淖颂荹f q y]T。本发明降低了小型无人机系统的成本；减小了航向参考系统的复杂程度；提高了航姿估计的精度。
文档编号G01C21/18GK102692225SQ20111007260
公开日2012年9月26日 申请日期2011年3月24日 优先权日2011年3月24日
发明者李怀建, 李蒙, 杜小菁, 涂海峰, 郭康 申请人:北京理工大学
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
