Problem designing a control law

Question

I am trying to build a posture regulation control which works with acceleration inputs.

Doing it with velocity inputs,I have well understood how it works. Moreover,for now, the controller only controls the position $x,y$ and sends to zero these two coordinates, so it brings $(x,y)=(0,0)$ and does not control the orientation.

I have built the acceleration level control for this case, and what I get is:

where we can see that the $x$ and $y$ coordinates go to zero, and $\theta$ is not controlled, as I expected. Even if the acceleration profile is a bit weird (and this makes me think that the error may be in the implementation, but I am not sure):

I think the acceleration is weird because I was expecting a profile of the type:

where on top there is the linear acceleration and the bottom one is the angular acceleration.

So I am not sure I am doing well. In this case, I have defined the polar coordinates:

$R = \sqrt{x^2+y^2}$

$\gamma=atan2(y.x)-\theta$

and the controller is:

 x = state(1);
 y = state(2);
 theta = state(3);
 v_state = state(4);
 omega_state = state(5);
 theta_w = state(6);

k_1 = gain(1);
k_2 = gain(2);
k_3 = gain(3);
k_4 = gain(4);
k_5 = gain(5);
temp = x^2+y^2;
R = sqrt(temp);
gamma = atan2(y,x)-theta;
delta = gamma + theta;
sinc = sin(gamma)*sign(cos(gamma));
v=k_1Rsign(cos(gamma));
omega = k_2(k_1sinc(gamma+k_3delta)/(2gamma + k_3delta));
a = k_4(v-v_state);
alpha = k_4(omega-omega_state);
inputs = [a;alpha];

where $v$ and $\omega$ can be obtained with the same procedure that I have written below, or also from the feedback linearization as it is done in the paper.

Moreover, v_state and omega_state are respectively linear and angular velocities of the system, that now have become states, since I want acceleration inputs.

And the model, written in Matlab , is:

a = inputs(1);
alpha = inputs(2);

x = state(1);
y = state(2);
theta = state(3);
v_state = state(4);
omega_state = state(5);
x_dot = -v_statecos(theta)+yomega_state;
y_dot = -v_statesin(theta)-xomega_state;
theta_dot = omega_state;
theta_w_dot = -omega_state;
v_dot = a;
omega_dot = alpha;

Note: the above codes are from Simulink blocks.

Now, I would like to control also the final orientation,and bring it to zero, so I define the polar coordinate(in the code above I have already written the control law that should control also the final orientation that I am going to derive below):

$\delta = \theta + \gamma$

and if I take the derivatives of all these polar coordinates, I get the following kinematic model:

$\dot{R}=-vcos\gamma$

$\dot{\gamma}=v\frac{sin\gamma}{R}-2\omega$

$\dot{\delta}=v\frac{sin\gamma}{R}-\omega$

and the objective is to have $R=0, \gamma=0,\delta=0$.

Now, to derive the control law that controls also the orientation I use a Lyapunov function:

$V = \frac{1}{2}(R^2+\gamma^2+\delta^2)>0$

$\dot{V}=R\dot{R}+\gamma\dot{\gamma}+\delta\dot{\delta}= R(-vcos\gamma)+\gamma(v\frac{vsin\gamma}{R}-2\omega)+\delta(\frac{vsin\gamma}{R}-\omega)= v(-Rcos\gamma + \gamma\frac{sin\gamma}{R}-\delta \frac{sin\gamma}{R})-\omega(2\gamma+\delta)$

and this has to be less than or equal to zero for the Lyapunov criterion.

From here, I derive that

$v = k_1\cdot Rcos\gamma$

$\omega = \frac{k_1\cdot sin\gamma\cdot cos\gamma(\gamma+k_2\delta)}{2\gamma+k_2\delta}$

but if i try these and use them as desired linear and angular velocities, I get :

so the orientation still does not go to zero.

I don't understand what I am doing wrong, and I have been on this for a while now and I am really stuck. The reasoning seems correct, at least to me, but apparently I am doing something wrong that I don't get.

Can somebody please help me understand what I am doing wrong? Do you think my approach is correct, or should I try another way for solving the problem?

For the implementation I have used Matlab and Simulink.

Could the problem be connected to the initial configuration? I really don't get what I am doing wrong, i am trying from a while now, but I am really stuck.

