0

我正在尝试使用 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()
4

0 回答 0