|
|
(原文件名:AGV running.jpg)
上图是前段时间和公司做的AGV车，功能都实现了,可以无线呼叫，到标志位停止，不过运行状况一般，有较大晃动，特别是载满货以后过弯道时经常出轨。
描述如下：
正常行驶时速度1~2M/S。这里我们只做到了1M/S。当速度再加快时，载上货物运行正常，但是空车运行时不太稳定，停止时易出轨。而在过弯道时，空车正常，载货时易出轨，所以这里我们采用了直道和弯道分开的策略，让弯道时减速。但由于载货对车体本身影响较大，过弯道有时还是不太稳定。
以下是系统中用到的PID循迹算法。虽然循迹功能实现了，但是当载货较重时，车身还是存在较大的晃动，感觉和机械机构也有关，因为通过挂钩拉货，挂钩摩擦力对车体由较大影响，当然这里和算法本身及其参数设计也有较大关系。
/********************
* 结构体定义区 *
********************/
typedef struct PID
{
int16_t pConst; // 比例常数 Proportional Const
int16_t iConst; // 积分常数 Integral Const
int16_t dConst; // 微分常数 Derivative Const
int16_t position;
int16_t hisPosition;
int16_t lastPosition[10];
}PID;
/***********************************************************
* 函数名称：PID参数初始化
* 功能描述：初始化PID参数，并实现P、I、D三个参数的整定
* 参数列表：
* 返回结果：无
***********************************************************/
void PIDInit(PID *iPID)
{
memset(iPID, 0, sizeof(iPID)); //将所有值清零
iPID->pConst = 2; // 比例常数 Proportional Const
iPID->iConst = 0; // 积分常数 Integral Const
iPID->dConst = 8; // 微分常数 Derivative Const
}
/***********************************************************
* 函数名称：PID控制程序
* 功能描述：
* 参数列表：
* 返回结果：无
***********************************************************/
void PIDCalc( PID *cPID)
{
int16_t pGain; //P增益
int16_t iGain; //I增益
int16_t dGain; //D增益
int16_t pidGain; //控制量
cPID->position = Discretized(PIN_RED & 0X1F); //对采集的循迹信号进行离散化处理得到位置信息
cPID->hisPosition += cPID->position; //记录偏差之和
cPID->lastPosition[3] = cPID->lastPosition[2];
cPID->lastPosition[2] = cPID->lastPosition[1]; //
cPID->lastPosition[1] = cPID->lastPosition[0]; //
cPID->lastPosition[0] = cPID->position; //
pGain = (cPID->pConst) * (cPID->position); //计算比例分量(P)=比例系数*本次位置差
iGain = (cPID->iConst) * (cPID->hisPosition); //计算积分分量(I)=积分系数*偏差之和
dGain = (cPID->dConst) * ((cPID->position) - (cPID->lastPosition[3])); //计算微分分量(D)=微分系数*(本次位置差-前3次的位置差)
//由于采样比较快，用本次位置-前3次位置才有足够大的控制 pidGain = pGain + iGain + dGain; //P分量和D分量相加，得到控制量。
if (pidGain > 320) pidGain = 320; //模糊化处理，防止增量溢出
if (pidGain < -320) pidGain = -320;
pidGain = pidGain/10;
SetMotors(pidGain); //将控制量传入电机控制函数进行控制
}
/***********************************************************
* 函数名称：电机调速
* 功能描述：通过得到的pidValue来改变占空比大小
* 参数列表：
* 返回结果：
***********************************************************/
void SetMotors(int16_t pidValue)
{
// PWM_CON = _BV(ENA) | _BV(ENB) | _BV(CTRL0) |!_BV(CTRL1) | _BV(CTRL2) |!_BV(CTRL3); //left
// PWM_CON = _BV(ENA) | _BV(ENB) | !_BV(CTRL0) | _BV(CTRL1) |!_BV(CTRL2) | _BV(CTRL3); //right
// PWM_CON = _BV(ENA) | _BV(ENB) | _BV(CTRL0) |!_BV(CTRL1) |!_BV(CTRL2) | _BV(CTRL3); //back
PWM_CON = _BV(ENA) | _BV(ENB) | !_BV(CTRL0) | _BV(CTRL1) | _BV(CTRL2) |!_BV(CTRL3); //forward
g_dutyL = 36 + pidValue; //改变PWM值
g_dutyR = 36 - pidValue;
}
/***********************************************************
* 函数名称：Discretized
* 功能描述：对从循迹板采集的数据进行离散化处理
* 参数列表：
*
* 返回结果：
*
***********************************************************/
int16_t Discretized(uint8_t temp)
{
switch (temp) //将位置信号进行离散化
{
case 0x1b: g_position = 0;
break;
case 0x1d: g_position = 10; //正，左加右减，向右转
break;
case 0x17: g_position = -10; //负，左加右减，右转
break;
case 0x1e: g_position = 20; //正，左减右加，极左转
break;
case 0x0f: g_position = -20; //负，左加右减，极右转
break;
case 0x1c: g_position = 40 ; //正，左减右加，极左转
break;
case 0x07: g_position = -40 ; //负，左加右减，极右转
break;
default: g_position = 0;
break;
}
return g_position;
}
这上面的参数中10、20、40以及pConst、iConst、dConst都是先定了个大致范围，然后根据实际效果更改而得到的。希望大家对这这个算法和这个车多提些建议吧…… |
阿莫论坛20周年了！感谢大家的支持与爱护！！
一只鸟敢站在脆弱的枝条上歇脚，它依仗的不是枝条不会断，而是自己有翅膀，会飞。
|
发表于 2010-5-19 08:41:29
