Обчисліть положення робота диференціального приводу

Як ви обчислюєте або оновлюєте положення робота диференціального приводу за допомогою покрокових датчиків?

До кожного з двох диференціальних коліс встановлений один поступовий датчик. Обидва датчики визначають відстань $\Delta left$ resp. $\Delta right$ їх колесо котилося протягом відомого часу $\Delta t$ .

Спочатку припустимо, що центр між обома колесами позначає положення робота. У цьому випадку позицію можна обчислити як:

$$x = \frac{x_{left}+x_{right}}{2} \\ y = \frac{y_{left}+y_{right}}{2}$$

"Виведення" цих рівнянь під припущенням, що обидва колеса котяться по прямій лінії (що має бути приблизно правильним для невеликих відстаней), я отримую:

$$\frac{\Delta x}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)cos(\theta) \\ \frac{\Delta y}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)sin(\theta)$$

Де - кут орієнтації робота. Для зміни цього кута я знайшов рівняння$\theta$

$$\frac{\Delta \theta}{\Delta t} = \frac{1}{w} \left( \frac{\Delta left}{\Delta t} - \frac{\Delta right}{\Delta t}\right)$$

Де - відстань між обома колесами.$w$

Оскільки і Δ y залежать від θ , мені цікаво, чи слід спочатку обчислити нове θ , додавши Δ θ, чи я повинен скоріше використовувати "старий" θ ? Чи є підстави використовувати одне над іншим?$\Delta x$$\Delta y$$\theta$$\theta$$\Delta \theta$$\theta$

Тоді, припустимо, тепер центр між обома колесами не позначає положення робота. Натомість я хочу використовувати точку, яка позначає геометричний центр обмежувальної коробки робота. Тоді і y змінюємо на:$x$$y$

$$x = \frac{x_{left}+x_{right}}{2} + l\, cos(\theta)\\ y = \frac{y_{left}+y_{right}}{2} + l\, sin(\theta)$$

"Виведення" першого дає:

$$\frac{\Delta x}{\Delta t} = \frac{1}{2}\left( \frac{\Delta left}{\Delta t} + \frac{\Delta right}{\Delta t}\right)cos(\theta) - l\,sin(\theta)\,\frac{\Delta \theta}{\Delta t}$$

Тепер існує залежність від . Це причина для використання "нового" θ ?$\Delta \theta$$\theta$

Чи є кращий метод зробити симуляційне оновлення позиції та орієнтації? Може використовуватися складні числа (той самий підхід, що і з кватерніонами в 3D?) Або однорідні координати?

Щоб відповісти на ваше перше запитання: якщо ви дійсно хочете знайти справжні кінематичні рівняння для диференціального приводу, я б не почав наближатися, вважаючи, що кожне колесо рухається по прямій лінії. Натомість знайдіть радіус повороту, обчисліть центральну точку дуги, а потім обчисліть наступну точку робота. Радіус повороту був би нескінченним, якщо робот рухається прямо, але в прямому випадку математика проста.

Отже, просто уявіть, що протягом кожного кроку часу або кожного разу, коли ви обчислюєте зміну покрокових датчиків, робот рухається від точки А до точки В на дузі так: Ось приклад коду з спрощеною математикою:

// leftDelta and rightDelta = distance that the left and right wheel have moved along
//  the ground

if (fabs(leftDelta - rightDelta) < 1.0e-6) { // basically going straight
    new_x = x + leftDelta * cos(heading);
    new_y = y + rightDelta * sin(heading);
    new_heading = heading;
} else {
    float R = unitsAxisWidth * (leftDelta + rightDelta) / (2 * (rightDelta - leftDelta)),
          wd = (rightDelta - leftDelta) / unitsAxisWidth;

    new_x = x + R * sin(wd + heading) - R * sin(heading);
    new_y = y - R * cos(wd + heading) + R * cos(heading);
    new_heading = boundAngle(heading + wd);
}

Я використовував аналогічну математику в тренажері, щоб продемонструвати різні способи управління: http://www.cs.utexas.edu/~rjnevels/RobotSimulator4/demos/SteeringDemo/

Для повторного розрахунку не має значення, чи знайдете ви $\Delta\theta$ до або після подання заяви $\theta$ до $\Delta{x}, \Delta{y}$розрахунок. Ви завжди будете чергувати позицію та розрахунок орієнтації.

У практичному сенсі це може бути краще обчислити $\Delta\theta$ після розрахунку $\Delta{x}, \Delta{y}$, оскільки ваша перша ітерація через цикл вимагає початкового значення $\theta$.

Пам'ятайте, що це спосіб вимірювання, схильний до помилок, так чи інакше - це пара одновимірних вимірювань, що подають 2D-наближення тривимірного світу. Навіть якщо ви зможете отримати$\Delta{t}$ дуже близько $0$, вона все одно не враховуватиме ковзання коліс та нерівну місцевість.