基于视觉反馈的板球系统定位控制

武凡凯， 付 伟， 张釜荣, 谢慕君*

(1.长春工业大学 电气与电子工程学院， 吉林 长春 130012; 2.吉林石化公司有机合成厂， 吉林 吉林 132000)

0 引 言

板球系统是一个多变量、非线性控制对象，是球杆系统[1]的二维扩展。其作为一个具有双输入、双输出的两自由度机械系统，经常作为经典模型用于检测各种控制理论，并对一些动态系统具有重要的研究意义。板球系统的控制对象是球盘，使其偏转来控制球盘上的小球，目的是为了得到小球在球盘坐标系中的坐标。文中主要是对板球系统做摄像机的标定，并将结果应用到实物中。板球系统如图1所示。

图1 板球系统示意图

1 板球系统物理建模

板球系统X轴和Y轴彼此垂直，为了更方便研究，我们把它当作是互相垂直方向上的两个球杆，即球杆系统的二维扩展。 球盘具有两个自由度，分别是绕X轴旋转和绕Y轴旋转，其与绕X轴转动的倾角α和绕Y轴转动的倾角β相对应。当球盘的倾斜角度很小时，可以获得α和β与X轴电机转角θm1和Y轴电机转角θm2之间相对应的关系：

式中:d1----电机轴与X轴连接杆下端点的距离；

d2----电机轴与Y轴连接杆下端点的距离；

L1----球盘中心支点与X轴连接杆上端点的距离；

L2----球盘中心支点与Y轴连接杆上端点的距离。

板球机械示意图如图2所示。

图2 板球机械示意图

对于本系统，d1=d2=23 mm，L1=L2=98 mm。

通过对图2系统进行动力学分析，使用拉格朗日方程，获得上述模型完整的非线性耦合的动力学方程：

X方向：

Y方向：

式中：mb----小球的质量；

rb----小球的半径；

Ib----小球绕质心的转动惯量；

h----球盘平面与其回转中心的距离；

α----球盘绕X轴旋转的角度；

β----球盘绕Y轴旋转的角度；

xb、yb----小球在球盘坐标系下的坐标。

考虑小球在球盘的工作点附近时，对式(2)、式(3)进行线性化处理，得到线性化方程：

(4)

2 摄像机标定

摄像机的标定[2-3]是获得摄像机的内外部参数，得到摄像机的几何模型和所获得图像信息与真实世界信息的几何成像关系[4]，是获取运动物体真实位置不可或缺的一步，也是视觉传感方式实现的前提条件。

文中采用线性模型标定方法对板球系统的摄像机进行标定，景物点世界坐标系(Xw，Yw，Zw)与其投影点图像坐标(u,v)的关系如下:

(5)

式中:ax=f/dx，ay=f/dy；

R----3×3旋转矩阵；

P[(Px,Py,Pz)T]----世界坐标系原点在摄像机坐标系中的位置；

M1----完全由ax，ay，u0，v0决定，由于ax，ay，u0，v0只与由摄像机内部结构有关，为摄像机内部参数；

M2----完全由摄像机相对于世界坐标系的方位决定，为摄像机外部参数；

M----3×4矩阵，称为投影矩阵。

将式(5)写成：

(6)

式中: (Xwi,Ywi，Zwi,1)----空间第i个点的坐标；

(ui,vi，1)----第i点的图像坐标;

mij----投影矩阵M的第i行j列元素，其中m34=PZ，PZ不为零[5]，M矩阵乘以一个除零以外的任意常数，其并不会使(Xw,Yw，Zw)与(u,v)的关系改变，所以可令m34=1。

展开式(6)，消去Zci，可得如下两个关于mij的线性方程：

m11Xwi+m12Ywi+m13Zwi+m14-uim31Xwi-uim32Ywi-uim33Zwi=uim34

m21Xwi+m22Ywi+m23Zwi+m24-vim31Xwi-vim32Ywi-vim33Zwi=vim34(7)

如果有n个已知点，它们的图像坐标(ui,vi),(i=1,2,…,n)与其世界坐标(Xwi,Ywi，Zwi),(i=1,2,…,n)是已知的，则可以得到关于矩阵元素的2n个线性方程。矩阵形式方程如下：

(8)

上述方程的未知数为11个，记做m，则式(8)可以化为：

