WBack
约 3533 字
初始化
void CWback::plan(const CVisionModule *pVision)
{
if (pVision->getCycle() - _lastCycle > PARAM::Vision::FRAME_RATE * 0.1)
{
setState(BEGINNING);
} // 超时重置
int player = task().executor;
int flag = task().player.flag;
int index = task().player.rotdir; // 我是第几辆后卫
int guardNum = task().player.kick_flag; // 一共有几辆后卫
int defendNum = task().ball.Sender; // 要防守的敌方车号
int bestEnemyNum = ZSkillUtils::instance()->getTheirBestPlayer();
const PlayerVisionT &bestEnemy = pVision->theirPlayer(bestEnemyNum);
const PlayerVisionT &defendEnemy = pVision->theirPlayer(defendNum);
const MobileVisionT &ball = pVision->ball();
const PlayerVisionT &me = pVision->ourPlayer(player);
bool frared = RobotSensor::Instance()->IsInfraredOn(player);
if (frared)
{
fraredOn = fraredOn > maxTime ? maxTime : fraredOn + 1;
fraredOff = 0;
}
else
{
fraredOff = fraredOff > maxTime ? maxTime : fraredOff + 1;
fraredOn = 0;
}
// 对持球红外传感器做滞后滤波:
// 检测到球时 fraredOn 累计计数,fraredOff 清零
// 未检测到时反之
// 两个计数器均上限为 maxTime=1024,防止溢出
// 这样可以避免传感器短暂抖动导致误判是否持球。
double seat = 0;
for (int i = 0; i < PARAM::Field::MAX_PLAYER; ++i)
{ // 遍历所有我方球员,找出所有担任后卫角色的车
const PlayerVisionT &player = pVision->ourPlayer(i);
if (!player.Valid())
continue;
if (TaskMediator::Instance()->isBack(i))
{
GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0 + seat, 200),
QString("%1").arg(i).toLatin1());
seat += 200;
// 在y=200 的水平线上依次显示其车号
}
}这段代码是 CWback::plan() 函数的前半部分,负责后卫机器人的决策规划初始化。逐块解析如下:
1. 超时重置
if (pVision->getCycle() - _lastCycle > PARAM::Vision::FRAME_RATE * 0.1)
setState(BEGINNING);若距上次执行超过 0.1 秒(约 6 帧),将状态机重置为初始状态,防止旧状态残留。
2. 任务参数读取
int player = task().executor; // 执行该技能的车号
int flag = task().player.flag; // 运动标志位
int index = task().player.rotdir; // 该车是第几辆后卫
int guardNum = task().player.kick_flag; // 后卫总数
int defendNum = task().ball.Sender; // 要重点防守的敌方车号从上层任务调度中提取后卫相关参数。
3. 视觉信息获取
const PlayerVisionT &bestEnemy = pVision->theirPlayer(bestEnemyNum); // 最优敌方球员
const PlayerVisionT &defendEnemy = pVision->theirPlayer(defendNum); // 指定防守目标
const MobileVisionT &ball = pVision->ball(); // 球
const PlayerVisionT &me = pVision->ourPlayer(player); // 本车4. 红外传感器防抖
fraredOn = fraredOn > maxTime ? maxTime : fraredOn + 1;
fraredOff = fraredOff > maxTime ? maxTime : fraredOff + 1;对持球红外传感器做滞后滤波:
- 检测到球时
fraredOn累计计数,fraredOff清零 - 未检测到时反之
- 两个计数器均上限为
maxTime=1024,防止溢出
这样可以避免传感器短暂抖动导致误判是否持球。
5. 遍历寻找所有后卫
for (int i = 0; i < PARAM::Field::MAX_PLAYER; ++i)
{
if (!player.Valid()) continue;
if (TaskMediator::Instance()->isBack(i))
{
GDebugEngine::Instance()->gui_debug_msg(CGeoPoint(0 + seat, 200),
QString("%1").arg(i).toLatin1());
seat += 200;
}
}遍历所有我方球员,找出所有担任后卫角色的车,在调试界面 y=200 的水平线上依次显示其车号,相邻间距 200 单位,用于可视化调试。
后卫站位与朝向
TaskT GuardTask(task());
CGeoPoint guardPos =
GuardPos::Instance()->backPos(guardNum, judgeindex(pVision, player), player, -1);
// 先用 -1(无指定防守目标)做一次保底初始化,避免后续 guardPos 未赋值的空指针风险
if (defendNum < 0)
{ // 无指定敌人,纯位置防守
guardPos = GuardPos::Instance()->backPos(guardNum, judgeindex(pVision, player), player, -1);
}
else
{
guardPos =
GuardPos::Instance()->backPos(guardNum, judgeindex(pVision, player), player, defendNum);
GuardTask.player.angle = (defendEnemy.Pos() - me.Pos()).dir();
// 针对指定敌方车号
}
if (Utils::InOurPenaltyArea(ball.Pos(), MIN_DIST_TO_PENALTY) || !ball.Valid())
{ // 球在己方禁区内或球丢失
// 改为朝向最优敌方球员的方向,因为此时球已不可追,转为盯防最危险的敌人
GuardTask.player.angle = (bestEnemy.Pos() - OUR_GOAL).dir();
}
else
{ // 正常情况,始终面向来球方向,便于及时拦截。
GuardTask.player.angle = (ball.Pos() - OUR_GOAL).dir();
}这段代码负责计算后卫的防守站位和朝向角度,分两个层次决策:
1. 计算防守站位 guardPos
TaskT GuardTask(task());
CGeoPoint guardPos = GuardPos::Instance()->backPos(guardNum, judgeindex(pVision, player),
player, -1); // KE avoid nil value先用 -1(无指定防守目标)做一次保底初始化,避免后续 guardPos 未赋值的空指针风险。
if (defendNum < 0)
{
guardPos = GuardPos::Instance()->backPos(..., player, -1); // 无指定敌人,纯位置防守
}
else
{
guardPos = GuardPos::Instance()->backPos(..., player, defendNum); // 针对指定敌方车号
GuardTask.player.angle = (defendEnemy.Pos() - me.Pos()).dir(); // 朝向对应敌人
}backPos() 接收四个参数:
| 参数 | 含义 |
|---|---|
guardNum | 后卫总数 |
judgeindex(...) | 本车在所有后卫中从左到右的排名(按相对己方球门角度排序) |
player | 本车车号 |
defendNum | 要盯防的敌方车号,-1 表示不盯人 |
根据这些参数,GuardPos 模块会计算出当前帧该后卫应站的防守坐位点。
2. 覆盖朝向角度
if (Utils::InOurPenaltyArea(ball.Pos(), MIN_DIST_TO_PENALTY) || !ball.Valid())
{
// 球在己方禁区内或球丢失
GuardTask.player.angle = (bestEnemy.Pos() - OUR_GOAL).dir();
}
else
{
// 正常情况
GuardTask.player.angle = (ball.Pos() - OUR_GOAL).dir();
}无论上一步朝向如何设置,这里会强制覆盖朝向角:
- 正常情况:车头朝向从
OUR_GOAL(己方球门)指向球的方向,即始终面向来球方向,便于及时拦截。 - 球在禁区内或无效:改为朝向从
OUR_GOAL指向最优敌方球员的方向,因为此时球已不可追,转为盯防最危险的敌人。
截球评估,传球模式
TaskT GuardTask(task());
CGeoPoint guardPos =
GuardPos::Instance()->backPos(guardNum, judgeindex(pVision, player), player, -1);
// 先用 -1(无指定防守目标)做一次保底初始化,避免后续 guardPos 未赋值的空指针风险
if (defendNum < 0)
{ // 无指定敌人,纯位置防守
guardPos = GuardPos::Instance()->backPos(guardNum, judgeindex(pVision, player), player, -1);
}
else
{
guardPos =
GuardPos::Instance()->backPos(guardNum, judgeindex(pVision, player), player, defendNum);
GuardTask.player.angle = (defendEnemy.Pos() - me.Pos()).dir();
// 针对指定敌方车号
}
if (Utils::InOurPenaltyArea(ball.Pos(), MIN_DIST_TO_PENALTY) || !ball.Valid())
{ // 球在己方禁区内或球丢失
// 改为朝向最优敌方球员的方向,因为此时球已不可追,转为盯防最危险的敌人
GuardTask.player.angle = (bestEnemy.Pos() - OUR_GOAL).dir();
}
else
{ // 正常情况,始终面向来球方向,便于及时拦截。
GuardTask.player.angle = (ball.Pos() - OUR_GOAL).dir();
}
CGeoPoint interPos = ZSkillUtils::instance()->getOurInterPoint(player);
double interTime = ZSkillUtils::instance()->getOurInterTime(player);
double bestInterTime = 99999;
double bestTheirInterTime = 99999;
double bestInterNum = -1;
for (int i = 0; i < PARAM::Field::MAX_PLAYER; ++i)
{ // 遍历所有球员,同时统计两侧截球能力:
if (ZSkillUtils::instance()->getTheirInterTime(i) < bestTheirInterTime)
{ // 统计敌方最短截球时间(所有敌方车都参与)
bestTheirInterTime = ZSkillUtils::instance()->getTheirInterTime(i);
}
const PlayerVisionT &me = pVision->ourPlayer(i);
if (!me.Valid() || TaskMediator::Instance()->isGoalie(i))
continue;
if (ZSkillUtils::instance()->getOurInterTime(i) < bestInterTime)
{ // 统计我方最短截球时间(跳过无效车和守门员)
bestInterTime = ZSkillUtils::instance()->getOurInterTime(i);
bestInterNum = i; // 记录最快截球的我方车号
}
} // bestInterNum 在后面暂未使用,无需理会
bool isflat = true;
double chippower =
BallModel::instance()->chipPassVel(pVision, MessiDecision::Instance()->passPos());
double flatpower = BallModel::instance()->flatPassVel(
pVision, MessiDecision::Instance()->passPos(), MessiDecision::Instance()->receiverNum(),
bufferTime, PASS_ANGLE_ERROR);
// 平挑射的力度
// judge flat or chip
isflat =
MessiDecision::Instance()->judgePassModule(player, MessiDecision::Instance()->passPos());
// 问 Messi 应该平射还是挑射
if (RobotSensor::Instance()->IsKickerOn(player) > 0) // 检测到踢球装置开启
{
if (_flytimediff < 1 && !isflat) // 仅挑传时,且只记录第一帧
{
_flytimediff++;
MessiDecision::Instance()->setisflyTime(true);
MessiDecision::Instance()->setfirstChipDir(me.Dir());
MessiDecision::Instance()->setfirstChipPos(pVision->ball().RawPos());
// 把球的信息发给 Messi ,因为从视觉拿可能不准
}
}
else
{
_flytimediff = 0;
}这段代码分为两个独立部分:截球态势评估 和 传球模式判断。
1. 截球时间评估
CGeoPoint interPos = ZSkillUtils::instance()->getOurInterPoint(player); // 本车预计截球点
double interTime = ZSkillUtils::instance()->getOurInterTime(player); // 本车预计截球时间
double bestInterTime = 99999; // 我方最短截球时间
double bestTheirInterTime = 99999; // 敌方最短截球时间
double bestInterNum = -1; // 我方最快截球车号遍历所有球员,同时统计两侧截球能力:
for (int i = 0; i < PARAM::Field::MAX_PLAYER; ++i)
{
// 统计敌方最短截球时间(所有敌方车都参与)
if (ZSkillUtils::instance()->getTheirInterTime(i) < bestTheirInterTime)
bestTheirInterTime = ...;
// 统计我方最短截球时间(跳过无效车和守门员)
if (!me.Valid() || TaskMediator::Instance()->isGoalie(i)) continue;
if (ZSkillUtils::instance()->getOurInterTime(i) < bestInterTime)
{
bestInterTime = ...;
bestInterNum = i; // 记录最快截球的我方车号
}
}注意:这些变量(interPos、bestInterTime 等)在本段代码之后均未被实际使用,属于被注释掉的旧逻辑的残留数据准备,计算结果被浪费了。
2. 传球模式判断 + 挑球时间记录
① 计算两种传球所需力度
double chippower = BallModel::instance()->chipPassVel(...); // 挑球所需速度
double flatpower = BallModel::instance()->flatPassVel(...); // 平推传球所需速度② 判断应使用平传还是挑传
isflat = MessiDecision::Instance()->judgePassModule(player, passPos());
// true = 平传,false = 挑传(chip)③ 记录首次挑球时刻(用于飞行时间估算)
if (RobotSensor::Instance()->IsKickerOn(player) > 0) // 检测到踢球动作
{
if (_flytimediff < 1 && !isflat) // 仅挑传时,且只记录第一帧
{
_flytimediff++;
MessiDecision::Instance()->setisflyTime(true); // 标记球在飞行中
MessiDecision::Instance()->setfirstChipDir(me.Dir()); // 记录起始朝向
MessiDecision::Instance()->setfirstChipPos(ball.RawPos()); // 记录起始球位
}
}
else
{
_flytimediff = 0; // 踢球结束,重置计数
}_flytimediff 作为单次触发锁,确保飞行参数只在挑球发生的第一帧被记录一次,供后续轨迹预测使用。
后卫卡死检测
double inVel;
CVector me2ball = ball.Pos() - me.RawPos();
if (pVision->ball().Vel().mod2() > FRICTION * me2ball.mod())
inVel = sqrt(pVision->ball().Vel().mod2() - FRICTION * me2ball.mod());
// 计算球到达本车时的剩余速度 inVel,考虑摩擦衰减
else
inVel = pVision->ball().Vel().mod();
// 若摩擦已使能量耗尽(根号内为负),则退化为直接取当前球速。
double beta = Utils::Normalize(pVision->ball().Vel().dir() + PARAM::Math::PI -
(MessiDecision::Instance()->passPos() - me.RawPos()).dir());
// 计算 beta(入射角),即球的运动方向(反向)与本车到传球目标方向之间的夹角
double alpha = atan2(
(MessiDecision::Instance()->passPos() - me.RawPos()).mod() * sin(beta) / 0.42 + 1e-5,
inVel + (MessiDecision::Instance()->passPos() - me.RawPos()).mod() * cos(beta) / 0.42 +
1e-5);
// 计算 alpha(击球补偿角):
// 本质是利用速度叠加原理,求击球时需要额外偏转多少角度,才能让球沿摩擦减速后仍准确飞向目标
float bestdist = 99999;
for (int i = 0; i < PARAM::Field::MAX_PLAYER; ++i)
{ // 统计所有敌方球员中离球最近的距离
if (pVision->theirPlayer(i).Pos().dist(ball.Pos()) < bestdist)
{
bestdist = pVision->theirPlayer(i).Pos().dist(ball.Pos());
}
}
/////////////////////////////////////////////////////
/////// noprogressjudge ,判断后卫是否卡着 /////////
////////////////////////////////////////////////////
if (ball.Vel().mod() < 1000 && !Utils::InOurPenaltyArea(ball.Pos(), 1000) &&
me.RawPos().dist(guardPos) < 300 && me.RawPos().dist(guardPos) > 100)
_noprogresscnt++;
if (me.RawPos().dist(guardPos) <= 100 || me.RawPos().dist(guardPos) >= 300)
_noprogresscnt = 0;
if (_noprogresscnt >= 50 && _noprogresscnt % 50 == 0)
_noprogress = true;
else if ((_noprogresscnt > 50 && _noprogresscnt % 50 > 10) || _noprogresscnt == 0)
_noprogress = false;
// 即每卡住约 50帧(≈0.8秒) 触发一次,持续约 10帧 后自动解除,形成周期性脉冲。
// 触发期间上层逻辑会开启 DSS 模式强制冲破阻碍。
CGeoPoint theirbestPos = pVision->theirPlayer(ZSkillUtils::instance()->getTheirBestPlayer()).Pos();
GuardTask.player.pos = guardPos;
GuardTask.player.flag = flag;
if (!Utils::InOurPenaltyArea(me.Pos(), DSS_DIST_FROM_PENALTY) || _noprogress)
GuardTask.player.flag = PlayerStatus::ALLOW_DSS;
if (WorldModel::Instance()->CurrentRefereeMsg() == "GameStop")
{ // GameStop(停球)期间,比赛规则要求机器人不得高速运动,限制最大加速度为 2m/s²,防止违规冲撞
GuardTask.player.max_acceleration = 2000;
}
GuardTask.player.rotate_acc = 15;
setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(GuardTask));
_lastCycle = pVision->getCycle(); // last frame
CStatedTask::plan(pVision);
}这段代码包含三个独立功能块:
第一部分:传球偏转角计算
这是为旧版传球逻辑准备的几何计算,结果本身当前未被实际使用。
① 计算球到达本车时的剩余速度 inVel,考虑摩擦衰减:
vin=vball2−μ⋅dme2ball
若摩擦已使能量耗尽(根号内为负),则退化为直接取当前球速。
② 计算 beta(入射角):
β=normalize(θball_vel+π−θme→passPos)
即球的运动方向(反向)与本车到传球目标方向之间的夹角——球"来的方向"相对于"要传去的方向"的偏差。
③ 计算 alpha(击球补偿角):
α=arctan2(0.42dpassPos⋅sinβ, vin+0.42dpassPos⋅cosβ)
本质是利用速度叠加原理,求击球时需要额外偏转多少角度,才能让球沿摩擦减速后仍准确飞向目标。0.42 为经验系数(近似传球速度比例)。
第二部分:最近敌方距球距离
float bestdist = 99999;
for (int i = 0; i < PARAM::Field::MAX_PLAYER; ++i)
bestdist = min(theirPlayer[i].dist(ball));统计所有敌方球员中离球最近的距离。同样是注释掉逻辑的残留,bestdist 后续未使用。
第三部分:后卫卡死检测
检测后卫是否长期停滞无法到达目标点(被阻挡/卡住),核心是对 _noprogresscnt 的维护:
累计条件(同时满足才计数):
| 条件 | 含义 |
|---|---|
ball.Vel().mod() < 1000 | 球速慢,场面稳定 |
!InOurPenaltyArea(ball, 1000) | 球不在禁区 |
100 < dist(me, guardPos) < 300 | 卡在"快到但未到"的中间距离 |
清零条件:距目标 ≤100(已到达)或 ≥300(还差很远)。
卡死标志的脉冲触发逻辑:
_noprogresscnt: 0 ——→ 50 → _noprogress = true (触发)
50 ——→ 60 → _noprogress = false (自动恢复)
60 ——→ 100 → 无变化
100 → _noprogress = true (再次触发)
...即每卡住约 50帧(≈0.8秒) 触发一次,持续约 10帧 后自动解除,形成周期性脉冲。触发期间上层逻辑会开启 DSS 模式强制冲破阻碍。
设置目标位置和基础标志位
GuardTask.player.pos = guardPos; // 移动目标 = 前面计算好的防守站位点
GuardTask.player.flag = flag; // 使用从上层任务继承的运动标志DSS 模式开启条件
if (!Utils::InOurPenaltyArea(me.Pos(), DSS_DIST_FROM_PENALTY) || _noprogress)
GuardTask.player.flag = PlayerStatus::ALLOW_DSS;DSS(Dynamic Speed Strategy) 是一种允许机器人更激进加速的运动模式,有两种情况下开启:
| 条件 | 原因 |
|---|---|
本车不在禁区扩展范围内(PLAYER_SIZE * 11) | 禁区附近需要精确控制,避免冲撞守门员或误入禁区,故关闭 DSS |
_noprogress == true(检测到卡死) | 强制开启 DSS 冲破障碍,解除卡死状态 |
停球规则下的加速度限制
if (WorldModel::Instance()->CurrentRefereeMsg() == "GameStop")
GuardTask.player.max_acceleration = 2000;GameStop(停球)期间,比赛规则要求机器人不得高速运动,限制最大加速度为 2000(单位:mm/s²),防止违规冲撞。
旋转加速度限制与指令提交
GuardTask.player.rotate_acc = 15; // 限制转向加速度,防止车身旋转过猛影响稳定性
setSubTask(TaskFactoryV2::Instance()->SmartGotoPosition(GuardTask)); // 提交给底层路径规划
_lastCycle = pVision->getCycle(); // 记录本帧时间戳,用于下帧超时检测
CStatedTask::plan(pVision); // 调用父类状态机继续执行SmartGotoPosition 是带避障能力的智能移动原语,接收完整的 GuardTask(含位置、朝向、标志位、加速度限制)后自动规划路径。
整体数据流
guardPos(前面计算)
↓
GuardTask.player.pos = guardPos
GuardTask.player.angle(已在前面设置)
↓
按条件附加 ALLOW_DSS / max_acceleration / rotate_acc
↓
SmartGotoPosition(GuardTask) → 底层运动执行这是整个 plan() 函数的出口,前面所有判断(防守位置、朝向、卡死检测)最终都在这里收敛为一条底层运动指令。
贡献者
更新日志
66dbc-chipsolver StaticGeiBallV2 WBack于