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.
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
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
omega_s are now the linear and angular velocities, which are states now 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.