Unicycle Model

Unicycle Model

Given:

A unicycle trying to get to the goal

Dynamic Constraints

\[\dot{x}_1(t)=x_4(t)cos(u_1(t))\]
\[\dot{x}_2(t)=x_4(t)sin(u_1(t))\]
\[\dot{x}_3(t)=u_1(t)\]
\[\dot{x}_4(t)=u_2(t)\]

Boundary Conditions

\[{x}_1(0)=0 \qquad {x}_1(t_f)=free\]
\[{x}_2(0)=pi/2\qquad {x}_2(t_f)=free\]
\[{x}_3(0)=0.5\qquad {x}_3(t_f)=free\]
\[{x}_4(0)=0\qquad {x}_4(t_f)=free\]

Find:

The control signals that minimize distance to goal $(x_g,y_g)$ within $tplan$

\[J=({x}_1(t_f)-x_g)^2 + ({x}_2(t_f)-y_g)^2)\]

Solution:

Packages that will be used

using NLOptControl

Define the Problem

Next let's write down the boundary conditions into an array:

X0=[0,0,pi/2,0]
XL=[-10,-10,-pi,0]
XU=[10,10,pi,1]
CL=[-1,-3]
CU=[1,3]

Define the Problem

n = define(numStates=4,numControls=2,X0=X0,XL=XL,XU=XU,CL=CL,CU=CU)

State and Control Names

names=[:x,:y,:psi,:ux]
descriptions=["X (m)","Y (m)","Yaw Angle (rad)","Longitudinal Velocity (m/s)"]
states!(n,names,descriptions=descriptions)
names = [:r,:ax]
descriptions=["Yaw Rate (rad/s)","Longitudinal Acceleration (m/s^2)"];
controls!(n,names,descriptions=descriptions)

Differential Equations

dx = [:(ux[j]*cos(psi[j])),:(ux[j]*sin(psi[j])),:(r[j]),:(ax[j])]
dynamics!(n,dx)

Define and Configure the Problem:

tplan = 7.0
configure!(n;(:Nck=>[50]),(:finalTimeDV=>false), (:tf=>tplan))

Objective Function

x = n.r.ocp.x[:,1]; y = n.r.ocp.x[:,2]; # pointers to JuMP variables
xg = -2; yg = 4;
@NLobjective(n.ocp.mdl, Min, (x[end]-xg)^2 + (y[end]-yg)^2)

Optimize

optimize!(n)

Post Process

plotSettings(;(:size=>(700,700)))
allPlots(n)
0 1 2 3 4 5 6 7 -10 -5 0 5 10 time (s) X (m) max min mpc 0 1 2 3 4 5 6 7 -10 -5 0 5 10 time (s) Y (m) max min mpc 0 1 2 3 4 5 6 7 -3 -2 -1 0 1 2 3 time (s) Yaw Angle (rad) max min mpc 0 1 2 3 4 5 6 7 0.00 0.25 0.50 0.75 1.00 time (s) Longitudinal Velocity (m/s) max min mpc 0 1 2 3 4 5 6 7 -1.0 -0.5 0.0 0.5 1.0 time (s) Yaw Rate (rad/s) max min mpc 0 1 2 3 4 5 6 7 -3 -2 -1 0 1 2 3 time (s) Longitudinal Acceleration (m/s^2) max min mpc

Taking a closer look at the position:

plotSettings(;(:size=>(400,400)));
statePlot(n,1,1,2;(:lims=>false))
xlims!(-3,2);
ylims!(0,5);
/home/travis/.julia/v0.6/GR/src/../deps/gr/bin/gksqt: error while loading shared libraries: libQt5Widgets.so.5: cannot open shared object file: No such file or directory
connect: Connection refused
GKS: can't connect to GKS socket application
Did you start 'gksqt'?

GKS: Open failed in routine OPEN_WS
GKS: GKS not in proper state. GKS must be either in the state WSOP or WSAC in routine ACTIVATE_WS