公式
距离差值计算 \[ \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}\)为系数