通过角速度计算里程计（对于两轮机器人）？

Question

我需要对机器人进行编程以检测它的位置，但由于我没有外部监视器/摄像头，因此机器人需要在内部计算其位置。但无论我做多少研究，我都无法做到这一点。因为机器人也需要避开障碍物，所以速度和方向总是在不断变化。

到目前为止，我得到的代码是：

initX, initY (co-ordinates), initial Angle (0 radians)

omega = (rightSpeed - leftSpeed) * (radius of each wheel/ distance between wheels) Angle = Angle + omega

velocity = (rightSpeed + leftSpeed) / 2 distance = velocity * change in time

x' = x + (distance * sin(Angle)) y' = y + (distance * cos(Angle))

我正在使用一个用Arduino编码的Elisa-3机器人（因此是C），这是等效代码：

[NB：rightt / leftt是Elisa-3独有的速度值。将它们乘以“mySpeedUnit”会将它们转换为“m / s”值。 PAUSE_1_SEC同样是内置的等效“1”秒，因此timeInSeconds将始终是以秒为单位的经过时间。 radwheels相当于（车轮半径/车轮之间的距离）。出于某种原因，如果我没有使用内置常量值，Arduino讨厌分裂，所以我在这里使用原始值。]

//odometry //get current time and time elapsed

tempTime = getTime100MicroSec();

timeInSeconds = (tempTime - time)/PAUSE_1_SEC;

//angles

float r = rightt*mySpeedUnit;

float l = leftt*mySpeedUnit;

float omega = (rightt-leftt) * radwheels;

angle = angle + (omega*timeInSeconds);

angle = correct(angle);

//velocity

float v = (r+l)*0.5;

float s = v*timeInSeconds; //distance travelled

//x, y calculations using distance travelled

xpos = xpos + (s*sin(angle));

ypos = ypos + (s*cos(angle));

time = tempTime;

无论我编码多少次，这个机器人都拒绝合作，所以我希望有人能够帮助我。我愿意尽我所能回答你的任何问题。

提前谢谢。