Problem designing acceleration inputs controller

Question

I am reading this paper: https://www.researchgate.net/publication/224635390_The_motion_control_problem_for_the_CyberCarpet

in which is implemented a cartesian regulator to bring the position $(x,y)$ to $(0,0)$. I have built it as in the paper, so with velocity inputs, and the output is the following:

the final velocities has to be zero, and since I can only control position and orientation, I think it is correct that I get the plot on the right.

The problem is a posture regulatoin prolem. If we consider for example a unicycle, we have that the desired behavior is the following:

so with this type of controller the unicycle is driven to the origin.

from here: https://people.kth.se/~dimos/pdfs/IET15_Zambelli.pdf

where $R$,$\gamma$ and $\delta$ are the polar coordinates for the system. $\gamma$ can be defined as a pointing error, and $\delta$ is the orientation on the final position.

I use polar coordinates, because I have read in many papers that in this way we can use Lyapunov function to find the controller.

The controller is the following(written in a Simulink block):

temp = x^2+y^2;

%polar coordinates

R = sqrt(temp);                   
gamma = atan(tan(y/x))-theta;
delta = gamma + theta;          %not used now

sinc = sin(gamma)*sign(cos(gamma));


%linear and angular velocity
if R == 0 
    v=k_1*((R^2)*sign(cos(gamma)));

% steering velocity

    omega = k_1*R*sinc;      
else
    v=k_1*R*sign(cos(gamma));
    omega = k_1*sin(gamma)*sign(cos(gamma));

end

inputs = [v;omega];

which is obtained throgh feedback linearization, but it can also be obtained using a Lyapunov method.

Now, I am trying to go a little furhter, and instead of using velocity inputs, I would like to use acceleration inputs. What I have done is the following:

I have defined v and omega as states of the system, and I have built a cascade controller. So, to the controller I hve just added these lines:

a = k_2*(v-v_s);               %linear acceleration
alpha = k_2*(omega-omega_s);          %angular acceleration

inputs = [a;alpha];               %acceleration inputs

where v_s and omega_s are now the linear and angular velocities, which are states now and v and omega are now the desired linear and angular velocity respectively. And the model of the system (written in nother Simulink block) becomes:

x_dot = -v_s*cos(theta)+y*omega_s;
y_dot = -v_s*sin(theta)-x*omega_s;
theta_dot = omega_s;
theta_w_dot = -omega_s;
v_dot = a;
omega_dot = alpha;

state_dot = [x_dot;y_dot;theta_dot;v_dot;omega_dot];    %out of the block I integrate this and send it back as [x;y;theta;v;omega], which are the states of the system

and if I do so, now I get the following plots:

so it diverges in practically everithing!

I don't understand what I am doing wrong. For the definition of the acceleration inputs I am trying to follow this paper, which is for a uniccyle but it should be similar, since they are both non-holonomic systems: https://www.researchgate.net/publication/276184094_Stabilized_Feedback_Control_of_Unicycle_Mobile_Robots (at pag. 4)

My final objective is actually to build a controller for this system that converges both in position and orientation, so apply the concept written in the latter paper, at pag.3, to my system of study.

But I think now that the problem lies in the definition of the acceleration law.

Can somebody please help me understand what I am doing wrong?

[EDIT]If I try to substitute gamma = atan(tan(y/x))-theta; with gamma = atan2(y,x)-theta; I get:

which I think it is a little better then before, even if the acceleration profile is a bit weird I think.

Could you give me some help understanding if up to now I have done somehting wrong? The acceleration profile that I get seems to be not really ok to me, what do you think?

[EDIT] I wrote sinc as an abbreviation for sin*cos , sorry for the confusion, when I wrote it I really wasn't thinking about the sinc function.

Answer 1

You've been asking lots of questions along these lines, and I've been there before - I know what it is to be at your wits end trying to find a solution. I'm going to try to help, but part of my "help" is to point out that I don't understand how your equations are derived. To that end, I'm going to show why I understand how they're derived. I'll start with some equations you've given:

x_dot = -v_s*cos(theta)+y*omega_s;
y_dot = -v_s*sin(theta)-x*omega_s;
theta_dot = omega_s;
theta_w_dot = -omega_s;
v_dot = a;
omega_dot = alpha;

Okay, working from the bottom to the top, with the help of MathJax, it looks like

$$ \dot{\omega} = \alpha \\ \dot{v} = a \\ $$

Those make sense to me. Derivative of an angular speed $\omega$ is an angular acceleration $\alpha$, and the derivative of a linear speed $v$ is a linear acceleration $a$. Got it.

Moving on:

$$ \dot{\theta} = \omega_s \\ $$

and you said in your text that (replacing code with Mathjax):