I have also tried to not make the values of $\gamma$ and $\delta$ don't go to large, so I have added this lines of code:

gamma_t = atan2(y,x)-theta;
delta_t = gamma_t + theta;

gamma = mod(gamma_t+pi,2pi)-pi;
delta = mod(delta_t+pi,2pi)-pi;

but this didn't solve the problem.

About the plots, if it can add something, I have done in the following way:

x=configuration.signals.values(:,1);
y=configuration.signals.values(:,2);
theta=configuration.signals.values(:,3);
t=configuration.time;
a=inputs.signals.values(:,1);
alpha=inputs.signals.values(:,2);

figure(1);
plot(t,x,t,y,t,theta);grid
legend('x','y','theta');
figure(2);
plot(t,a,t,alpha);grid
legend('a','alpha');

if I have to add something to the question to make it clearer please tell me. I really don't know how to move on.

I am trying to read several papers and references, such as:

Stabilized Feedback Control of Unicycle Mobile Robots

Posture regulation for unicycle-like robots with prescribed performance guarantees

and they seem to use the same approch I have used, so there has to be some mistake in some step I have done, which I do not understand.

Can someone please help me?

Answer 1

Something is wrong with your Lyapunov analysis.

\begin{align} \dot{V}&=R\dot{R}+\gamma\dot{\gamma}+\delta\dot{\delta} \\ &= R(-vcos\gamma)+\gamma(v\frac{sin\gamma}{R}-2\omega)+\delta(v\frac{sin\gamma}{R}-\omega) \\ &= v(-Rcos\gamma + \gamma\frac{sin\gamma}{R}+\delta \frac{sin\gamma}{R})-\omega(2\gamma+\delta) \end{align} You suggested using, \begin{align} v &= k_1\cdot Rcos\gamma \\ \omega &= \frac{k_1\cdot sin\gamma\cdot cos\gamma(\gamma+k_2\delta)}{2\gamma+k_2\delta} \end{align} Substituiting this in we get that, \begin{align} \dot{V} &= k_1\cdot Rcos\gamma(-Rcos\gamma + \gamma\frac{sin\gamma}{R}+\delta \frac{sin\gamma}{R})-\frac{k_1\cdot sin\gamma\cdot cos\gamma(\gamma+k_2\delta)}{2\gamma+k_2\delta}(2\gamma+\delta) \\ &= -k_{1}R^{2}cos^{2}\gamma + k_{1}(\gamma + \delta)cos\gamma sin\gamma -\frac{k_1\cdot sin\gamma\cdot cos\gamma(\gamma+k_2\delta)}{2\gamma+k_2\delta}(2\gamma+\delta) \end{align} If we assume that $k_{2} =1$ then, \begin{align} \dot{V} &= -k_{1}R^{2}cos^{2}\gamma + k_{1}(\gamma + \delta)cos\gamma sin\gamma -\frac{k_1\cdot sin\gamma\cdot cos\gamma(\gamma+\delta)}{2\gamma+\delta}(2\gamma+\delta) \\ &= -k_{1}R^{2}cos^{2}\gamma + k_{1}(\gamma + \delta)cos\gamma sin\gamma - k_1\cdot sin\gamma\cdot cos\gamma(\gamma+\delta) \\ &= -k_{1}R^{2}cos^{2}\gamma \end{align} and $\dot{V}\leq 0$ if $k_{1}>0$. Therefore, $\dot{V}=0$ when $\gamma = \frac{\pi}{2} + \pi k$ or $R = 0$. Though your dynamics when $R=0$ are not well defined.

If $k_{1}\neq 1$ then \begin{align} \dot{V} &= -k_{1}R^{2}cos^{2}\gamma + \left(k_{1}(\gamma + \delta) -\frac{k_1(\gamma+k_2\delta)}{2\gamma+k_2\delta}(2\gamma+\delta)\right)cos\gamma sin\gamma \\ &= -k_{1}R^{2}cos^{2}\gamma + k_{1}\left((\gamma + \delta) -\frac{(\gamma+k_2\delta)}{2\gamma+k_2\delta}(2\gamma+\delta)\right)\frac{1}{2}sin(2\gamma) \end{align} As you increase $\gamma$ the second term will oscilate from being positive and negative and therefore $\dot{V}\not\leq 0$.