Lesson 8:运动控制-角速度标定

1.角速度标定原理及其代码

与线速度标定的原理类似,角速度标定即发送控制指令控制机器人旋转一定的角度,观察机器人旋转的角度是否与控制指令一致,由于硬件本身的精密度限制,只要误差在可接受的范围内即可。

在这里我们命令控制机器人顺时针旋转360度,如下是角速度标定的代码:

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, Quaternion
from nav_msgs.msg import Odometry
import tf
from math import radians, copysign
from transform_utils import quat_to_angle, normalize_angle
import PyKDL
from math import pi

class CalibrateAngular():
    def __init__(self):
        # Give the node a name
        rospy.init_node('calibrate_angular', anonymous=False)

        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)

        # How fast will we check the odometry values?
        self.rate = 10
        r = rospy.Rate(self.rate)

        # The test angle is 360 degrees
        self.test_angle = 2*pi #这里注意,在ROS中使用的弧度,不能直接写360

        self.speed = 0.1 # radians per second
        self.tolerance = 1 # degrees converted to radians
        self.odom_angular_scale_correction = 1
        self.start_test = True

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

        # The base frame is usually base_link or base_footprint
        self.base_frame = rospy.get_param('~base_frame', '/base_link')

        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))

        rospy.loginfo("Bring up rqt_reconfigure to control the test.")

        reverse = 1

        while not rospy.is_shutdown():
            if self.start_test:
                # Get the current rotation angle from tf
                self.odom_angle = self.get_odom_angle()
                rospy.loginfo("self.odom_angle: "+str(self.odom_angle))

                last_angle = self.odom_angle
                turn_angle = 0
                self.test_angle *= reverse
                error = self.test_angle - turn_angle
                rospy.loginfo("errir: "+str(error))

                # Alternate directions between tests
                reverse = -reverse

                while abs(error) > self.tolerance and self.start_test:
                    if rospy.is_shutdown():
                        return
                    rospy.loginfo("*************************** ")
                    # Rotate the robot to reduce the error
                    move_cmd = Twist()
                    move_cmd.angular.z = copysign(self.speed, error)
                    rospy.loginfo("move_cmd.angular.z: "+str(move_cmd.angular.z))
                    self.cmd_vel.publish(move_cmd)
                    r.sleep()

                    # Get the current rotation angle from tf                   
                    self.odom_angle = self.get_odom_angle()
                    rospy.loginfo("current rotation angle: "+str(self.odom_angle))

                    # Compute how far we have gone since the last measurement
                    delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)
                    rospy.loginfo("delta_angle: "+str(delta_angle))

                    # Add to our total angle so far
                    turn_angle += abs(delta_angle)
                    rospy.loginfo("turn_angle: "+str(turn_angle))

                    # Compute the new error
                    error = self.test_angle - turn_angle
                    rospy.loginfo("error: "+str(error))

                    # Store the current angle for the next comparison
                    last_angle = self.odom_angle

                # Stop the robot
                self.cmd_vel.publish(Twist())

                # Update the status flag
                self.start_test = False
                params = {'start_test': False}
                dyn_client.update_configuration(params)

            rospy.sleep(0.5)

        # Stop the robot
        self.cmd_vel.publish(Twist())

    def get_odom_angle(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        # Convert the rotation from a quaternion to an Euler angle

        return quat_to_angle(Quaternion(*rot))

    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)
if __name__ == '__main__':
    try:
        CalibrateAngular()
    except:
        rospy.loginfo("Calibration terminated.")

2. 代码说明

首先导入需要的包

import rospy
from geometry_msgs.msg import Twist, Point
from math import copysign, sqrt, pow
import tf

ROS标准化初始化代码,初始化话节点,设定发布的频率


class CalibrateLinear():
    def __init__(self):
        # Give the node a name
        rospy.init_node('calibrate_linear', anonymous=False)

        # Set rospy to execute a shutdown function when terminating the script
        rospy.on_shutdown(self.shutdown)

        # How fast will we check the odometry values?
        self.rate = 10
        r = rospy.Rate(self.rate)

设定标定的参数

test_angle:标定旋转的角度,这里使用的单位是弧度;speed:前进的线速度;tolerance:精度范围;odom_angular_scale_correction:里程数据的缩放比例;start_test:开始测试的开关

        # The test angle is 360 degrees
        self.test_angle = 2*pi #

        self.speed = 0.1 # radians per second
        self.tolerance = 1 # degrees converted to radians
        self.odom_angular_scale_correction = 1
        self.start_test = True

