Visual Servoing Trajectory Tracking of Nonholonomic Mobile Robots Without Direct Position Measuremen

1. 移动机器人（两轮差动）的运动方程

$\dot{p}(t)=\begin{pmatrix}\dot{x}(t)\\\dot{y}(t)\\\dot{\theta}(t)\end{pmatrix}=\begin{pmatrix}cos\theta(t)&0\\sin\theta(t)&0\\0&1\end{pmatrix}\begin{pmatrix}v(t)\\w(t)\end{pmatrix}$
$p(t)$ — 机器人的运动轨迹，含在世界坐标系下的位置变化$x(t),y(t)$，朝向与世界坐标系$x$轴的夹角变化$\theta(t)$
$v(t),w(t)$ — 机器人的线速度与角速度

---

2. 轨迹偏差

$\Delta{p(t)}=p_d(t)-p(t)=\begin{pmatrix}\Delta{x(t)}\\\Delta{}y(t)\\\Delta\theta(t)\end{pmatrix}=\begin{pmatrix}x_d(t)-x(t)\\y_d(t)-y(t)\\\theta_d(t)-\theta(t)\end{pmatrix}$
$e(t)=\begin{pmatrix}e_x(t)\\e_y(t)\\e_\theta(t)\end{pmatrix}=\begin{pmatrix}cos\theta(t)&sin\theta(t)&0\\-sin\theta(t)&cos\theta(t)&0\\0&0&1\end{pmatrix}\Delta{p}(t)$
$\Delta{p(t)}$ — 轨迹偏差
$e(t)$ — 转换后的偏差，含义如下图(非配图，符号不同)，图中的$(x_e,y_e,\theta_e)$对应上式中的$e(t)$，$p_r,p_c$对应$p_d(t),p(t)$。

---

第二个公式中的参数矩阵是满秩的，意味着若$e(t)$收敛于零，则$\Delta{p}(t)$收敛于零。
$e(t)$对$t$求导：
$\dot{e}(t)=\begin{pmatrix}v_d(t)cose_\theta(t)\\v_d(t)sine_\theta(t)\\w_d(t)\end{pmatrix}+\begin{pmatrix}-1&e_y(t)\\0&-e_x(t)\\0&-1\end{pmatrix}\begin{pmatrix}v(t)\\w(t)\end{pmatrix}$

---

以$e_x(t)$为例证明上式(为表述方便，省去$(t)$)：
$e_x=cos\theta(x_d-x)+sin\theta(y_d-y)$
$\begin{align}\dot{e}_x&=-\dot\theta{sin}\theta(x_d-x)+\dot\theta{cos}\theta(\dot{y_d}-y)&+cos\theta(\dot{x}_d-\dot{x})+sin\theta(\dot{y}_d-\dot{y})\\&=-e_yw&+\dot{x}_dcos\theta-\dot{x}cos\theta+\dot{y}_dsin\theta-\dot{y}sin\theta\\&=-e_yw-v&+\dot{x}_dcos\theta+\dot{y}_dsin\theta\\&=-e_yw-v&+\dot{x}_d(cos\theta_dcose_\theta+sin\theta_dsine_\theta)+\dot{y}_d(sin\theta_dcose_\theta-cos\theta_dsine_\theta)\\&=-e_yw-v&+(\dot{x}_dcos\theta_d+\dot{y}_dsin\theta_d)cose_\theta+(\dot{x}_dsin\theta_d-\dot{y}_dcos\theta_d)sine_\theta\\&=-e_yw-v+v_dcose_\theta\end{align}$
其中：
$\dot{x}_dcos\theta_d+\dot{y}_dsin\theta_d=v_d$
$\dot{x}_dsin\theta_d-\dot{y}_dcos\theta_d=0$

---

未完待续