专利名称：一种适用于任意运动微小型系统的定姿系统的制作方法
技术领域：
本发明涉及一种适用于任意运动微小型系统的定姿系统，适用于微小型 系统任意运动状态下航向的确定。
背景技术：
任何运动体都需要高精度导航系统和控制系统，高精度导航系统为运动
体提供当前的位置、速度和姿态等运动信息；高精度控制系统依据导航系统 提供的运动信息控制运动体按照要求的方式运动，其中姿态信息的精度对控 制系统性能有决定性的影响。
大型运动体利用高精度、大体积、高成本惯性导航系统或者星敏感器已 经实现了任意运动情况下的姿态确定。但大型运动体所用的定姿设备由于体 积庞大、成本极高而无法应用于微小型系统任意运动情况下的姿态确定。微 小型系统(微小飞行器、微小水下航行器、微小地面机器人)机动性大且随 机性强，姿态确定的难度很大。
加速度计和倾角传感器是目前测量微小型系统横滚角和俯仰角最常用 的方式，但由于两种传感器均无法很好的区别微小型系统运动加速度和重力 加速度，因此不能给出微小型系统任意运动情况下的横滚角和俯仰角，只适 合于静止和匀速直线运动情况。而且上述两种传感器均不能给出航向角。
惯性导航系统原理上能够测量微小型系统在任意运动状态下的横滚角、 俯仰角、航向角等三维姿态信息，但它需要初始姿态基准作为积分的初值， 而且微小型惯性导航系统的误差随时间急剧增加， 一般不能独立工作。
磁传感器能够测量三个正交方向的磁场级度.，,为了换算为三维姿态信
息，还必须首先依赖其它手段，在微小型系统任意运动状态下确定三维磁传 感器测量方向与当地水平面之间的坐标转换关系，即必须预先确定横滚角和 俯仰角。目前多数磁传感器定姿系统都依赖加速度计或倾角传感器给出静态 和匀速直线运动状态下的4t滚角和俯仰角，而在其它才几动运动状态下^f兹传感 器基本不可用。
可见，任何现有的姿态测量方式均无法准确确定微小型系统在任意运动 状态下的三维姿态信息。本发明提出一种适用于任意运动微小型系统的
MIMU ( Micro Inertial Measure - ment Unit，微惯性测量单元)和GPS辅助 MMS (Micro Magnetic Sensors,微磁传感器)定姿系统，利用GPS、 MIMU、 MMS之间迭代解算关系，不断修正MIMU误差并给出微小型系统任意运动 状态下的三维姿态信息，具有广阔的应用前景。
发明内容
本发明的技术解决问题是克服现有技术的不足，提供一种适用于任意 运动微小型系统的定姿系统，该系统利用MIMU和GPS辅助的MMS定姿， 为任意运动状态下的微小型系统提供高精度三维姿态信息。
本发明的技术解决方案是 一种适用于任意运动微小型系统的定姿系 统，其特征在于包括微磁传感器MMS模块、微惯性测量单元MIMU模块、 GPS模块和Kalman滤波模块，在微小型系统任意运动状态下，MIMU模块 根据输出数据通过捷联导航算法分解出微小型系统重力加速度；MMS模块 利用所述的重力加速度通过定姿算法实时解算微小型系统的横滚角、俯仰角 和航向角三维姿态信息；同时，利用恒虚警CFAR滤波器平滑掉GPS输出 速度和加速度信息中的噪声；三维姿悉信息一方面送至Kalman滤波模块， 与GPS给出的位置和速度信息一道通过信息融合算法间歇校正MIMU误差， 另一方面利用三维姿态信息实时更新C/坐标转换矩阵；坐标转换矩阵 将被送给MIMU模块和GPS模块，将水平坐标系的信息转换到载体坐标系
下,
本发明的原理是 一般的载体坐标系见图2所示，定义为微小型系统纵 轴前进方向为Y轴，向上为Z轴，X轴依据右手定则确定。MIMU、 GPS、 MMS各自的三维正交测量方向均与载体坐标系三轴重合，见图3所示，GPS 位于载体坐标系的原点，MMS、 MIMU、三个正交测量轴分别与载体坐标系 三轴重合。
MIMU冲莫块由三维正交安装的硅MEMS陀螺和加速度计构成，共六个 惯性器件。硅MEMS陀螺测量微小型系统角速度，硅MEMS加速度计测量 微小型系统线加速度。在静止状态下硅MEMS加速度计敏感重力加速度在 载体坐标系的投影，从而给出载体坐标系与当地水平坐标系的坐标转换矩阵 以及横滚角、俯仰角；在其它任意运动状态下，MIMU模块的测量值经过捷 联导航算法，利用微小型系统运动线速度、微小型系统运动角速度以及地球 自转角速度，实时分解加速度计测量值中微小型系统运动线加速度、哥式加 速度、向心加速度、重力加速度等不同成分。利用重力加速度在载体坐标系
<formula>formula see original document page 6</formula>以及重力加速度在当地水平坐标系投影[O 0 g]之间
的关系，计算微小型系统的横滚角y和俯仰角^，
<formula>formula see original document page 6</formula>MMS模块由安装于三个正交方向的微小型磁传感器组成，测量当前位 置三个正交方向的磁场强度，从而给出当前位置磁场矢量的方向和大小。依 据横滚角y和俯仰角P计算地磁场矢量在当地水平坐标系下的投影
<formula>formula see original document page 6</formula>
信息融合算法利用当前时刻GPS给出的三维位置及速度倌息、MMS给 出的三维姿态信息，通过Kalman滤波器校正MIMU的导航误差和惯性器件 误差，提高重力加速度在载体坐标系的计算精度。
本发明与现有技术相比的优点在于(1 ) MIMU模块结合捷联导航算 法不断实时分解出加速度计测量值中重力加速度的成分，并给出重力加速度 在载体坐标系的投影，解决了由于加速度计测量值无法分辨运动加速度和重 力加速度的问题，从而得到载体坐标系与当地水平坐标系之间的转换关系， 即横滚角和俯仰角；(2)采用CFAR滤波器抑制GPS输出数据的噪声，提 高了微小型系统位置、速度的测量精度。(3)本发明采用GPS三维位置、 速度信息以及MMS给出的三维姿态信息不断校正MIMU的误差，保证了长 时间工作情况下MIMU测量精度以及分解重力加速度在载体坐标系投影的 精度；(4)MMS模块结合定姿算法能够测量载体坐标系三维方向磁场强度， 利用已获得的横滚角和俯仰角，循环迭代计算微小型系统任意运动状态下的 三维姿态信息。
图1为本发明的适用于任意运动微小型系统的MIMU和GPS辅助MMS 定姿系统组成框图2为载体坐标系示意图3为本发明的适用于任意运动微小型系统的MIMU和GPS辅助MMS 定姿系统在载体坐标系的安装方法；
图4为本发明的适用于任意运动微小型系统的MIMU和GPS辅助MMS 定姿系统工作流程图。 '
具体实施例方式
图1为适用于任意运动微小型系统的MIMU (微惯性测量单元)和GPS
辅助MMS (微磁传感器)定姿系统组成框图，包括硬件和软件两部分，硬 件部分由MMS模块、MIMU模块、GPS模块组成；软件算法部分包括捷联 导航、信息融合以及定姿算法。在微小型系统任意运动状态下，利用MIMU 模块的数据和捷联导航算法分解出微小型系统重力加速度，MMS模块利用 重力加速度和定姿算法实时解算微小型系统的横滚角、俯仰角和航向角，采 用CFAR (恒虚警)滤波器平滑GPS输出噪声，由GPS模块、三维姿态信 息以及信息融合算法间歇性的校正MIMU误差，提高重力加速度和姿态解 算精度。图4描述了图1中捷联导航算法、定姿算法、信息融合算法基于硬 件输出的数据确定姿态的工作流程。
首先，MIMU和GPS辅助的MMS定姿系统在静止状态下开始工作，由
MIMU模块中加速度计给出重力加速度在载体坐标系的投影<formula>formula see original document page 8</formula>
已知重力加速度在当地水平坐标系的投影为<formula>formula see original document page 8</formula>
<formula>formula see original document page 8</formula>地磁场在水平坐标系下的投影<formula>formula see original document page 8</formula>
由于地磁场的磁力线方向总是指向磁北的，即/C/Zf的合向量指向磁北，
》兹;杭向为
再用f减去地磁偏角/1,就是数字磁罗盘与真北的夹角即航向角
被发起.这样的接口有利地独立于要传递的工件的位置的数量.所
述接口可以是串行打印机接口 /特别是RS-232/422接口 ，或是并行打 印机接口 ，特别是Centronics接口 .
关于通过机床机械加工的状态的信息有利地包含用于工件和/或 工具的传递指令.这样的传递指令优选地包含工件和/或工具的箱位 置号和定位位置、和/或传递装置的回转(pivoting)动作，这种信息 是对于在机床处交换部件所必需的信息.
尤其，在根据本发明的系统中，同步化装置附加地被提供用于 对机床和传递装置协调和步进计时.在所有情况下，所述同步化装 置优选地均包含机床和传递装置的数字输入/输出端。尤其，所述同 步化装置也被用于分别响应于所发出的信息和所发出的传递指令而 反馈握手(handshaking);这是必需的，因为打印机接口是单向的。 类似于打印机接口 ，数字输入/输出端通常已经存在于机床中并且允 许来自控制单元的信号被送出或者被读入到所述控制单元中.
机床的打印机接口的初始化优选地通过加栽用于发布信息到传 递装置的宏来执行； 一旦接收到信息，传递装置的控制单元就通过 激活相应的协议来起作用.正如先前所提到的，如栽宏可以简单地 由机床的操作员通过对适当的ISO代码编程来执行.
尤其，所述传递装置作为对其控制单元已经从机床的打印机接 口接收到了交换指令的响应而执行对工件或工具的交换.
此外，可以规定，所述机床通过发送相应的信号到其数字输出 端来用信号通知工件和/或工具的交换准备就绪；所述信号被馈入 传递装置的数字输入端并且一旦所述机床不再准备交换就立即被重 置。然而，也可以规定所述机床的控制单元生成请求交换工件或工 具的循环重复信号，所述信号被发送到打印机接口.通过这些特征， 可以保证， 一旦所述机床不再准备用于交换工件或工具，所述交换 指令就被清除，因此，提高了所述机床以及所述传递装置的安全性.
优选地，所述机床的控制单元与校验和一起发出交换信号到所 述打印机接口.通过所述校验和，保证所述所传输的信息的数据完 整性.
在所述机床侧，提供包舍所述机床的控制单元、打印机接口和 数字输入端/输出端的控制架，而在所述传递装置侧，提供包含所述
—w —"<formula>formula see original document page 10</formula>
MIMU模块实时连续输出陀螺、加速度计测量值，并将其输入捷联导 航解算，求取载体坐标系下的重力加速度<formula>formula see original document page 10</formula>
上式各项均为矢量，/6为加速度计直接测量值(直接测量值在载体坐标系 内)；假设￡为GPS给出的炜度，当地地理坐标系内地球自转角速度cw:可 分解为
<formula>formula see original document page 10</formula>当地地理坐标系内微小型系统运动产生的相对地球坐标系的角速度为
<formula>formula see original document page 10</formula>为利用航向角信息分解GPS速度获得的微小型系统在当地地理坐标 系内的北向速度和东向速度信息，,及力分别为地球的子午面半径和卯酉面 半径。
得到后，即可采用与静止状态相同的算法计算微小
型系统横滚角、俯仰角和航向角。
信息融合利用当前获得的三维速度、三维位置和三维姿态信息，采用标
准Kalman滤波器校正MIMU的误差。Kalman滤波器状态变量
<formula>formula see original document page 11</formula>
Kalman滤波器观测变量是上文获得的三维速度、三维位置和三维姿态信息 与MIMU输出的三维速度、三维位置和三维姿态信息的差值。<formula>formula see original document page 11</formula>根据惯性导航标准误差方程建立Kalman滤波器的状态方程与观测方程，其 余计算与标准Kalman滤波器相同。
权利要求
1、一种适用于任意运动微小型系统的定姿系统，其特征在于包括微磁传感器MMS模块、微惯性测量单元MIMU模块、GPS模块和Kalman滤波模块，在微小型系统任意运动状态下，MIMU模块根据输出数据通过捷联导航算法分解出微小型系统重力加速度；MMS模块利用所述的重力加速度通过定姿算法实时解算微小型系统的横滚角、俯仰角和航向角三维姿态信息；同时，利用恒虚警CFAR滤波器平滑掉GPS输出速度和加速度信息中的噪声；三维姿态信息一方面送至Kalman滤波模块，与GPS给出的位置和速度信息一道通过信息融合算法间歇校正MIMU误差，另一方面利用三维姿态信息实时更新Cnb坐标转换矩阵；坐标转换矩阵Cnb将被送给MIMU模块和GPS模块，将水平坐标系的信息转换到载体坐标系下。
2、 根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其 特征在于所述的MMS模块由安装于三个正交方向的微小型磁传感器组成， 测量当前位置三个正交方向的地球石兹场强度，从而给出当前位置地球石兹场矢 量的方向和大小。
3、 根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其 特征在于所述的MIMU模块中的硅MEMS陀螺测量微小型系统角速度， 硅MEMS加速度计测量微小型系统线加速度；在静止状态下，硅MEMS加 速度计敏感重力加速度在三维正交方向的投影，给出载体坐标系与当地水平 坐标系的坐标转换矩阵以及横滚角、俯仰角。
4、 根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其 特征在于所述的CFAR滤波器釆用滑动平均的方法抑制GPS输出数据的 噪声，根据机动性设置滑动窗的时间长度r-MAf ， M为滑动个数，At为时 间间隙，采用先进先出的方式确定滑动窗内参与平均的数据，利用当前时刻 之前r时段内数据经过CFAR滤波器抑制GPS数据的噪声。
5、 根据权利要求I所述的适用于任意运动微小型系统的定姿泉统，其 特征在于所述的捷联导航算法在载体坐标系下建立基本力学编排方程，从 而从加速度计测量值中分解出重力加速度在载体坐标系三轴的投影。6、 根据权利要求1所述的适用于任意运动微小型系统的定姿系统，其 特征在于所述的MMS模块利用所述的重力加速度通过定姿算法实时解算 微小型系统的横滚角、俯仰角和航向角三维姿态信息的过程如下利用重力加速度在载体坐标系的投影n尸Sjc gy Sz以及重力加速度在当地水平坐标系的投影[O 0 gf之间的关系，计算微小型系统的横滚角z和俯仰角P,<formula>formula see original document page 3</formula>依据y和P计算当前地磁场矢量在水平坐标系下的投影:《=sin 6> sin y +《cos 6 -《sin 0 cos y 考虑磁偏角丄的情况下计算航向角- :
全文摘要
一种适用于任意运动微小型系统的定姿系统，其特征在于包括微磁传感器MMS模块、微惯性测量单元MIMU模块、GPS模块和Kalman滤波模块，在微小型系统任意运动状态下，MIMU模块根据输出数据通过捷联导航算法分解出微小型系统重力加速度；MMS模块利用所述的重力加速度通过定姿算法实时解算微小型系统的横滚角、俯仰角和航向角三维姿态信息；同时，利用恒虚警CFAR滤波器平滑掉GPS输出速度和加速度信息中的噪声；三维姿态信息一方面送至Kalman滤波模块，与GPS给出的位置和速度信息一道通过信息融合算法间歇校正MIMU误差，另一方面利用三维姿态信息实时更新C<sub>n</sub><sup>b</sup>坐标转换矩阵；坐标转换矩阵C<sub>n</sub><sup>b</sup>将被送给MIMU模块和GPS模块，将水平坐标系的信息转换到载体坐标系下。本发明利用了GPS、MIMU、MMS之间的迭代解算关系，解决了任意运动状态下确定运动体航向的难题，具有广阔的应用前景。
文档编号G05D1/08GK101109959SQ200710119968
公开日2008年1月23日 申请日期2007年8月6日 优先权日2007年8月6日
发明者房建成, 曹娟娟, 蔚 盛, 杰 秦, 马珍珍, 马艳武 申请人:北京航空航天大学