Km=U(9)

式中:K----式(8)左边2n×11矩阵;

m----未知的11维向量;

U----式(8)右边的2n维向量。

当2n>11,即n≥6时，对上述线性方程用最小二乘法[6-8]求解。

设定球盘的三个位置如图3所示。

(a) 位置1(球盘水平)(b) 位置2(球盘与水平位置呈夹角α1(β1))(c) 位置3(球盘与水平位置呈夹角α2(β2))

图3 球盘的三个位置

当球盘处于位置2时，板球绕X轴旋转的角度α1与其绕Y轴旋转的角度β1的值为:α1=β1=arctan0.19。当球盘处于位置3时，其绕X轴旋转的角度α2与其绕Y轴旋转的角度β2的值为:α2=β2=-arctan0.19。

通过计算获得标定点的图像坐标和标定点在世界坐标系中的坐标，见表1。

表1 标定点坐标

选用3组18个标定点通过最小二乘法求得矩阵如下:

(10)

3 小球空间位置解算

计算出M矩阵的各个元素后，对于空间任意一点P实际坐标(Xw,Yw，Zw)，可以很容易由M矩阵求得其图像坐标(u,v)；然而却无法由图像坐标p(u,v)通过逆过程得到空间点P的实际坐标(Xw,Yw，Zw)。

在板球系统中，通过电机的转动能够得到球盘在任何时候绕X轴和绕Y轴的转动角度,通过三维空间旋转矩阵的计算，可得球盘的平面方程,如图4所示。

图4 板球转动示意图

直线OP与球盘平面方程的交点就是小球质心在空间坐标系中的坐标点。

3.1 球盘平面方程

球盘绕X轴、Y轴转动角度分别为α、β,球盘的旋转矩阵为:

(11)

根据旋转矩阵获得球盘平面方程为:

Xwsinβcosα-Ywsinα+Zwcosβcosα=0(12)

3.2 小球实际坐标计算

假设在图像坐标系中，小球的质心坐标是(u,v),通过M矩阵可得图像坐标系与世界坐标系之间的映射关系，其线性方程为:

(m11-m31u)Xw+(m12-m32u)Yw+(m13-m33u)Zc=um34-m14

(m21-m31v)Xw+(m22-m32v)Yw+(m33-m33v)Zc=vm34-m14(13)

联立球盘平面方程与上述线性方程所组成的线性方程组为:

Xwsinβcosα-Ywsinα+Zwcosβcosα=0

(m11-m31u)Xw+(m12-m32u)Yw+(m13-m33u)Zw=um34-m14

(m21-m31v)Xw+(m22-m32v)Yw+(m33-m33v)Zw=vm34-m14(14)

系统实际运行时，根据测得的α、β及通过视觉反馈获得的小球图像坐标(u,v)代入上述方程组，可计算出在世界坐标系下小球的实际坐标(Xw,Yw，Zw)。

4 系统控制结构及控制器设计

针对板球系统的小球定位控制，系统采用双闭环结构，板球系统的闭环控制系统结构如图5所示。

图5 板球闭环控制系统结构

由式(4)可以看出，板球坐标系中小球的X坐标值xb仅与β有关，Y轴坐标值yb仅与α有关，所以此系统就可以作为两个独立子系统进行同步控制。

外环进行小球定位闭环控制，采用视觉传感器进行小球的位置测量；内环进行球盘的倾角闭环控制，光电编码器进行角度测量。双环均采用PID控制器[9-10]。

设系统给定值r(t),实际输出值为c(t)，则误差e(t)=r(t)-c(t)，通过PID调节后的输出值u(t)为：

(15)

则传递函数为：

(16)

式中：Kp、Ti和Td----分别为PID控制器的比例系数、积分时间常数和微分时间常数。

5 实验及结论

通过上述摄像机标定及PID控制器设计后，在固高公司的板球实验系统上进行了多组定位实验，小球的定位误差均在1 mm以内，达到了预期的控制效果。选取三组实验数据见表2。

表2 小球实验数据

第一组实验对应的小球实时控制定位如图6所示。

由图6可知，“+”为正方向，小球初始位置(0,0)，预期位置(50,50)，X方向上误差为0.79 mm，Y方向上误差为0.93 mm，小球的实际位置(49.21,50.93)。