板球控制系统制作
本文主要介绍板球系统的设计过程,尤其是程序设计。3.2.1节和附录三讲述了板球系统的调试方法。
1 板球系统简介
板球系统的基本模型是在一个平台上放置一个小球,以小球在平台上的位置作为反馈信息,通过调整平台的倾斜程度来控制小球的运动轨迹。通常板球系统通过处理摄像头采集的图像,分析出小球的位置及运动状态,并将这些信息反馈给主控,从而实现系统感知。自动控制算法是在机器视觉的基础上实现的,主要用于维持系统平衡。
2 机械结构及工作原理
板球系统工作做原理如图下:
其中:
核心控制单元:山外科技 MK60DN512ZVLQ10单片机核心板
运动控制单元:S-D5舵机
图像采集单元:鹰眼摄像头
平 台 : 300 * 300 * 3mm的哑光黑色亚克力板
小 球 : 1.8cm白色硬质小球
机械结构如下图(引用网上图片):
板球系统的机械结构设计基本大同小异。本文采用硬质氧化锆小球作为被控对象,采用一块300 * 300 * 3mm的普通哑光黑色亚克力板作为平台,采用摄像头作为图像采集单元,采用舵机作为运动控制单元。板球系统的机械结构模型为:在平台下方的几何中心固定一个万向节,并用连接杆将平台支撑起来,将两个舵机分别放在平台下方相邻两边的中间位置,用连接杆将平台与舵机连接起来,最后将摄像头固定在平台上方。核心控制单元是由MK60单片机及其外围电路组成的,所有单元之间的信号传输依靠连接线。
关于板球系统的机械结构设计,大家可以参考一下这篇博客:https://blog.csdn.net/automan05/article/details/84501990
3 程序控制流程
3.1 寻找小球位置
3.1.1 利用特征检测圆(本文最终采用的方式)
利用特征检测圆的思想较为简单,该方式在检测圆之前首先要对图像做二值化处理。主要检测步骤如下:
1、检测物体宽度
2、检测物体高度
3、判断物体高度与宽度是否满足小球半径要求
4、检测物体是否为正方形,不为正方形则默认物体是圆形
该方式类似于模型匹配,检测的示意图如图3.6所示
其中I(i,j)表示第i行第j列图像像素点,P(x,y)点为圆心,n为物体宽度,h为物体高度。当检测到白点时,程序开始计算从该点开始连续白点的个数n,若n满足小球半径范围,则推算物体j 轴中点:
再从点(x,j_1)处开始,计算i轴方向连续白点的个数h,并记录底端坐标I2(i_2,j_2),若h满足小球半径范围,则推算物体i轴的中点:
此时已经得到了物体的中心位置P(x,y)。最后通过判断点P1,P2,P3,P4周围是否存在白点,进行圆形检测。其中:
若四点周围无白点,则认定为圆。该过程中若有一个条件不满足都会认定该物体不为圆,并进行下一格像素点检测。该方式检测效率较高,但抗干扰能力弱,可能出现错误识别的情况,可以通过一些参数设置减少错误识别的情况。
3.1.2 霍夫变换寻找圆
霍夫变换检测圆是目前机器视觉中较为流行的一种检测方式,具有较好的抗噪声能力,且能一次检测多个圆。 在进行霍夫变换前一般先要将图片进行二值化处理,并利用边缘检测算子将图片中的对象轮廓提取出来,霍夫变换只检测对象的轮廓。
关于霍夫变换的原理网上有很多介绍,本文不再累述。在使用霍夫变换检测圆时,参数空间已经变成了3个维度,计算量大。当然也可以采用霍夫梯度法,将参数空间降为2维,但计算依旧巨大。最终本文没有采用霍夫变换的来检测小球,只是用matlab进行了一下方案可行性验证,且没有对霍夫梯度法进行深入研究。matlab检测效果如下:
霍夫变换检测直线应用示例:
霍夫变换检测圆的效果图:
3.2 PID控制
3.2.1 PID介绍
网上已经有很多关于介绍PID及其调试方法的文章了,本节只是简单描述一下,帮助理解板球系统模型。PID的离散形式如下:
其中Kp,Ki,Kd都为常数,k为采样周期的序号,e(k)为期望值与被控对象实际输出值的差(也就是PID控制算法的输入)。从式中可以看出比例项是对偏差的线性放大,可以快速调节系统偏差,积分项可以累积系统偏差,主要用于消除系统静差,微分项用于计算系统偏差的变化率,具有预测作用可以使系统响应更为平滑。
从图2.2可以看出,平台的倾斜角度是由X轴、Y轴方向的两个舵机控制。因此程序中应该有X、Y两个方向的PID,分别控制X轴、Y轴两个舵机的输出。X轴PID函数的输入是小球在X轴方向上的偏差,输出是X轴舵机的PWM值;Y轴PID函数的输入是小球在Y轴方向上的偏差,输出是Y轴舵机的PWM值;
最后是关于系统静差。 在水桶模型(一个理解PID的经典模型)中系统静差表现为水桶一直装不满水(因为水桶会漏水),那板球系统的系统静差表现为什么呢?个人认为是小球始终到不了目标位置。通过受力分析可以知道小球是受到摩擦力的(当然除了摩擦力外还有平台不平整等因素),也就是说,在不使用积分项的情况下,即使平台是倾斜的,小球也可能处于相对静止状态,一旦小球静止,那么小球就一直到不了目标位置。但实际上板球系统中小球受到的摩擦力是很小的,只要稍微加大P项,系统静差就可以忽略不计了。在调试板球系统遇到系统静差的情况很少,更多的是遇到加大P项导致系统震荡的问题(小球在目标位置附近来回滚动,就是停不下来)。因此本文在调试时是先调P项,在调D项,最后调试I项。
3.2.2 串级PID控制(位置环PID-速度环PID)
串级PID通过增加系统的控制量,从而增加了系统的稳定性,因此采用串级PID控制更为合适,串级PID控制流程如下图所示:
采用了位置-速度PID控制模型,其中速度环为内环,位置环为外环。由于摄像头与平台始终保持正对(若不能保证平台运动时始终保持正对,则只需要保证平台摆动幅度不是太大即可。若摆动幅度也大,则需要考虑图像校正了,例如:在平台四个角做标记,在图像处理时识别出这些标记,在通过比较标记的图像距离和实际距离,粗略的校正图像),因此图像坐标系与平台的坐标系可以一致,小球在图像中的位置可以直接认为就是小球在平台中的实际位置。一定时间内,小球的运动距离s与运动时间t之比为小球在该时间段内的的平均速度v,其中通过定时器控制t为150ms,由于时间较短因此可以近似认为小球在t时间段内为匀速直线运动。
积分分离是指当小球的实际位置与设定位置距离较近时,积分项才开始累加,一旦小球的实际位置与设定位置距离较远,积分项将清零。积分过饱和处理是指限定积分项的累加和,防止累加和过大。这两项处理很好的解决了积分项容易失控的问题。
4 小结
经过测试,小球的坐标计算准确。小球在平台上保持稳定且能够沿着固定轨迹运动,板球系统取得了良好的控制效果。
5 参考资料
1 、https://blog.csdn.net/automan05/article/details/84501990
6 附录
由于代码太多,因此附录只粘了部分重要的程序。
附录一:板球系统主程序:
#include "common.h"
#include "include.h"
#define FREQ 50 //SD-5舵机频率(Hz)
//X轴舵机幅值(FTM0_CH3_PIN )
//---------------------------------------------------xxxHz
#define PWM_X_MAX 910 //
#define PWM_X_MIN 630 //
#define PWM_X_MID 770 //
//Y轴舵机幅值(FTM0_CH2_PIN )
//注意:由于机械结构原因,Y轴的最大值是向下边打角
//而最小值是向上打角
//---------------------------------------------------xxxHz
#define PWM_Y_MIN 647 //
#define PWM_Y_MAX 927 //
#define PWM_Y_MID 787 //
uint32 xSteerDuty = PWM_X_MID; //x轴pwm
uint32 ySteerDuty = PWM_Y_MID; //x轴pwm
uint8 imgbuff[CAMERA_H][CAMERA_W/8]; //定义存储接收图像的数组(压缩)
uint8 img_finish[newLCD_H][newLCD_W/8]; //裁剪后的图像大小
uint8 searchFlag = 0; //寻找小球的结果:0-没找到 1-找到
Site_t ballSite = {newLCD_W/2,newLCD_H/2}; //小球的位置
speed ballSpeed = {0,0}; //小球的速度
enum ballTask{ //小球任务表
point,
line,
square,
circular
};
const Site_t ballMotionLine[2]={{15,56},{88,56}};
const Site_t ballMotionSquare[4]={{15,20},{15,92},{88,92},{88,20}};
//const Site_t ballMotionCircular;
//pid初始化
double kp = 0.22, ki = 0.00 , kd = 0.35;// //位置pid初始参数 0.22 0.0 0.35(目前最合理的参数)
double skp = 4.30, ski = 0.0, skd = 2.20;// //速度pid初始参数 4.3 0.0 2.2 (目前最合理的参数)
Pid xSpeedPid;
Pid ySpeedPid;
Pid xSitePid;
Pid ySitePid;
int32 tickTock = 0; //滴答计数器,为上层提供时间,每次滴答的基准时间为PIT1的定时时间,如有其它需求须自行更改
/*
* @brief:计算小球速度
* note:速度计算是利用距离除以时间
* 该函数在pit1中断中运行
*/
void calculateSpeed(void){
int32 time = 1; //这里假设每次时间一样
static Site_t lastSite = {newLCD_H/2,newLCD_W/2};
static uint8 lastSearchFlag = 0;
if(searchFlag != lastSearchFlag){
lastSite.x = ballSite.x;
lastSite.y = ballSite.y;
}
ballSpeed.x = (ballSite.x-lastSite.x)/time;
ballSpeed.y = (ballSite.y-lastSite.y)/time;
lastSite.x = ballSite.x;
lastSite.y = ballSite.y;
lastSearchFlag = searchFlag;
}
/*!
* @brief main函数
* @since
* @note
*/
void main(){
Site_t lcdSite={0,0}; //LCD开窗位置
Size_t lcdSize={newLCD_W, newLCD_H}; //LCD显示区域图像大小
int32 xPID = 0,yPID = 0 ; //x,y轴串级pid输出值
int32 lastTick = 0; //上次时间数
int32 count = 0; //控制轨迹切换
enum ballTask myTask = square; //小球轨迹
double xkimax = 30, ykimax = 30; //积分极限值
DisableInterrupts; //关总中断
//配置 K60 的优先级
NVIC_SetPriorityGrouping(4); //设置优先级分组,4bit 抢占优先级,没有亚优先级
NVIC_SetPriority(PORTA_IRQn,0); //配置优先级(摄像头)
NVIC_SetPriority(DMA0_IRQn,1); //配置优先级(图像传输)
NVIC_SetPriority(PIT0_IRQn,2); //配置优先级(舵机)
NVIC_SetPriority(PIT1_IRQn,3); //配置优先级(测速)
//设置中断服务函数
set_vector_handler(PORTA_VECTORn , PORTA_IRQHandler); //设置 PORTA 的中断服务函数为 PORTA_IRQHandler
set_vector_handler(DMA0_VECTORn , DMA0_IRQHandler); //设置 DMA0 的中断服务函数为 PORTA_IRQHandler
set_vector_handler(PIT0_VECTORn ,PIT0_IRQHandler); //设置PIT0的中断服务函数为 PIT0_IRQHandler
set_vector_handler(PIT1_VECTORn ,PIT1_IRQHandler); //设置PIT1的中断服务函数为 PIT1_IRQHandler
//环境初始化
led_init(LED3);
pit_init_ms(PIT0,20); //初始化PIT0
pit_init_ms(PIT1,150); //初始化PIT1
camera_init(&imgbuff[0][0]); //摄像头初始化
LCD_init(); //LCD初始化
ftm_pwm_init(FTM0, FTM_CH2,FREQ,PWM_Y_MID); //y轴舵机初始化
ftm_pwm_init(FTM0, FTM_CH3,FREQ,PWM_X_MID); //x轴舵机初始化
initPid(&xSitePid, newLCD_W/2, kp, ki, kd,PWM_X_MAX-PWM_X_MID, PWM_X_MIN-PWM_X_MID); //x轴位置pid结构体初始化
initPid(&ySitePid, newLCD_H/2, kp, ki, kd,PWM_Y_MAX-PWM_Y_MID, PWM_Y_MIN-PWM_Y_MID); //y轴位置pid结构体初始化
initPid(&xSpeedPid, 0, skp, ski, skd,PWM_X_MAX-PWM_X_MID,PWM_X_MIN-PWM_X_MID); //x轴速度pid结构体初始化
initPid(&ySpeedPid, 0, skp, ski, skd,PWM_Y_MAX-PWM_Y_MID, PWM_Y_MIN-PWM_Y_MID); //y轴速度pid结构体初始化
xSitePid.Actual = newLCD_W/2;
ySitePid.Actual = newLCD_H/2;
enable_irq (PIT0_IRQn); //使能PIT0中断
enable_irq (PIT1_IRQn); //使能PIT1中断
EnableInterrupts; //开总中断
while(1){
camera_get_img(); //摄像头获取图像
imgCrop(imgbuff,img_finish); //将图像裁剪
searchFlag = Search(img_finish,&ballSite); //寻找小球
if(!searchFlag){
//如果没有找到小球:
led(LED3,LED_ON); //没找到小球就点亮led3
initPid(&xSitePid, newLCD_W/2, kp, ki, kd,PWM_X_MAX-PWM_X_MID, PWM_X_MIN-PWM_X_MID); //x轴位置pid结构体初始化
initPid(&ySitePid, newLCD_H/2, kp, ki, kd,PWM_Y_MAX-PWM_Y_MID, PWM_Y_MIN-PWM_Y_MID); //y轴位置pid结构体初始化
initPid(&xSpeedPid, 0, skp, ski, skd,PWM_X_MAX-PWM_X_MID,PWM_X_MIN-PWM_X_MID); //x轴速度pid结构体初始化
initPid(&ySpeedPid, 0, skp, ski, skd,PWM_Y_MAX-PWM_Y_MID, PWM_Y_MIN-PWM_Y_MID); //y轴速度pid结构体初始化
xSteerDuty = (uint32)PWM_X_MID;
ySteerDuty = (uint32)PWM_Y_MID;
continue;
}
led(LED3,LED_OFF);
//小球运动轨迹控制
if((tickTock-lastTick+1000)%1000 > 20){
switch(myTask){
case point :xSitePid.Set = newLCD_W/2;
ySitePid.Set = newLCD_H/2;
break;
case line :xSitePid.Set = ballMotionLine[count%=2].x;
ySitePid.Set = ballMotionLine[count].y;
break;
case square:xSitePid.Set = ballMotionSquare[count%=4].x;
ySitePid.Set = ballMotionSquare[count].y;
break;
case circular:
default :xSitePid.Set = newLCD_W/2;ySitePid.Set = newLCD_H/2;break;
}
count++;
lastTick = tickTock;
}
//防止积分过饱和
if(xSitePid.err_last2 * xSitePid.Ki >= xkimax) xSitePid.err_last2 = (int32)(xkimax / xSitePid.Ki) ;
if(xSitePid.err_last2 * xSitePid.Ki <= -xkimax)xSitePid.err_last2 = (int32)(-xkimax / xSitePid.Ki);
if(ySitePid.err_last2 * ySitePid.Ki >= ykimax) ySitePid.err_last2 = (int32)(ykimax / ySitePid.Ki) ;
if(ySitePid.err_last2 * ySitePid.Ki <= -ykimax)ySitePid.err_last2 = (int32)(-ykimax / ySitePid.Ki);
//积分分离
if(abs(xSitePid.Set - ballSite.x) > 20)xSitePid.err_last2 = 0; //大于20就不积分了
if(abs(ySitePid.Set - ballSite.y) > 20)ySitePid.err_last2 = 0;
//位置pid
calculatePid(&xSitePid,ballSite.x,&xSpeedPid.Set);
calculatePid(&ySitePid,ballSite.y,&ySpeedPid.Set);
//------------速度环调试-----------------
// xSpeedPid.Set = 0;
// ySpeedPid.Set = 0;
//--------------------------------------
//速度pid
calculatePid(&xSpeedPid,ballSpeed.x,&xPID);
calculatePid(&ySpeedPid,ballSpeed.y,&yPID);
//计算pwm
xSteerDuty = (uint32)(PWM_X_MID+xPID);
ySteerDuty = (uint32)(PWM_Y_MID+yPID);
//调试信息
LCD_Img_Binary(lcdSite,lcdSize,&img_finish[0][0]); //LCD显示图像
markTarget(&ballSite, xSitePid.Set, ySitePid.Set); //在LCD上标记小球位置和目标位置
}
}
附录二:小球识别程序
/*
* @brief 搜索小球
* @parma:img[][newLCD_W/8] 需要搜索的图像
* @param: site 小球位置
* @ret : 1 搜索成功 0 搜索失败
*/
uint8 Search(uint8 img[][newLCD_W/8],Site_t *const site){
uint8 e = 0; //临时变量
int32 n = 0; //累计小球半径
int32 h=0, w=0, i=0;
for(h = newLCD_H-1; h>=0; h--){
for(w = 0; w<newLCD_W/8; w++){
if(img[h][w] != 0xff) { //1黑0白
i = 0;
while(i < 8){
e = img[h][w]<<(i++);
e &= 0x80;
if(!e) n++;
}
}
else {
n = 0;
continue;
}
//存在下一格像素时,若下一个不等于0xff,且下一格为奇数(或者为0x00),则对下一格继续累加半径(这个条件有点问题,但不知道该怎么改)
if(w < newLCD_W/8-1 && (img[h][w+1] % 2 !=0 || !img[h][w+1]) && img[h][w+1] != 0xff) continue;
//如果小球横向半径合适,就很可能找到了小球
if(n >= xRMin && n <= xRMax){
//计算小球横向的中心位置
i = 0; //这里i,e将保存一个值用于计算小球横向坐标
e = 0;
while((w-i) >= 0 && img[h][w-i] != 0xff) i++;
while((img[h][w-i+1]<<(++e)&0x80) && (e<0x08));
site->x = 8 * (w-i+1) + e + n / 2;
//检查小球纵向半径是否合适,并计算小球纵向的中心位置
i = 1; //这里i,e将保存一个值用于计算小球纵向坐标
e = 1;
while((h-i)>=0 && !((img[h-i][site->x/8]>>(7-site->x%8)) & 0x01)) i++;
while((h+e)<newLCD_H && !((img[h+e][site->x/8]>>(7-site->x%8)) & 0x01)) e++;
if((e+i)<xRMin || (e+i)>xRMax) { n = 0; continue; } //判断小球纵向直径是否合适
site->y = h - i + (e+i)/2;
/*
** 到这里,(e+i)-2为纵向直径,site->x为圆心x轴坐标,site->y为圆心y轴坐标
** 提醒一下,n不是横向直径,横向直径可以根据纵向直径推算出来。
** 为了方便以后程序的优化,这些变量最好不要在赋值了!!!
*/
//最后再判断一次,是否为圆形
/*
** 不得已加入了a,其实核心语句就continue所在的四行。
** 其实刚开始写这段程序时存在数组越界的情况,为了避免越界,所以加入了这些多余的判断语句。
** 越界是因为这个函数在开始设计时没考虑到为数组留出多余空间,要是传入的数组是imgbuff,
** 那就根本不用担心越界的问题了。不过我不想改动这个程序了,太麻烦了。
*/
int32 a = h+e,
n = site->x-1-(e+i-2)/2;
if(a >= newLCD_H) a = newLCD_H-1;
if(n < 0) n = 0;
if(!((img[a][(n)/8]>>(7-(n)%8)) & 0x01)) { n = 0; continue; }
n = site->x+1+(e+i-2)/2;
if(n >= newLCD_H) n = newLCD_H-1;
if(!((img[a][(n)/8]>>(7-(n)%8)) & 0x01)) { n = 0; continue; }
a = h-i;
if(a < 0) a = 0;
if(!((img[a][(n)/8]>>(7-(n)%8)) & 0x01)) { n = 0; continue; }
n = site->x-1-(e+i-2)/2;
if(n < 0) n = 0;
if(!((img[a][(n)/8]>>(7-(n)%8)) & 0x01)) { n = 0; continue; }
//坐标范围限制
if(site->x > newLCD_W)
site->x = newLCD_W;
else if(site->x < 0)
site->x = 0;
if(site->y > newLCD_H)
site->y = newLCD_H;
else if(site->y < 0)
site->y = 0;
return 0x01;
}
n = 0;
}
}
return 0x00;
}
附录三:调试日志(含调试方法)
//file:readme.txt
//by:weilan
//date:****/**/**
滚球控制系统:目前已经可以完成固定点、直线、方形轨迹,圆形轨迹没有实现,不要求的话可能不会去实现了。
由于没有加I项(ki),现在存在一点静差,不过这个没关系,通过加大P项(kp),可以减小静差,
但是要注意加大P项也会使系统趋向于不稳定。硬件电路还没有完成,控制该系统是需要通过改
程序来实现。
(1)材料 舵机:sd-5 ; 摄像头:鹰眼 90度镜头 ; 30*30*3哑光黑色亚克力板 ; 18~22mm白色小球 ; 主控:MK60DN
(2)环境 IAR7.8 , 山外MK60程序库
(3)操作 通过设置myTask(main函数内),控制小球运动轨迹。
(4)不足 1:由于硬件系统还没完成,目前操作该系统还需要通过改程序的方式来实现
2:小球的识别是基于横向半径(18~22mm),半径合适就认为是小球,因此会存在错误识别的情况。可以
通过加入纵向半径,减轻错误识别的情况。为了加大适应能力,程序中加入了距离比例(HandleImg.c中)。
3: 由于没有加入I项,系统目前还存在静差。但是I项容易让系统产生震荡,要是精度要求不高的话可以不加I项,
通过加大P项来减小静差。
4:系统存在抖动,原因未知。
(5)参考资料:https://blog.csdn.net/wb790238030/article/details/92809538
https://blog.csdn.net/automan05/article/details/84501990
//date:****/**/**
优化:小球的识别是基于横向和纵向半径相比对,目前错误识别的情况已经很少了。
pid最合理参数值:
位置pid初始参数 kp:0.14 ki:0.0 kd:0.18
速度pid初始参数 skp:4.8 ski:0.0 skd:2.2
//date:****/**/**
优化:(1)解决了当小球突然出现,或小球突然消失时平台会乱晃的问题。
(2)将小球的目的地用黄点标记出来,方便调试。
pid最合理参数值:
位置pid初始参数 kp:0.23 ki:0.0 kd:0.18
速度pid初始参数 skp:4.8 ski:0.0 skd:2.2
目前发现增加kp可以使小球静差降低,但在到达目的地时小球会在目的地前来回晃动,
通过增加skd似乎能缓解这一问题,原因未知。
//date:****/**/**
优化:(1)通过圆形判断程序,减少了错误识别的情况。
(2)增加LED3,方便调试
//date:****/**/**
优化:重新调试了pid参数,这次的调试效果不错!系统抖动的原因找到了:skp过大。
本次调试,解决了这个问题,目前系统已经不抖动了。
pid最合理参数值:
double kp = 0.22, ki = 0.00 , kd = 0.35;// //位置pid初始参数
double skp = 4.30, ski = 0.0, skd = 2.20;// //速度pid初始参数
//date:****/**/**
目前已经没有什么太大问题了,系统很稳定,小球的运动也很流畅。有时间的话在完善一下吧!可以试着用C++实现一下,练练手。
//date:****/**/**
补充调试小技巧:调试串级PID的时候应该先调试内环,待内环效果较好后在调试外环。调试板球系统时,是先调速度环,将速度环的目标值
设置为0,即让小球保持静止,main函数中的这个注释就是调速度环的:
//------------速度环调试-----------------
// xSpeedPid.Set = 0;
// ySpeedPid.Set = 0;
//--------------------------------------
调试的效果应该是将小球丢向平台后,平台能快速让小球静止。
调试完速度环后就可以调位置环了,同样先设置一个目标位置(我将小球的目标位置设置平台正中间,因为设到边沿位置小球会经常
滚出平台,然后我要到处找球,哈哈!),然后先调p,让小球能尽快靠近平台,并且不滚出平台是最好的,有点晃动没关系。p调
好后就要调d项了,
调d项的一个理想效果是小球能快速到达目标位置,并且在到达目标位置后能快速平稳的停下,如果d项调试效果一直不理想,就要
考虑考虑先前调好的速度环参数是否合理了,可以适当调整一下之前调好的参数。其实d项调好后板球系统就已经完成95%了。通常
情况下,平台是很平整的,几乎不会有太多静差,小球的位置比较准确,i项适当加一下就好。