2023华为软件精英挑战赛初赛总结
所有的代码都是为了被debug而设计出来的。
在这写bug与debug的无穷螺旋中，
【我们】被永远地禁锢着。
这究竟是诅咒，还是惩罚？
交予【我们】这个无解难题的神啊，
是否有一天，【我们】也会对你刀剑相向？
团队介绍
\ \quad 本团队由两名非计算机专业的大四学生和一名计算机专业的大三学生组成。团队在粤港澳赛区中排名第11。关于此次复赛的情况，可参考以下链接：
\ \quad 本次初赛的代码见于以下链接：
初赛赛题介绍
\ \quad 题目目标：选手程序操控（表现为在与判题器实时交互时，选手程序输出相关的指令，让判题器执行相关的操作）4个机器人执行前进（forward）、旋转（rotate）、购买（buy）、出售（sell）、销毁（destroy）5个指令，往来于最多9种类型的工作台间（生产型工作台可消耗低价值产品，在一定时间后生产出高价值产品，消费型工作台只接收产品而不生产产品。工作台可同时具有生产和消费两种属性），完成物品商贸任务，以赚取差价获得利润。程序运行3min（9000帧），在程序运行结束后，选手拥有的资金数即为最终分数，所获得的资金越高越好。
\ \quad 程序与判题器的交互：程序通过标准输入和标准输出与判题器进行交互。判题器运行帧率为每秒 50 帧，对于每一帧，判题器都会把场上的实时信息通过标准输入传递给选手程序， 同时从选手程序读取机器人的操控指令作用到各个机器人上。每一帧有 1000/50=20ms 的时间，由于判题器需保留 5ms 执行物理计算来模拟真实场景，故选手程序需要在15ms 内做出每一帧的决策，如果超过15ms 未做出决策，则系统将直接忽略这一帧的控制进入下一帧，并且在直到选手程序返回控制指令之前， 不会再发送状态数据给选手程序。
团队解决方案
\ \quad 本文只介绍初赛训练赛的方案组成。该方案由机器人的运动控制、碰撞检测与避免策略、工作台选择策略组成。对于初赛正式赛，我们团队针对不同的图进行了不同的工作台选择策略的优化，由于没有对算法进行根本性的改动，故不在此赘述。
机器人运动控制
\ \quad 机器人的运动控制的思路是，在机器人开始运动前，先为机器人设置一个目的地坐标，机器人从当前的坐标移动到该目的地坐标。在运动的过程中，对机器人的方向和速度进行控制。
方向控制
\ \quad 根据机器人的朝向与机器人和对应工作台的连线之间夹角dAngle（后者的角度减去前者，dAngle \in [- \pi, \pi]），把机器人的旋转角速度设置为50×dAngle。以下对这个角速度的设置进行一些补充说明。50×dAngle \in [- \pi, \pi ]。当dAngle较大时，把角速度设置为最大值更合适。即，当|dAngle|＞3.6°时，程序会把角速度的绝对值设置为\pi。；当|dAngle|<3.6°时，程序会把角速度设置为50×dAngle。50这个参数的设置是为了契合赛题的一帧（20ms）一控制的设定，这样，在一帧后，机器人恰好能转过dAngle的角度。而且我们也发现，机器人的角加速度其实很大，把机器人的角速度控制视为在理想条件下进行也没有什么大碍（然而在后面的测试中发现，机器人装载货物前后的加速度性能和控制性能还是会有可观的差异）。
速度控制（秋名山）
\ \quad 在机器人的角加速度足够大，角速度控制足够精确的情形下，是不需要进行速度控制的，只需要把机器人的朝向瞬间对准，然后冲向目标就行。但是，机器人的角加速度并不是无穷大，角速度也只能做到每20ms控制一次，因此，机器人的朝向不能瞬间对准，在机器人全速冲向目标时，就会出现偏差（机器人在转弯的角速度过大，线速度过大时，甚至出现了漂移的现象）。所以，需要在机器人对不准目标时，进行速度控制。本团队实现了两个版本的速度控制，赤城山版本是初赛中使用的，秋名山版本是初赛后再制作的。由于本人对秋名山版本的速度控制更为熟悉，此处仅介绍秋名山版本的速度控制。
赤城山
秋名山
\ \quad 如图，机器人的位置为A，目标工作台的位置为B。机器人此时的速度为V，此时，机器人的速度方向并未对准工作台B，角度偏差了 {\theta}_1 。注意，CB与CE不共线。
\theta_1 ， \theta_2 ， \theta_3 ， \theta_4 的计算参见以下代码（使用代码来计算角度很容易会出现不易察觉的bug，例如正负号相反，锐角与钝角取反，因此需要对这些角度的计算加上很强的控制，首先把所有可能的情况列出来，然后把这些角度控制到一个合适的范围再进行后续的计算）。
double vx = lineSpeed[0], vy = lineSpeed[1];
double angleV = atan2(vy, vx);
double deltaX = tarX - this->x, deltaY = tarY - this->y;
double angleTar2Bot = atan2(deltaY, deltaX);
double theta1 = angleTar2Bot - angleV;
theta1 = theta1 > M_PI ? theta1 - 2 * M_PI : (theta1 <= -M_PI ? theta1 + 2 * M_PI : theta1); // 将theta1保持在[-pi,pi]
double theta3 = fabs(angleTar2Bot) > M_PI_2 ? (M_PI - fabs(angleTar2Bot)) : fabs(angleTar2Bot); // 强行取锐角
double theta2 = M_PI_2 - theta3;
// offset原始值为0.4，对应于机器人在工作台的感知范围内，
// 但当机器人携带货物时，offset会稍作降低，让机器人更偏
// 向进入速度控制
double offset = 0.4;
if (gootsId) // 持有产品后，对机器人对速度控制保守一些
offset = 0.86 * 0.4;
double offset_x, offset_y; // 点C的位置计算
double offset1_x, offset1_y, offset2_x, offset2_y;
if (deltaY * deltaX < 0) { // 线段AB与x轴的夹角成钝角
offset1_x = tarX + offset * cos(fabs(theta2));
offset1_y = tarY + offset * sin(fabs(theta2));
offset2_x = tarX - offset * cos(fabs(theta2));
offset2_y = tarY - offset * sin(fabs(theta2));
}
else { // 线段AB与x轴的夹角成锐角
offset1_x = tarX - offset * cos(fabs(theta2));
offset1_y = tarY + offset * sin(fabs(theta2));
offset2_x = tarX + offset * cos(fabs(theta2));
offset2_y = tarY - offset * sin(fabs(theta2));
}
double angleOffset12Bot = atan2(offset1_y - this->y, offset1_x - this->x);
double angleOffset22Bot = atan2(offset2_y - this->y, offset2_x - this->x);
double angleGap1 = fabs(angleOffset12Bot - angleV);
if (angleGap1 > M_PI) {
angleGap1 = 2 * M_PI - angleGap1;
}
double angleGap2 = fabs(angleOffset22Bot - angleV);
if (angleGap2 > M_PI) {
angleGap2 = 2 * M_PI - angleGap2;
}
if (angleGap2 > angleGap1) {
offset_x = offset1_x;
offset_y = offset1_y;
}
else {
offset_x = offset2_x;
offset_y = offset2_y;
}
double angleOffset2Bot = atan2(offset_y - this->y, offset_x - this->x);
double theta4 = angleOffset2Bot - angleV;
theta4 = theta4 > M_PI ? 2 * M_PI - theta4 : (theta4 <= -M_PI ? theta4 + 2 * M_PI : theta4);
\ \quad 以机器人与工作台的连线与x轴成钝角，机器人的速度方向延长线更偏向于工作台的右侧（机器人看向工作台的视角）为例（即上图所示的情况），进行角度控制的具体思路是，考虑机器人与工作台的感知范围（<0.4m），先看机器人以最大速度（6m/s）和最大角速度（ \pi / s）能否进入最容易进入的工作台最远感知范围中，即图中点C的位置。由于机器人的角速度恒定，其轨迹应该是一条圆弧，故能否保持最大速度进入点C，即看机器人此时在最大角速度和最大速度下跑出的圆弧的半径（6 / \pi )，是否小于在点A与机器人速度方向相切，且过点C的圆弧的半径R。R的计算公式见下：
double dist1 = sqrt(pow(this->x - offset_x, 2) + pow(this->y - offset_y, 2));
double R = dist1 / (2 * fabs(sin(theta4)));
\ \quad 比较公式见下：
if ((6.0 / M_PI) < R) // 旋转死区之外，无须减速
return 6;
\ \quad 此时如果该条件不能满足，就需要进行降速处理，且尽可能使得该降低后的速度，结合当前机器人的角速度，能恰好使得机器人的运动轨迹经过目标点B。该圆弧轨迹的半径r的计算公式如下：
double dist2 = sqrt(pow(this->x - tarX, 2) + pow(this->y - tarY, 2));
double r = dist2 / 2 / fabs(sin(theta1));
\ \quad 通过r，就可以计算出此时机器人应该具有的速度minSpeed：
double minSpeed = r * fabs(omega);
\ \quad 至此，完成机器人秋名山版本的速度控制。秋名山版本的速度控制能在未仔细调整参数的情况下，在线下把正式赛的图3做到101.7w。
机器人碰撞检测与碰撞避免
\ \quad 由于机器人间的碰撞会导致货物价值的损失，甚至会导致机器人因粘连在一起而卡死，因此有必要进行碰撞检测与碰撞避免。初赛中使用的碰撞检测与碰撞避免策略比较简单，且只讨论和规避了两个进行相向运动（两个速度矢量的点积小于0，如果有多个即将发生碰撞机器人，程序将会把其中一个机器人的速度置为原来的一半，且不再把它和它所对应的碰撞目标视为即将发生碰撞的机器人，避免死锁）的机器人之间的碰撞。但十分有效。其思路是源于VO算法。
碰撞检测
\ \quad 利用相对运动的概念，进行机器人的碰撞检测。
\ \quad 对于两个机器人A和B，判断它们正在互相靠近的代码是：
double delta_vx = botj_vx - boti_vx, delta_vy = botj_vy - boti_vy;
double deltax = xj - xi, deltay = yj - yi;
// 向量的点积，检查相对位置向量与相对速度向量是否反向（成钝角），反向则说明两个机器人正在靠近
if (delta_vx * deltax + delta_vy * deltay > 0)
continue;
\ \quad 判断它们在做相向运动的代码是：
// 向量的点积，同向运动的先不管
if (botj_vx * boti_vx + botj_vy * boti_vy > 0)
continue;
\ \quad 判断它们是否会发生碰撞的代码是：
dist = calDist(xi, yi, xj, yj);
angleTowPoint = atan2(yj - yi, xj - xi);
cosAngle = cos(angleTowPoint);
sinAngle = sin(angleTowPoint);
vi_par = boti_vx * cosAngle + boti_vy * sinAngle; // i在两点连线的水平速度
vi_ver = boti_vx * sinAngle - boti_vy * cosAngle; // i在两点连线的垂直速度
vj_par = botj_vx * cosAngle + botj_vy * sinAngle; // j在两点连线的水平速度
vj_ver = botj_vx * sinAngle - botj_vy * cosAngle; // j在两点连线的垂直速度
vij_par = vj_par - vi_par; // 从i看j，j向i直接靠近的速度
vij_ver = vj_ver - vi_ver; // 从i看j，j向i垂直远离的速度
t_catch = dist / vij_par;
if (fabs(t_catch) > f3_crash_t) // 碰撞时间还剩比较多的话，不作处理
continue;
}
// 说明这时候它们的速度保持不变的话，过了t_catch的时间，它们就会相撞
if (fabs(vij_ver * t_catch) < (ri + rj)) {
++crashNum[i];
++crashNum[j];
crashId[i][j] = 1;
crashId[j][i] = 1;
}
碰撞避免
\ \quad 在两个机器人即将接近时，让它们都向同一个方向，以 \pi 的角速度旋转，直至它们不再将对方视为碰撞目标（实际上在这里还额外保持多了一帧的碰撞避免动作）。对于具体是顺时针方向旋转还是逆时针方向旋转，则利用两个机器人的在它们连线上的垂直相对速度，使得旋转后，能否可以顺着这个垂直相对速度，或者说能否可以加大这个垂直相对速度，来决定顺逆方向。
if (deltax * delta_vy - deltay * delta_vx < 0) { // 向量的叉积，值为负，大家以pi的角速度旋转
rotate_dir[i][j] = true;
rotate_dir[j][i] = true;
}
else { // 值为正，大家以-pi的角速度旋转
rotate_dir[i][j] = false;
rotate_dir[j][i] = false;
}
工作台选择策略
按对选择工作台
\ \quad 选择工作台时，不以某一个进货工作台或某一个出货工作台为选择目标，而是以一对的进货的工作台和出货的工作台为选择目标。每一对的工作台都拥有其权值。当机器人更新工作台目标时，将遍历场上所有合理的工作台对，选权值最大的工作台对，分别作为进货工作台目标和出货工作台目标。在选择和释放工作台时，都要将相应的工作台的输入端和输出端置位和复位，以免机器人共同选择了一个目标，引起混乱。
int loopId1ToId2(int tp1, int tp2, int botId, int goodId, int& wbId1, int& wbId2, double& maxWeight);
权值函数
\ \quad 初赛的权值函数设置综合考虑了运送某种货物的收益，运送某种货物能否为工作台补齐原料，取得某种货物并把它运送至目标工作台销售的距离。
// 运送货物的最大收益数组，把第一位空出
const int valueArr[8] = { 0,3000,3200,3400,15000,15000,20000,29000 };
double value = valueArr[goodId];
int mstate = wbArr[wbId2].materialsState;
for (int i = 0; i < 8; ++i) {
if (mstate & 1) // 判断wbArr[wbid2]需不需要这个材料
value += valueArr[i] / 2; // 为了补全另外加的权重
mstate = mstate >> 1;
}
double _disBot2Wb1 = calDist(botArr[botId].x, botArr[botId].y, wbArr[wbId1].x, wbArr[wbId1].y);
return pow(value, 1.0) / (pow(_disBot2Wb1 + wbDis[wbId1][wbId2], 2.0));
不同机器人同时锁定一个工作台的输出端和多个不同的输入端
\ \quad 同一个工作台可以被不同的机器人同时选择，或者说锁定，只是需要额外投入精心的控制。给所有工作台的输出端设置一个状态位，给所有工作台的所有输入端分别设置一个状态位（实际实现时，是采用了位运算，仅用一个整数标识多个输入端的状态）。在某个工作台的输出端或多个不同的输入端被机器人选择时，就将对应位置位，当机器人完成了相应的操作后，就将对应位复位。
// 数组的第0项表示输入端是否被作为卖出的目标, 标识工作台的输入端被承载哪种类型货物的机器人谁选中，二进制位表示，例如，0b110，意味着它的输入端被持有1号货物和2号货物的机器人锁定。第1项表示是否被作为买入的目标
vector<vector<int>> wbIfTar{ 50,vector<int>(2, 0) };
// 置位
if (wbId1 != -1 && wbId2 != -1) {
botTarId[botId] = { 1,wbId1,wbId2 };
wbIfTar[wbId1][1] = 1;
if (wbArr[wbId2].wbType != 8 && wbArr[wbId2].wbType != 9)
wbIfTar[wbId2][0] = (wbIfTar[wbId2][0] | (1 << wbArr[wbId1].wbType));
return 0;
}
else {
botTarId[botId] = { 0, 0, 0 };
return 1;
}
// 取走货物后将工作台的输出端复位
wbIfTar[botTarId[i][1]][1] = 0;
// 卖出货物后将工作台的对应输入端复位
if (wbArr[bot.workbenchId].wbType != 8 && wbArr[bot.workbenchId].wbType != 9)
wbIfTar[botTarId[i][2]][0] ^= (1 << bot.gootsId);
防死锁
\ \quad 在每一帧中，遍历所有工作台的原料格，统计出当前每种类型的货物被需要的数量A。
initProductNeed(productNeed);
\ \quad 然后，遍历所有的机器人，统计出每种货物被四个机器人持有的数量与被四个机器人意向取得的数量之和B，A-B即是当前每种货物在此刻的将来被需要的数量。机器人在帧内寻找新的进货目标时，将忽视掉某种此刻的将来被需要的数量为0的货物，以此避免货物死锁。另外还需要在帧内，当某种货物被工作台推入生产时，增加对应类型的货物在此刻的将来被需要的数量，以完成帧内状态实时更新。
for (int i = 0; i < 4; ++i) {
// 如果持有产品，那它肯定是以或者将以某个工作台为目标，这样就消耗掉1个产品需求
if (botArr[i].gootsId > 0 && botArr[i].gootsId < 7) // 不管上一次有没有update成功，只要持有了产品，就会消耗掉1个产品需求
--productNeed[botArr[i].gootsId];
// 如果不持有产品，且之前能成功更新状态，且目前意图去拿某个产品(购入)，这样也消耗掉1个产品需求
if (botArr[i].gootsId == 0 && successUpdate[i]) // 如果之前不能成功更新状态，说明它在卖出产品后，找不到下一个目标，而botTarget[i]仍然等于它卖出产品的工作台id
--productNeed[wbArr[botTarId[i][1]].wbType];
}
动态更新工作台目标选择
\ \quad 如果当前机器人正在奔向进货（buy操作，买入产品）工作台，那么就试图动态更新它进货的目标工作台，因为有时候机器人在奔向进货的工作台的途中，某些工作台突然生产出了产品且距离更为接近，或者某些可用了的工作台与机器人的距离更为接近了（多见于目标工作台对称分布，旧目标工作台在背后，新目标工作台在前方，此时进行动态更新，就可以免去调头操作），这时候动态更新进货的目标工作台就有利可图（权值函数的计算方式与之前的一致）。
\ \quad 如果当前机器人正在奔向出货（sell操作，售出产品）工作台，那么久试图动态更新它出货的目标工作台，因为有时候机器人在奔向进货的工作台的途中，某些工作台突然空出了原料格且距离更为接近，或者某些可用了的工作台与机器人的距离更为接近了（多见于目标工作台对称分布，旧目标工作台在背后，新目标工作台在前方，此时进行动态更新，就可以免去调头操作），这时候动态更新出货的目标工作台就有利可图（权值函数的计算方式与之前的类似）。
\ \quad 然而在实际跑图时，有时候只用动态更新进货目标工作台比较好，有时候只用动态更新出货目标工作台比较好，有时候都不用比较好，比较难把握。
初赛总结
\ \quad 这次初赛打得很辛苦，但也算打得不错。从3.10号训练赛开始到3.26号正式赛结束，每天都能去实现新的思路，每天也都在上分。作为一个非计算机专业，没有接受过算法训练的本科生来说，我一开始的目标只是进个32强（复赛线）。但是后面排名每天都在涨，到了最后几天，还能稳定在前十，训练赛也以赛区第三的身份结束。到了正式赛的最后一天，还出乎意料地拿到过一次榜首（然后那个下午就开始浪了）。当然，这次比赛也打得很辛苦。尤其是排行榜实时更新，搞得自己每天都枕戈待旦，晚上12点睡早上5点起，中午只睡20分钟，其余时间都在写代码，这样的状态维持了半个月，简直比高考还辛苦。
\ \quad 这次初赛也有很多做得不好的地方。首先是没有建立一个统领全局的头文件管理代码，代码架构也写得不好，导致在进入复赛后，我们额外花了两天的时间来处理这些问题。然后是写bug写得太多，虽然都能把这些bug找到并修复好，但这一点还是严重拉低了效率，希望自己以后能多写一些代码来保持住感觉吧。最后是始终没有用到物品的剩余生产时间，这一点直到复赛结束后才被解决，而且我们在复赛现场问了另外一个团队，发现他们团队考虑上这个因素后，分数有了很大的提高，我们在这一点上吃了不少亏。