01Note 01 of 04: 笔记 01 / 04：The K230 Gimbal Tracking LoopK230 云台追靶回路

By the afternoon of day three, everything else on the car moved. The tracking ran steadily; only the gimbal on top refused to cooperate. It would swing to the end of its travel and stay there, or correct itself further and further away from the target, and once that was sorted it would follow the target partway and then simply slide off. The three of us had more or less written off the aiming part. The K230 code was my teammate's, and I went through it from the top and came out with three faults: the servo limits were wrong, the output direction was reversed, and an if/else was missing a layer of branching. With all three fixed the gimbal came alive. They look unrelated, but each one sits in a different link of the same loop.

第三天下午，车上别的部分都动起来了，循迹也跑得稳，就剩上面那个云台不配合：要么一下甩到行程尽头不回来，要么越修越偏，好不容易方向对了，追着靶子走到半路，又自己滑开了。三个人基本已经认定瞄准这块交不了差。K230 的代码是负责视觉的队友写的，我从头往下读，最后揪出三处毛病：舵机的限位没设好，输出方向反了，一组 if/else 少写了一层分支。三处改完，云台一下子活了。这三处看着八竿子打不着，放回同一条回路里，正好一处卡一环。

From Pixel Error to Angle从像素误差到角度

The loop itself is short. The camera grabs a frame and scales it down to the model's 320 × 320 input, padding the leftover strip with grey, and the model looks for its one and only class, yuan, Chinese for circle. The centre of the box minus the screen centre, divided by 400 horizontally and 240 vertically and multiplied by 100, gives how far the target sits from the centre as a percentage of half the screen. That percentage goes into the PID, and the PID's output never reaches the servo directly:

回路本身不长。摄像头抓一帧，按比例缩到模型要的 320 × 320，空出来的边用灰色补齐，模型在里面找它唯一认识的类别 yuan，也就是圆。框的中心减去屏幕中心，横向除以 400、纵向除以 240，再乘 100，得到靶子偏离中心占了半个屏幕的百分之几。这个百分数交给 PID，PID 算出来的东西并不直接交给舵机：

pan_current_angle -= pan_pid.compute(pan_error * 100) * 0.15
tilt_current_angle += tilt_pid.compute(tilt_error * 100) * 0.15

That line deserves a second look. The PID decides how far to move in this frame, and the angle is the running total of those moves. Once its output is accumulated like that, the proportional term already acts as an integral on position: as long as the target is off-centre, the angle keeps creeping toward it and only stops at zero error. The integral term then integrates a second time on top, which is why the pan axis gets a ki of just 0.001. The increment is also applied per frame, so when inference slows down, the gimbal slows down with it. The controller swaps its gains by error size as well: above half the output limit kp goes up by half and the integral is switched off, and close to the centre kp drops while the integral is boosted to grind away the last bit of offset. Varying gains by operating point like this is called gain scheduling; here it comes in three crude steps.

这一行值得多看两眼。PID 管的是这一帧转多少，角度是一帧一帧加出来的。输出这么一累加，比例项自己就成了对位置的积分：靶子只要还偏着，角度就一直往那边挪，误差归零才停。积分项再在上面积一层，所以水平轴的 ki 只给了 0.001。增量按帧算还有个副作用，推理一慢，云台跟着慢。控制器还会按误差大小换参数：误差超过输出上限的一半，kp 加五成，积分直接关掉；靠近中心以后 kp 收一点，积分加一点，把最后那点偏差磨掉。这种按工况换参数的做法叫增益调度，这里是最粗糙的三档版本。

Servo Limits and Windup舵机限位与积分饱和

The first fault was the most visible one: the servo kept turning out of range. Both servos take 50 Hz PWM, where pulses from 0.5 ms to 2.5 ms span the travel, 2.5% to 12.5% duty; the OpenMV servo tutorial uses the same convention. set_servo_angle maps the angle onto that range around a centre value and clips it to a minimum and a maximum. With the limits wrong, the servo is told to go past where it can physically go, and the accumulating angle makes things worse. The target is still off-centre, so every frame adds more to the angle variable, which runs further and further past the travel. When the target comes back, the variable first has to unwind all that excess before the servo moves at all. That is integrator windup, met through a servo. In the final code the pan angle variable is clipped to ±90° before mapping, and both duty cycles are held inside their own ranges; the tilt axis only gets 7.5% to 12%, so the laser can neither nod down at the floor nor tip up at the ceiling.

