一种基于视觉伺服的agv滑模控制方法
技术领域
1.本发明涉及工业机器人技术领域，具体涉及一种基于视觉伺服的agv滑模控制方法。
背景技术：2.agv的控制算法有经典的pid控制算法、自适应控制算法、智能控制算法、滑模变结构控制算法等。对于不同的控制要求我们需要选用最适合的控制算法。agv现在通用的引导方式有电磁引导、磁带引导、惯性引导、超声定位引导、激光引导、视觉导引等。在不同的应用场合需要不同的引导方式以满足工业的需求。
技术实现要素：3.本发明旨在提供一种基于视觉伺服的agv滑模控制算法，通过视觉伺服能够精确的获取agv的实时位置；通过滑模控制算法，能够使agv实现非常好的轨迹跟踪。
4.为达成上述目的，本发明提供如下技术方案：
5.一种基于视觉伺服的agv滑模控制方法，包括如下步骤：
6.步骤1)建立摄像机成像模型，通过摄像机获取特征点，并得到当前图像像素坐标,然后通过坐标变换得到世界坐标；
7.步骤2)对agv进行运动学建模，建立agv的运动学模型；
8.步骤3)根据所述运动学模型设计对应的滑模控制器；
9.步骤4)将步骤1)中获得的世界坐标加入到所设计的滑模控制器，使agv运动到目标点。
10.进一步地，所述步骤1)中坐标转换的过程为，首先从像素坐标系转换到图像物理坐标系，然后再从图像物理坐标系转换到摄像机坐标系，然后再从摄像机坐标系转换到世界坐标系。
11.进一步地，所述步骤1)中坐标转换的具体过程如下：
12.1.1)将图像像素坐标通过像素转换矩阵转换到图像物理坐标：
[0013][0014]
式中：[u v 1]
t
表示图像像素坐标系，[x y 1]
t
表示图像物理坐标系；dx、dy分别表示x方向和y方向的一个像素分别所占的单位长度数量；u0、v0表示图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数；
[0015]
1.2)将图像物理坐标通过焦距对角矩阵和畸变系数转换到摄像机坐标：
[0016][0017]
式中：[x
c y
c z
c
]
t
表示摄像机坐标系，[x y 1]
t
表示归一化后的图像物理坐标系；f表示焦距；
[0018]
1.3)对外参矩阵进行求解，其中r是旋转矩阵，t是平移向量；
[0019]
1.4)将摄像机坐标通过下式转换到世界坐标：
[0020][0021]
式中：[x
c y
c z
c
]
t
表示摄像机坐标系，[x
m y
m z
m
]
t
表示世界坐标系。
[0022]
进一步地，所述外参矩阵的求解过程为，通过对当前图像以及目标点的图像进行匹配，估计和优化两幅图像对应的数学映射关系，结合通过摄像机标定获取的摄像机内部参数，采用快速分解算法计算出摄像机的外参矩阵
[0023]
进一步地，所述外参矩阵的求解过程具体如下：
[0024]
1.3.1)射影单应矩阵g的估计与优化
[0025]
将当前图像以及目标点的图像之间特征点对应的像素p
i
，p
i*
通过矩阵g关联：
[0026]
p
i
＝λ
i
gp
i*
[0027]
式中：p
i
＝[1,u
i
,v
i
]
t
；p
i*
＝[1,u
i*
,v
i*
]
t
；像素坐标(u
i
,v
i
)和(u
i*
,v
i*
)分别表示当前图像与目标图像上的像素坐标；λ
i
表示静态特征点o
i
(i＝1,2,3
…
,n)在当前摄像机坐标系f和期望摄像机坐标系f
*
下的深度之比；
[0028]
整理后得到：
[0029][0030]
消除g
11
λ
i
之后得到：
[0031][0032]
利用上述两个线性约束方程，在相差一个比例因子的情况下，对四个以上特征点对应的像素坐标，采用最小二乘法对射影单应矩阵g中的8个未知变量进行估计，最终获得射影单应矩阵g；
[0033]
1.3.2)欧式单应矩阵h的估计
[0034]
定义3x3欧式单应矩阵：
[0035]
h＝a
‑1ga
[0036]
建立欧式单应矩阵h与移动机器人相对位姿参数的关系，表示为：
[0037][0038]
n
*
＝[n
x*
,n
y*
,n
z*
]
t
是摄像机期望坐标系下目标平面f
*
的单位法向量，其各项值是单位法向量n
*
在坐标系f
*
下各坐标轴的投影；θ为摄像机当前位姿与期望位姿之间的旋转角度；
[0039]
1.3.3)矩阵快速分解，求解出r和t
[0040]
对agv当前位置和期望位置之间的比例平移量t进行估计，t的具体表示如下：
[0041][0042]
欧式单应矩阵与移动机器人相对位姿参数之间的对应关系如下：
[0043][0044]
其中，h
ij
(i＝1,2,3；j＝1,2,3)表示欧式矩阵h的第i行，第j列的参数值，n为法向量；
[0045]
根据所述对应关系求解，求得θ，t的解，其中
[0046]
最终得到外参矩阵
[0047]
进一步地，所述步骤2)的建模过程如下：
[0048]
2.1)agv的位姿由向量q＝[x
m y
m
θ
m
]
t
表示，用v
m
和w
m
分别表示agv前进时整体的线速度和角速度；从而建立agv的运动学模型：
[0049][0050]
2.2)令q
r
＝[x
r y
r θ
r
]
t
为期望位姿的坐标，q
e
＝[x
e y
e θ
e
]
t
为位姿误差坐标，则agv的位姿误差方程为：
[0051][0052]
进一步地，所述步骤3)中滑模控制器的设计过程如下：
[0053]
3.1)设计切换函数如下：
[0054][0055]
其中s1，s2都趋向于0时，x
e
趋向于0，那么y
e
也趋向于0；其中s1、s2是所设计的切换
函数的两个量；
[0056]
3.2)采用趋近律与高增益相结合的方法设计滑模控制器：
[0057][0058]
其中(x
r
,y
r
,θ
r
)和(v
r
,ω
r
)分别表示agv的期望位姿和速度，β表示虚拟控制量，ε1，ε2>0,0<a<1,k1,k2>0，ε1、ε2、a、k1、k2都为常数。
[0059]
进一步地，所述步骤4)的具体过程如下：
[0060]
4.1)将步骤1)中的世界坐标(x
m
,y
m
,θ
m
)作为agv的实时坐标，与设定坐标(x
r
,y
r
,θ
r
)共同输入步骤3中的agv的轮式移动机器人位姿误差方程：
[0061][0062]
4.2)由agv位姿误差方程输出的(x
e
,y
e
,θ
e
)输入到步骤4中所设计的滑模控制器
[0063][0064]
4.3)滑模控制器的输出角速度与线速度输入到agv，通过运动分解，将v，ω转换到ω1和ω2，其中ω1和ω2分别表示左轮的角速度和右轮的角速度；
[0065]
4.4)agv通过步骤1)获取此时agv的世界坐标输入到步骤3中的误差方程，形成闭环控制。
[0066]
本发明与现有技术相对比，其有益效果在于：
[0067]
1、本发明采用了一种单应矩阵的位姿估计方法，通过对单应矩阵的快速分解，提取agv的相对位姿参数。它是一种从二维图像中获取三维姿态信息的过程，在通常情况下可取得唯一解，并且算法效率较高。
[0068]
2、本发明采用的滑模控制算法，相对于经典的pid算法，抗干扰能力更强，鲁棒性更好。
具体实施方式
[0069]
为了使本发明的目的、技术方案及优点更加清楚明白，下面通过实施例对本发明作进一步具体的说明。
[0070]
实施例：一种基于视觉伺服的agv滑模控制方法，具体步骤如下：
[0071]
步骤1)建立摄像机成像模型，通过agv上面的摄像机获取图片，得到特征点的实时像素坐标，通过对摄像机的标定以及坐标变换，获取agv的世界坐标；坐标转换是从像素坐标系转换到图像物理坐标系，然后再从图像物理坐标系转换到摄像机坐标系，然后再从摄
像机坐标系转换到世界坐标系；其中：世界坐标系是指物体在真实世界的坐标；摄像机坐标系是指以摄像机镜头的光心为摄像机坐标系的原点，以平行于图像的x和y方向为x
c
轴和y
c
轴，z
c
轴和光轴平行，x
c
，y
c
，z
c
互相垂直；图像物理坐标系以主轴和图像平面交点为坐标原点，x轴y轴方向分别平行于u轴和v轴；像素坐标系是指以所拍的相片左上角为顶点，水平方向为u轴，竖直方向为v轴。
[0072]
坐标变换的具体步骤为：
[0073]
1.1)将图像像素坐标通过像素转换矩阵转换到图像物理坐标：
[0074][0075]
式中：[u v 1]
t
表示图像像素坐标系，[x y 1]
t
表示图像物理坐标系；dx、dy分别表示x方向和y方向的一个像素分别所占的单位长度数量；u0、v0表示图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数。
[0076]
1.2)将图像物理坐标通过焦距对角矩阵和畸变系数转换到摄像机坐标：
[0077][0078]
式中：[x
c y
c z
c
]
t
表示摄像机坐标系，[x y 1]
t
表示归一化后的图像物理坐标系；f表示焦距。
[0079]
1.3)对外参矩阵进行求解，其中r是旋转矩阵，t是平移向量。
[0080]
其求解过程如下：
[0081]
1.3.1)射影单应矩阵g的估计与优化
[0082]
根据几何知识，射影单应矩阵g，表示摄像机在运动过程中，在不同位置拍摄的图像之间的一一映射关系。平面景物上的静态特征点o
i
(i＝1,2,3
…
,n)在移动机器人运动到当前坐标f与目标坐标f
*
的投影齐次像素坐标分别表示为p
i
＝[1,u
i
,v
i
]
t
，p
i*
＝[1,u
i*
,v
i*
]
t
，像素坐标(u
i
,v
i
)和(u
i*
,v
i*
)分别表示当前图像与目标图像上的像素坐标。根据图像间的几何知识将当前图像以及目标点的图像之间特征点对应的像素p
i
，p
i*
通过矩阵g关联：
[0083]
p
i
＝λ
i
gp
i*
[0084]
式中：λ
i
表示静态特征点o
i
在当前摄像机坐标系f和期望摄像机坐标系f
*
下的深度之比。
[0085]
整理后得到：
[0086][0087]
消除g
11
λ
i
之后得到：
[0088][0089]
利用上述两个线性约束方程，在相差一个比例因子的情况下，对四个以上特征点对应的像素坐标，采用最小二乘法对射影单应矩阵g中的8个未知变量进行估计，最终获得射影单应矩阵g。
[0090]
1.3.2)欧式单应矩阵h的估计
[0091]
射影单应矩阵g表示当前图像l与目标图像l
*
中特征点对应的齐次像素坐标p
i
和p
i*
之间的几何变换关系，它隐含着摄像机当前位姿与期望位姿之间的旋转角度，摄像机当前位置与期望位置之间的平移向量以及摄像机内部参数之间的相互关系。定义3x3欧式单应矩阵：
[0092]
h＝a
‑1ga
[0093]
建立欧式单应矩阵h与移动机器人相对位姿参数的关系，表示为：
[0094][0095]
n
*
＝[n
x*
,n
y*
,n
z*
]
t
是摄像机期望坐标系下目标平面f
*
的单位法向量，其各项值是单位法向量n
*
在坐标系f
*
下各坐标轴的投影；θ为摄像机当前位姿与期望位姿之间的旋转角度；由于放置目标景物所在的平面不垂直于agv的运动平面，所以单位法向量n
*
的n
z*
不为0，即n
z*
≠0；而d
*
表示坐标系f
*
的原点到目标平面的垂直距离。
[0096]
1.3.3)矩阵快速分解，求解出r和t
[0097]
对于欧式单应矩阵h的分解算法，实质上就是提取摄像机三维信息的过程。考虑到单目视觉系统深度信息未知，只能在相差一个比例因子的情况下对agv当前位置和期望位置之间的比例平移量t进行估计，t的具体表示如下：
[0098][0099]
欧式单应矩阵与移动机器人相对位姿参数之间的对应关系如下：
[0100][0101]
其中，h
ij
(i＝1,2,3；j＝1,2,3)表示欧式矩阵h的第i行，第j列的参数值，n为法向量；
[0102]
根据以上对应关系，可凭借如下情况进行讨论：
[0103]
当h
13
≠0,h
23
≠0时，对于这种情况，摄像机当前位置坐标和期望位置坐标之间的相对位姿参数θ，t和单位法向量n
*
均存在两组解。
[0104]
当h
13
＝0,h
23
≠0时，摄像机当前位置坐标和期望位置坐标之间的相对位姿参数θ，t和单位法向量n
*
也均存在两组解。
[0105]
当h
13
＝0,h
23
＝0时，摄像机当前位置坐标和期望位置坐标之间的相对位姿参数θ，t和单位法向量n
*
也均存在一组解。
[0106]
通过上述求解得出θ，t的解，其中
[0107]
最终求得外参矩阵
[0108]
1.4)将摄像机坐标通过外参矩阵转换到世界坐标。世界坐标系与摄像机坐标系进行转换的时候，先将其用齐次坐标来表示，左乘一个3x4的外参矩阵，得到世界坐标系与摄像机坐标系的关系式：
[0109][0110]
式中：[x
c y
c z
c
]
t
表示摄像机坐标系，[x
m y
m z
m
]
t
表示世界坐标系。
[0111]
将步骤1.3)求得的外参矩阵代入该式得到世界坐标。
[0112]
步骤2)对agv进行运动学建模，建立agv的运动学模型；建模过程如下：
[0113]
2.1)agv的位姿由向量q＝[x
m y
m θ
m
]
t
表示，用v
m
和w
m
分别表示agv前进时整体的线速度和角速度；从而建立agv的运动学模型：
[0114][0115]
2.2)令q
r
＝[x
r y
r θ
r
]
t
为期望位姿的坐标，q
e
＝[x
e y
e θ
e
]
t
为位姿误差坐标，则agv的位姿误差方程为：
[0116][0117]
步骤3)根据所述运动学模型设计对应的滑模控制器；滑模控制器的设计过程如下：
[0118]
3.1)设计切换函数如下：
[0119][0120]
其中s1，s2都趋向于0时，x
e
趋向于0，那么y
e
也趋向于0；其中s1、s2是所设计的切换函数的两个量；
[0121]
3.2)由于所设计的切换函数的切换动作造成控制不连续性，使得agv存在抖振；采用趋近律与高增益相结合的方法削弱系统在切换面附近的抖振，先采用趋近律方法，再结合高增益方法的连续化思想设计滑模控制器，所述设计滑模控制律为：
[0122]
[0123]
其中(x
r
,y
r
,θ
r
)和(v
r
,ω
r
)分别表示agv的期望位姿和速度，β表示虚拟控制量，ε1，ε2>0,0<a<1,k1,k2>0，ε1、ε2、a、k1、k2都为常数。
[0124]
步骤4)将步骤1)中获得的世界坐标加入到所设计的滑模控制器，使agv运动到目标点。具体过程如下：
[0125]
4.1)将步骤1)中的世界坐标(x
m
,y
m
,θ
m
)作为agv的实时坐标，与设定坐标(x
r
,y
r
,θ
r
)共同输入步骤3中的agv的轮式移动机器人位姿误差方程：
[0126][0127]
4.2)由agv位姿误差方程输出的(x
e
,y
e
,θ
e
)输入到步骤4中所设计的滑模控制器
[0128][0129]
4.3)滑模控制器的输出角速度与线速度输入到agv，通过运动分解，将v，ω转换到ω1和ω2，其中ω1和ω2分别表示左轮的角速度和右轮的角速度；
[0130]
4.4)agv通过步骤1)获取此时agv的世界坐标输入到步骤3中的误差方程，形成闭环控制。
[0131]
上述实施例只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人士能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所作的等效变化或修饰，都应涵盖在本发明的保护范围之内。
技术特征：1.一种基于视觉伺服的agv滑模控制方法，其特征在于，包括如下步骤：步骤1)建立摄像机成像模型，通过摄像机获取特征点，并得到当前图像像素坐标,然后通过坐标变换得到世界坐标；步骤2)对agv进行运动学建模，建立agv的运动学模型；步骤3)根据所述运动学模型设计对应的滑模控制器；步骤4)将步骤1)中获得的世界坐标加入到所设计的滑模控制器，使agv运动到目标点。2.根据权利要求1所述的一种基于视觉伺服的agv滑模控制方法，其特征在于，所述步骤1)中坐标转换的过程为，首先从像素坐标系转换到图像物理坐标系，然后再从图像物理坐标系转换到摄像机坐标系，然后再从摄像机坐标系转换到世界坐标系。3.根据权利要求2所述的一种基于视觉伺服的agv滑模控制方法，其特征在于，所述步骤1)中坐标转换的具体过程如下：1.1)将图像像素坐标通过像素转换矩阵转换到图像物理坐标：式中：[u v 1]
t
表示图像像素坐标系，[x y 1]
t
表示图像物理坐标系；dx、dy分别表示x方向和y方向的一个像素分别所占的单位长度数量；u0、v0表示图像的中心像素坐标和图像原点像素坐标之间相差的横向和纵向像素数；1.2)将图像物理坐标通过焦距对角矩阵和畸变系数转换到摄像机坐标：式中：[x
c y
c z
c
]
t
表示摄像机坐标系，[x y 1]
t
表示归一化后的图像物理坐标系；f表示焦距；1.3)对外参矩阵进行求解，其中r是旋转矩阵，t是平移向量；1.4)将摄像机坐标通过下式转换到世界坐标：式中：[x
c y
c z
c
]
t
表示摄像机坐标系，[x
m y
m z
m
]
t
表示世界坐标系。4.根据权利要求3所述的一种基于视觉伺服的agv滑模控制方法，其特征在于，所述外参矩阵的求解过程为，通过对当前图像以及目标点的图像进行匹配，估计和优化两幅图像对应的数学映射关系，结合通过摄像机标定获取的摄像机内部参数，采用快速分解
算法计算出摄像机的外参矩阵5.根据权利要求4所述的一种基于视觉伺服的agv滑模控制方法，其特征在于，所述外参矩阵的求解过程具体如下：1.3.1)射影单应矩阵g的估计与优化将当前图像以及目标点的图像之间特征点对应的像素p
i
，p
i*
通过矩阵g关联：p
i
＝λ
i
gp
i*
式中：p
i
＝[1,u
i
,v
i
]
t
；p
i*
＝[1,u
i*
,v
i*
]
t
；像素坐标(u
i
,v
i
)和(u
i*
,v
i*
)分别表示当前图像与目标图像上的像素坐标；λ
i
表示静态特征点o
i
(i＝1,2,3
…
,n)在当前摄像机坐标系f和期望摄像机坐标系f
*
下的深度之比；整理后得到：消除g
11
λ
i
之后得到：利用上述两个线性约束方程，在相差一个比例因子的情况下，对四个以上特征点对应的像素坐标，采用最小二乘法对射影单应矩阵g中的8个未知变量进行估计，最终获得射影单应矩阵g；1.3.2)欧式单应矩阵h的估计定义3x3欧式单应矩阵：h＝a
‑1ga建立欧式单应矩阵h与移动机器人相对位姿参数的关系，表示为：n
*
＝[n
x*
,n
y*
,n
z*
]
t
是摄像机期望坐标系下目标平面f
*
的单位法向量，其各项值是单位法向量n
*
在坐标系f
*
下各坐标轴的投影；θ为摄像机当前位姿与期望位姿之间的旋转角度；1.3.3)矩阵快速分解，求解出r和t对agv当前位置和期望位置之间的比例平移量t进行估计，t的具体表示如下：欧式单应矩阵与移动机器人相对位姿参数之间的对应关系如下：
其中，h
ij
(i＝1,2,3；j＝1,2,3)表示欧式矩阵h的第i行，第j列的参数值，n为法向量；根据所述对应关系求解，求得θ，t的解，其中最终得到外参矩阵6.根据权利要求1所述的一种基于视觉伺服的agv滑模控制方法，其特征在于，所述步骤2)的建模过程如下：2.1)agv的位姿由向量q＝[x
m y
m θ
m
]
t
表示，用v
m
和w
m
分别表示agv前进时整体的线速度和角速度；从而建立agv的运动学模型：2.2)令q
r
＝[x
r y
r θ
r
]
t
为期望位姿的坐标，q
e
＝[x
e y
e θ
e
]
t
为位姿误差坐标，则agv的位姿误差方程为：。7.根据权利要求6所述的一种基于视觉伺服的agv滑模控制方法，其特征在于，所述步骤3)中滑模控制器的设计过程如下：3.1)设计切换函数如下：其中s1，s2都趋向于0时，x
e
趋向于0，那么y
e
也趋向于0；其中s1、s2是所设计的切换函数的两个量；3.2)采用趋近律与高增益相结合的方法设计滑模控制器：其中(x
r
,y
r
,θ
r
)和(v
r
,ω
r
)分别表示agv的期望位姿和速度，β表示虚拟控制量，ε1，ε2>0,0<a<1,k1,k2>0，ε1、ε2、a、k1、k2都为常数。8.根据权利要求7所述的一种基于视觉伺服的agv滑模控制方法，其特征在于，所述步骤4)的具体过程如下：4.1)将步骤1)中的世界坐标作为agv的实时坐标，与设定坐标共同输入步骤3中的agv的轮式移动机器人位姿误差方程：
4.2)由agv位姿误差方程输出的(x
e
,y
e
,θ
e
)输入到步骤4中所设计的滑模控制器4.3)滑模控制器的输出角速度与线速度输入到agv，通过运动分解，将v，ω转换到ω1和ω2，其中ω1和ω2分别表示左轮的角速度和右轮的角速度；4.4)agv通过步骤1)获取此时agv的世界坐标输入到步骤3中的误差方程，形成闭环控制。
技术总结本发明公开了一种基于视觉伺服的AGV滑模控制方法，包括如下步骤：步骤1)建立摄像机成像模型，通过摄像机获取特征点，并得到当前图像像素坐标,然后通过坐标变换得到世界坐标；步骤2)对AGV进行运动学建模，建立AGV的运动学模型；步骤3)根据所述运动学模型设计对应的滑模控制器；步骤4)将步骤1)中获得的世界坐标加入到所设计的滑模控制器，使AGV运动到目标点。本发明通过视觉伺服能够精确的获取AGV的实时位置；通过滑模控制算法，能够使AGV实现良好的轨迹跟踪。轨迹跟踪。
技术研发人员：邢科新 林叶贵 李星宏
受保护的技术使用者：绍兴敏动科技有限公司
技术研发日：2021.04.07
技术公布日：2021/6/29
