CN108955724A - 一种基于指南车原理的全路况三维定向自校正方法 - Google Patents

Abstract

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

Description

一种基于指南车原理的全路况三维定向自校正方法

技术领域

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

背景技术

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

发明内容

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

本发明是这样实现的：

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

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

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

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

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

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

ω_针+ω_车＝0

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

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

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

K＝0

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

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

K＝cosα

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

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

进一步，所述的步骤二中小车左右轮角速度与转速的关系为：

ω₁＝2πn₁

ω₂＝2πn₂

可以推得：

ω₁R＝2πn₁·R

ω₂R＝2πn₂·R

其中，R为车轮的半径。

进一步，所述的小车的瞬时的角速度具体计算方法为：

设定在Δt时间内小车转过Δθ的角度，则有：

(L+P)·Δθ-ρ·Δθ＝(v₂-v₁)·Δt

L·Δθ＝(v₂-v₁)·Δt

其中，L是小车左轮与右轮的轮距、ρ为小车转弯时左轮轨迹的曲率半径；

所以小车的瞬时的转速为：

其中，Δt为瞬时时间；Δθ为Δt时间内小车转过的角度。

进一步，所述的步骤七中修正系数计算过程为：设定斜面上小角度为Δθ₂，底面上对应小角度为Δθ₁，两平面之间夹角为α，也即车轮轴的相对倾角α，具体如下：

首先，Δθ₂绕一周进行积分的值为∫Δθ₂，通过几何关系可轻易得到∫Δθ₂的值，即：

同时，Δθ₁积分一周为2π，算得误差，即修正系数为：

进一步，所述的步骤八的中在具有坡度的路面小车运动校正过程为：

按照平面上的公式算得为：

现引进修正系数K：

是按照在水平面上的公式计算得到的转速值，但小车此时是在斜曲面上运动的，此时有误差，因此，校正后的指南针转速为：

式中，n_针为为校正后的指南针转速；为在水平面上的指南针转速。

本发明与现有技术相比的有益效果在于：

1)利用本发明的方法，本发明通过红外编码器读取出车轮的转速，通过单片机对指南针的转速实时控制，达到转向时指南针方向不变的效果；同时解决了传统技术中单纯依靠齿轮传动仍旧存在的系统误差，特殊尺寸的齿轮加工存在一定难度、齿轮易磨损，日常维修保养较为复杂的缺陷；

2)本发明还针对指南车在曲面运动时的误差，提出了一种定向校正方法，引入修正系数，得到校正后的转速，极大地提高了指南车的精度，同时电机及单片机控制提高定向精度；

3)该方法还解决了现有的指南车在曲面运动时指向飘移、在坑洼地面运动误差积累过大、制造加工装配引起误差的问题，并且该方法适用于月球车及火星车在极限工作环境下的自主定位导航、在室内外实现与北斗卫星导航系统相结合的组合导航；

4)本发明的应用范围广泛，可以应用于大型公共场所及旅游场所的自动指向装置、月球车及火星车的天线定向、坦克行进间辅助定向瞄准、挖掘机转动时机械臂自动定向及战场转移紧急时雷达、火炮的自动定向。

附图说明

图1是本发明实施例中锥面示意图；

图2是本发明实施例中锥面截取部分示意图；

图3是本发明实施例中小车运动轨迹；

其中，1-坡度；2-斜面小角度，3-地面小角度，4-小车左轮与右轮的轮距，5-小车转弯时左轮轨迹的曲率半径，6-小车瞬时转过的角度，7-右轮轨迹，8-左轮轨迹。

具体实施方式

为使本发明的目的、技术方案及效果更加清楚，明确，以下参照附图并举实例对本发明进一步详细说明。应当指出此处所描述的具体实施仅用以解释本发明，并不用于限定本发明。

本发明的方法具体如下：

通过红外编码器读取出左右两个车轮的转速，就可通过单片机对指南针的转速实时控制，达到转向时指南针方向不变的效果。

设左轮角速度为ω₁，转速为n₁(单位：r/s)，右轮角速度为ω₂，转速为n₂(单位：r/s)。则有：

ω₁＝2πn₁

ω₂＝2πn₂

由此可以推得，两轮的轮速和转速之间的关系为：

v₁＝ω₁R＝2πn₁·R

v₂＝ω₂R＝2πn₂·R

如图3所示，小车在Δt(即瞬时时间)时间内小车转过Δθ的角度，即小车瞬时转过的角度6，右轮轨迹7与左轮轨迹8之间的小车左轮与右轮的轮距4，设定L为小车左轮与右轮的轮距，设定小车转弯时左轮轨迹的曲率半径5为ρ，则有：

