How do I linearize the following system using taylor series expansion:
$\dot x = v cos\theta \\ \dot y = v sin\theta \\ \dot \theta = u$
Here, $\theta$ is the heading direction of my robot, measured counter clockwise with respect to $x$ axis.
$v$ is the linear velocity of the robot,
$u$ is the angular velocity of the robot.
$\begingroup$
$\endgroup$
4
-
$\begingroup$ for each equation, take the derivative with respect to each state variable which eventually will yield a matrix. $\endgroup$– CroCoMay 29, 2016 at 18:00
-
1$\begingroup$ The method you are taking is the Jacobian method right? $\endgroup$– SantoshMay 29, 2016 at 19:05
-
$\begingroup$ That's right... $\endgroup$– CroCoMay 29, 2016 at 20:39
-
$\begingroup$ Should that be written as answer? $\endgroup$– IanMay 31, 2016 at 18:47
Add a comment
|
1 Answer
$\begingroup$
$\endgroup$
5
The short answer to this question is that linearization won't work, and here's why:
Differential drive robots can be modeled with unicycle dynamics of the form: $$\dot{z}=\left[\begin{matrix}\dot{x}\\ \dot{y} \\ \dot{\theta} \end{matrix}\right] = \left[\begin{matrix}cos(\theta)&0\\sin(\theta)&0\\0&1\end{matrix}\right] \left[\begin{matrix}v\\\omega\end{matrix}\right],$$ where $x$ and $y$ are Cartesian coordinates of the robot, and $\theta \in (-\pi,\pi]$ is the angle between the heading and the $x$-axis. The input vector $u=\left[v, \omega \right]^T$ consists of linear and angular velocity inputs.
We can re-write this in affine form, such that: $$\dot{z}=f(z)+g(z)u.$$ Notice that $f(z)=0$, which is equivalent to saying that there is no drift in the system. For clarity, let $\mathbf{g}(z,u)=g(z)u.$ The Jacobian, which is essentially a matrix of truncated Taylor series expansions, is used to linearize nonlinear systems about an equilibrium point, but the eigenvalues of the Jacobian matrix, evaluated at equilibrium, must be nonzero in order for the linearization to hold (there are some important theorems that come into play here, which are covered in linear algebra and linear systems textbooks). The Jacobian for state matrix A of the possible linearized unicycle system is given by: $$A:=D\mathbf{g}(z,u)=\frac{\partial \mathbf{g}(z,u)}{\partial z}=\left[\begin{matrix}\frac{\partial g_1(z,u)}{\partial x}&\frac{\partial g_1(z,u)}{\partial y}&\frac{\partial g_1(z,u)}{\partial \theta}\\\frac{\partial g_2(z,u)}{\partial x}&\frac{\partial g_2(z,u)}{\partial y}&\frac{\partial g_2(z,u)}{\partial \theta}\\\frac{\partial g_3(z,u)}{\partial x}&\frac{\partial g_3(z,u)}{\partial y}&\frac{\partial g_3(z,u)}{\partial \theta}\end{matrix}\right]=\left[\begin{matrix}0&0&-v\text{sin}(\theta)\\0&0&v\text{cos}(\theta)\\0&0&0\end{matrix}\right],$$ where $g_n$ is the $n^{th}$ function of $\mathbf{g}(z,u)$. Each element in the Jacobian is evaluated as follows, using $n=1$ as an example: $$\frac{\partial g_1(z,u)}{\partial x}=\frac{\partial (v cos{\theta})}{\partial x}=0.$$
By inspection, we can see that the eigenvalues of the Jacobian are zero for all $z$, never mind at any equilibrium point, so this system cannot be linearized. Despite not being able to linearize the system, non-linear control of unicycle robots is straightforward and well studied.
-
$\begingroup$ How do you expand from a 3x2 state matrix to a 3x3 state matrix? If your states were originally $[v,w]^T$, a 2x1 state vector, what are they now? $\endgroup$– Chuck ♦Jun 21, 2016 at 13:21
-
$\begingroup$ Good question! Correct me if I'm wrong, but this is how I understand the Jacobian. There are 3 state variables ($x$, $y$, and $\theta$), and there are 3 functions working on the state variables ($\mathbf{g}(z,u)=g(z)u$). The Jacobian maps from $\mathbf{g}(z,u)\in\mathbb{R}^3$ to the update of $\dot{z}\in\mathbb{R}^3$, and is in $\mathbb{R}^{3\times 3}$. $\endgroup$ Jun 21, 2016 at 16:30
-
$\begingroup$ If the state variables are $[x,y,\theta]^T$, then what is the state matrix? All zeros? Perhaps this is the point you're trying to make. But then I still don't see how you get to your $Dg(z)$ matrix above - what are the $g_N$ terms in $\partial g_N$? Is $g(z)$ not a 3x2 matrix? How are you indexing $g(z)$? I'm not trying to nit-pick, I just think you've glossed over a couple steps (maybe?) and I'm genuinely curious how you've gotten your answer. $\endgroup$– Chuck ♦Jun 21, 2016 at 18:14
-
$\begingroup$ The discussion is good, and hopefully other readers will benefit! The goal is to take the nonlinear system $\dot{z}=\mathbf{g}(z,u)$ and linearize it to $\dot{z}=Ax+Bu.$ To do this we must compute the Jacobian matrices $A:=\frac{\partial \mathbf{g}(z,u)}{\partial z}\in\mathbb{R}^{3\times 3}$ and $B:=\frac{\partial \mathbf{g}(z,u)}{\partial u}\in\mathbb{R}^{3\times 2}$. After computing A, it was easy to determine that linearization would fail about any equilibrium point (none exist!), so I didn't bother computing B. In short, we are both right--you correctly highlight that I left a step out! $\endgroup$ Jun 21, 2016 at 21:14
-
1$\begingroup$ I found a great paper talking about the problem of linearization of unicycle robot models: dis.uniroma1.it/~labrob/pub/papers/Ramsete01.pdf -- it goes into good detail. In particular, the author highlights the problem in section 2. $\endgroup$ Jul 1, 2016 at 13:16