智能车仿真课设PID控制环节部分代码(仅供参考)

使用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时可以只挑整数,体验好一些.
关于误差函数:原始代码只取了一个点.这里取了两个点,也可以取三个甚至更多的点,甚至使用非线性的组合,但粗略尝试之后发现性能不增反减,故作罢.但相比一点式,提升十分明显.
希望可以帮到大家!