where $v_s$ and $\omega_s$ are now the linear and angular velocities, which are states now and $v$ and $\omega$ are now the desired linear and angular velocity respectively.

You've got a $\dot{\theta_w}$ that I don't know what it's doing, but otherwise I'm still on board with your definitions - they're all identity so far. Where I have trouble is with the next line. Again, in Mathjax, you're saying:

$$ \dot{x} = -v_s \cos{\theta} + y (\omega_s) \\ \dot{y} = -v_s \sin{\theta} - x (\omega_s) \\ $$

So you've got $v_s$ as the linear velocity, so I can understand the sine/cosine of an angle times the linear velocity to get the components of linear velocity, but what is this $x(\omega_s)$ and $y(\omega_s)$ business? I'm assuming that it's your x/y position, but the x/y position should have nothing to do with your speed.

What you're saying is that, by changing your reference point for zero position, you're changing your speed. If $x = 0$ then $x(\omega_s) = 0$, and if $x = 100$ then $x(\omega_s) = 100*\omega_s$.

It feels like you're trying to combine a linear speed and a tangential speed, but the tangential speed would be an angular speed times a radius of curvature, but there's a lot lacking from that. It seems almost like you're maybe trying to do something like with a Coriolis force, but that would be a force that would impact acceleration, not velocity. And there's no $r$ for radius of curvature or rotating frame distance, etc.

I would think you should omit the $x(\omega_s)$ and $y(\omega_s)$ terms completely, but again I'm not understanding how they're derived or what they mean.

Continuing up:

a = k_2*(v-v_s);               %linear acceleration
alpha = k_2*(omega-omega_s);          %angular acceleration

it looks like you've got acceleration as a linear function of the difference in reference and state speed, but what is $k_2$? The more important problem for me is that you have the same gain $k_2$ being used to drive both your linear and angular accelerations. This might make sense if you were doing some vector operation, but if you did it longhand like that then I'd expect a $k_1$ for the linear acceleration and a $k_2$ for the angular. Since I'm not seeing where $k_2$ (or $k_1$) is defined I can't give any feedback on that.

Now finally I work my way back up to your first set of equations, which have me totally lost. I would expect that your reference would be either a constant setpoint, like v=0, or the result of some profile, like v = MySpeedProfileGenerator(t). What I'm seeing instead is that your linear and angular speeds are again functions of distance, but not the x/y positions; just the radial distance $R$ from origin to the x/y point.

That is, you have:

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

and that's fine, but then why do your targets $v$ and $\theta$ depend on whether or not $R==0$? And, IF $R$ is zero, then:

if R == 0 
    v=k_1*((R^2)*sign(cos(gamma)));
    omega = k_1*R*sinc;      
else

is just a really fancy way of writing

if R == 0 
    v = 0;
    omega = 0;      
else

But I'm also not understanding the rest of it - what is $\theta$? What is $\gamma$ supposed to be? Finally, the biggest thing I don't understand here, what is this sinc function you've got, and what is it doing? You have:

sinc = sin(gamma)*sign(cos(gamma));

But the sinc function is

sinc = sin(x)/x;

so maybe you're abbreviating some other name? But why does the sign (not sine!) of cos(gamma) keep coming up?

---

I would almost guess you're trying to constrain it to run in a circle or something, the way these angles keep coming up, and that you're using an $R$ to drive your speed and... angle? I'm assuming theta is an input, but theta doesn't drive your values directly, gamma does, and that's a function of position.

The crazy thing for me is that I can't tell how or even if your position ever gets back to anything. You have position driving $R$ and driving $\gamma$, which means position also drives everything else, and then you're outputting $\dot{x}$ and $\dot{y}$, but you're never doing anything with x or y.

I thought you had given some drawings previously but I can't find those now; they're not in your list of questions for this site. I think that you should go back to the problem statement for your project and do the following:

1. Clearly define what you're trying to control - is it x/y position? Radial position? Heading? Is this a SISO system or MIMO?
2. Clearly define the kinematic relationships at play - this means you need a labeled drawing of your setup.
3. Pick Cartesian or Polar, and stick with it. You're begging for accidents by switching back and forth mid-block.

:EDIT:

As I was writing the last line of this answer, I had a sudden flash - I think maybe you're trying to convert from Polar to Cartesian in your x_dot and y_dot equations! But you're not doing it correctly. For one, you're using the linear speed instead of the radial speed. For two, you're using the wrong equation (but you're so close!):

You have:

x_dot = -v_s*cos(theta) + y*omega_s;

where the proper polar-to-Cartesian conversion would be:

x_dot = R_dot*cos(theta) - r*omega_s*sin(theta);

I hope all of this leads you to some insight into your problem.