Bresenham Algorithm

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

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

算法思路

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

(x_{1}, y_{1}), (x_{2}, y_{2})

$(x_1,y_1),(x_2,y_2)$

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

Δ x = x_{2} - x_{1} Δ y = y_{2} - y_{1} m = \frac{Δ y}{Δ x} b = y_{1} - m x_{1} = y_{2} - m x_{2} 直 线 方 程 为 ： y = m x + b

$\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_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}

$\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} = 2 m \cdot (x_{k} + 1) \cdot r - 2 y_{k} \cdot r + 2 b - r d_{1} - d_{2} > 0 - > d_{2} < d_{1} d_{1} - d_{2} < 0 - > 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} & = Δ x (d_{1} - d_{2}) \\ = [2 \frac{Δ y}{Δ x} \cdot (x_{k} + 1) \cdot r - 2 y_{k} \cdot r + 2 b - r] \cdot Δ x \\ = 2 Δ y \cdot (x_{k} + 1) \cdot r - 2 y_{k} Δ x \cdot r + 2 b Δ x - r Δ x \\ = 2 Δ y \cdot x_{k} \cdot r + 2 Δ y \cdot r - 2 y_{k} Δ x \cdot r + 2 b Δ x - r Δ x \\ = 2 Δ y \cdot x_{k} \cdot r - 2 Δ x \cdot y_{k} \cdot r + c \end{aligned}

$\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 Δ y \cdot (x_{k} + 1) \cdot r - 2 Δ x \cdot y_{k + 1} \cdot r + c \\ p_{k + 1} - p_{k} & = 2 Δ y \cdot r - 2 Δ x (y_{k + 1} - y_{k}) \cdot r \\ p_{k + 1} & = p_{k} + 2 Δ y \cdot r - 2 Δ x (y_{k + 1} - y_{k}) \cdot r \end{aligned}

$\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 & = m x_{0} \cdot r + b \\ Δ x \cdot y_{o} r & = Δ y \cdot x_{0} r + b \cdot Δ x \\ p_{0} & = 2 Δ y \cdot x_{0} \cdot r - 2 Δ x \cdot y_{0} \cdot r + c \\ = 2 Δ y \cdot x_{0} \cdot r - 2 Δ y \cdot x_{0} r - 2 b \cdot Δ x + c \\ = - 2 b Δ x + 2 Δ y \cdot r + 2 b Δ x - r Δ x \\ = 2 Δ y \cdot r - Δ x \cdot r \\ = (2 Δ y - Δ x) \cdot r \end{aligned}

$\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}$