三坐标转台算法的思考

明天要去一家客户解决转台问题，当其他同事搞了四个月还没有搞好，我也没有信心去一次就能搞定。所以我今天就在做一些准备，就想从原理上先解决一下转台的算法，这样到了现场就比较容易地分析问题出在哪里。

作为一个附加的设备，控制柜中并没有能直接读出坐标值的功能，读到的值依旧是XYZ三个轴上的读数头的示值。而转台唯一能提供的就是当前旋转的角度。

而校准转台时又可以得到转台相对于机器坐标系的轴线坐标\[\begin{cases}x=x_0+it\\y=y_0+jt\\z=z_0+kt\end{cases}(t\in R)\]，那么可以得到当转台旋转角度为\(0^\circ\)时的坐标系仿射矩阵为(抱歉这里打成转置矩阵了，我用的算法库里行列就是反的╮(╯_╰)╭）\[A=\begin{bmatrix}\frac{k}{\sqrt{1-j^2}}&0&\frac{-i}{\sqrt{1-j^2}}&-x\\\frac{-i*j}{\sqrt{1-j^2}}&\sqrt{1-j^2}&\frac{-j*k}{\sqrt{1-j^2}}&-y\\i&j&k&-z\\0&0&0&1\end{bmatrix}^T\]。

当转台旋转了\(\theta^\circ\)时，坐标系为原坐标系乘以一个旋转矩阵\[A’=A*\begin{bmatrix}1&0&0&0\\0&\cos{\theta}&-\sin{\theta}&0\\0&\sin{\theta}&\cos{\theta}&0\\0&0&0&1\end{bmatrix}\]

因为三坐标取点是离散的，那么每个角度采的点只是\(\theta\)不同而已，那么就可以通过\(A’\)矩阵将每个点转换到转台的坐标系上。