This microsite is associated to the paper entitled :
Controlling independently the heading and the position of a Dubins car using Lie brackets


In this paper, we give a control approach to follow a trajectory for a Dubins car controlling the heading independently. The difficulty is that the Dubins car should have a heading corresponding to the argument of the vector speed of the vehicle. This non holonomic constraint can be relaxed thanks to a specific control which uses Lie brackets. We will show such a control will allow us, at least from a theoretical point of view, to stabilize independently the moving position of the car and the heading.

Speed control

from roblib import *

def f(X,u):
    θ=X[2,0]
    u=u.flatten()
    u1,u2=list(u[0:2])
    return array([[u1*cos(θ)], [u1*sin(θ)],[u2]])

def control_speed(a,t,delta):
    a=a.flatten()
    eps= sign(a[2])
    def A(t): return (-1)**np.floor(t/2)*(np.floor(t)%2)
    u = sqrt(4*abs(a[2])/delta)*array([[eps*A(t/delta+1)],[A(t/delta)]])+array([[a[0]],[a[1]]])
    return u

dt= 0.01
delta=sqrt(dt)
ax=init_figure(-1,1,-1,1)
tmax=10
def godir(X,a,col,dk):
    for t in arange(0,tmax,dt):
         if int(t/dt)%dk==0:
             draw_tank(X,col,0.005,1)
         u=control_speed(a,t,delta)
         X=X+dt*f(X,u)
z1=0.02
godir(array([[z1],[z1],[1]]),array([[0.1],[0.0],[0.0]]),'darkblue',30)
godir(array([[z1],[-z1],[1]]),array([[0.0],[0.0],[0.1]]),'green',1)
godir(array([[-z1],[-z1],[1]]),array([[-0.1],[0.0],[0.0]]),'darkred',30)
godir(array([[-z1],[z1],[1]]),array([[0.0],[0.0],[-0.1]]),'magenta',1)

Control with respect to the Cardinal directions

def control_a(X,dw):
    X=X.flatten()
    x,y,θ=list(X[0:3])
    A = array([ [cos(θ), 0, sin(θ) ], [sin(θ), 0,-cos(θ)], [0 ,1, 0]])
    a=inv(A) @ dw
    return a

def godir(X,dw,col):
    X0=X
    for t in arange(0,tmax,dt):
        draw_tank(X,col,0.005,1)
        a=control_a(X0,dw)
        u=control_speed(a,t,delta)
        X=X+dt*f(X,u)

dt= 0.01
delta=sqrt(dt)
tmax=3
z1=0.05
godir(array([[z1],[-z1],[1]]),array([[0.1],[0.0],[0.0]]),'darkblue')
godir(array([[z1],[2*z1],[1]]),array([[0.0],[0.1],[0.0]]),'green')
godir(array([[-z1],[z1],[1]]),array([[-0.1],[0.0],[0.0]]),'darkred')
godir(array([[-z1],[-2*z1],[1]]),array([[0.0],[-0.1],[0.0]]),'magenta')

Lissajou for the first order Dubins car

dt= 0.01
delta=sqrt(dt)
X=array([[-1],[0],[1]])
for t in arange(0,20,dt):
    p=array([[5*sin(0.01*t)],[5*sin(0.02*t)],[0]])
    dw=array([[0.2*tanh((p[0,0]-X[0,0]))],[0.2*tanh((p[1,0]-X[1,0]))],[0.2*(p[2,0]-X[2,0])]])
    draw_tank(X,'darkblue',0.005,1)
    draw_tank(p,'red',0.005,1)
    a=control_a(X,dw)
    u=control_speed(a,t,delta)
    X=X+dt*f(X,u)

Lissajou for the second order Dubins car

def control_K(X,v):
    v=v.flatten()
    v1,v2=list(v[0:2])
    K=30
    b=array([[K*(v1-X[3,0])],[K*(v2-X[4,0])]])
    return b

dt= 0.01
delta=sqrt(dt)
X=array([[-0.1],[0],[1],[0],[0]])
tmax=500  #4 10 60 #500
for t in arange(0,tmax,dt):
    p=array([[5*sin(0.01*t)],[5*sin(0.02*t)],[0]])
    dw=array([[0.2*tanh((p[0,0]-X[0,0]))],[0.2*tanh((p[1,0]-X[1,0]))],[0.2*(p[2,0]-X[2,0])]])
    draw_tank(X,'darkblue',0.005,1)
    draw_tank(p,'red',0.0005,1)
    a=control_a(X,dw)
    v=control_speed(a,t,delta)
    u=control_K(X,v)
    X=X+dt*f(X,u)