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

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

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

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;

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

先感谢您 .