[发明专利]一种基于AHRS的单轴旋转角测量方法

摘要

一种基于AHRS的单轴旋转角测量方法，它有五大步骤一、将传感器固连在被测物体上，使转轴尽量在传感器的俯仰和倾斜轴组成的平面内；AHRS系统上电后，在静止状态测量陀螺输出的上电误差；二、被测物体沿转轴快速往复转动，以测量转轴在AHRS系统坐标系内的方位；三、当被测物体处于零位时，记录当前的俯仰角θ0和滚转角φ0；作为初始的状态，之后的角度变化以此状态为基准进行转换；四、根据零位及轴转向，获取旋转角度α俯仰角θα和滚转角φα的关系；五、采用插值法求取α。本发明解决了现有测角方案中，测角元件安装复杂的问题，且具有较高的采样率。

主权项

一种基于AHRS的单轴旋转角测量方法，其特征在于：该方法具体步骤如下：步骤一：将传感器固连在被测物体上，使转轴尽量在传感器的俯仰和倾斜轴组成的平面内；AHRS系统上电后，在静止状态测量陀螺输出的上电误差，后续测量需减掉此误差以缩小测量的角速度的误差；步骤二：被测物体沿转轴快速往复转动，以测量转轴在AHRS系统坐标系内的方位；实际上，AHRS系统测得的角速度能直接读出，假定为(wx wy wz)T，则旋转轴方向为rT=(a,b,c)T=wxwx2+wy2+wz2wywx2+wy2+wz2wzwx2+wy2+wz2T---(0.1)]]>上式中，a、b、c表示旋转轴的方向向量，且a2+b2+c2＝1快速转动是为了减小AHRS的系统误差在角速度中所占的比例，从而得到比较精确的转轴信息；由于角速度测量的误差很小，因此得到的转轴信息是十分精确的；步骤三：当被测物体处于零位时，记录当前的俯仰角θ0和滚转角φ0；作为初始的状态，之后的角度变化以此状态为基准进行转换；假设，在该状态下AHRS坐标系与世界坐标系间的旋转矩阵为TRW，俯仰角θ0和滚转角φ0与TRW满足：RTW001=sinθ0cosθ0sinφ0cosθ0cosφ0---(0.2)]]>步骤四：根据零位及轴转向，获取旋转角度α、俯仰角θα和滚转角φα的关系，具体获取方式如下：1)根据轴向Tr＝(a,b,c)T求取旋转角度α的AHRS坐标系{T(α)}与初始位置的AHRS坐标系{T}之间的旋转矩阵T(α)RT所对应的四元数，如下：qT(α)T=cosα2+asinα2i+bsinα2j+csinα2k---(0.3)]]>2)根据四元数T(α)qT求取T(α)RT，如下式：RT(α)T=q02+q12-q22-q322(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)q02+q22-q12-q322(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)q02+q32-q12-q22,---(0.4)]]>其中，3)由于俯仰角θα和滚转角φα满足如下关系式：RT(α)W001=sinθαcosθαsinφαcosθαcosφα---(0.5)]]>且T(α)RW＝T(α)RTTRW，将(0.2)，(0.5)代入，可得sinθαcosθαsinφαcosθαcosφα=RT(α)Tsinθ0cosθ0sinφ0cosθ0cosφ0---(0.6)]]>在式(0.6)中，仅旋转角度α是未知的，通过该关系式获取α的估计值；步骤五：采用插值法求取α，具体求解步骤如下：1)取αi＝‑180+0.1(i‑1),i＝1,2,...,3601     (0.7)对于每一个αi，求取xi,yi,zi,i＝1,2,...,3601,如下式xiyizi=RT(αi)Tsinθ0cosθ0sinφ0cosθ0cosφ0---(0.8)]]>2)根据当前读取的θα，φα计算xc＝sinθα,yc＝cosθαsinφα,zc＝cosθαcosφα，然后分别计算xc,yc,zc在xi,yi,zi,i＝1,2,...,3601中的插值角度和插值处的斜率；对于xc来说，插值点是如下三种情况的点：(i)当(xi‑xc)(xi+1‑xc)＜0时，(ii)当对于所有的i均有xi＞xc,则kx＝0.0001,αx＝αi,对于满足xi‑xi‑1≤0且xi+1‑xi≥0的i；(iii)当对于所有的i均有xi＜xc,则kx＝0.0001,αx＝αi,对于满足xi‑xi‑1≥0且xi+1‑xi≤0的i；记xc所对应的差值点的集合为与其对应的斜率集合为Kx；同理求得yc所对应的差值点的集合为与其对应的斜率集合为Ky，zc所对应的差值点的集合为与其对应的斜率集合为Kz；3)从插值角度集合X,Y,Z中选取合适的角度估计值，通过以下优化问题求得的[αx*,αy*,αz*]=argminX×Y×Z(d(αx-αy)+d(αx-αz)+d(αz-αy))---(0.9)]]>其中，相应的斜率记为4)根据所求角度，加权获取估计值，方式如下α^=kxαx*+kyαy*+kyαy*,---(0.10)]]>其中kx=|kx*||kx*|+|ky*|+|kz*|,ky=|ky*||kx*|+|ky*|+|kz*|,kz=|kz*||kx*|+|ky*|+|kz*|---(0.11).]]>