# Obstacle Avoidance in a Three-Dimensional Dynamic Environment Based on Fuzzy Dynamic Windows
## 2 Formulation of the Collision-Avoidance Problem  
In this paper, we mainly focus on the 3D real-time trajectory planning of the kind of space-based robots, such as SPHERES [38] and astronaut assistant robots [39]. The agent can work in a space station or specific working range in which the flying distance is much shorter than the radius of the orbit. In addition, the algorithm can also be extended to other types of agent 3D trajectory planning. This section provides the mathematical model of the motion and the attitude of the spatial agent. In a 3D environment, the attitude of the agent is represented by the pitch angle \(\theta\), yaw angle \(\psi\), and roll angle \(\phi\), as shown in Figure 1. Under reference coordinate system (\(X_{f}\), \(Y_{f}\), \(Z_{f}\)), the equation of the agent's motion is written as:  
\[\left[ \begin{array}{c}X_{f(t+1)}\\ Y_{f(t+1)}\\ Z_{f(t+1)}\end{array} \right]=\left[ \begin{array}{c}X_{f(t)}\\ Y_{f(t)}\\ Z_{f(t)}\end{array} \right]+\Delta t*R^{f}_{b}\left[ \begin{array}{c}v^{b}_{g} \\ v^{b}_{g}\\ v^{b}_{g}\end{array} \right] \tag{1}\]  
where the \(\Delta t\) is the sampling time. The conversion between the reference coordinate system \(\alpha X_{f}Y_{f}Z_{f}\) and the body coordinate system \(\alpha X_{b}Y_{b}Z_{b}\) is in the order of 321. Therefore, the transformation matrix from the body coordinate system to the reference coordinate system is:  
\[\mathbf{R}^{f}_{b}=\left[ \begin{array}{ccc}c_{\psi}c_{\theta}&-s_{\psi}c_{ \theta}+c_{\psi}s_{\theta}s_{\phi}&s_{\psi}s_{\phi}+c_{\psi}s_{\theta}c_{\phi} \\ s_{\psi}c_{\theta}&c_{\psi}c_{\phi}+s_{\psi}s_{\theta}s_{\phi}&-c_{\psi}s_{ \phi}+s_{\psi}s_{\theta}c_{\phi}\\ -s_{\theta}&c_{\theta}s_{\phi}&c_{\theta}c_{\phi}\end{array} \right] \tag{2}\]  
where \(c\) and \(s\) are the cosine and sine, respectively. The relationship between the angular velocity component and Euler angle is:  
\[\mathbf{R}(\mathbf{\zeta})=\left[ \begin{array}{ccc}1&0&-s_{\theta}\\ 0&c_{\phi}&s_{\phi}c_{\theta}\\ 0&-s_{\phi}&c_{\phi}c_{\phi}\end{array} \right] \tag{3}\]\[\dot{\xi}=R(\xi)^{-1}\omega \tag{4}\]  
where \(\xi=[\phi,\,\theta,\,\psi]^{\mathrm{T}}\), \(\omega=[\omega_{x},\,\omega_{y},\,\omega_{z}]^{\mathrm{T}}\), \(\omega_{x}\), \(\omega_{y}\) and \(\omega_{z}\) are the components of the angular velocity \(\omega\) in the axis of the body coordinate. The equation of the attitude of the agent is written as:  
\[\left[ \begin{array}{c}\phi_{(t+1)}\\ \theta_{(t+1)}\\ \psi_{(t+1)}\end{array} \right]=\left[ \begin{array}{c}\phi_{(t)}\\ \theta_{(t)}\\ \psi_{(t)}\end{array} \right]+\dot{\xi}*\Delta t \tag{5}\]  
The trajectory of the agent is generated by Equations (1) and (5). Due to the limitation of the sensor, the activity range that the agent can perceive in a certain time is limited; for example, the effective detection distance of a vehicle-mounted lidar is within 6-8 m. Taking the geometric center of the agent as the origin, the range can be obtained by:  
\[S(t)=\left\{\mathbf{p}\in\mathbb{R}^{3}:\left\|\mathbf{p}(t)-\mathbf{p}\right\|\leq\mathrm{ r}_{s}\right\} \tag{6}\]  
where \(\mathbf{P}=[x,y,z]\) is the position of the agent, \(\mathbf{P}(t)\) is the point within the detection range of the agent. \(\mathrm{r}_{s}\) is the detection radius of the sensor. Within this range, the space occupied by obstacles can be expressed as:  
\[(t)=\left\{\mathbf{p}\in\mathbb{R}^{3}:\left\|\mathbf{p}(t)-\mathbf{p}_{o}(t)\right\|\leq \mathrm{r}_{o}\right\} \tag{7}\]  
where \(\mathbf{P}_{o}(t)=[x_{o}(t),\,y_{o}(t),\,z_{o}(t)]\) is the position of the obstacles, \(\mathrm{r}_{o}\) is the radius of the obstacles. In this paper, obstacles and agents are assumed to be spherical with the radius of \(R_{obstacle}\) and \(R_{agent}\). Users can also refer to the reference [40] to configure agents and obstacles by different boundary constraints. In addition, the obstacle information can be obtained by the sensors [41].