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