math - 在差速驱动里程计中考虑车轮未对准

Question

我有一个差速驱动机器人，使用里程表来推断它的位置。

我正在使用标准方程:

WheelBase = 35.5cm;
WheelRadius = 5cm;
WheelCircumference = (WheelRadius * 2 * Math.PI);
WheelCircumferencePerEncoderClick = WheelCircumference / 360;

DistanceLeft = WheelCircumferencePerEncoderClick * EncoderCountLeft
DistanceRight = WheelCircumferencePerEncoderClick * EncoderCountRight

DistanceTravelled = (DistanceRight + DistanceLeft) / 2
AngleChange (Theta) = (DistanceRight - DistanceLeft) / WheelBase

我的(DIY)底盘有一个小特点，在轴距(35.5 厘米)的过程中，车轮未对齐，左轮为 6.39 毫米(我是软件人而不是硬件人!)比右轮更“向前”。 (轮子是机器人的中间。)

我不确定如何计算我必须添加到我的公式中才能给我正确的值。它不会对机器人产生太大影响，除非它在现场转弯，而且我的值很差，我认为这是造成它。

我的第一个想法是在网格上绘制车轮位置，并计算它们位置线的斜率，然后用它来乘以……一些东西。

我在正确的轨道上吗？有人可以帮忙吗？我四处寻找这个错误，大多数人似乎都忽略了它(因为他们使用的是专业机箱)。

Answer 1

必须对 AngleChanged 的​​公式进行修正，如下所示:

取 L 和 R 分别为左右轮在一个刻度内行驶的距离。
让 B 表示标称轴距的长度(不是倾斜轴距的长度)。
令 E 代表左轮误差。也就是说，左轮从其理想位置向前偏移的距离。
我们试图找到 θ，即轴距角度在一个刻度内的变化。

首先，(预)计算倾斜轴距和理想轴距之间的角度:

φ = arctan(E / B).

使用一些基本几何(如果您愿意，我可以发布详细信息)，我们可以按如下方式计算 theta:

σ = arctan( ( E+ L - R) / B )
θ = φ - σ

当 E=0 时，这会简化为您之前的实现，并且具有直观可理解的结果为 E -> +inf.，我们的公式似乎是正确的。

笔记:
您可能希望在计算 σ (sigma) 时消除计算上丑陋的反正切。在这些情况下，通常的做法(实际上，您在之前的公式中使用过它)使用近似值 arctan(x) = x , 对于小 x。
这里的问题是，虽然数量 (L-R)/B 可能非常小，但误差添加 E/B 可能会增长到无法接受的大。您可以尝试通过去掉 arctan 并使用 sigma = (E+L-R)/B 来计算 sigma，但如果您想要更好的近似值，您应该使用 arctan(a+x) 约 0 的一阶泰勒级数:

arctan(a+x) = arctan(a) + x/(1 + a^2)

应用于 sigma 的计算，近似值现在看起来像这样:

σ = arctan(E / B) + (L - R) / (B + E^2 / B)

请注意，arctan(E/B) 已经预先计算为 φ。这是对 sigma 更好的近似值，应该会产生更精确的 theta 计算。