# Design of Autonomous Mobile Robot for Cleaning in the Environment with Obstacles
## 3 Control Algorithm
As long as the robot remains in the avoidance region, its desired coordinates are periodically updated at certain discrete instants of time. The time period \(t_{u}\) of this update process is large in comparison to the main control loop sample time._  
Assumption 1 tells that the desired path of the virtual robot should be planned in such a way that in the steady state, the virtual robot should remain outside of the collision avoidance regions. Assumption 2 means that the tracking process is temporarily suspended when the follower robot gets into collision regions because collision avoidance has a higher priority. Once the robot is outside of the collision detection region, it updates the reference to the new values. In addition, when the robot is in the collision avoidance region, its reference trajectory is periodically updated. It supports leaving the unstable equilibrium points (which occurs, for example, when one robot is located precisely between the other robots and its goal) if the reference trajectory is exciting enough. In rare cases, the robot may get stuck at a saddle point, but the set of such points is of measure zero and is not considered further. The error with respect to the coordinate frame fixed to the robot can be calculated as follows:  
\[\left[ \begin{array}{c}e_{x}\\ e_{y}\\ e_{\theta}\end{array} \right]=\left[ \begin{array}{ccc}\cos(\theta)&\sin( \theta)&0\\ -\sin(\theta)&\cos(\theta)&0\\ 0&0&1\end{array} \right]\left[ \begin{array}{c}p_{x}\\ p_{y}\\ p_{\theta}\end{array} \right]. \tag{5}\]  
The error dynamics can be written with the above equations and non-holonomic constraint \(\dot{y}\cos(\theta)-\dot{x}\sin(\theta)=0\) as follows:\[\dot{e}_{x} =e_{y}\omega-v+v_{d}\cos e_{\theta}\] \[\dot{e}_{y} =-e_{x}\omega+v_{d}\sin e_{\theta}\] \[\dot{e}_{\theta} =\omega_{d}-\omega, \tag{6}\]  
where \(v_{d}\) and \(\omega_{d}\) are controls of the reference virtual robot. The position error and collision avoidance terms can be used to calculate position correction variables as follows:  
\[P_{x} =p_{x}-\sum_{j=1}^{M}\frac{\partial V_{aj}}{\partial x}\] \[P_{y} =p_{y}-\sum_{j=1}^{M}\frac{\partial V_{aj}}{\partial y}. \tag{7}\]  
\(V_{aj}\) depends on \(x\) and \(y\) according to Equation (3); \(M\) is a number of static obstacles in the task space. The robot avoids collisions with them. The correction variables can be transformed to the local coordinate frame fixed to the geometric center of the mobile platform:  
\[\left[ \begin{array}{c}E_{x}\\ E_{y}\\ e_{\theta}\end{array} \right]=\left[ \begin{array}{ccc}\cos(\theta)&\sin( \theta)&0\\ -\sin(\theta)&\cos(\theta)&0\\ 0&0&1\end{array} \right]\left[ \begin{array}{c}P_{x}\\ P_{y}\\ p_{\theta}\end{array} \right]. \tag{8}\]  
Equation (8) can be transformed to the following form [8]:  
\[E_{x} =p_{x}\cos(\theta)+p_{y}\sin(\theta)+\sum_{j=1}^{M}\frac{ \partial V_{aj}}{\partial e_{x}}\] \[E_{y} =-p_{x}\sin(\theta)+p_{y}\cos(\theta)+\sum_{j=1}^{M}\frac{ \partial V_{aj}}{\partial e_{y}}\] \[e_{\theta} =p_{\theta} \tag{9}\]  
where each derivative of the APF is transformed from the global coordinate frame to the local coordinate frame fixed to the robot. Finally, the correction variables expressed with respect to the local coordinate frame are as follows:  
\[E_{x} =e_{x}+\sum_{j=1}^{M}\frac{\partial V_{aj}}{\partial e_{x}}\] \[E_{y} =e_{y}+\sum_{j=1}^{M}\frac{\partial V_{aj}}{\partial e_{y}}. \tag{10}\]  
The trajectory tracking algorithm combined with collision avoidance is as follows:  
\[v =v_{d}+c_{2}E_{x} \tag{11}\] \[\omega =\omega_{d}+h(t,E_{y})+c_{1}e_{\theta}\]  
where \(c_{1}\) and \(c_{2}\) are the positive constant design parameters, while \(h(t,E_{y})\) depends on \(E_{y}\) and continuously differentiable function. \(h(t,E_{y})\) plays a vital role in the persistent excitation of the virtual robots angular velocity. **Assumption 3**.