车辆控制:航向角跳变问题
问题描述
对车辆或者其他移动机器人进行轨迹跟踪等控制时,常常会对航向角误差进行控制,定义期望航向角为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 , − π < e r r _ y a w < π e r r _ y a w − 2 ∗ p i , e r r _ y a w > = π e r r _ y a w + 2 ∗ p i , e r r _ y a w < = − π err\_yaw=\Bigg\{ \begin{matrix} err\_yaw, \quad -\pi<err\_yaw<\pi \\ err\_yaw-2*pi, \quad err\_yaw>=\pi \\ err\_yaw+2*pi, \quad err\_yaw<=-\pi \end{matrix} err_yaw={err_yaw,−π<err_yaw<πerr_yaw−2∗pi,err_yaw>=πerr_yaw+2∗pi,err_yaw<=−π
经过方案2处理后的情况变为:
des_yaw | actual_yaw | err_yaw | 动作 |
---|---|---|---|
30° | 20° | -10° | 左转10° |
30° | 340° | -50° | 左转50° |
这样问题就能够彻底解决。