公式
距离差值计算
\[ \rho = \sqrt{(x_{robot} - x_{target})^2 + (y_{robot} - y_{target})^2}. \]
距离决定机器人的运行速度
角度差值计算
\[ \alpha = (\arctan2(y_{diff}, x_{diff}) - \theta + \pi) mod (2\pi) - \pi \]
得到\([-\pi,\pi]\)区间内的值
调整角度需要用到\(\beta\)
\[ \beta = (\theta_{goal} - \theta - \alpha + \pi) mod (2\pi) - \pi \]
为了抵消机器人在目的地点附近的\(\alpha\)的影响，\(-\alpha\)引入进来
速度计算，线速度：
\[ v = \left\{\begin{matrix} k_{\rho}\rho,\quad |\alpha|\leq\frac{\pi}{2}\\ -k_{\rho}\rho,\quad |\alpha|>\frac{\pi}{2}\\ \end{matrix}\right. \]
角速度：
\[ \omega=k_{\alpha}\alpha-k_{\beta}\beta \]
其中\(k_{\rho},k_{\alpha},k_{\beta}\)为系数