使用Linux系统,ros机器人操作系统,gazebo仿真软件,利用rospy等python包所带工具进行智能小车运动仿真.在给定的地图当中跑完一圈,1分钟之内为满分.现将笔者所编写之代码与小伙伴们分享,希望能够抛砖引玉.本文只做参考,切忌全部代码照抄哦~
该代码最终验收成绩为54-55秒之间.视虚拟机内存,电脑电量等,性能会有一定波动.建议虚拟机内存拉到14G以上(笔者验收时的配置)
PID调试方法:先调P,逐渐增大直至出现震荡;再调D,在保证转弯不耗费太多时间的情况下不损失太多速度.微分控制可以预见error的变化,从而使得调节过程更加平稳顺滑,表现出来就是过弯过的稳.积分控制一般用来减小稳态误差,这里不使用积分控制.说起来容易做起来难,涉及到的其他参数还是比较多,而且参数之间强耦合,牵一发动全身,需要大量时间.将代码分享出来,也是为了小伙伴们调参的时候可以舒服一点.
#!/usr/bin/env python
# BEGIN ALL
import rospy, cv2, cv_bridge, numpy, time
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
class PID:
def __init__(self, P, I, D):
self.Kp = P
self.Ki = I
self.Kd = D
self.current_time = time.time() #用来计算时间差,留作微分控制用
self.last_time = self.current_time
self.PTerm = 0.0
self.ITerm = 0.0
self.DTerm = 0.0
self.last_error = 0.0 #上次的误差,留作微分控制用
self.output = 0.0 #PID控制器输出
self.bridge = cv_bridge.CvBridge()
self.image_sub = rospy.Subscriber('camera/image_raw', Image, self.image_callback)
self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.twist = Twist()
def image_callback(self, msg): #主要函数,该函数的调用次数与性能与电脑状况(如电量,虚拟机内存等严重相关)
image = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_white = numpy.array([0, 0, 221])
upper_white = numpy.array([180, 30, 255])
mask = cv2.inRange(hsv, lower_white, upper_white) #将摄像头看到的图像做二值化处理
mask1 = mask.copy()
mask2 = mask.copy()
h, w, d = image.shape
search_top1 = 3 * h / 4 + 20
search_bot1 = 3 * h / 4 + 40
mask1[0:search_top1, 0:w] = 0
mask1[search_bot1:h, 0:w] = 0
M1 = cv2.moments(mask1)
search_top2 = 3 * h / 4 - 70
search_bot2 = 3 * h / 4 - 50
mask2[0:search_top2, 0:w] = 0
mask2[search_bot2:h, 0:w] = 0
M2 = cv2.moments(mask2) #计算两张二值化图片的矩
if (M1['m00'] > 0):
cx1 = int(M1['m10'] / M1['m00'])
cy1 = int(M1['m01'] / M1['m00'])
cv2.circle(image, (cx1, cy1), 20, (0, 0, 255), -1)
if (M2['m00'] > 0):
cx2 = int(M2['m10'] / M2['m00'])
cy2 = int(M2['m01'] / M2['m00'])
cv2.circle(image, (cx2, cy2), 20, (0, 0, 255), -1) #将两个重心点在图像中
err = cx1 * 0.55 + cx2 * 0.45 - w / 2 #误差函数设计,原始加权为0.5 0.5,可进行微调,改进效果并不明显
self.current_time = time.time() #不要忘记时间的处理,微分控制的关键
delta_time = self.current_time - self.last_time #计算时间差
self.PTerm = self.Kp * err
self.DTerm = (err - self.last_error) * self.Kd #参考位置式PID公式即可
self.output = self.PTerm + self.ITerm + self.DTerm #计算PID控制器输出
output = self.output
self.twist.linear.x = -2.2 * (1 - abs(err) / 770) #线速度,err的一次函数
self.twist.angular.z = float(self.output) / 45 #角速度,使用PID控制器,主要目的在于使小车转弯平滑且不明显减速
self.cmd_vel_pub.publish(self.twist)
self.last_error = err # 将这次的err赋给last_error
cv2.imshow("mask", mask) #输出二值化图像
cv2.waitKey(3)
P = 2
I = 0
D = 6
rospy.init_node('follower')
follower = PID(P, I, D)
rospy.spin()
这里并没有将PID控制器的输出直接赋给角速度,而是除以了一个分母(原始代码是除以50),这样再调PID时可以只挑整数,体验好一些.
关于误差函数:原始代码只取了一个点.这里取了两个点,也可以取三个甚至更多的点,甚至使用非线性的组合,但粗略尝试之后发现性能不增反减,故作罢.但相比一点式,提升十分明显.
希望可以帮到大家!