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

更多创客作品，请关注笔者网站园丁鸟，搜集全球极具创意，且有价值的创客作品

---

让机器人动起来容易，但要精确控制是比较难的，这与控制算法、硬件都有比较大的相关性。对于轮式机器人，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.")

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113

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()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39

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

rosrun diego_nav calibrate_linear.py 
1

如果能一次运行刚好是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
1
2
3
4
5
6

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