公式

ρ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