ROS机器人Diego 1#制作（四）base controller—线速度的标定详解架构师

让机器人动起来容易，但要精确控制是比较难的，这与控制算法、硬件都有比较大的相关性。对于轮式机器人，base controller代码写好，机器人动起来后，首先做的就是需要对线速度，和角速度进行标定，以保证机器人可以按照指令精确的的形式，这篇博文中将讲述线速度的标定方法，下一篇中介绍角速度的标定方法。

线速度的标定，就是设置一个1m的距离，代码控制让机器人行驶1m，看是否刚好是1m，误差在可接受的范围内即可，如下是进行标定的代码：

#!/usr/bin/env python 
import rospy 
from geometry_msgs.msg import Twist, Point 
from math import copysign, sqrt, pow 
import tf 
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) 
 
        # Set the distance to travel 
        self.test_distance = 1.0 # meters 
        self.speed = 1.0 # meters per second 
        self.tolerance = 0.01 # meters 
        self.odom_linear_scale_correction = 1.0 
        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 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 not rospy.is_shutdown(): 
            # Stop the robot by default 
            move_cmd = Twist() 
 
            if self.start_test: 
                # Get the current position from the tf transform between the odom and base frames 
                self.position = self.get_position() 
 
                # Compute the Euclidean distance from the target point 
                distance = sqrt(pow((self.position.x - x_start), 2) + 
                                pow((self.position.y - y_start), 2)) 
 
                # Correct the estimated distance by the correction factor 
                distance *= self.odom_linear_scale_correction 
                # How close are we? 
                error =  distance - self.test_distance 
 
                # Are we close enough? 
                if not self.start_test or abs(error) <  self.tolerance: 
                    self.start_test = False 
                    params = False 
                    rospy.loginfo(params) 
                else: 
                    # If not, move in the appropriate direction 
                    move_cmd.linear.x = copysign(self.speed, -1 * error) 
            else: 
                self.position = self.get_position() 
                x_start = self.position.x 
                y_start = self.position.y 
 
            self.cmd_vel.publish(move_cmd) 
            r.sleep() 
 
        # Stop the robot 
        self.cmd_vel.publish(Twist()) 
 
    def get_position(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 
 
        return Point(*trans) 
 
    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: 
        CalibrateLinear() 
        rospy.spin() 
    except: 
        rospy.loginfo("Calibration terminated.") 

ROS标准的代码，我们这里不多解释，请自行到Ros wiki学习，这里只解释一下标定有关逻辑的代码，标定的逻辑主要在while循环里面，请看下面的代码解释：

x_start = self.position.x  #设定起始位置的x坐标 
y_start = self.position.y  #设定起始位置的y坐标 
 
        move_cmd = Twist() 
 
        while not rospy.is_shutdown(): 
            # Stop the robot by default 
            move_cmd = Twist() 
 
            if self.start_test: 
                # Get the current position from the tf transform between the odom and base frames 
                self.position = self.get_position() #获取当前的位置信息 
 
                # 计算当前位置与起始位置的距离 
                distance = sqrt(pow((self.position.x - x_start), 2) + 
                                pow((self.position.y - y_start), 2)) 
 
                # Correct the estimated distance by the correction factor 
                distance *= self.odom_linear_scale_correction 
 
                # 计算与目标位置的距离 
                error =  distance - self.test_distance 
 
                # Are we close enough? 
                if not self.start_test or abs(error) <  self.tolerance: #如果已经到达目标位置，则停止 
                    self.start_test = False 
                    params = False 
                    rospy.loginfo(params) 
                else: 
                    # 如果还没有到达，则继续前进，如果已经超出了目标位置，这控制电机反转，退回 
                    move_cmd.linear.x = copysign(self.speed, -1 * error) 
            else: 
                self.position = self.get_position() 
                x_start = self.position.x 
                y_start = self.position.y 
 
            self.cmd_vel.publish(move_cmd)#发布控制Twist消息 
            r.sleep() 

接下来运行如下命令，控制小车前进

rosrun diego_nav calibrate_linear.py 

如果能一次运行刚好是1m那当然是理想的效果，如果不理想，可能要调整my_arduino_params.yaml文件中有关机器人的参数了，首先需要检查机器人的参数是否与实际的想否，如果不想否修改为与实际想否的数据，另外要注意的是ROS里面使用的单位是 米，一定要注意单位的换算；

# === Robot drivetrain parameters 
wheel_diameter: 0.02900 
wheel_track: 0.18 
encoder_resolution: 2 # from Pololu for 131:1 motors 
gear_reduction: 75.0 
motors_reversed: True

即使是数据与实际的测量数据符合，但也可能达不到你的要求，这可能与电机的性能有关系，本人的经验适当的调整wheel_diameter参数，即可以达到满意的效果，当然要达到高精度的控制效果，硬件的精度也要非常高。