辅助瞄准初步实现

[imuncle]

trafficstars

时间过得很快，转眼寒假就结束了，又是新的一轮调车。寒假前成功与视觉组对接，接收到了视觉组发送的角度信息。是时候用上它了。

视觉组发送的角度是相对角度，比如现在我的云台在3点钟方向，目标在2点钟方向，视觉传递给电控的数据就是-30°，即让云台向左转动30度。

视觉计算出的角度很大程度上依赖于摄像头的单目测距，但往往总是差一点距离，导致角度不是完全准确，所以我实现辅助瞄准的时候大部分时候都在强行校准角度信息。

辅助瞄准基本实现

先说说我是怎么实现辅助瞄准的。

就拿yaw轴来说事，在我的代码中，云台电机位置环PID的反馈值是编码器的值，也就是real_position，具体的可以查看我前面写的文章。在正常模式下，云台电机的位置环期望值是由遥控器控制的，在辅助瞄准模式下，这个期望值则由视觉的角度数据确定。实现代码很简单：

if(hero.workstate == RemoteControl)         //遥控器控制模式
{
        if(Remote.rc.s1 == 2 && !Is_Error(1<<12))
	{
		hero.gimbal.YawAngle = yaw_motor.fdbPosition - PC_Data.yaw * 22.755f;
		hero.gimbal.PitchAngle = pitch_motor.real_position - PC_Data.pitch * 22.755f;
	}
	else
	{
		hero.gimbal.YawAngle = hero.gimbal.YawBiasAngle - (Remote.rc.ch2 - 1024);
		hero.gimbal.PitchAngle = hero.gimbal.PitchBiasAngle + (Remote.rc.ch3 - 1024);
	}
}

我直接在当前云台编码器的基础上进行角度的加减，代码中的22.755是单位换算，视觉的数据单位是°，而编码器一圈是8192，之间的转换倍数是22.755。

至于角度到底是加还是减则要看实际的摄像头和电机的安装方式确定。

角度修正

如果按照上述的方式实现，假设目标机器人和击打的机器人的位置是静止不动的，那么云台位置环的期望值应该是一个定值，这样云台才能最快速地指向目标。可以想象，如果视觉计算出的角度偏小，那么云台响应不及时，如果角度偏大，则云台会超调，严重会发生振荡。

实际情况是由于单目测距的结果偏大，导致角度偏小，那么我是怎么发现这个问题的呢？其实很简单，只需要采集两组数据就行了。

比如当目标静止时，采集到了以下两组数据：

yaw轴编码器值	视觉计算的角度
2200	-15
1500	10

两组数据的编码器差值是700，换算成角度为30.762°，而视觉测得的角度差为25°，所以可以认为视觉的角度小了1.23倍，于是相关的代码应该手动修改为：

hero.gimbal.YawAngle = yaw_motor.fdbPosition + PC_Data.yaw * 27.988f;

但这样始终不是万全之策，因为视觉的测距偏差不是理想的呈现倍数变化，所以在2m处标定的数据在瞄准5m处的目标时就会出现问题。

存在的问题

上面说到的角度不准确是问题之一，另一个严重的问题，也是困扰广大参赛队的问题，就是延时问题，可以上一个动态图给大家感受一下：

重点关注上图的左侧部分，可以明显看到瞄准有延迟，在面对高速变化移动的装甲板时基本瞄不准。这里有多方面的原因，首先是摄像头的固有延时，对于摄像头的延时的测量，可以直接在电脑屏幕上打开一个时钟，然后对比摄像头与屏幕中的时钟的时间差，基本上就是摄像头的固有延时，经过测试，摄像头的延时大概60ms，对于这个问题，加钱才能解决。

第二原因是识别算法的耗时，视觉算法处理一帧图像大概20ms,。这个解决办法是加入一些滤波算法。

还有一个原因就是电控的锅了，电机的响应不够及时。这个可以通过调整电机的PID参数解决，我的经验是将速度环的参数P往小调，将位置环的参数P往大调，一个减小，一个增大，能维持平衡。

改进方向

接下来的改进方向当然还是分为电控和视觉两个方面。

电控方面

• 加一点简单的滤波，让数据更加平滑
• 优化PID参数，加快响应速度
• 完善意外情况的处理逻辑，比如丢失目标时、切换状态时的逻辑处理等

视觉方面

• 估计得加卡尔曼滤波处理一下

[lucainiaoge]

角度修正这里

两组数据的编码器差值是700，换算成角度为30.762°，而视觉测得的角度差为25°，所以可以认为视觉的角度小了1.23倍，于是相关的代码应该手动修改为： hero.gimbal.YawAngle = yaw_motor.fdbPosition + PC_Data.yaw * 27.988f;

或许可以考虑一下动态修正 calibration_factor = (yaw.bias_angle - yaw.last_bias_angle)/22.755/(vision_angle-last_vision_angle) hero.gimbal.YawAngle = yaw_motor.fdbPosition + PC_Data.yaw * calibration_factor;

[imuncle]

@lucainiaoge 非常好的想法！！ 👍 但有一个小错误，在此更正一下：

calibration_factor = (yaw.fdbPosition - yaw.last_fdbPosition)/(vision_angle-last_vision_angle)
hero.gimbal.YawAngle = yaw_motor.fdbPosition + PC_Data.yaw * calibration_factor;

那个22.755应该去掉，另外bias_angle的本意是角度偏差，在这里指的是云台中间位置处的电机编码器值，是一个常量。