[发明专利]一种基于指南车原理的全路况三维定向自校正方法

摘要

本发明公开了一种基于指南车原理的全路况三维定向自校正方法，属于定位导航与控制技术领域，本发明采用电机和单片机对指南车的工作方式进行改善，只需要红外编码器读取出两个齿轮的转速，反馈到单片机对指南车以及中心轴的转速实时控制，达到指南效果；此外，本发明还针对指南车在曲面运动时的误差，提出了一种定向校正方法，引入了修正系数，得到校正后的转速，极大地提高了指南车的精度。相比于传统的机械式指南车，本发明消除了在曲面运动时指向飘移、在坑洼地面运动误差积累对指向精度的影响，并且该方法实现容易，所需仪器设备少，算法简单，对搭载平台的要求不高，是一套完整的自主定位导航方法。

主权项

1.一种基于指南车原理的全路况三维定向自校正方法，其特征在于，步骤如下：步骤一：通过红外编码器读取出两个车轮的转速；其中，左轮角速度为ω1，转速为n1，单位：r/s，右轮角速度为ω2，转速为n2，单位：r/s；步骤二：编码器将转速值自动输入单片机；步骤三：通过单片机计算小车的瞬时的角速度ω车，其中其中，L是小车左轮与右轮的轮距，R为车轮的半径；步骤四：通过单片机计算小车上指南针的角速度ω针，其中ω针+ω车＝0步骤五：通过单片机计算指南针的转速为：步骤六：通过水平仪实时读取车轮轴的相对倾角α；若相对倾角α保持不变，则不需要校正；即K＝0若倾角α在取样间隔内发生变化，则需要修正；步骤七：计算修正系数K：K＝cosα步骤八：计算校正后的指南针转速：其中n_针为为校正后的指南针转速；为在水平面上的指南针转速；n₁为小车左轮转速、n₂为小车右轮转速。