板球控制系统制作
本文主要介绍板球系统的设计过程，尤其是程序设计。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项适当加一下就好。
8791
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
