ROS机器人Diego 1#制作（四）base controller---线速度的标定

更多创客作品，请关注笔者网站园丁鸟，搜集全球极具创意，且有价值的创客作品
ROS机器人知识请关注，diegorobot
业余时间完成的一款在线统计过程分析工具SPC，及SPC知识分享网站qdo

---

让机器人动起来容易，但要精确控制是比较难的，这与控制算法、硬件都有比较大的相关性。对于轮式机器人，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参数，即可以达到满意的效果，当然要达到高精度的控制效果，硬件的精度也要非常高。