发布Twist消息,控制小车前进

        # Publisher to control the robot's speed
        self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5)

 初始化base_frame, odom_frame,tf listener

        # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot
        self.base_frame = rospy.get_param('~base_frame', '/base_link')

        # The odom frame is usually just /odom
        self.odom_frame = rospy.get_param('~odom_frame', '/odom')

        # Initialize the tf listener
        self.tf_listener = tf.TransformListener()

        # Give tf some time to fill its buffer
        rospy.sleep(2)

        # Make sure we see the odom and base frames
        self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0))

        rospy.loginfo("Bring up rqt_reconfigure to control the test.")

初始化初始化位置

        self.position = Point()

        # Get the starting position from the tf transform between the odom and base frames
        self.position = self.get_position()

        x_start = self.position.x
        y_start = self.position.y

        move_cmd = Twist()

接下来的while循环代码控制机器人向前行驶1m,具体可以看代码解释

        while not rospy.is_shutdown():
            if self.start_test:
                # Get the current rotation angle from tf根据tf的信息获取当前旋转角度
                self.odom_angle = self.get_odom_angle()
                rospy.loginfo("self.odom_angle: "+str(self.odom_angle))

                last_angle = self.odom_angle
                turn_angle = 0
                self.test_angle *= reverse
                error = self.test_angle - turn_angle
                rospy.loginfo("errir: "+str(error))

                # Alternate directions between tests可以控制旋转的方向
                reverse = -reverse
                
                #error是与目标旋转角度的差距,while循环的目的就是让error无限接近0。
                while abs(error) > self.tolerance and self.start_test:
                    if rospy.is_shutdown():
                        return
                    rospy.loginfo("*************************** ")
                    # Rotate the robot to reduce the error旋转机器人,使得error越来越小
                    move_cmd = Twist()
                    move_cmd.angular.z = copysign(self.speed, error)
                    rospy.loginfo("move_cmd.angular.z: "+str(move_cmd.angular.z))
                    self.cmd_vel.publish(move_cmd)
                    r.sleep()

                    # Get the current rotation angle from tf根据获取当前的旋转角度            
                    self.odom_angle = self.get_odom_angle()
                    rospy.loginfo("current rotation angle: "+str(self.odom_angle))

                    # Compute how far we have gone since the last measurement计算还剩余多少角度需要旋转
                    delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle)
                    rospy.loginfo("delta_angle: "+str(delta_angle))

                    # Add to our total angle so far
                    turn_angle += abs(delta_angle)
                    rospy.loginfo("turn_angle: "+str(turn_angle))

                    # Compute the new error计算新的error
                    error = self.test_angle - turn_angle
                    rospy.loginfo("error: "+str(error))

                    # Store the current angle for the next comparison存储当前的角度,为下一次计算
                    last_angle = self.odom_angle

                # Stop the robot停止机器人
                self.cmd_vel.publish(Twist())

                # Update the status flag更新状态
                self.start_test = False
                params = {'start_test': False}
                dyn_client.update_configuration(params)

            rospy.sleep(0.5)

根据odom获取当前的行进角度

    def get_odom_angle(self):
        # Get the current transform between the odom and base frames
        try:
            (trans, rot)  = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.loginfo("TF Exception")
            return

        # Convert the rotation from a quaternion to an Euler angle

        return quat_to_angle(Quaternion(*rot))

停止节点

    def shutdown(self):
        # Always stop the robot when shutting down the node
        rospy.loginfo("Stopping the robot...")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

3.标定

有了上面的标定节点,我们可以标定机器人的角速度。首先找一块平坦可以行进2*2m的空间,把机器人按照一定的方向放置,执行如下命令,观察机器人是否旋转360度:

rosrun diego_nav calibrate_angular.py

影响角速度的主要参数是wheel_track,如果配置文件中的参数与实际数据一致,但还是达不到控制精度,如果发现机器人不能按照要求旋转固定角度,可以适当调整此参数来做补偿,直到达到满意的效果。

如果要达到高精度的控制效果,必须使用高精度的硬件。

发表评论

Scroll to top
%d 博主赞过: