车辆控制:航向角跳变问题
问题描述
对车辆或者其他移动机器人进行轨迹跟踪等控制时,常常会对航向角误差进行控制,定义期望航向角为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 , −