ROS机器人Diego制作4-线速度的标定

ROS机器人Diego制作4-线速度的标定

说明：

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

线速度的标定方法：

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

#!/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.")

• Python的ROS语法，参考：ROS与Python入门教程
• Pyhon基础教程，参考：python入门教程
• 代码片段分析：

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      #编码器一圈的脉冲计数
gear_reduction: 75.0       #减速比
motors_reversed: True

• 参数设置可以参考：搭建ROS小车底盘-第四篇上位机程序
• 其中说明一些测试例子，可以根据自己实际情况调整。
• 即使是数据与实际的测量数据符合，但也可能达不到你的要求，这可能与电机的性能有关系
• 本人的经验适当的调整wheel_diameter参数，即可以达到满意的效果
• 当然要达到高精度的控制效果，硬件的精度也要非常高。