三轮全向ROS机器人标定

三轮全向ROS机器人标定

准备工作：

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

2、创建软件包

3、源码

启动文件.

配置文件ams.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

标定程序的源码.py：

#!/usr/bin/env python
# -*- coding: UTF-8 -*-"""Copyright: 2016-2018 ROS小课堂 www.corvin.cnAuthor: corvinDescription:该源码文件是标定三轮全向移动小车的底盘线速度的代码,标定的主要过程就是根据设定的移动距离,然后向小车的移动话题中发布移动速度.在此时,不断的监听小车自身的基坐标系与odom坐标系之间的距离.当检测到两坐标系之间的距离已经小于容忍范围内,说明小车已经到了指定的移动距离.此时就需要测量小车的实际移动距离跟坐标系间检测的距离是否一样,两者之间的误差是否可以接受,如果不接受就需要修改里程计的修正值.History:20180907: initial this file.
"""import tf
import rospy
from math import copysign, sqrt, pow
from geometry_msgs.msg import Twist, Pointclass CalibrateLinear():def __init__(self):rospy.init_node('calibrate_linear_node', anonymous=False)#execute a shutdown function when terminating the scriptrospy.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 speedself.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 odomself.base_frame = rospy.get_param('~base_frame', '/base_footprint')self.odom_frame = rospy.get_param('~odom_frame', '/odom')#initialize the tf listener and waitself.tf_listener = tf.TransformListener()rospy.sleep(2)#make sure we see the odom and base framesself.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 framesself.position = self.get_position()self.x_start  = self.position.xself.y_start  = self.position.y#print start calibrate summary infoself.print_summary()while not rospy.is_shutdown():#get the starting position from the tf between the odom and base framesself.position = self.get_position()check_rate.sleep() #sleep for while loopif self.start_test:#compute the euclidean distance from the target pointdistance = sqrt(pow((self.position.x - self.x_start), 2) +pow((self.position.y - self.y_start), 2))#correct the estimate distance by the correction factordistance *= self.odom_linear_scaleerror = self.test_distance - distancerospy.loginfo("-->rest_distance: " + str(error))move_cmd = Twist()if error < self.tolerance:self.start_test = Falseself.cmd_vel.publish(Twist())  #stop the robotrospy.logwarn("Now stop move robot !")else:move_cmd.linear.x = self.speedself.cmd_vel.publish(move_cmd) #continue moveelse:  #end testactual_dist = input("Please input actual distance:")linear_scale_error = float(actual_dist)/self.test_distanceself.odom_linear_scale *= linear_scale_errorrospy.logwarn("Now get new linear_scale: " + str(self.odom_linear_scale))self.print_summary()self.start_test = Trueself.x_start = self.position.xself.y_start = self.position.ydef 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 !")returnreturn 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的文件.：

基本的标定数据：

以下为示例数据：test_distance: 2.0  # mlinear_speed: 0.17  # m/stolerance_linear: 0.005  # 0.5cmlinear_scale: 1.0check_rate: 15cmd_topic: /cmd_velbase_frame: base_footprintodom_frame: odom

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

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

原文请看：