(L+ρ)·Δθ-ρ·Δθ＝(v₂-v₁)·Δt

L·Δθ＝(v₂-v₁)·Δt

所以小车在此瞬时的转速为：

为保证指南针始终指向一个方向，应有两个转速合成为零，即：

ω_针+ω_车＝0

由此可得，指南针的转速为：

分析可得，我们只需要获得两轮的转速，在指南针下加装电机或者舵机，就可通过单片机对指南针的转速进行调节，达到指向不变的目的。

此外，本发明还可以针对双轮在曲面时实现定向的自校正方法

用电机控制指南车，解决了误差过大、不便加工及维修保养的弊端。经过对指南车实际使用场景的分析后，发现仍有提升与改进空间。

小车在行驶中必定会遇到坑洼的路况，有可能其中一个轮子压过地面的小突起。更为常见的是，小车行驶的路面不都是水平的。当小车在具有一定坡度的路面转弯时，对于指向造成的误差也会非常之大。以上情况下，指南针的指向会出现严重偏差，急需改进。

古代指南车的设计者是基于路面水平无杂物的情况设计了指南车，没有考虑路面起伏、存在坡度时造成的车身倾斜。本实施例中假设一种情况，如图1所示，小车在坡度1的曲面上转弯，设定坡度1为α。如图2所示。现截取一段作为研究对象，斜面小角度2为Δθ₂，地面小角度3为Δθ₁，两平面之间夹角为α(坡度1)。

首先，Δθ₂绕一周进行积分的值为∫Δθ₂，通过几何关系可轻易得到∫Δθ₂的值：

同时，Δθ₁积分一周为2π，所以可算得误差为：

此时小车在斜面上运动，按照平面上的公式算得为：

现引进一个修正系数K：

即可得到校正后的指南针转速：

应用此方法，可以使小车顺利适应各种复杂路况，即使是路面不平、碾压石块等情况发生时，也可使指南针方向保持不变。实现方法是，在小车车轮轴上加装水平仪，实时读取轮轴的倾斜角度值，通过单片机控制，即可将指南车的精度和适用性大大提升。

Claims (5)

1.一种基于指南车原理的全路况三维定向自校正方法，其特征在于，步骤如下：

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

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

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

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

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

ω_针+ω_车＝0

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

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

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

K＝0

若倾角α在取样间隔内发生变化，则需要修正；

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

K＝cosα

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

其中n_针为为校正后的指南针转速；为在水平面上的指南针转速；n₁为小车左轮转速、n₂为小车右轮转速。

2.根据权利要求1所述的一种基于指南车原理的全路况三维定向自校正方法，其特征在于，所述的步骤二中小车左右轮角速度与转速的关系为：

ω₁＝2πn₁

ω₂＝2πn₂

可以推得：

ω₁R＝2πn₁·R

ω₂R＝2πn₂·R

其中，R为车轮的半径。

3.根据权利要求1所述的一种基于指南车原理的全路况三维定向自校正方法，其特征在于，所述的小车的瞬时的角速度具体计算方法为：

设定在Δt时间内小车转过Δθ的角度，则有：

(L+ρ)·Δθ-ρ·Δθ＝(v₂-v₁)·Δt

L·Δθ＝(v₂-v₁)·Δt

其中，L是小车左轮与右轮的轮距、ρ为小车转弯时左轮轨迹的曲率半径；

小车的瞬时的转速为：

其中，Δt为瞬时时间；Δθ为Δt时间内小车转过的角度。

4.根据权利要求1所述的一种基于指南车原理的全路况三维定向自校正方法，其特征在于，所述的步骤七中修正系数计算过程为：设定斜面上小角度为Δθ₂，底面上对应小角度为Δθ₁，两平面之间夹角为α，也即车轮轴的相对倾角α，具体如下：

首先，Δθ₂绕一周进行积分的值为∫Δθ₂，通过几何关系得到∫Δθ₂的值，即：

同时，Δθ₁积分一周为2π，算得误差，即修正系数为：

5.根据权利要求4所述的一种基于指南车原理的全路况三维定向自校正方法，其特征在于，所述的步骤八的中在具有坡度的路面小车运动校正过程为：

按照平面上的公式算得为：

现引进修正系数K：

校正后的指南针转速为：

式中，n_针为为校正后的指南针转速；为在水平面上的指南针转速。