三轮全向ROS机器人标定

三轮全向ROS机器人标定

准备工作：
我们在标定前需要做好充分的准备工作，首先要保证控制小车移动的下位机代码和上位机代码已经调试好，而且下位机的PID已经整定好了，小车可以四处移动了。这里的代码我们是使用ros_arduino_bridge改造的，当我们后面把小车的线速度标定好以后，我们需要把线速度标定的参数值要保存在控制小车移动的上位机代码配置参数中。
2、创建软件包

3、源码
启动文件calibrate_mobilebase_linear.launch

<!--startup mobilebase arduino launch -->
<include file="$(find ros_arduino_python)/launch/arduino.launch" />

<node pkg="carebot_calibration" type="calibrate_mobilebase_linear.py" name="calibrate_mobilebase_linear" output="screen">
    <rosparam file="$(find carebot_calibration)/config/linear_calibrate_params.yaml" command="load" />
</node>

配置文件linear_calibrate_params.yaml：

#######################################################################
# Copyright: 2016-2018 ROS小课堂 www.corvin.cn
#######################################################################
# Author: corvin
#######################################################################
# Description:
#  该参数文件是为校准小车的线速度而设置，大家可以根据需要酌情来修改各
#  个参数，各参数功能介绍分别是：
#  test_distance:测试小车需要移动的距离,默认是2米。
#  linear_speed:小车移动时速度多大。
#  tolerance_linear:在测试移动时可以容忍的误差。
#  linear_scale:小车在移动中线速度精度,根据小车实际走的距离去除以
#    test_distance得到的数据就是,如果小车走的还是不准确的话就继续
#    运行一次,根据实际走的距离乘以当前的linear_scale作为新的linear_scale.
#  check_rate:根据小车底盘发布里程计坐标的频率来设置检查的频率.
#  cmd_topic:小车底盘订阅控制其移动的话题名称.
#  base_frame:小车底盘的基坐标系.
#  odom_frame:小车里程计的坐标系,我们需要查询这两个坐标系之间的距离来
#    判断我们的小车是否移动到了指定的距离.
#  我们最终标定完以后,只需要记下这个linear_scale参数就可以了.我们需要将
#  该参数配置在控制我们小车底盘移动的上位机代码中.这样我们的小车以后在
#  移动时距离精度就会很好了.
#######################################################################
# History:
#  20180111:初始化该参数文件.
#  20180911:增加check_rate和cmd_topic两个参数.
#######################################################################
test_distance: 2.0  # m
linear_speed: 0.17  # m/s
tolerance_linear: 0.005  # 0.5cm
linear_scale: 1.0
check_rate: 15
cmd_topic: /cmd_vel
base_frame: base_footprint
odom_frame: odom

标定程序的源码calibrate_mobilebase_linear.py：

#!/usr/bin/env python
# -*- coding: UTF-8 -*-
 
"""
 Copyright: 2016-2018 ROS小课堂 www.corvin.cn
 Author: corvin
 Description:
  该源码文件是标定三轮全向移动小车的底盘线速度的代码,标定的主要过程
  就是根据设定的移动距离,然后向小车的移动话题中发布移动速度.在此时,
  不断的监听小车自身的基坐标系与odom坐标系之间的距离.当检测到两坐标
  系之间的距离已经小于容忍范围内,说明小车已经到了指定的移动距离.
  此时就需要测量小车的实际移动距离跟坐标系间检测的距离是否一样,
  两者之间的误差是否可以接受,如果不接受就需要修改里程计的修正值.
 History:
  20180907: initial this file.
"""
 
import tf
import rospy
from math import copysign, sqrt, pow
from geometry_msgs.msg import Twist, Point
 
