Python 计算路径跟踪的横向和角度误差

我在ROS从事一个与路径跟踪相关的项目。我有一个Roomba机器人，它必须遵循一条路径

要编写路径跟随器AI，我需要计算横向和角度误差

我有我的机器人的3个信息（x，y和偏航）和点路径的坐标（x，y）

为了计算横向误差，我使用以下公式：

dx = path_x - robot_x 
dy = path_y - robot_y

path_yaw = math.atan2(dy, dx) 

cos_yaw = math.cos(yaw)
sin_yaw = math.sin(yaw)

error_lat_ = dx * s_yaw - dy * c_yaw

error_ang_ = norm_angle(robot_yaw - path_yaw)

这个公式给了我错误的结果。让我们看看是否将robot_x和robot_y设置为0

对于上述点（x，y），结果如下：

好：

dx = path_x - robot_x 
dy = path_y - robot_y

path_yaw = math.atan2(dy, dx) 

cos_yaw = math.cos(yaw)
sin_yaw = math.sin(yaw)

error_lat_ = dx * s_yaw - dy * c_yaw

error_ang_ = norm_angle(robot_yaw - path_yaw)

• （1，0）=0
• （-1,0）=3.14

错误

• （0，1）=0
• （0，-1）=0

对于（0，1）和（0，-1），侧向应为1

为了计算角度误差，我使用以下公式：

dx = path_x - robot_x 
dy = path_y - robot_y

path_yaw = math.atan2(dy, dx) 

cos_yaw = math.cos(yaw)
sin_yaw = math.sin(yaw)

error_lat_ = dx * s_yaw - dy * c_yaw

error_ang_ = norm_angle(robot_yaw - path_yaw)

norme_角度：规格化以保持在Pi和-Pi之间

对于上述点（x，y），结果如下：

好

• （1，0）=0
• （-1,0）=3.14

错误

• （0,1）=1.57
• （0，-1）=-1.57

（0，1）的正确结果应为-1.57符号不好

你知道这些公式是不正确的还是我做错了什么吗

注意：当y不是0时，结果是错误的