介绍

一个简单的应用背景基于图像中的位置误差，设计合理的云台关节速度，控制摄像头使得目标保持在视野中央。

模型

一个双轮驱动的小车上面搭载一个两自由度(pan+tilt)的摄像机云台，摄像机固定在云台上。坐标系设定如右图所示。c--摄像头坐标系，b--tilt坐标系，a--pan坐标系，r--以双驱动轮轴线中点为原点的小车坐标系。

求解

基于图像的伺服控制，首先要求解图像坐标系下空间点的运动和关节运动速度之间的关系。

$\begin{bmatrix} ^{c}{\dot{x}}\\ ^{c}{\dot{y}}\\^{c}{ \dot{z}} \end{bmatrix}=F({^{r}{v}}_{c} ,\omega_r,\omega_\alpha ,\omega_\beta)$

对于静态目标有以下等式成立

$^{c}{\dot{P}_t}=-^{c}{v_c}-^{c}{w_c}\times ^{c}{P_t}$

$^{c}{v_c}=^{c}{R_r}^{r}{v_c}=^{c}{R_r}\begin{bmatrix} V_r\\ 0\\ 0 \end{bmatrix}$

$^{c}{\omega _c}=^{c}{R_r}\begin{bmatrix} 0\\ 0\\ \omega_r+\omega_\alpha \end{bmatrix}+^{c}{R_b}\begin{bmatrix} \omega_\beta\\ 0\\ 0 \end{bmatrix}$

$^{a}{R_r}=^{b}{R_r}=R(Y,-\alpha)R(X,-90^{\circ})R(Z,-90^{\circ})=\begin{bmatrix} -\sin \alpha &\cos\alpha &0 \\ 0& 0 &1 \\ \cos\alpha&\sin\alpha &0 \end{bmatrix}$

（右乘联体，左乘基）

$^{c}{R_a}=^{c}{R_b}=R(X,-\beta)=\begin{bmatrix} 1 &0 &0 \\ 0&\cos\beta &\sin\beta \\ 0& -\sin\beta& \cos\beta \end{bmatrix}$

$^{c}{R_r}=^{c}{R_a}^{a}{R_r}=\begin{bmatrix} -\sin\alpha & \cos\alpha &0 \\ \cos\alpha\sin\beta&\sin\alpha\sin\beta &\cos\beta \\ \cos\alpha\cos\beta& \sin\alpha\cos\beta &-\sin\beta \end{bmatrix}$

$^{c}{\dot{P}_t}=\begin{bmatrix} ^{c}{\dot{x}}\\ ^{c}{\dot{y}}\\ ^{c}{\dot{z}} \end{bmatrix}=\begin{bmatrix} V_r\sin\alpha-(\omega_\alpha+\omega_r)\cos\beta^c{z}-(\omega_\alpha+\omega_r)\sin\beta^c{y}\\ -V_r\cos\alpha\sin\beta+(\omega_\alpha+\omega_r)\sin\beta^c{x}+\omega_\beta{^c{z}}\\ -V_r\cos\alpha\cos\beta+(\omega_\alpha+\omega_r)\cos\beta^c{x}-\omega_\beta{^c{y}} \end{bmatrix}$

摄像机内参模型：