class CalibrateLinear():
    def __init__(self):
        rospy.init_node('calibrate_linear_node', anonymous=False)


    #execute a shutdown function when terminating the script
    rospy.on_shutdown(self.shutdown)

    self.test_distance = rospy.get_param("~test_distance", 2.0)
    self.speed     = rospy.get_param("~linear_speed", 0.17)
    self.tolerance = rospy.get_param("~tolerance_linear", 0.005)
    self.odom_linear_scale = rospy.get_param("~linear_scale", 1.000)
    self.rate  = rospy.get_param("~check_rate", 15)
    check_rate = rospy.Rate(self.rate)
    self.start_test = True  #default when startup run calibrate

    #Publisher to control the robot's speed
    self.cmd_topic = rospy.get_param("~cmd_topic", '/cmd_vel')
    self.cmd_vel   = rospy.Publisher(self.cmd_topic, Twist, queue_size=5)

    #The base frame is base_footprint for the robot,odom_frame is odom
    self.base_frame = rospy.get_param('~base_frame', '/base_footprint')
    self.odom_frame = rospy.get_param('~odom_frame', '/odom')

    #initialize the tf listener and wait
    self.tf_listener = tf.TransformListener()
    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(30.0))
    self.position = Point()

    #get the starting position from the tf between the odom and base frames
    self.position = self.get_position()
    self.x_start  = self.position.x
    self.y_start  = self.position.y

    #print start calibrate summary info
    self.print_summary()

    while not rospy.is_shutdown():
        #get the starting position from the tf between the odom and base frames
        self.position = self.get_position()
        check_rate.sleep() #sleep for while loop

        if self.start_test:
            #compute the euclidean distance from the target point
            distance = sqrt(pow((self.position.x - self.x_start), 2) +
                            pow((self.position.y - self.y_start), 2))

            #correct the estimate distance by the correction factor
            distance *= self.odom_linear_scale
            error = self.test_distance - distance
            rospy.loginfo("-->rest_distance: " + str(error))

            move_cmd = Twist()
            if error < self.tolerance:
                self.start_test = False
                self.cmd_vel.publish(Twist())  #stop the robot
                rospy.logwarn("Now stop move robot !")
            else:
                move_cmd.linear.x = self.speed
                self.cmd_vel.publish(move_cmd) #continue move
        else:  #end test
            actual_dist = input("Please input actual distance:")
            linear_scale_error = float(actual_dist)/self.test_distance
            self.odom_linear_scale *= linear_scale_error
            rospy.logwarn("Now get new linear_scale: " + str(self.odom_linear_scale))
            self.print_summary()
            self.start_test = True
            self.x_start = self.position.x
            self.y_start = self.position.y

def print_summary(self):
    rospy.logwarn("~~~~~~Now Start Linear Speed Calibration~~~~~~")
    rospy.logwarn("-> test_distance: " + str(self.test_distance))
    rospy.logwarn("-> linear_speed: "  + str(self.speed))
    rospy.logwarn("-> move_time: "  + str(self.test_distance/self.speed))
    rospy.logwarn("-> cmd_topic: "  + str(self.cmd_topic))
    rospy.logwarn("-> distance_tolerance: " + str(self.tolerance))
    rospy.logwarn("-> linear_scale: " + str(self.odom_linear_scale))

#get the current transform between the odom and base frames
def get_position(self):
    try:
        (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0))
    except (tf.Exception, tf.ConnectivityException, tf.LookupException):
        rospy.logerr("lookup TF exception !")
        return
    return Point(*trans)

#Always stop the robot when shutting down the node
def shutdown(self):
    rospy.logwarn("shutdown test node,stopping the robot")
    self.cmd_vel.publish(Twist())
    rospy.sleep(1)


if __name__ == '__main__':
    try:
        CalibrateLinear()
        rospy.spin()
    except:
        rospy.logerr("Calibration terminated by unknown problems!")

rviz的launch文件rviz_display.launch：

<!--
  Copyright: 2016-2018 ROS小课堂 www.corvin.cn
  Author: corvin
  Description:可以在小车的线速度和角速度标定过程中，在rviz中查看整个过程.
  History:
    20180915: initial this file.
-->
<launch>
  <!-- startup rviz with config file -->
  <node name="rviz" type="rviz" pkg="rviz" args="-d $(find carebot_calibration)/config/rviz_config.rviz" />
</launch>

基本的标定数据：

以下为示例数据：
    test_distance: 2.0  # m
    linear_speed: 0.17  # m/s
    tolerance_linear: 0.005  # 0.5cm
    linear_scale: 1.0
    check_rate: 15
    cmd_topic: /cmd_vel
    base_frame: base_footprint
    odom_frame: odom

需要根据设定的距离与机器人实际行走的距离进行误差分析，进而修改linear_scale数值来重新进行调整。

第二次标定时必须把机器人放回原位。

原文请看：http://www.corvin.cn/916.html