Bresenham Algorithm

为了通过雷达构建占用栅格地图，需要计算雷达扫过的障碍栅格和非障碍栅格。如下图，

于是采用\(Bresenham Algorithm\)快速计算占用栅格集合。

算法思路

首先假设两个坐标点,分别为雷达测量的起点和终点

\[(x_1,y_1),(x_2,y_2) \]

且满足\(x_2>x_1,y_2>y_1\)，根据起始点信息计算直线方程：

\[\Delta x=x_2-x_1\\ \Delta y=y_2-y_1\\ m=\frac{\Delta y}{\Delta x}\\ b=y_1-mx_1=y_2-mx_2 \\直线方程为：y=mx+b \]

假设已经计算到点\((x_k,y_k)\)，下一个占用栅格坐标仅有两种情况

注意这里的\(x_k,y_k\)并非二维连续空间坐标，而是占用栅格地图的索引，其中包含栅格边长信息\(r\)实际坐标应为\((x_k\cdot r,y_k\cdot r)\)

\[(x_k+1,y_k)\\ (x_k+1,y_k+1)\\ \]

实际上我们更希望选取与真实直线距离更近的占用栅格坐标。

首先计算直线与轴\(x=(x_k+1)\cdot r\)交点\(y\),再分别计算图中\(d_1,d_2\)

\[\begin{aligned} d_1&=y-y_k\cdot r=m(x_k+1)\cdot r+b-y_k\cdot r\\ d_2&=(y_k+1)\cdot r-y=(y_k+1)\cdot r-m(x_k+1)\cdot r-b \end{aligned} \]

对\(d_1,d_2\)做差

\[d_1-d_2=2m\cdot (x_k+1)\cdot r-2y_k\cdot r+2b-r\\ d_1-d_2>0->d_2<d_1\\ d_1-d_2<0->d_1<d_2\\ \]

所以结果大于零代表距离\(y_k+1\)更近，反之为\(y_k\)。基本思路很简单，接下来需要做的是需要如何减少算法计算量。

减少浮点运算

\(d_1-d_2\)中仍然具有浮点运算，对于一个二选一的问题，不需要精确的浮点运算，其实更有价值的是符号(正负)信息。引入

\[\begin{aligned} p_k&=\Delta x(d_1-d_2)\\ &=[2\frac{\Delta y}{\Delta x}\cdot (x_k+1)\cdot r-2y_k\cdot r+2b-r]\cdot \Delta x\\ &=2\Delta y\cdot (x_k+1)\cdot r-2y_k\Delta x\cdot r+2b\Delta x-r\Delta x\\ &=2\Delta y\cdot x_k\cdot r+2\Delta y\cdot r-2y_k\Delta x\cdot r+2b\Delta x-r\Delta x\\ &=2\Delta y\cdot x_k\cdot r-2\Delta x\cdot y_k\cdot r+c \end{aligned} \]

其中\(c=2\Delta y\cdot r+2b\Delta x-r\Delta x\)为常量不随坐标发生变化，且由于假设\(x_2>x_1\)可得\(\Delta x>0\),所以\(p_k\)与\(d_1-d_2\)同号。

所以可以将\(p_k=f(x_k,y_k)\)作为下一步占用栅格旋转的决策依据，继续计算\(p_{k+1}\)，

\(x_{k+1}-x_k=1\)，\(x\)轴每次步进一个单位，(\(y_{k+1}-y_k\))为1或0；\(p_k>0\)为1反之为0

\[\begin{aligned} p_{k+1}&=2\Delta y\cdot (x_k+1)\cdot r-2\Delta x\cdot y_{k+1}\cdot r+c\\ p_{k+1}-p_k&=2\Delta y\cdot r-2\Delta x(y_{k+1}-y_k)\cdot r\\ p_{k+1}&=p_k+2\Delta y\cdot r-2\Delta x(y_{k+1}-y_k)\cdot r \end{aligned} \]

进而得到递推形式\(p_{k+1}=p_k+2\Delta y-2\Delta x(y_{k+1}-y_k)\)。

起点\((x_0,y_0)\)处的\(p_0\)为：

\[\begin{aligned} y_0\cdot r&=mx_0\cdot r+b\\ \Delta x\cdot y_or&=\Delta y\cdot x_0r+b\cdot \Delta x\\ p_0&=2\Delta y\cdot x_0\cdot r-2\Delta x\cdot y_0\cdot r+c\\ &=2\Delta y\cdot x_0\cdot r-2\Delta y\cdot x_0r-2b\cdot \Delta x+c\\ &=-2b\Delta x+2\Delta y\cdot r+2b\Delta x-r\Delta x\\ &=2\Delta y\cdot r-\Delta x \cdot r \\ &=(2\Delta y-\Delta x)\cdot r \end{aligned} \]