python - 欧拉角引起的机器人定向问题
问题描述
我正在尝试使用 python 和为 ROS 和 Gazebo、RViz 创建的 4 轮机器人来实现基本路径规划算法。
我的算法唯一需要做的就是使我的机器人面向 x,y 平面中的给定(使用鼠标)点。
我面临的问题是我正在从四元数转换为欧拉角,并且我总是(无论我如何操作我的代码)遇到如下情况:
Theta to target:257.106918658
Theta_deg:-179.85
dTheta:-436.96
Theta to target:257.106918658
Theta_deg:-179.85
dTheta:-436.96
Theta to target:257.118158708
Theta_deg:179.99
dTheta:-77.13
Theta to target:257.118158708
Theta_deg:179.99
dTheta:-77.13
Theta to target:257.118158708
Theta_deg:179.99
dTheta:-77.13
Theta to target:257.118158708
Theta_deg:179.99
所以:
Theta to target: Euler [-180, 180] 机器人质心与给定点连线的角度 [deg]。
Theta_deg: Euler [-180, 180] 方向(机器人正面的垂直矢量)到 X 轴的角度 [deg]。
从上面的数据可以看出,Theta_deg
经历了一个非连续的步骤:
Theta_deg:-179.99
至
Theta_deg:179.99
我知道这是使用欧拉角的问题。我该如何克服这个问题?
相关代码:
while not rospy.is_shutdown():
theta_deg = (round(180*(theta/math.pi),2))
old_distance = round(distance_to_target,2)
theta_to_target =180*(math.pi/2+math.atan2((y-goal.y),(x-goal.x)))/math.pi
dTheta =(round(((theta_deg)-(theta_to_target)),2))
dtt = round((math.sqrt((goal.x - x)**2 + (goal.y - y)**2)),2)
if dtt < 0.1:
print "<<< Destination Reached >>>"
else:
theta_to_target =180*(math.pi/2+math.atan2((y-goal.y),(x-goal.x)))/math.pi
dTheta =(round(((theta_deg)-(theta_to_target)),2))
print "Theta to target:" + str(theta_to_target)
print "Theta_deg:" + str(theta_deg)
print "dTheta:" + str(dTheta)
# if dTheta<-360: dTheta=dTheta+360
# if dTheta>360: dTheta=dTheta-360
if abs(dTheta)>10:
if abs(theta_to_target - theta_deg)>180:
speed.angular.z = -0.2
speed.linear.x = 0.0
pub.publish(speed)
else:
speed.angular.z = +0.2
speed.linear.x = 0.0
pub.publish(speed)
else:
speed.linear.x = 0.6
pub.publish(speed)
r.sleep()
解决方案
推荐阅读
- swift - 是否可以对 NavigationLink 点击执行操作?
- .net-core - 具有 Null 值的 Blazor 参数
- google-chrome - 使用 chrome Inspect Element 从网站下载视频
- azure-devops - Azure Devops Server 2019 限制选择展开为 8 列
- php - Laravel sql 查询在从 ajax 调用时出错
- javascript - 获取 youtube 视频缩略图
- amazon-web-services - 如何使用 terragrunt 导入模块?
- php - PHP:如何删除所有子文件夹(只有 1MB 或更小)并清除其所有文件?
- asp.net-mvc - 模型中的日期格式无法正常工作并显示秒数
- android - 使用 Google Maps Flutter 进行 Android 预启动测试的 StackOverflowError