Расчет одометрии по угловой скорости (для двухколесного робота)?

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))

Я использую робота Elisa-3, закодированный с Arduino (отсюда 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;

Независимо от того, сколько раз я кодирую это, этот робот отказывается сотрудничать, поэтому я надеюсь, что кто-то здесь сможет мне помочь. Я готов ответить на любые ваши вопросы в меру своих возможностей.

Заранее спасибо.