在视觉导航中，如何把各个传感器得到的空间信息进行融合是一个很棘手的问题。我们通过建立空间坐标转换方程利用高速的搜索算法得到了一种新的计算精度更高的坐标融合算法。

坐标关系图

现在假设我们已经从机器人的陀螺仪和电机编码器获得了一组机器人的坐标。同时我们又通过摄像头获得的视觉数据计算出了机器人的另一组坐标。虽然这两组坐标在数值上并不相等，但是显然它们之间存在着转换关系。如何把这两组坐标融合起来然后得到一组更精确的坐标呢？猛地一看，这似乎并不是一个很困难的问题，只要找到坐标系的变换关系就可以了。但是视觉算法提供的坐标其坐标原点的选取是很随机的。而且因为视觉坐标的没有确定的比例尺，所以我们还必须通过电机编码器来给视觉坐标进行标定。这个问题的难度随之增加。

当机器人在空间中连续的运动时，我们就会得到两组连续对应的坐标值。我们就要通过这两组坐标值，建立一系列的方程。通过这些方程解出坐标系的变换关系。然而求解时却发现，这个方程是一个非线性方程组，解起来非常困难。

经过我们的研究发现，摄像头本体坐标系和陀螺仪的本体坐标系之间是存在固定转换关系的。如果知道这个转换关系，这个方程就比较容易解。这个转换关系是由摄像头和陀螺仪之间的机械位置决定的，只要测量出来就可以了。然而我们并不满足通过测量而得出这个变换关系，因为一旦测量，就会引入测量误差。如果能只通过计算得到就不会有这个误差。最终我们找到了一种迭代的快速搜索算法解决了这个问题。

如果想要详细了解这个问题欢迎到社区看看。