基于增量PID算法的无人机跟踪模块

其中O-XY为图像像素坐标系，而C-XYZ为摄像机坐标系。

图1 图像像素坐标系和摄像机坐标系示意图

他们的转换关系如下：

 

 

建立如下坐标系：

图2 摄像机坐标系示意图

其中设置摄像头的角度为向下倾斜45度，那么地面上的P2正好能够投影在摄像头CMOS平面上的正中央P2’点，如果物点离得太远或太近，则投影在CMOS平面的其他地方，甚至投影在CMOS平面之外。无人机在跟踪的时候，应该尽量保证物点的投影像点尽量保持在镜头所拍摄画面的中心。当跟踪目标为P1点时，无人机应当携带摄像头像Z轴的正方向进行运动。当目标为为P2点时，无人机应当向Z轴的负方向进行运动。

当投影点在图像的偏上或者偏下部位时，此时无人机应当向前后进行移动；而当投影点在图像的偏左或者偏右部位时，此时无人机应当旋转偏航角，在此方针进行分析时，仅先考虑前者情况。

图3 摄像机坐标系平面图

图4 拍摄所得的图像

 

所以应当仿真的函数功能为：