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

说明书

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

技术领域

本发明涉及定位导航与控制技术领域，具体是一种基于指南车原理的全路况三维定向自校正方法，该方法是在古代指南车定向原理的基础上，对指南车进行自动控制，从而提高指南车的工作精度和复杂路况下的自校正方法。

背景技术

指南车作为中国古代机械设计领域的杰出成果，时至今日，人们对他的复原和改进仍然没有停歇。通过与现代加工工艺和相关技术的结合，指南车的外观和工作精度都有了很大的提高。一方面，人们提出差动轮系的设计，采用行星轮作为指南车内部结构的关键技术，对指南车两个轮子的转向通过齿轮传动转换到中心轴上，并控制中心轴同速反向转动，从而达到指南效果。另一方面，人们将万向轮加到指南车上，同时采用电力驱动，使指南车的运动更加灵活和自主化。此外，人们还对指南车的外形进行了多种多样的设计。但由于机械装置原本的系统误差，如构件的加工精度，各部件的装配误差等等，极大地影响了指南车的工作精度。

发明内容

本发明针对现有技术中存在的问题，公开了一种基于指南车原理的全路况三维定向自校正方法，通过电机和单片机等对指南车的工作方式进行改善，替换掉原本的齿轮传动装置，只需要红外编码器读取出两个齿轮的转速，反馈到单片机对指南车以及中心轴的转速实时控制，达到指南效果，解决了现有技术中存在的误差缺陷。

本发明是这样实现的：

本发明公开了一种基于指南车原理的全路况三维定向自校正方法，方法如下：

步骤一：通过红外编码器读取出两个车轮的转速；其中，左轮角速度为ω₁，转速为n₁，单位：r/s，右轮角速度为ω₂，转速为n₂，单位：r/s；

步骤二：编码器将转速值自动输入单片机；

步骤三：通过单片机计算小车的瞬时的角速度ω_车，其中

其中，L是小车左轮与右轮的轮距，R为车轮的半径；

步骤四：通过单片机计算小车上指南针的角速度ω_针，其中ω_针+ω_车＝0

步骤五：通过单片机计算指南针的转速为：

步骤六：通过水平仪实时读取车轮轴的相对倾角α；

若相对倾角α保持不变，则不需要校正；即

K＝0

若倾角α在取样间隔内发生变化，则需要修正，进行下一步计算；

步骤七：计算修正系数K：

K＝cosα

步骤八：计算校正后的指南针转速：

其中n₁为小车左轮转速、n₂为小车右轮转速。