1.一种基于目标检测和模型预测控制的板球系统控制方法,其特征在于,包括以下步骤:
1)为了提高检测的精度,将相机采集到的彩色图像进行颜色处理,只采集和小球颜色相同的图像,再将图像进行中值滤波,开操作、闭操作一系列去操作处理,再对其进行二值化处理,使其符合霍夫圆检测算法的输入图像要求;
2)将颜色检测与霍夫圆检测相结合检测到的小球的像素位置通过相机外参标定转到机械臂坐标系下;
3)通过步骤2)得到的像素坐标在基坐标系下的坐标,将板子固定在机械臂法兰末端,分别将小球初始状态以及期望位置下的像素坐标转化为机械臂基坐标系下的坐标;
4)通过步骤3)得到的初始位置和期望位置,可以建立板球系统模型,利用模型预测控制算法设计一个三维角度关于位置的控制器;
所述步骤4)控制器具体设计如下:
4.1)定义小球的初始位置和初始速度,小球的期望位置和期望速度,由此建立板球系统模型:其中,P0、V0分别为小球初始位置和初始速度;Pd、Vd分别为小球的期望位置和期望速度;
f(θk)为当前时刻小球的加速度,T为上一时刻与当前时刻的时间差;Pk、Vk分别为当前时刻的位置和速度,Pk+1、Vk+1分别为下一时刻的位置和速度;公式(2‑3)利用当前时刻的位置、速度以及加速度来计算下一时刻的位置和速度;
4.2)板球系统的控制问题转化为根据当前时刻小球的位置、速度解算出最优角度的最优化问题,采用单步MPC预测方式,计算最优控制量;约束条件过多可能压缩了最优解空间的大小,导致在某一时刻规划问题找不到最优解,也可能使搜索最优解的计算机程序花费大量的时间,在两次采样时间间隔内无法完成控制计算,造成失控现象;所以针对于板球系统,除了必要的等式约束外,只对板子的倾斜角度变量幅值上做约束,防止板子倾斜角度过大,小球滚动太快,直接脱离板面;
目标函数:
约束条件:
其中,正定矩阵Qp、Qv、R自定义为
θ为板子的倾斜角度,θx、θy分别是板子在x轴、y轴方向上的倾斜角度;
5)步骤4)得出来的角度是板子的倾角,板子和法兰固定在一起,所以板子的倾角就是机械臂末端法兰的倾斜角度,法兰相对于机械臂基坐标系只做了平移变换,并没有旋转变T换,即板子相对于基坐标系没有旋转变换,世界坐标系下的三维加速度为(0,0,9.8),但是T机械臂的z轴是向上的,所以机械臂基坐标系下的三维加速度为g=(0,0,9.8) ;
因为机械臂欧拉角转换为旋转矩阵形式是Z‑Y‑X形式,即旋转顺序为第一次先绕z轴旋转γ角度,第二次绕y轴旋转β角度,第三次绕x轴旋转α角度,所以三维欧拉角转换为基坐标系下的旋转矩阵为:Rzyx=Rz*Ry*Rx (1‑4)所以,小球的加速度与角度的关系为a=Rzyx*g,z轴方向的加速度不影响小球的运动,所以取x和y轴方向上的加速度即可;
6)根据小球的初始位置P0和初始速度V0,期望位置Pd和期望速度Vd,由步骤5)欧拉角和旋转矩阵的关系,求得欧拉角和加速度的关系,再由步骤4)中的目标函数可以得到最优三维欧拉角;
7)将步骤6)中解出的三维欧拉角转换为旋转矩阵,为了让机械臂末端在初始位置旋转来调整板子的倾角,以此来调整小球的位置,所以平移矩阵为初始状态下机械臂法兰相对于基坐标系下的平移矩阵,以此作为机械臂末端期望的齐次变换矩阵;
8)根据小球的实时位置和期望位置的偏差,不断通过控制器求得调整板子所需要的欧拉角,通过机械臂调节末端法兰的倾角来调整小球在板子上的位置,直到小球到达期望的位置。
2.根据权利要求1所述的一种基于目标检测和模型预测控制的板球系统控制方法,其特征在于,所述步骤7)中,将步骤6)中解出的三维欧拉角转换为旋转矩阵,为了让机械臂末端在初始位置旋转来调整板子的倾角,以此来调整小球的位置,所以平移矩阵为初始状态下机械臂法兰相对于基坐标系下的平移矩阵,将旋转矩阵与平移矩阵相合以此作为机械臂末端法兰期望的齐次变换矩阵;目的是让当前机械臂末端法兰的齐次变换矩阵跟随期望的末端法兰齐次变换矩阵而变化,步骤8)中根据小球的实时位置和期望位置的偏差,不断通过控制器求得调整板子所需要的欧拉角,这样就能够调整板子的倾角,继而调整小球的位置。