ROS学习（四）机器人的移动与矫正

移动底座的坐标系

机器人在移动的时候使用的是右手坐标系。如图：

这里的右手坐标系和课本上的不太一样：

不过本质是一样的，进行反转后其实会重合。

我们以图1作为解释。
x轴指向机器人的正前方，y轴指向正左方，z轴指向正上方。

ROS的移动控制架构

整个架构从对发动机的直接控制，到路径规划SLAM，不同层代表着不同的抽象。

1. Motor Speeds
   发动机控制器。这一层包含了发动机、轮子、编码器。一个编码器每转一圈回触发固定数量的ticks，从而纪录对应轮子转了多少圈。加上预先知道的轮子直径和轮子之间的距离，就可以把纪录的数据转换成移动距离，进而可以知道速度等，这种方法叫做测程法。

2. Base Controller
   基控制器。经过上一层的抽象，机器人的目标速度可以按照现实世界的计量单位来表示。另外还会采用PID控制的形式。PID表示Proportional Integral Derivative，比例积分倒数控制器。PID会根据轮子当前的实际速度和理论速度的差，速度对于时间的导数和积分来纠正轮子的速度。

3. /cmd_vel + /odom
   驱动程序和PID控制器都会被整合在基控制器的ROS节点中。基控制器会在/odom话题下发布测量数据，并在/cmd_vel话题下监听运动指令。

4. move_base
   move_base可以为某一个机器人对应某一参照框架设定目标位置和方向，move_base包会场时让机器人必考障碍物并移动到目标位置。move_base包含一个十分复杂的路径规划，它在为机器人选择路径时，结合了测量数据，局部和全局代价地图。还会根据在配置文件中设定的最小和最大速度，自动的调整线速度、角速度、加速度。

5. Path Planner
   ROS机器人可以使用SLAM的gmapping包来绘制一张它所在的的环境的地图。地图的绘制最好是用激光扫描仪，但用Kinect或者Xtion的深度照相机来模拟激光扫描仪也是可以的。

6. amcl
   导航与定位。当地图绘制完成后，ROS中的amcl包就可以用过机器人当前的扫描和测量数据来自动定位。

/cmd_vel + /odom下的移动控制

ROS使用Twist消息发布运动指令给基控制器。

he@he-VirtualBox:~/catkin_ws$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z

可以发现Twist消息组成如上。Twist由两个Vector3表示。linear表示线速度，angular表示角速度。
我们想让机器人以0.1m/s的速度向前走他的形式如下:

‘{linear: {x: 0.2, y: 0, z: 0}, angular: {x: 0, y: 0, z: 0.5}}’

运动参数的校准

基础版-从内部进行修正

在实际运动中，因为地面材质的问题，机器人的理论移从与实际产生差异，所以就需要进行校准。

得有一个实体机器人才可以进行操作。

利用rqt_reconfigure命令进行校准。另机器人向前行走一米或者旋转一圈，与实际运动情况进行对比。

具体步骤如下：

• 用卷尺测量并纪录下机器人的实际移动距离。
• 用实际移动距离除以目标移动距离，并记录下这个值。
• 回到rqt_reconfigure的gui界面，用参数odom_linear_scale_correction乘以上一步得到的比值所得的乘积更新这个参数。
• 把机器人放回卷尺的起始端，在rqt_reconfigure窗口中勾选start_test选框，重复测试。
• 不断重复测试，知道得到满意结果。一米测试精确度达到一厘米左右。

进阶版本–从外部进行修正

内部修正的精度和可靠性依赖于机器人内部的感应器、校准过程的精度和周围环境的条件（比如轮子轻微打滑，从而打乱编码器的计数和移动距离之间的对应关系）。
机器人的外部测量是获得自己的位置和方向，是对内部测量的补充。

ROS开放了一些工具包：
ar_pose、ar_kinect、ar_track_alvar：利用基准线准线等固定在墙上的可视标志。
ccny_rgbd_tools、rgbdslam：利用环境中的可视的标识来做定位。
gps：室外位置估算
evolution robotics、northstart：其他主动定位技术。

