[发明专利]一种基于磁/惯性器件的脊柱测量仪

权利要求书

1.一种基于磁/惯性器件的脊柱测量仪，其特征在于脊柱姿态解算步骤：

(1)参照医疗行业人定义体姿态坐标系，脊柱三个角度——后凸角θ、侧凸角γ、侧弯角ψ，脊柱测量仪坐标系和地理坐标系的关系，即姿态矩阵姿态矩阵也可用四元数表示：其中b为脊柱坐标系和n为地理坐标系；

(2)四元数微分方程：

可采用一阶龙格库塔法进行求解得：其中为四元数一阶微分，Q为四元数，ω为角速度，ω_x、ω_y和ω_z分别为x、y和z轴的角速度，t为采样时刻，T为采样周期；

(3)脊柱坐标系三轴上的重力加速度分量，可通过姿态矩阵乘以重力矢量获得其中g为重力加速度，g_xc,g_yc,g_zc为计算的脊柱坐标系三个轴向上的重力加速度分量；

(4)计算脊柱坐标系三个轴向上重力加速度分量直接采用了测量仪中的加速度计数据来计算后凸角θ_a和侧凸角γ_a为

(5)利用每[0 T]的采样间隔内大量采集加速度计、陀螺仪和磁强计x、y和z轴的加速度三轴角速度和三轴磁场数据根据其中d为周期内的采集数据，为采集数据均值，n为采集数据数，分别计算采集数据的方差为一个周期内由陀螺仪数据计算的姿态增量的方差没有改变，方差还是

(6)融合解算初始值确定：如果磁数据正常，则初始值为ψ_m通过磁数据计算获得；如果磁数据异常或无磁数据，侧弯初始值无法确定，可假定ψ为0，即初始值为

(7)更新由陀螺仪数据计算的角度：其中θ_g,γ_g,ψ_g为由角速度数据解算的角度，θ_gl,γ_gl,ψ_gl为上一次由角速度数据解算的角度，也可采用四元数姿态更新解算；

(8)更新融合解算的角度：其中融合系数；

(9)磁强计数据正常时更新融合系数：其中和分别为θ_a、γ_a和ψ_m的均方差；

(10)如果磁数据异常或无磁数据时，侧弯角融合解算的角度ψ采用x和y轴的加速度进行校正补偿；如果姿态矩阵准确，则计算的三轴重力加速度矢量和加速度计三个轴向上输出的加速度矢量方向完全是一致的，但姿态矩阵存在误差，设侧弯角误差为Δψ，该误差正比于矢量和矢量的叉乘中的a_x*g_yc-a_y*g_xc项，设为e_z，设比例系数为则p₁为静态误差系数，p₂为动态误差系数；

(11)值的确定：从开机工作到再放置30s，系统自动记录这段每个解算周期内的姿态变化值Δψ、误差e_z和误差积分值即采用递推最小二乘法拟合出则z轴角速度为其中n为30s内姿态的解算次数、为z轴平均角速度，dT即采样周期T；

(12)侧弯角为ψ＝ψ_gl+ω_zdT，则磁数据异常或无磁数据时

(13)如果姿态数据存在一个方向变化，说明去除零偏不完全，可进行常值系数补偿，也可进行转台标定来补偿。