1.三轮全向移动小车的线速度标定
corvin_zhang
0x00 线速度为何需要标定？
首先我们来明确标定的定义，标定是在测量时，对测试设备的精度进行复核，并及时对误差进行消除的动态过程。根据该定义我们就能了解，其实任何的设备在刚制造出来后，无论你的制造过程有多精密，误差总是存在的。只不过有时候误差可能在我们的接受范围内，所以有些设备就不必标定了。拿我们的三轮全向移动小车来说的话，这个标定过程是不可少的。因为小车是一个具有机械安装结构和电机驱动的设备，机械结构安装会存在误差，电机的性能也会各不相同。这样即使我们的控制程序是一样的，那在具有安装误差和电机性能不同的小车上运行起来移动效果肯定也是不一样的。所以，我们需要每制作好一台轮式移动小车后，都需要对其移动的线速度和转动的角速度进行标定。只有在标定后，小车的移动距离理论值才会跟实际测量值一样。这样我们小车在进行自动导航时，往目标点移动时才能更好的进行定位。
0x01 线速度标定前的准备
我们在标定前需要做好充分的准备工作，首先要保证控制小车移动的下位机代码和上位机代码已经调试好，而且下位机的PID已经整定好了，小车可以四处移动了。这里的代码我们是使用ros_arduino_bridge改造的，当我们后面把小车的线速度标定好以后，我们需要把线速度标定的参数值要保存在控制小车移动的上位机代码配置参数中。
其次要准备好标定用的测试环境，我画了一个简图，如下图所示，我们需要找出一块空地，布置好测试环境才能开始标定我们的小车：
0x02 创建软件包
由于创建软件包的命令我们前面已经介绍过很多次了，这里就不再演示创建软件包的命令了，我们来直接查看创建好的软件包目录结构，如下图所示：
下面我们就来依次查看下各源码文件的内容，首先来查看下launch文件：
（1）用于启动线速度标定流程的启动文件calibrate_mobilebase_linear.launch
<!-- Copyright: 2016-2018 ROS小课堂 www.corvin.cn Author: corvin Description:该launch文件是用于对三轮全向移动小车底盘的线速度进行标定.通过设置往前移动的 指定距离,然后实地测量小车底盘的实际移动距离与设定的移动距离的误差.根据该误差修正配置 参数,从而可以减小该误差,使小车底盘移动距离更准确,为后面的自动导航做好准备工作. 该启动文件主要启动流程如下: 1.启动小车的urdf文件,这样方便在RViz中查看整个标定过程. 2.启动小车控制底盘移动的上位机程序. 3.启动标定程序,从配置文件中读取测试移动距离、移动速度、控制移动的话题等配置信息. 根据该信息来控制小车移动,测试完成后,需要拿出米尺来测量小车的实际移动距离与配置 的移动距离的差值,然后标定程序会根据测量值来修正配置参数,直到实际的移动距离与配 置的移动距离之间的误差在可容忍范围内即可.最后的linear_scale参数就是最终的标定参数, 到这里线速度标定过程完毕,最后我们将linear_scale写入到控制小车移动的上位机配置文件中即可. History: 20180907:initial this comment. --> <launch> <!--startup robot urdf description --> <include file="$(find carebot_description)/launch/carebot_description.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> </launch>
（2）下面来查看下标定程序运行需要的配置文件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
（3）下面就来查看线速度标定程序的源码calibrate_mobilebase_linear.py，该源码使用python来编写的：
#!/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!")
（4）最后来查看下启动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>
0x03 开始线速度标定
查看完代码后，当编译完成后就可以来开始线速度标定了，我们启动线速度标定的命令如下：
roslaunch carebot_calibration calibrate_mobilebase_linear.launch
当运行该命令后，就会发现小车开始往前按照线速度为0.17m/s的速度往前移动，命令的输出信息如下动图所示：
在测试中，我们在开始时会发现如下的信息，这是本次测试的一个参数信息：
下面可以来查看下小车的实际移动效果，由于我这里办公室场地有限，就没有布置那么完整的测试环境，如下动图所示：
下面经过测量发现想要小车移动2米的距离，但是经过实际测量却发现小车只移动了1.73米，线速度没有经过标定的小车移动起来误差还是很大的，如下图所示：
此时，我们就需要记录下该实际移动距离值1.73米，将其输入到终端的提示信息中，如下图所示的部分：
那么使用新的linear_scale=0.865来查看实际的移动效果，如下图所示：
如果你已经对该移动效果很满意的话，那就把最终得到的linear_scale值记录下来，将其保存在小车移动的上位机程序的配置文件中即可，这样以后小车在移动时就会默认使用标定好的参数来控制小车移动了。这样小车的移动距离就会很准确了。具体是修改如下配置文件my_arduino_params.yaml：
ros_arduino_bridge/ros_arduino_python/config/my_arduino_params.yaml
0x04 在Rviz中查看标定过程
我们除了在实际环境中可以查看到标定流程，在rviz中也可以查看到可视化的过程，启动rviz的命令如下：
roslaunch carebot_calibration rviz_display.launch
运行起来的效果如下图所示：
需要注意在Rviz中一个方格的距离是一米，当我们在标定以前会发现在Rviz中小车就是走了2米，说明我们理论上是驱动小车走了2米的距离。但是我们在实际环境中测量小车的移动距离时，发现移动距离只有1.73米。这就说明我们的理论值与实际值不相符，因此我们就需要标定来使实际移动距离与理论值要一样才行。这样我们在实际环境中移动的距离才能跟我们的仿真环境移动距离一样。
0x05 分析造成误差的原因
下面我们还可以来简单分析下造成理论距离和实际移动距离不一样的原因，就是因为小车的车轮在地面上打滑造成的，可能我们从表面上看小车车轮跟地面没有明显的打滑。但是其实只要有一点点的打滑，累积起来的误差就会很大了。例如我们可以假设车轮的周长是1米，理论上车轮转动一圈，车轮应该往前移动了1米。但是由于地面摩擦力不够，摩擦系数有点小，造成小车车轮转动一圈只往前移动了0.99米，误差有1cm。看起来好像很小，但是当车轮转动100圈，那么误差就是1米了，就好比车轮少转了整整一圈。由此可见，误差随着累积会造成非常严重的后果，那么我们该如何解决这个问题呢？
其实我们只有得到这个误差的系数就可以了，还拿上面的例子来说的话就是，我们理论移动了1米，但是实际却只移动了0.99米。那么我们移动的系数就是0.99，那么我们要想实际移动1米的话，理论上就需要移动1/0.99=1.01010101米。我们做线速度标定的过程，其实就是为了得到这个系数。在有了这个系数后，我们就能比较好的控制小车的实际移动距离了。但是，如果我们让理论移动了1米多，实际移动了1米，这样就会造成我们的仿真和实际移动效果不统一的情况。那么我们如何做才能让理论移动距离和实际的移动距离相同呢？就是理论上控制小车移动1米，实际测量发现小车确实也移动了1米呢？
我们就只能处理这个理论移动距离了，我们之所以可以得到理论值移动了1米是根据电机的编码器反馈来计算得到的。例如电机的参数是：减速比90，编码器分辨率一圈16个脉冲，测量编码器的AB相两路输出，电平变化作为中断计数的话。我们就可以得到，电机转动一圈，那么编码器的输出中断数就是90*16*4=5760个。所以我们统计到5760个中断就知道电机转动了一圈，那就是车轮转动了一圈，那就是理论上移动了1米。所以我们就需要对这个5760个中断计数作为1圈的计数值做下优化就可以了。下面截图就是从ros_arduino_bridge的base_controller.py中的代码：
这样我们就能根据比例系数0.99得到实际需要测量的中断计数值必须达到5760/0.99=5818个，本来我们一切测得5760个中断就认为转一圈了。现在必须测得5818个中断才认为转一圈，这样我们实际移动的距离也就是一圈的距离了。现在我们就能把理论距离和实际距离给统一起来了。
对于这个问题，更深入的思考一下原因。根据我们在高中学过的知识，我们知道一个物体的摩擦力f=μN，这个μ就是地面的摩擦系数，N就是小车自身的重力（N=mg，m就是小车的质量,g就是重力常数）。所以根据这个公式我们就明白了，在进行线速度标定的时候最好是在小车最终成型的时候，就是小车的质量不会有变化的时候。可别在小车只有一个底盘的时候就早早的标定好了线速度，然后就又开始往小车上加各种配件，导致小车的质量变化的话。那以前标定的线速度就需要重新来过了。还有就是这个地面摩擦系数μ，它会导致我们的小车在不同的路面上摩擦力不同。所以理论上我们的小车需要在不同的路面上工作时，就需要做相应路面的标定过程。可以试想一下，现在有两种路面：一个是摩擦力很大的水泥路面，另外一个是全是水的玻璃路面。那小车在水泥路面上走时，车轮转1圈就能走1米。在全是水的玻璃路面上，要想往前走1米，车轮就可能需要转2圈才行啊。这就是不同的地面摩擦系数造成的，所以我们就需要为不同路面不同摩擦系数情况下分别做线速度标定才行。
现在我们就来总结下标定的注意事项：
1.只有在小车组装完毕，整车质量不会再有变化时才能做线速度标定。
2.为了得到最好的移动效果，我们需要在不同的路面下（不同的摩擦系数下）分别都要做线速度标定。
0x06 参考资料
[1].diego robot运动控制-线速度标定. http://www.diegorobot.com/wp/?p=658&lang=zh
[2].turtlebot实现线速度标定.https://www.ncnynl.com/archives/201707/1812.html
[3].周学伟-矫正自己的机器人.http://www.cnblogs.com/zxouxuewei/p/5482302.html
0x07 Feedback
大家在按照教程操作过程中有任何问题，可以关注ROS小课堂的官方微信公众号，在公众号中给我发消息反馈问题即可，我基本上每天都会处理公众号中的留言！当然如果你要是顺便给ROS小课堂打个赏，我也会感激不尽的，打赏30块还会邀请进ROS小课堂的微信群与更多志同道合的小伙伴一起学习和交流！
