(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210899583.0 (22)申请日 2022.07.28 (71)申请人 浙江工业大 学 地址 310014 浙江省杭州市拱 墅区潮王路 18号 (72)发明人 刘安东　张华富　金哲豪　朱华中　 (74)专利代理 机构 杭州浙科专利事务所(普通 合伙) 33213 专利代理师 汤明 (51)Int.Cl. B25J 9/16(2006.01) G06T 7/13(2017.01) G06T 7/168(2017.01) G06T 7/60(2017.01) G06T 7/73(2017.01)G06T 7/80(2017.01) G06T 7/90(2017.01) (54)发明名称 一种基于目标检测和模型预测控制的板球 系统控制方法 (57)摘要 本发明公开了一种基于目标检测和模型预 测控制的板球系统控制方法， 属于计算机视觉处 理领域和控制领域； 本发明在霍夫圆检测的基础 上引入颜色检测， 提出了一种将颜色检测与霍夫 圆检测算法相结合的算法， 减少环 境信息对霍夫 圆检测算法的干扰。 并提出一种利用MPC算法来 根据小球位置求 解板子倾斜角度的方法。 权利要求书3页 说明书8页 附图2页 CN 115519540 A 2022.12.27 CN 115519540 A 1.一种基于目标检测和模型预测控制的板球系统控制方法， 其特征在于， 包括以下步 骤： 1)为了提高检测的精度， 将相机采集到的彩色图像进行颜色处理， 只采集和小球颜色 相同的图像， 再将图像进行中值滤波， 开操作、 闭操作一系列去操作处理， 再对其进行二值 化处理， 使其符合霍夫圆检测算法的输入图像要求； 2)将颜色检测与霍夫圆检测相结合检测到的小球的像素位置通过相机外参标定转到 机械臂坐标系下； 3)通过步骤2)得到的像素坐标在基坐标系下的坐标， 将板子固定在机械臂法兰末端， 分别将小球初始状态以及期望位置下的像素坐标转 化为机械臂基坐标系下的坐标； 4)通过步骤3)得到的初始位置和期望位置， 可以建立板球系统模型， 利用模型预测控 制算法设计一个三维角度关于位置的控制器； 5)步骤4)得出来的角度是板子的倾角， 板子和法兰固定在一起， 所以板子的倾角就是 机械臂末端法兰的倾斜角度， 法兰相对于机械臂基坐标系只做了平移变换， 并没有旋转变 换， 即板子相对于基坐标系没有旋转变换， 世界坐标系下的三维加速度为(0,0,9.8)T， 但是 机械臂的z轴是向上的， 所以机 械臂基坐标系下的三维加速度为g＝(0,0,9.8)T； 因为机械臂欧拉角转换为旋转矩阵形式是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)中解出的三维欧拉角转换为旋转矩阵， 为了让机械臂末端在初始位置旋转 来调整板子的倾角， 以此来调整小球的位置， 所以平移矩阵为初始状态下机械臂法兰相对 于基坐标系下的平 移矩阵， 以此作为机 械臂末端期望的齐次变换矩阵；权　利　要　求　书 1/3 页 2 CN 115519540 A 28)根据小球的实时位置和期望位置的偏差， 不断通过控制器求得调整板子所需要的欧 拉角， 通过机械臂调节末端法兰的倾角来调整小球在板子上 的位置， 直到小球到达期望的 位置。 2.根据权利要求1所述的一种基于目标检测和模型预测控制的板球系统控制方法， 其 特征在于， 所述 步骤4)控制器具体设计如下： 4.1)定义小球的初始位置和初始速度， 小球的期望位置和期望速度， 由此建立板球系 统模型： 其中， P0、 V0分别为小球初 始位置和初始速度； Pd、 Vd分别为小球的期望位置和期望速度； f( θk)为当前时刻小球的加速度， T为上一时刻与当前时刻的时间差； Pk、 Vk分别为当前时刻 的位置和速度， Pk+1、 Vk+1分别为下一时刻的位置和速度； 公式(2 ‑3)利用当前时刻的位置、 速 度以及加速度来计算下一时刻的位置和速度； 4.2)板球系统的控制问题转化为根据当前时刻小球的位置、 速度解算出最优角度的最 优化问题， 采用单步MPC预测方式， 计算最优控制量； 约束条件过多可能压缩了最优解空间 的大小， 导致在某一时刻规划问题找不到最优解， 也可能使搜索最优解的计算机程序花费 大量的时间， 在两次采样时间间隔内无法完成控制计算， 造成失控现象； 所以针对于板球系 统， 除了必 要的等式约束外， 只对板子的倾斜角度变量幅值上做约束， 防止板子倾斜角度过 大， 小球滚动太快， 直接脱离 板面； 目标函数: 约束条件: 权　利　要　求　书 2/3 页 3 CN 115519540 A 3