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

权利要求书

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_针为为校正后的指南针转速；为在水平面上的指南针转速。