此文系第16届智能车智能视觉组-上海交通大学AuTop战队开源算法讲解第十一部分--激光打靶，专栏及开源方案链接：

llo：第16届智能车智能视觉组-上海交通大学AuTop战队开源汇总

最近有些小伙伴在问我激光打靶时，如何计算舵机的转角。所以特此补充一篇文章来进行介绍。

注：该篇文章需要一定程度的相机成像模型和坐标变换的知识，如果有机器人学的基础知识会更好。

一、坐标系定义

一个常规的二自由度舵机云台如下图所示

在上图中，我们将世界坐标系称为

  （w: world）、yaw轴舵机坐标系称为

  （y: yaw）、pitch轴舵机坐标系称为 

 （p: pitch）、激光坐标系称为

  （l: laser）、相机坐标系称为

  （c: camera）、靶牌坐标系称为

  （t: target）。

同时，当舵机发生旋转时，舵机坐标系本身不会跟着旋转。以pitch轴舵机为例，当pitch轴舵机低头后，对应的坐标系如下图所示

二、模型建立

三、参数标定

3.1 手眼标定

3.2 相机-激光标定

即标定

  。标定的关键在于建立方程，有了方程只需要进行优化求解即可。为此，我们采用以下方法：

1. 保持激光开启且激光点投射在标定板上，小幅度移动标定板并拍摄若干张照片。

2. 对上述图片进行去畸变操作

3. 识别（或手动选取）激光点在图片中的位置

4. 根据PnP计算得到标定板坐标系和相机坐标系之间的变换关系（

  ）

5. 根据激光点和标定板角点之间的关系，使用透视变换计算出激光点 P 在标定板坐标系下的坐标 

6. 得到方程 

7. 优化求解得到

可以注意到，当激光坐标系绕z轴旋转时，上述等式同样恒成立，这意味着

  的解不唯一。但这并不影响对舵机角度的计算，所以任取一个特解即可。

四、模型简化

第二节模型建立时，我们发现想要解出舵机角度，需要解一个复杂的三角方程，这个方程的解析解并不容易求取，而采用数值解则比较麻烦。所以是否可以通过巧妙的设计机械结构，从而简化舵机角度的求解。

在上图中，相机、激光、p轴舵机、yaw轴舵机处于同一铅垂线，且相机、激光坐标系完全平行，同时激光离地距离正好是靶牌离地的距离（15cm）。

此时打靶时可以保持p轴舵机不动，仅需移动y轴舵机即可，并且y轴舵机转角的计算完全不需要进行坐标系间的标定，同时也不需要求解PnP，只需要标定相机内参，使用小孔成像模型求反正切即可得到y轴舵机转角。

假设相机内参矩阵为（忽略畸变，如果有畸变需要首先去畸变）

靶牌中心点在图像上的坐标为

  ，则y轴舵机转角为

这个方法可以说是最简单的打靶方式，我们也同样使用了这个方案，但需要一定程度的机械安装精度保证。

SJTU-AuTop完整开源方案链接，如果觉得我们的方案对您有帮助，请在github上帮忙点个star吧:)

llo：第16届智能车智能视觉组-上海交通大学AuTop战队开源汇总