Calculate (three wheeled) mobile robot position and heading

876 Views Asked by At

I've tried to use odometry data (distance (dist) and steering angle ($\alpha$) from front wheel of mobile robot to calculate position of rear wheels using following:

tricycle model

$\beta = dist / d * \tan (\alpha)$
($\beta$ = turning angle)

$R = \frac {dist} {\beta} $

$R.x = x - \sin(\theta) * R$

$R.y = y + cos(\theta) * R$

$\theta' = (\theta + \beta) \mod 2\pi$

$x' = R.x + sin(\theta') * R$

$y' = R.y - cos(\theta') * R$

It calculates $\theta'$ well with small steering angles, but it looks as $\theta'$ gets much too big when steering angle gets bigger than somewhere around $\frac {\pi} {3}$

1

There are 1 best solutions below

0
On

Perhaps it is good enough to approximate with a bicycle model. Imagine a fictitious rear wheel placed midway along the rear axle of the tricycle. Then you have created a vehicle that resembles a bike. The front wheel of the vehicle can pivot freely, but the rear wheel axis is aligned rigidly with the body axis of the moving vehicle.

Here is a solution to the bike problem.

Denote the position vectors of the front and rear hubs of the bike by $ F(t)$ and $R(t)$. Then the distance between these hubs is a fixed constant length $L$. The body axis of the vehicle points in the direction of the displacement vector from $R$ to $F$. Its compass bearing is described by some angle $\theta(t)$, hence the displacement vector can be written as $ F-R= \vec {RF} = L\vec u= L<\cos (\theta), \sin(\theta)>$. Using complex exponential notation for the last expression we obtain

$F- R= L e^{i\theta}$. By differentiating $$(1) \qquad F(t)- R(t) = L e^{ i \theta(t)}$$ you deduce that $$ (2) \qquad \dot F - \dot R = L i \dot \theta e^{i \theta} $$ But the rear wheel must travel in the same direction as the body axis $\vec u$ because it is aligned rigidly with the body frame. Thus the rear velocity vector is $(3 A) \qquad \dot R= C \vec u$ where $C$ is the speed of the rear wheel. The front wheel velocity vector is aligned with the direction that the front wheel is turned, which depends on the steering angle. Thus $ (3B) \qquad \dot F= k e^{i (\theta +\alpha)}$ where $k$ is the speed of the front wheel and $ \alpha$ is the steering angle, measured relative to the body axis vector $\vec u$. Inserting (3) into (2) and canceling $e^{i \theta}$ across all terms one deduces $$(4) \qquad k e^{i\alpha} = c+ i L \dot \theta$$ Interpreting the left and right sides of (4) as the polar and Cartesian forms of the same complex quantity, we deduce

$$(5A)\qquad k \cos \alpha= c$$ and $$(5B) \qquad k\sin \alpha = L\dot \theta$$

Your data provides knowledge of $k$ (speed of front wheel) and $\alpha$ (steering angle relative to body frame axis). Thus you can use (5) to deduce the speed $c$ of the rear wheel and the rate of turning of the body $\dot \theta= k \sin \alpha /L$. By integrating both equations you can find the distance traveled by the rear wheel and the bearing of the body $\theta$. Then integrating the velocity vector of the rear wheel $\dot R= c(t) e^{i \theta(t)}$ you can solve for the position of the rear wheel: $ R(t)= \int c(t) e^{i \theta(t)} \ dt$

The analysis above assumes that there is no slipping. Note that this analysis predicts that the rear speed $c$ is always smaller than the front speed $k$, and therefore the rear wheel will always travel fewer miles than the front.