车辆控制：航向角跳变问题（-pi-＞pi 或 0-＞2*pi）

车辆控制：航向角跳变问题

问题描述

对车辆或者其他移动机器人进行轨迹跟踪等控制时，常常会对航向角误差进行控制，定义期望航向角为des_yaw,实际航向角为actual_yaw,则航向角误差为err_yaw=actual_yaw-des_yaw。由传感器采集的航向角的范围在[-pi,pi]或[0,2*pi],于是在-pi->pi（0->2*pi）切换时，会出现问题。
我们拿车辆控制举例，假设航向角的范围在[0,2*pi]之间，当err_yaw<0执行器左转，反之执行器右转，具体举例以下两种情况：

des_yaw	actual_yaw	err_yaw	动作
30°	20°	-10°	左转10°
30°	340°	310°	右转310°

分析完上面两种情况就发现问题了，第一种情况是正常情况，第二种情况err_yaw =310°导致车辆执行器通过右转310°来达到期望航向角，这显然不合理，通过左转50°才是更合理的选择，但是如果不进行处理，控制器则会执行右转310°。

解决方案

• 方案1： 处理航向角，把航向角从[0,2*pi]转换为[-pi,pi]，则可以消除0°附近的跳变问题，但是治标不治本，在pi附近仍然会存在跳变问题。
• 方案2：处理航向角误差err_yaw，把航向角误差转换到[-pi,pi]中
  $$err\_yaw=\{\begin{array}{c}err\_yaw,-\pi <err\_yaw<\pi \\ err\_yaw-2\ast pi,err\_yaw>=\pi \\ err\_yaw+2\ast pi,err\_yaw<=-\pi \end{array}$$
  经过方案2处理后的情况变为：

des_yaw	actual_yaw	err_yaw	动作
30°	20°	-10°	左转10°
30°	340°	-50°	左转50°

这样问题就能够彻底解决。