公式

ρABΘΘ_goalαβ

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

参考

https://github.com/AtsushiSakai/PythonRobotics/blob/master/docs/modules/control/move_to_a_pose_control/move_to_a_pose_control_main.rst