第一处毛病最直观：舵机老是转出界。两个舵机吃的都是 50 Hz 的 PWM，脉宽 0.5 ms 到 2.5 ms 对应整个行程，也就是 2.5% 到 12.5% 的占空比，OpenMV 的舵机教程用的也是这套约定。set_servo_angle 以一个中心值为基准，把角度映射成占空比，再夹在上下限之间。限位一错，舵机就被指到它物理上去不了的地方；角度又是累加出来的，这下雪上加霜：靶子还偏着，每一帧都往角度变量上加，变量越跑越远，早就超出了行程。等靶子回来，变量得先把多出来的那一大截退回去，舵机才肯动。这就是积分饱和，只不过是在舵机身上撞见的。最终代码里，水平那一路的角度变量先夹在 ±90° 再映射，两路占空比各自夹在自己的范围里；俯仰轴只给了 7.5% 到 12%，激光既不会低头扫到地上，也不会抬头打到天花板。

Loop Sign回路方向

The second fault looked almost deliberate, as if the gimbal were dodging the target: every correction made the error bigger. Whether one axis should subtract and the other add cannot be read off the code. The camera image is mirrored horizontally (set_hmirror(True)), the servos are mounted a particular way round, and the duty-to-angle mapping carries its own sign. Multiply the signs of every link around the loop and the product has to be negative for the loop to pull the target back toward the centre. Flip one link and it becomes positive feedback: the target sits right of centre, the gimbal turns further left, the target slides further right, and the angle accumulates all the way to the limit. The comment still sitting in set_servo_angle, about turning a plus into a minus to invert the servo output, is the scar that fix left.

第二处毛病看着简直像故意的，云台仿佛在躲着靶子走：误差越修越大。哪个轴该减、哪个轴该加，光看代码是看不出来的。摄像头画面做了水平镜像（set_hmirror(True)），舵机有它自己的装法，占空比到角度的映射也带着一个正负号。把回路一圈上每一环的符号乘起来，结果得是负的，回路才会把靶子往中心拉。只要有一环反了，就成了正反馈：靶子在中心右边，云台往左转，靶子在画面里滑得更靠右，角度一路累加到限位为止。set_servo_angle 里那行“将加号改为减号，实现舵机输出取反”的注释，就是这次修改留下的疤。

Branches and Lost Targets分支与丢靶

The third fault took the longest. Every frame, the program has to pick one path out of several situations: target seen and inside the centre zone, seen but off-centre, lost after having been seen, and never seen at all. Inside 25 pixels of the centre on both axes it pulls LASER low and the laser comes on; outside that, the laser goes off, and 250 ms without a detection also switches it off. Only when the target has never been seen does the gimbal search, stepping the pan duty cycle up by 0.2% every 100 ms. The if/else that handled these situations was missing a layer, so one of them had a way in and no way out. Once the program wandered in, the gimbal stopped chasing, which from the outside looked exactly like tracking partway and then sliding off. Filling in that branch was the last of the three fixes.

第三处毛病花的时间最长。每一帧，程序都得在几种情况里挑一条路：看到了靶子而且在中心区里，看到了但还偏着，看到过又丢了，从来没看到过。两个方向都进了中心 25 个像素，就把 LASER 拉低，激光亮；出了这个范围就灭，连续 250 ms 没有检测结果也灭。只有从来没看到过靶子的时候，云台才会去找，水平占空比每 100 ms 加 0.2%，一路扫过去。处理这几种情况的那组 if/else 少了一层，其中一种情况有进口没出口，程序一走进去，云台就不追了，从外面看，正好就是追到一半滑开、再也不接着追。把这层分支补上，是三处里最后修好的一处。

The same branches decided how the extensions were lost at the judging. The search branch is guarded by a flag that is set the first time the target is detected and never cleared again. At a right-angle corner the car body swung round, the target was thrown out of frame, and the program landed in the lost-after-seen situation, which only turns the laser off and leaves the gimbal parked where it was. The car's turn rate, which the MPU6050 on the MSPM0 side measures all the time, never reaches the K230; the two controllers run on their own. The way I now understand the corner problem, the turn rate should be fed forward to the gimbal, because a camera alone only finds out about the turn once the target has already left the picture.

测评时发挥部分是怎么丢的，也落在这几条分支上。找靶那条分支被一个标志位挡着，第一次看到靶子时置位，之后再也不清零。车过直角弯，车身一转，靶子被甩出画面，程序进的是看到过又丢了那条路，这条路只关激光，云台就停在原地。车身转得多快，MSPM0 那边的 MPU6050 一直在测，可这个数从来没送到 K230，两套控制器各跑各的。我现在对过弯的理解是，应该把车身的转速前馈给云台；光靠摄像头，转弯这件事要等靶子已经出了画面才知道。

On the third afternoon the gimbal came back to life because the loop was finally closed the right way round, within the right limits, with every situation given an exit. At the judging it lost the target at the corners for a different reason: what it was missing was how fast the car was turning, a piece of information this loop never had.

第三天下午云台能活过来，靠的是回路终于闭对了：方向对，限位对，每种情况都有出口。测评时它在直角弯上追不回来，缺的是另一样东西：车身转了多快，这个数这条回路从头到尾就没拿到过。