激光雷达定位算法在FPGA中的实现——section1 原理推导

1、前言

        关于算法在FPGA中的实现，本次利用业余的时间推出一个系列章节，专门记录从算法的远离推导、公式计算、Matlab的实现、Matlab定点化仿真、FPGA算法模块系统设计、FPGA算法模块代码开发与仿真、FPGA算法计算结果与Matlab定点化结果对比、上板调试等，在此做一下相关的记录和总结，做到温故知新。

2、激光雷达定位原理

        这里以Detector在Global Coordinate System（原点为O）中运动为背景，Detector本身有运动的正方向，因此可以以Detector Od为原点组成Local Coordianate System；

        系统中的Markers，提前设计好它们在Global Coordinate System中的global coordinate（xi , yi），Detector可以实时探测到Markers在Local Coordiante System中的local coordinate（xi' , yi'），通过Markers的global coordinate和local coordinate实时动态的计算出Detector的global coordinate，完成Detector在Global Coordinate System中的定位功能。注意，Global Coordinate System的原点为（0 , 0），Local Coordiante System的原点为Detector的位置，它在Global Coordiante System的global coordinate为（x , y），如下图所示：

3、原理公式推导

        下面推导下相关公式：

                

        公式化简：

                

        进一步化简：

                

        转化为矩阵乘法：

                

        我们要计算x和y，其他的都可以看成是常量，因此可以看成是如下矩阵运算：

                

        那么我们需要对A矩阵做转置和求逆运算：

                

        其中

                

                 

                

最后将激光雷达的全局坐标和角度信息矩阵X化简为：

                 ​​​​​​​

4、总结

        定位算法原理公式的推导设计线性代数相关的基础知识，需要提前进行准备。