更多创客作品,请关注笔者网站园丁鸟,搜集全球极具创意,且有价值的创客作品
让机器人动起来容易,但要精确控制是比较难的,这与控制算法、硬件都有比较大的相关性。对于轮式机器人,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.")
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()
接下来运行如下命令,控制小车前进
rosrun diego_nav calibrate_linear.py
如果能一次运行刚好是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
即使是数据与实际的测量数据符合,但也可能达不到你的要求,这可能与电机的性能有关系,本人的经验适当的调整wheel_diameter参数,即可以达到满意的效果,当然要达到高精度的控制效果,硬件的精度也要非常高。