$^{c}{z}\begin{bmatrix} u\\ v\\ 1 \end{bmatrix}=\begin{bmatrix} \alpha_{x}& 0& u_0\\ 0&\alpha_{y} &v_0 \\ 0&0 &1 \end{bmatrix}\begin{bmatrix} ^{c}{x}\\ ^{c}{y}\\ ^{c}{z} \end{bmatrix}=M\begin{bmatrix} ^{c}{x}\\ ^{c}{y}\\ ^{c}{z} \end{bmatrix}\Rightarrow \left\{\begin{matrix} u={\alpha_x}\frac{^c{x}}{^c{z}}+u_0\\ v=\alpha_y\frac{^c{y}}{^c{z}}+v_0 \end{matrix}\right.$

图像中的误差

$\left\{\begin{matrix} e_u=u-u_0\\ e_v=v-v_0 \end{matrix}\right.$

误差等式两边求导

$\begin{align*} \dot{e}_u&=\frac{\partial }{\partial t}(u-u_0)=\dot{u}=\alpha_x\cdot \frac{\partial }{\partial t}(\frac{^c{x}}{^c{z}})=\alpha_x \cdot {\frac{^c{\dot{x}^c{z}-^c{x}^c{\dot{z}}}}{^c{z}^2}}\\ &=\alpha_x\cdot\left \{ \frac{1}{^c{z}} \left [ V_r\sin\alpha-(\omega_\alpha+\omega_r)\cos\beta^c{z}-(\omega_\alpha+\omega_r)\sin\beta^c{y} \right ]-\frac{^c{x}}{^c{z^2}}\left [ -V_r\cos\alpha\cos\beta+(\omega_\alpha+\omega_r)\cos\beta^c{x}-\omega_\beta{^c{y}} \right ]\right \} \\ &=\alpha_x\left \{ V_r\left(\frac{1}{^c{z}} \sin\alpha+\frac{^c{x}}{^c{z^2}}\cos\alpha\cos\beta\right )+(\omega_\alpha+\omega_r)\left [ -\cos\beta-\frac{^c{y}}{^c{z}} \sin\beta -\frac{^c{x^2}}{^c{z^2}}\cos\beta \right ]+\omega_\beta\frac{^c{x}^c{y}}{^c{z^2}} \right \} \end{align*}$

$\left\{\begin{matrix} \frac{^c{x}}{^c{z}}=\frac{1}{\alpha_x}\left( u-u_0\right )=\frac{e_u}{\alpha_x}\\ \frac{^c{y}}{^c{z}}=\frac{1}{\alpha_y}\left( v-v_0\right )=\frac{e_v}{\alpha_y} \end{matrix}\right.$

$\begin{align*}\dot{e}_u= V_r\left[ \frac{1}{^c{z}}\left( \alpha_x\sin\alpha+e_u\cos\alpha\cos\beta\right )\right ]+(\omega_\alpha+\omega_r)\left [ -\alpha_x\cos\beta-\frac{\alpha_x}{\alpha_y}e_v\sin\beta -\frac{1}{\alpha_x}e_u^2\cos\beta \right ]+\omega_\beta{\frac{1}{\alpha_y}e_ue_v} \end{align*}$

同理可以求得

$\begin{align*} \dot{e}_v = V_r\left[\frac{1}{^c{z}}\left(-\alpha_y\cos\alpha\sin\beta+e_v\cos\alpha\cos\beta \right ) \right ]+\left(\omega_\alpha+\omega_r \right )\left[\frac{\alpha_y}{\alpha_x}\sin\beta{e_u}-\frac{1}{\alpha_x}e_ue_v\cos\beta \right ]+\omega_\beta\left[\alpha_y+\frac{1}{\alpha_y}e_v^2 \right ]\end{align*}$

其中除 $^c{z}$ 以外其他变量都可以通过传感器和图像处理求得，为了解决对 $^c{z}$ 估计，在目标静止情况下有 $^b{y}$ 不变

$\begin{bmatrix} ^b{x}\\ ^b{y}\\ ^b{z} \end{bmatrix}=^b{R_c}\begin{bmatrix} ^c{x}\\ ^c{y}\\ ^c{z} \end{bmatrix}=\begin{bmatrix} 1 & 0 &0 \\ 0& \cos\beta &-\sin\beta \\ 0& \sin\beta&\cos\beta \end{bmatrix}\begin{bmatrix} ^c{x}\\ ^c{y}\\ ^c{z} \end{bmatrix}$

$\frac{1}{^c{z}}=\frac{e_v\cos\beta-\alpha_y\sin\beta}{\alpha_y}\cdot \frac{1}{^b{y}}$

虽然 $^b{y}$ 也是未知的，但是由于其不变，可以进行动态的估计

代入上式可以得到

$\left\{\begin{matrix} \dot{e}_u=\lambda \Omega _1 -A\left(\omega_\alpha+\omega_r \right )+B\omega_\beta\\ \dot{e}_v=\lambda \Omega_2+C\left(\omega_\alpha+\omega_r \right )+D\omega_\beta \end{matrix}\right.$

其中

$\lambda=\frac{1}{^ b{y}}$

$\Omega _1 =\frac{V_r}{\alpha_y}\left(e_v\cos\alpha\cos\beta+\alpha_x\sin\alpha \right )\left(e_v\cos\beta-\alpha_y\sin\beta \right )$

$\Omega _2 =\frac{V_r}{\alpha_y}\left(e_v\cos\alpha\cos\beta-\alpha_y\cos\alpha\sin\beta \right )\left(e_v\cos\beta-\alpha_y\sin\beta \right )$

$A=\frac{\alpha_x^2\alpha_y\cos\beta+\alpha_x^2\sin\beta{e_v}+e_u^2\alpha_y\cos_\beta}{\alpha_x\alpha_y}$

$B=\frac{e_ue_v}{\alpha_y}$

$C=\frac{\alpha_y\sin\beta e_u-e_ue_v\cos\beta}{\alpha_x}$

$D= \frac{\alpha_y^2+e_v^2}{\alpha_y}$

令 $\dot{e}=-Ke$ 设计自适应控制器

$\omega_\alpha = -\omega_r+\frac{K_1De_u+\hat{\lambda }D\Omega_1-K_2Be_v-\hat{\lambda}B\Omega_2}{AD+BC}$

$\omega_\beta=-\frac{K_2Ae_v+\hat{\lambda}A\Omega_2+K_1Ce_u+\hat{\lambda}C\Omega_1}{AD+BC}$

$\hat{\lambda}$ 是对 $\lambda$ 的估计，设计如下

$\hat{\lambda}=r(\Omega_1e_u+\Omega_2e_v)$

基于图像的视觉伺服-详细推导

介绍

模型

求解

猜你喜欢