Ros使用测量(Odometry)标识内部的位置数据。

he@he-VirtualBox:~/catkin_ws$ rosmsg show nav_msgs/Odometry
std_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance

PoseWithCovariance子消息记录了机器人的位置和方向，而TwistWithCovariance子消息告诉了我们线速度和角速度。
convariance矩阵为pose和twist补充了不同的测量不确定性。
Header和child_frame_id定义了我们用来测量距离和角度的框架。他同时为每一个消息提供了一个时间戳，让我们知道在何时何地。

ROS在测量中约定俗成的用/odom作为父框架的id，而用/base_link（或/base_footprint）作为自框架的id。框架/base_link代表的是现实中的机器人，而/odom则是由测量数据中平移和旋转定义的。这些变换是相对于/odom框架来移动机器人的。如果我们在RViz上现实模拟机器人，并设置到固定的/odom框架，机器人的位置表示的是机器人”觉得”它相对于开始位置的坐标。

移动

一般移动

让机器人进行移动一般从ROS节点发布Twist消息
关键代码如下(Python版本)

self.cmd_vel = rospy.Publisher(‘/cmd_vel’, Twist, queue_size=1)
move_cmd = Twist()
move_cmd.linear.x = linear_speed
self.cmd_vel.publish(move_cmd)

基于测量的移动

一般移动中我们如果想让机器人移动1m，就要发布0.1m/s的移动速度并持续10s。
这个可能会不准确，因为会有前面提到的各种方法对机器人的移动进行修正。
通过转换自/odom和/base_link框架之间的测量信息，监视机器人的位置和方向。
关键代码如下

＃Enter the loop to move along a side
while distance < goal_distance and not rospy.is_shutdown():
    ＃Publish the Twist message and sleep 1 cycle         
    self.cmd_vel.publish(move_cmd)
    r.sleep()
    # Get the current position
    (position, rotation) = self.get_odom()
    # Compute the Euclidean distance from the start
    distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2))

不停的获取机器人的移动距离直到达到目标为止。他是怎么获取机器人的当前位置呢self.get_odom()。

    def get_odom(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), quat_to_angle(Quaternion(*rot)))

self.odom_frame与self.base_frame的赋值见如下代码。

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

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

        # Set the odom frame
        self.odom_frame = '/odom'

        # Find out if the robot uses /base_link or /base_footprint
        try:
            self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0))
            self.base_frame = '/base_footprint'
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            try:
                self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0))
                self.base_frame = '/base_link'
            except (tf.Exception, tf.ConnectivityException, tf.LookupException):
                rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint")
                rospy.signal_shutdown("tf Exception")   

我们创建一个TransformListener对象来监听框架之间的转换。注意tf需要一点时间去填满监听器缓存，所以增加了rospy.sleep(2)。为了获得机器人的位置和方向，需要在/odom框架和/base_footprint框架(TurtleBot使用)或/base_link(Pi Robot和Maxwell使用)之间进行转换。

为什么用TransformListener去访问测量信息，而不是直接订阅/odom话题呢。
因为发布在/odom话题的数据不总是全部的数据。例如:
TurtleBot使用一个单轴陀螺仪去对机器人的旋转进行额外的估算。这些数据实在robot_pose_ekf节点（该节点在TurtleBot的启动文件中启动）和来自轮子的编码器数据进行合并，然后一起对旋转进行比单一数据源更精确的估算。
然而robot_pose_ekf节点并不会吧数据重新发回/odom话题，/odom话题是保留给轮子的编码器数据的。相反的robot_pose_ekf节点把数据发布在/odom_conbined话题中。此外数据不是作为Odometry消息发布，而是作为PoseWithCovarianceStamped消息发布。然而，从/odom框架到/base_link的转换提供了我们需要的信息。
因此，总体来说用tf来件ing在/odom框架和/base_link(或/base_footprint)框架的转换，比只依赖/odom消息话题更安全。