车辆控制：航向角跳变问题（-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]中
  e r r _ y a w = { e r r _ y a w , −