一种基于张量积模型变换的移动机器人控制方法与流程

本发明涉及一种基于张量积模型变换的移动机器人控制方法，属于移动机器人控制领域。

背景技术：

观察近几年国内外关于基于图像的移动机器人视觉伺服控制方法的特点，主要集中在对图像雅可比矩阵的在线估计。曾祥进，黄心汉等人运用 Broyden动态法在线估计雅可比矩阵，从而控制机器人运动。该方法具有自适应性，但当无法识别机器人运动参数或图像中出现较大偏差时，无法保证该视觉伺服控制算法的稳定性。高振东等人采用局部拟合的方法来估计图像雅可比矩阵，可得到更加准确的图像雅可比矩阵的值，并能补偿处理图像带来的时延。但这些研究工作都没有考虑到内在的约束，包括驱动器饱和、最大速度、以及一些机器人动力学的状态变量的约束。

因此需要一种能够实时在线应用于移动机器人这种快速变化的动态系统。为了解决这一问题，优化问题必须在系统规定采样时间内得以解决。

技术实现要素：

目的：为解决上述不足，本发明提供一种基于张量积模型变换的移动机器人控制方法，在图像的视觉伺服在图像特征点和离散的雅可比矩阵基础上设计一种控制方法，将张量积模型变换引入优化控制，并将此方法应用于移动机器人。

本发明的技术方案如下：

一种基于张量积模型变换的移动机器人控制方法，所述移动机器人包括一台网络摄像机、PC机、视觉传感器、预测控制器和双轮驱动机器人车体，所述移动机器人的具体控制步骤如下：

步骤1)：首先在PC机设定机器人初始位姿，并根据移动机器人运动模型以及摄像机参数矩阵获得具体视觉特征点在图像平面的期望特征数据；

步骤2):根据步骤1中所述的移动机器人运动模型以及摄像机小孔模型原理，通过坐标转换得到相应的图像雅可比矩阵，并将雅可比矩阵离散化以张量形式存储；

步骤3):将步骤2中离线所得张量执行高阶奇异值分解，提取图像雅可比矩阵的顶点张量以及各时变参数对应的权值函数，并对权值函数进行归一化，得到雅可比顶点矩阵；

步骤4)：所述网络摄像机将拍摄到的图像传输至PC机处理，得到当前图像特征点在摄像机图像平面的坐标信息；

步骤5)：根据步骤3中所得的雅可比顶点矩阵和步骤4中得到的当前图像特征点在摄像机图像平面的坐标信息，通过线性矩阵不等式求解凸最优问题，得到当前时刻系统最优控制输入vc(k)，将此控制输入vc(k)传给电机驱动移动机器人运动；

步骤6)：在下一采样时刻重复上述步骤1-5的过程，直到满足摄像机拍到的图像特征误差e(k)小于设定阈值。

优选地，所述步骤1中移动机器人运动模型如下：

其中，定义xR,yR分别为全局坐标空间中X,Y坐标轴上的坐标值，即机器人的位置信息，分别为机器人沿X,Y方向的速度分量,θR为机器人的朝向与X轴的夹角，即前进方向的角度，为角速度，νR、ωR分别为机器人的线速度和角速度，则机器人在世界坐标系下的姿态向量为[xR,yR,θR]^T。

优选地，步骤2中采用所述移动机器人运动模型以及摄像机小孔模型原理，推导出的图像雅可比矩阵为：

其中ky为已知放大系数，目标点在摄像机成像平面坐标值s＝(u,v)，u0,v0为成像平面中心点坐标值,摄像机笛卡儿坐标空间中的点^CP＝(xc,yc,zc)；

根据视觉伺服理论，得到机器人的视觉伺服系统的离散状态空间模型如下：

其中，Ts为采样时间，每一个采样k时刻图像特征误差为e(k)＝s(k)-sd，其中s(k)为步骤4中得到当前图像特征点在摄像机图像平面的坐标信息，即当前图像特征值，sd为期望的图像特征坐标,即步骤1中获得的数据，Js(k) 即为k时刻图像雅克比矩阵，vc(k)为控制输入包含机器人的线速度和角速度；再根据TP模型变换，将雅可比矩阵离散化以张量形式存储，其中，TP 模型变换具体步骤如下：

雅可比矩阵由时变参量q(k)＝{u(k),v(k),xc(k)}定义，其中，q(k)属于闭合超立方体中的元素，在Ω中放置N维超矩形网格

其中Mn对应于第n维参数空间的采样网格线的数量，an,bn分别是三个时变参量在各个维度的最小值和最大值，在各个网格点上对系统矩阵采样离散化：

优选地，步骤3中所述顶点矩阵如下：

其中wn,m(qn)为权值矩阵，且满足：

