# A Vision-Based Approach for Autonomous Motion in Cluttered Environments
## 3 Methods
### Perception System
#### 3.2.4 The Occupancy-Grid-Mapping Algorithm  
In Sections 3.2.2 and 3.2.3, we introduced how to estimate the occupancy state of the cell observed by the camera from the depth map and pose information. In this section, we fuse observation data to build an environment map for robot navigation in real time, based on the occupancy grid map mapping algorithm. This algorithm mainly consists of two steps. The first step is to build a three-dimensional (3D) map of the environment by using an occupancy-grid-mapping algorithm. After that, the collision between the 3D map and the robot is determined, and then a 2D map for robot navigation is built.  
First, we use the occupancy-grid-mapping algorithm introduced in Reference [29] to establish a 3D occupancy map of the environment. The details are shown in Algorithm 1. The input of the algorithm is the log-likelihood ratios, \(l_{t}\), of the map at time \(t-1\); the robot state, \(x_{t}\); and the observation value, \(z_{t}\), at the current time, \(t\). The output of the algorithm is the updated likelihood ratios, \(l_{t}\), at the current time.  
Figure 4: Schematic diagram of sensor inversion model. The yellow cell is the observed obstacle, and the blues are idle cells calculated by the Bresenham algorithm.  
```
Input:\(\{l_{i-1,i}\},x_{t},z_{t}\) Output:\(l_{t,i}\) for all cells \(m_{i}\)do If\(m_{i}\) in perceptual field of \(z_{t}\)then \(l_{t,i}=l_{t-1,i}+inv\_model(m_{i},x_{t},z_{t})-l_{0}\) else \(l_{t,i}=l_{t-1,i}\) Endif Endfor return\(\{l_{t,i}\}\)
```  
**Algorithm 1**Occupancy grid mapping.  
The definition of log-likelihood ratios, \(l_{t,i}\), is shown in Formula (6), where \(x_{1:t}\) is the robot state information, and \(z_{1:t}\) is the observation value from time 1 to time \(t\), \(p(m_{i}|z_{1:t},x_{1:t})\) is the probability that the cell \(m_{i}\) to be occupied estimated by \(x_{1:t}\) and \(z_{1:t}\). Therefore, the log-likelihood ratios indicate the logarithmic value of the ratio of the probability of the cell \(m_{i}\) being occupied to the probability of being free.  
\[l_{t,i}=log\frac{p(m_{i}|z_{1:t},x_{1:t})}{1-p(m_{i}|z_{1:t},x_{1:t})} \tag{6}\]  
The literature [29] derives the update method of log-likelihood ratios of \(m_{i}\). If the cell \(m_{i}\) is not observed, its log-likelihood ratio remains unchanged. Otherwise, the inversion model of the sensor needs to be used to estimate its update amount. As shown in Formula (7), the inversion model of the sensor is the log-likelihood ratio of the cell \(m_{i}\) inferred from the robot's \(x_{t}\) and the observation \(z_{t}\). It can be obtained by using the algorithms introduced in Sections 3.2.2 and 3.2.3.  
\[inv\_model(m_{i},x_{t},z_{t})=l(m_{i}|z_{t},x_{t}) \tag{7}\]  
Since the robot is moving in a 2D plane, it is necessary to convert the 3D map to a 2D map for robot navigation. The corresponding algorithm is shown in Algorithm 2. The two-dimensional map needs to represent the space that the robot will collide with obstacles. The height of the robot is \(h\). For the coordinates \((x_{i},y_{i})\), \(0\leq z\leq h\); if the probability value of the cell \((x_{i},y_{i},z_{i})\) in the 3D map \(Map_{3d}(x_{i},y_{i},z_{i})\geq l_{thr}\), then the coordinate \((x_{i},y_{i},z_{i})\) will be considered as an obstacle where the robot will collide with obstacles. Thus, the 2D map at \((x_{i},y_{i})\) should be occupied; otherwise, it is idle.  
```
Input:\(Map\_3d\) Output:\(Mpa\_2d\) for\(x=x_{min}:x_{max}\)do for\(y=y_{min}:y_{max}\)do \(Map\_2d(x,y)=0\) for\(z=0:h\) if\(Map_{3d(x,y,z)}\geq l_{thr}\) \(Map\_2d(x,y)=1\) endif endfor endfor
```  
**Algorithm 2**Converting the 3D map into a 2D map.