Calcular a posição do robô de acionamento diferencial

Como você calcula ou atualiza a posição de um robô de acionamento diferencial com sensores incrementais?

Há um sensor incremental conectado a cada uma das duas rodas diferenciais. Ambos os sensores determinam a distância resp. sua roda rolou durante um tempo conhecido .Δ r i g h t Δ $\Delta left$$\Delta right$$\Delta t$

Primeiro, vamos assumir que o centro entre as duas rodas marca a posição do robô. Nesse caso, pode-se calcular a posição como:

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

"Derivando" essas equações sob o pressuposto de que ambas as rodas rolaram em uma linha reta (que deve ser aproximadamente correta para pequenas distâncias), recebo:

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

Onde é o ângulo de orientação do robô. Para a mudança desse ângulo, encontrei a equação$\theta$

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

Onde $w$ é a distância entre as duas rodas.

Porque $\Delta x$ e $\Delta y$ dependem $\theta$ , eu me pergunto se eu deveria primeiro calcular o novo $\theta$ adicionando $\Delta \theta$ ou se eu deveria preferir usar o "velho" $\theta$ ? Existe algum motivo para usar um sobre o outro?

Então, vamos agora assumir que o centro entre as duas rodas não marca a posição do robô. Em vez disso, quero usar um ponto que marque o centro geométrico da caixa delimitadora do robô. Então $x$ e $y$ mudam para:

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

"Derivando" o primeiro fornece:

$$\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}$$

Agora há uma dependência em . Esse é um motivo para usar o "novo" ?$\Delta \theta$$\theta$

Existe algum método melhor para fazer uma atualização simultânea de posição e orientação? Pode estar usando números complexos (a mesma abordagem dos quaternions em 3D?) Ou coordenadas homogêneas?

Para responder à sua primeira pergunta: se você realmente deseja encontrar as equações cinemáticas verdadeiras para acionamento diferencial, eu não começaria a aproximar-me assumindo que cada roda se moveu em linha reta. Em vez disso, encontre o raio de viragem, calcule o ponto central do arco e, em seguida, calcule o próximo ponto do robô. O raio de viragem seria infinito se o robô estivesse se movendo reto, mas no caso reto a matemática é simples.

Então, imagine que, a cada etapa do tempo, ou sempre que você calcula a alteração nos sensores incrementais, o robô viaja do ponto A ao ponto B em um arco como este: Aqui está um código de exemplo com a matemática simplificada:

// 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);
}

Usei matemática semelhante em um simulador para demonstrar diferentes maneiras de dirigir: http://www.cs.utexas.edu/~rjnevels/RobotSimulator4/demos/SteeringDemo/

$\Delta\theta$$\theta$$\Delta{x}, \Delta{y}$

$\Delta\theta$$\Delta{x}, \Delta{y}$$\theta$

$\Delta{t}$$0$