则根据上述顶点矩阵(6)和权值矩阵(7)，视觉伺服系统的离散状态空间模型(3)可转换为TP模型的形式：

其中Jer＝TsJsr，且满足如下系统约束：

优选地，所述步骤5中寻找最优系统控制输入vc(k)的具体步骤如下：

定义一个线性状态反馈vc(k+i)＝F(k)e(k+i)，其中，反馈控制律 F(k)＝YQ^-1，使得如下“准最坏情况”的无限时域目标函数最小：

其中，Qw＞0，Rw＞0为正定权值矩阵，因此，TP模型的视觉伺服系统 (8)通过线性矩阵不等式的凸最优问题求解，且最优解满足系统约束(9)：

最优目标函数满足如下线性不等式，其中γ为求解的线性目标函数，

|vcl(k)|≤vcl,max (14)，

||xj(k)+Je(qj(k))vc(k)+sd||2≤smax (15)，

其中Q为对称正定矩阵，l＝1,2,3表示系统控制输入的维数，符号*表示线性矩阵不等式的对称结构，j＝1,…,n表示特征点的数量， r＝1,...,M1×M2×M3表示TP顶点数量，Qw＞0，Rw＞0为正定权值矩阵， vcl,max,smax分别表示控制输入和步骤4中获得的图像特征值的上限，求得当前时刻的控制输入vc(k)。

本发明的有益效果：本发明提供一种基于张量积模型变换的移动机器人控制方法，可以将移动机器人从任意指定位姿驱动到期望视觉特征所在的位姿,与其他基于图像的视觉伺服控制方法不同，该方法将张量积模型变换原理引入到图像视觉伺服控制中，通过解线性矩阵不等式对摄像机视野及系统输入输出进行约束；与传统IBVS相比，避免直接求解图像雅可比矩阵的逆而无需考虑图像奇异问题；对比普通模型预测控制，在无限时域内执行，具有良好的实时性，保证稳定性的同时易于处理各个约束。

附图说明

图1为移动机器人模型示意图；

图2为控制系统框图。

具体实施方式

为了使本技术领域的人员更好地理解本申请中的技术方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。

如图1-2所示，一种基于张量积模型变换的移动机器人控制方法，所述移动机器人包括一台网络摄像机、PC机、视觉传感器、预测控制器和双轮驱动机器人车体，所述移动机器人的具体控制步骤如下：

步骤1)：首先在PC机设定机器人初始位姿，并根据移动机器人运动模型以及摄像机参数矩阵获得具体视觉特征点在图像平面的期望特征数据；

步骤2):根据步骤1中所述的移动机器人运动模型以及摄像机小孔模型原理，通过坐标转换得到相应的图像雅可比矩阵，并将雅可比矩阵离散化以张量形式存储；

步骤3):将步骤2中离线所得张量执行高阶奇异值分解，提取图像雅可比矩阵的顶点张量以及各时变参数对应的权值函数，并对权值函数进行归一化，得到雅可比顶点矩阵；

步骤4)：所述网络摄像机将拍摄到的图像传输至PC机处理，得到当前图像特征点在摄像机图像平面的坐标信息；

步骤5)：根据步骤3中所得的雅可比顶点矩阵和步骤4中得到的当前图像特征点在摄像机图像平面的坐标信息，通过线性矩阵不等式求解凸最优问题，得到当前时刻系统最优控制输入vc(k)，将此控制输入vc(k)传给电机驱动移动机器人运动；

步骤6)：在下一采样时刻重复上述步骤1-5的过程，直到满足摄像机拍到的图像特征误差e(k)小于设定阈值。

优选地，所述步骤1中移动机器人运动模型如下：

其中，定义xR,yR分别为全局坐标空间中X,Y坐标轴上的坐标值，即机器人的位置信息，分别为机器人沿X,Y方向的速度分量,θR为机器人的朝向与X轴的夹角，即前进方向的角度，为角速度，νR、ωR分别为机器人的线速度和角速度，则机器人在世界坐标系下的姿态向量为[xR,yR,θR]^T。

优选地，步骤2中采用所述移动机器人运动模型以及摄像机小孔模型原理，推导出的图像雅可比矩阵为：

其中ky为已知放大系数，目标点在摄像机成像平面坐标值s＝(u,v)，u0,v0为成像平面中心点坐标值,摄像机笛卡儿坐标空间中的点^CP＝(xc,yc,zc)；

根据视觉伺服理论，得到机器人的视觉伺服系统的离散状态空间模型如下：

