[发明专利]一种三分量感应线圈姿态测量方法及装置

权利要求书

1.一种三分量感应线圈姿态测量装置，其特征在于，三分量感应线圈姿态测量方法，包括以下步骤：

(1)读取每个姿态传感器的数据：以三分量感应线圈为中心，分别在前后位置放置k个姿态传感器、左右位置放置k个姿态传感器，k为大于等于1的自然数，令前后位置的姿态传感器所在的直线为X轴，测量三分量感应线圈的横滚角，令左右位置的姿态传感器所在的直线为Y轴，测量三分量感应线圈的俯仰角；同时在三分量感应线圈中心放置一个姿态传感器用于测量三分量感应线圈的横滚角和俯仰角；

(2)通过低通滤波器和高通滤波器对单个姿态传感器获得的数据进行滤波；

(3)将滤波后的位于三分量感应线圈前后左右位置的姿态传感器获得的数据与滤波后的三分量感应线圈中心的姿态传感器获得的数据做数据融合；

所述对单个姿态传感器获得的数据进行滤波具体包括：

根据加速度计计算姿态传感器的姿态：其中θ_a为加速度计得到的俯仰角，φ_a为加速度计得到的横滚角，a_x为x轴方向的加速度，a_y为y轴方向的加速度，a_z为z轴方向的加速度；g为重力加速度；

根据陀螺仪计算姿态传感器的姿态：其中θ_g为陀螺仪得到的俯仰角，φ_g为陀螺仪得到的横滚角，ω_y为y轴方向的角速度，ω_x为x轴方向的角速度；

将加速度计和陀螺仪所得的姿态数据做互补滤波，滤波后的俯仰角为：

θ(n)＝α[θ(n-1)+Tω_y(n)]+(1-α)θ_a(n)，其中l为常数；T为采样时间；n为时间序列，为大于等于1的自然数；ω_y为y轴方向的角速度；θ_a为加速度计得到的俯仰角；

滤波后的横滚角为：

φ(n)＝γ[φ(n-1)+Tω_x(n)]+(1-γ)φ_a(n)，其中f为常数；T为采样时间；n为时间序列，为大于等于1的自然数，ω_x为x轴方向的角速度，φ_a为加速度计得到的横滚角；

所述将位于三分量感应线圈前后左右位置的姿态传感器获得的数据与三分量感应线圈中心的姿态传感器获得的数据做数据融合，数据融合包括各个传感器的姿态值与其权值相乘，最后将各个传感器求得的姿态值及其权值的乘积求和，具体为：

三分量感应线圈的俯仰角：其中α₀为位于所述三分量感应线圈中心的姿态传感器的权值，θ_m为位于三分量感应线圈中心的姿态传感器所得的俯仰角，α_i为位于所述三分量感应线圈左右的各姿态传感器所得的俯仰角的权值，θ_i为位于所述三分量感应线圈左右的各姿态传感器所得的俯仰角，k为传感器的个数；

三分量感应线圈的横滚角：其中β₀为位于所述三分量感应线圈中心的姿态传感器的权值，φ_m为位于三分量感应线圈中心的姿态传感器所得的横滚角，β_i为位于所述三分量感应线圈前后的各姿态传感器所得的横滚角的权值，φ_i为位于所述三分量感应线圈前后的各姿态传感器所得的横滚角，k为传感器的个数；

三分量感应线圈姿态测量装置包括，传感器组、吊舱、中央处理器、低通滤波器以及高通滤波器，三分量感应线圈通过线绳固定在所述吊舱上，所述传感器组包括2k+1个姿态传感器，k为大于等于1的自然数，其中k个姿态传感器分别位于所述三分量感应线圈的前后位置，用于测量所述三分量感应线圈的横滚角、另外k个姿态传感器位于所述三分量感应线圈的左右位置，用于测量所述三分量感应线圈的俯仰角，另外一个所述姿态传感器位于所述三分量感应线圈中心，用于同时测量所述三分量感应线圈的横滚角和俯仰角；所述中央处理器与所述传感器组连接，用于读取所述传感器组的数据，并将读取的所述传感器组的数据进行计算；所述低通滤波器和所述高通滤波器分别与所述中央处理器连接，用于将所述传感器组所得的数据进行滤波；

所述中央处理器具体包括，数据融合单元，所述数据融合单元用于将所述位于三分量感应线圈前后的姿态传感器所得的横滚角值与其对应权值相乘、所述位于三分量感应线圈左右的姿态传感器所得的俯仰角值与其对应权值相乘以及三分量感应线圈中心姿态传感器测得的横滚角值与其对应权值相乘和俯仰角值与其对应权值相乘，最后将各个传感器求得的姿态值及其权值的乘积求和。