其中，Ts为采样时间，每一个采样k时刻图像特征误差为e(k)＝s(k)-sd，其中s(k)为步骤4中得到当前图像特征点在摄像机图像平面的坐标信息，即当前图像特征值，sd为期望的图像特征坐标,即步骤1中获得的数据，Js(k) 即为k时刻图像雅克比矩阵，vc(k)为控制输入包含机器人的线速度和角速度；再根据TP模型变换，将雅可比矩阵离散化以张量形式存储，其中，TP 模型变换具体步骤如下：

雅可比矩阵由时变参量q(k)＝{u(k),v(k),xc(k)}定义，其中，q(k)属于闭合超立方体中的元素，在Ω中放置N维超矩形网格

其中Mn对应于第n维参数空间的采样网格线的数量，an,bn分别是三个时变参量在各个维度的最小值和最大值，在各个网格点上对系统矩阵采样离散化：

优选地，步骤3中所述顶点矩阵如下：

其中wn,m(qn)为权值矩阵，且满足：

则根据上述顶点矩阵(6)和权值矩阵(7)，视觉伺服系统的离散状态空间模型(3)可转换为TP模型的形式：

其中Jer＝TsJsr，且满足如下系统约束：

为设计控制器，即找到最优系统控制输入vc(k)，使得离散采样时间内，图像特征误差e(k)逐渐收敛到零。

优选地，所述步骤5中寻找最优系统控制输入vc(k)的具体步骤如下：

定义一个线性状态反馈vc(k+i)＝F(k)e(k+i)，其中，反馈控制律 F(k)＝YQ^-1，使得如下“准最坏情况”的无限时域目标函数最小：

其中，Qw＞0，Rw＞0为正定权值矩阵，因此，TP模型的视觉伺服系统 (8)通过线性矩阵不等式的凸最优问题求解，且最优解满足系统约束(9)：

最优目标函数满足如下线性不等式，其中γ为求解的线性目标函数，

|vcl(k)|≤vcl,max (14)，

||xj(k)+Je(qj(k))vc(k)+sd||2≤smax (15)，

其中Q为对称正定矩阵，l＝1,2,3表示系统控制输入的维数，符号*表示线性矩阵不等式的对称结构，j＝1,…,n表示特征点的数量， r＝1,...,M1×M2×M3表示TP顶点数量，Qw＞0，Rw＞0为正定权值矩阵， vcl,max,smax分别表示控制输入和步骤4中获得的图像特征值的上限，求得当前时刻的控制输入vc(k)。

实施例1：

如附图1所示，移动机器人运动模型如下：

定义机器人在世界坐标系下的姿态向量为[xR,yR,θR]^T，其中，xR,yR为全局坐标空间中X,Y坐标轴上的坐标值，即机器人的位置信息，θR为机器人的朝向与X轴的夹角，即前进方向的角度，νR、ωR分别为机器人的线速度和角速度。

已知机器人左右轮中心之间的间距为2r，机器人的运动通过左右轮差动控制，则相对于左右轮速的机器人运动模型推导为：

存在如下非完整性约束：

如图1所示，假设摄像机坐标系与机器人坐标系之间只存在平移变换，摄像机内参数矩阵为：

则摄像机笛卡儿坐标空间中的点^CP＝(xc,yc,zc)可通过如下关系式映射到摄像机成像平面s＝(u,v)：

已知移动机器人具有三个自由度，即沿x、y方向的平移运动和绕z轴的旋转运动，可推导出图像雅可比矩阵：

将安装在移动机器人上的摄像机初始位姿设定在[-8,9,0.3]，摄像机参数为：焦距0.002，像素大小，像素中心为[512,512]，分辨率为1024×1024，采样周期40ms，放大系数为200，选取四个目标特征点，其世界坐标为 P＝[8 8 8 8；1 -1 1 -1；2 2 4 4]，通过MATLAB机器人工具箱仿真移动机器人手眼系统的视觉定位任务，验证算法的有效性，该验证方法属于常规技术手段，故而本发明未加详述。

根据TP模型变换，图像雅可比矩阵(2)可转换为12个线性时不变顶点矩阵的凸组合形式，M1×M2×M3＝3×2×2，控制输入约束-1≤vc(k)≤1。分解得到的12个顶点矩阵如下：

安装在移动机器人上的摄像机将拍摄到的图像传回PC机处理，得到当前图像特征点在摄像机图像平面的坐标信息，再根据线性矩阵不等式(12) -(15)求解凸最优问题，得到当前时刻系统最优的控制输入，将此控制信号传给电机驱动移动机器人运动，再在下一采样时刻重复上述过程，直到满足摄像机拍到的图像特征误差小于设定阈值，此处阈值可根据实际需要进行设定。

本发明在步骤2中提及的摄像机小孔模型原理、步骤3中提及的雅可比顶点矩阵求解过程以及视觉伺服理论均属于本领域技术人员掌握的常规技术手段，故而未加详述。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。