车道识别

校准

1.启动gazebo节点

roslaunch turtlebot3_gazebo turtlebot3_autorace_2020.launch

2.启动相机的内标定

roslaunch turtlebot3_autorace_camera intrinsic_camera_calibration.launch

3.启动相机外标定

roslaunch turtlebot3_autorace_camera extrinsic_camera_calibration.launch 

4.启动车道线检测校准节点

roslaunch turtlebot3_autorace_detect detect_lane.launch mode:=calibration

5.打开可视化

rqt

• 点击rqt左上角菜单栏Plugins -> Cisualization -> Image view，依次添加三个窗口且分别订阅/detect/image_lane/compressed、/detect/image_yellow_lane_marker/compressed和/detect/image_white_lane_marker/compressed三个主题

6.新终端，打开rqt_reconfigure工具

rosrun rqt_reconfigure rqt_reconfigure

• 点击detect_lane后调整参数来过滤黄色和白色，实际需要的过滤效果可以参考上面两图
• 首先应校准色相高低值：hue_white_l和hue_white_h
• 然后校准饱和度高低值：lightness_white_l和saturation_white_h的
• 最后是校准亮度高低值：lightness_white_l和saturation_white_h，由于源代码包含有亮度自动调节，所以只需将亮度的高值设置为255
• 黄色车道的过滤流程也跟上面一样

7.启动键盘控制节点

roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch

• 当你认为当前参数已经能稳定过滤出车道时，可以使用键盘控制TB3沿着车道中心走一段距离，观察话题/detect/image_lane/compressed的窗口里是否能正常过滤出车道

8.保存参数

新终端，将调整好的值写入turtlebot3_autorace_detect/param/lane/的lane.yaml文件中

---
detect:
  lane:
    white:
      hue_l: 0
      hue_h: 179
      saturation_l: 0
      saturation_h: 70
      lightness_l: 105
      lightness_h: 255
    yellow:
      hue_l: 10
      hue_h: 127
      saturation_l: 70
      saturation_h: 255
      lightness_l: 95
      lightness_h: 255

测试

1.关闭所有节点，启动gazebo

roslaunch turtlebot3_gazebo turtlebot3_autorace_2020.launch

2.启动相机内标定

roslaunch turtlebot3_autorace_camera intrinsic_camera_calibration.launch

3.启动相机外标定

roslaunch turtlebot3_autorace_camera extrinsic_camera_calibration.launch

4.打开车道线检测

roslaunch turtlebot3_autorace_detect detect_lane.launch

5.启动车道线跟随

roslaunch turtlebot3_autorace_driving turtlebot3_autorace_control_lane.launch

实现

detect_lane

turtlebot3_autorace_2020\turtlebot3_autorace_detect\nodes\detect_lane

cbFindLane

订阅图像的回调,并进行车道线检测

    def cbFindLane(self, image_msg):
        # 每三张图片识别一次
        if self.counter % 3 != 0:
            self.counter += 1
            return
        else:
            self.counter = 1
        # 图像转换
        if self.sub_image_type == "compressed":
            # converting compressed image01 to opencv image01
            np_arr = np.frombuffer(image_msg.data, np.uint8)
            cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
        elif self.sub_image_type == "raw":
            cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8")
        # ------------------ 识别车道线核心代码 ------------------ #
        # 获取黄色和白色的车道线
        # 返回: 白色点的个数  白色部分的掩膜
        white_fraction, cv_white_lane = self.maskWhiteLane(cv_image)
        # 返回: 黄色点的个数  黄色部分的掩膜
        yellow_fraction, cv_yellow_lane = self.maskYellowLane(cv_image)

        try:
            # 根据黄白掩膜结合上一次拟合的结果拟合出2次多项式的黄白线(避免离散的点影响拟合结果)
            if yellow_fraction > 3000:
                self.left_fitx, self.left_fit = self.fit_from_lines(self.left_fit, cv_yellow_lane)
                self.mov_avg_left = np.append(self.mov_avg_left, np.array([self.left_fit]), axis=0)

            if white_fraction > 3000:
                self.right_fitx, self.right_fit = self.fit_from_lines(self.right_fit, cv_white_lane)
                self.mov_avg_right = np.append(self.mov_avg_right, np.array([self.right_fit]), axis=0)
        except:
            # 根据黄白掩膜拟合出2次多项式的黄白线
            if yellow_fraction > 3000:
                self.left_fitx, self.left_fit = self.sliding_windown(cv_yellow_lane, 'left')
                self.mov_avg_left = np.array([self.left_fit])

            if white_fraction > 3000:
                self.right_fitx, self.right_fit = self.sliding_windown(cv_white_lane, 'right')
                self.mov_avg_right = np.array([self.right_fit])
        # 取最近的5次拟合的参数取平均值
        MOV_AVG_LENGTH = 5

        self.left_fit = np.array([np.mean(self.mov_avg_left[::-1][:, 0][0:MOV_AVG_LENGTH]),
                                  np.mean(self.mov_avg_left[::-1][:, 1][0:MOV_AVG_LENGTH]),
                                  np.mean(self.mov_avg_left[::-1][:, 2][0:MOV_AVG_LENGTH])])
        self.right_fit = np.array([np.mean(self.mov_avg_right[::-1][:, 0][0:MOV_AVG_LENGTH]),
                                   np.mean(self.mov_avg_right[::-1][:, 1][0:MOV_AVG_LENGTH]),
                                   np.mean(self.mov_avg_right[::-1][:, 2][0:MOV_AVG_LENGTH])])

        if self.mov_avg_left.shape[0] > 1000:
            self.mov_avg_left = self.mov_avg_left[0:MOV_AVG_LENGTH]

        if self.mov_avg_right.shape[0] > 1000:
            self.mov_avg_right = self.mov_avg_right[0:MOV_AVG_LENGTH]
        # 根据前面拟合的结果计算出车应该去的中心点的x坐标
        self.make_lane(cv_image, white_fraction, yellow_fraction)

maskWhiteLane

过滤出图像中的白色车道线

    def maskWhiteLane(self, image):
        # 把BGR转成HSV
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        # 定义白色在HSV中的区间
        lower_white = np.array([self.hue_white_l, self.saturation_white_l, self.lightness_white_l])
        upper_white = np.array([self.hue_white_h, self.saturation_white_h, self.lightness_white_h])

        # 获取图片中的白色颜色
        mask = cv2.inRange(hsv, lower_white, upper_white)

        # 计算白色点的个数
        fraction_num = np.count_nonzero(mask)
        # ------------------ 计算颜色的可信度(在车道线检测渐变中有使用) ------------------ #
        # 有白色的行数
        how_much_short = 0
        for i in range(0, 600):
            if np.count_nonzero(mask[i, ::]) > 0:
                how_much_short += 1
        # 黑色的行数
        how_much_short = 600 - how_much_short

        if how_much_short > 100:
            if self.reliability_white_line >= 5:
                self.reliability_white_line -= 5
        elif how_much_short <= 100:
            if self.reliability_white_line <= 99:
                self.reliability_white_line += 5

        # 如果是标定模式,发布检测到的车道线
        if self.is_calibration_mode:
            if self.pub_image_type == "compressed":
                self.pub_image_white_lane.publish(self.cvBridge.cv2_to_compressed_imgmsg(mask, "jpg"))
            elif self.pub_image_type == "raw":
                self.pub_image_white_lane.publish(self.cvBridge.cv2_to_imgmsg(mask, "bgr8"))

        return fraction_num, mask

maskYellowLane

过滤出图像中的黄色车道线

    def maskYellowLane(self, image):
        # 把BGR转成HSV
        hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)

        # 定义黄色的HSV区间
        lower_yellow = np.array([self.hue_yellow_l, self.saturation_yellow_l, self.lightness_yellow_l])
        upper_yellow = np.array([self.hue_yellow_h, self.saturation_yellow_h, self.lightness_yellow_h])

        # 获取图片中的黄色颜色
        mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
        # 计算黄色点的个数
        fraction_num = np.count_nonzero(mask)

        # ------------------ 计算颜色的可信度(在车道线检测渐变中有使用) ------------------ #
        how_much_short = 0

        for i in range(0, 600):
            if np.count_nonzero(mask[i, ::]) > 0:
                how_much_short += 1

        how_much_short = 600 - how_much_short

        if how_much_short > 100:
            if self.reliability_yellow_line >= 5:
                self.reliability_yellow_line -= 5
        elif how_much_short <= 100:
            if self.reliability_yellow_line <= 99:
                self.reliability_yellow_line += 5

        # 如果是标定模式,发布检测到的车道线
        if self.is_calibration_mode:
            if self.pub_image_type == "compressed":
                self.pub_image_yellow_lane.publish(self.cvBridge.cv2_to_compressed_imgmsg(mask, "jpg"))
            elif self.pub_image_type == "raw":
                self.pub_image_yellow_lane.publish(self.cvBridge.cv2_to_imgmsg(mask, "bgr8"))

        return fraction_num, mask

sliding_windown

根据车道线掩膜图像生成拟合的车道线曲线

    def sliding_windown(self, img_w, left_or_right):
        '''
        根据车道线掩膜图像生成拟合的曲线
        :param img_w: 掩膜图像
        :param left_or_right: 左侧车道线还是右侧车道线
        :return: lane_fit:拟合的2次多项式的参数  lane_fitx:拟合的曲线上的点x坐标
        '''
        # 对掩膜图片按列求和(为了确定检测到的车道线的列)
        histogram = np.sum(img_w[int(img_w.shape[0] / 2):, :], axis=0)

        # 中间列的列号
        midpoint = np.int(histogram.shape[0] / 2)
        # 获取车道线最明显的列号
        if left_or_right == 'left':
            lane_base = np.argmax(histogram[:midpoint])
        elif left_or_right == 'right':
            lane_base = np.argmax(histogram[midpoint:]) + midpoint

        # 把整个图片从下向上分成20分检测车道线(为了找到曲线车道线的趋势)
        nwindows = 20

        # 每一块检测的高度
        window_height = np.int(img_w.shape[0] / nwindows)

        # 把掩膜中检测到的颜色的x和y找出来
        nonzero = img_w.nonzero()
        nonzeroy = np.array(nonzero[0])
        nonzerox = np.array(nonzero[1])

        # 检测每一块的车道线的x坐标
        x_current = lane_base

        # 检测车道线的x的左右浮动
        margin = 50

        # 检测每一块的点如果超过这个值就可以更新x
        minpix = 50

        # 保存所有符合条件的车道线索引
        lane_inds = []

        # 开始检测
        for window in range(nwindows):
            # 确定检测范围
            win_y_low = img_w.shape[0] - (window + 1) * window_height
            win_y_high = img_w.shape[0] - window * window_height
            win_x_low = x_current - margin
            win_x_high = x_current + margin

            # 过滤在检测范围内的符合条件的点
            good_lane_inds = ((nonzeroy >= win_y_low) & (nonzeroy < win_y_high) & (nonzerox >= win_x_low) & (
                    nonzerox < win_x_high)).nonzero()[0]

            # 添加符合条件的点到列表中
            lane_inds.append(good_lane_inds)

            # 如果这块检测符合条件的点大于阈值就更新x
            if len(good_lane_inds) > minpix:
                x_current = np.int(np.mean(nonzerox[good_lane_inds]))

        # 把2维数组拼接成1维数组
        lane_inds = np.concatenate(lane_inds)

        # 获取所有满足条件的点
        x = nonzerox[lane_inds]
        y = nonzeroy[lane_inds]

        # 2次多项式拟合
        try:
            lane_fit = np.polyfit(y, x, 2)
            self.lane_fit_bef = lane_fit
        except:
            lane_fit = self.lane_fit_bef

        # 根据拟合的曲线生成曲线上的点
        ploty = np.linspace(0, img_w.shape[0] - 1, img_w.shape[0])
        lane_fitx = lane_fit[0] * ploty ** 2 + lane_fit[1] * ploty + lane_fit[2]

        return lane_fitx, lane_fit

fit_from_lines

结合上次拟合的曲线生成新的拟合的曲线

    def fit_from_lines(self, lane_fit, image):
        '''
        结合上次拟合的曲线生成新的拟合的曲线
        :param lane_fit: 上次拟合的曲线参数
        :param image: 掩膜图片
        :return: lane_fit:拟合的2次多项式的参数  lane_fitx:拟合的曲线上的点x坐标
        '''
        # 获取掩膜中的颜色点x和y的索引
        nonzero = image.nonzero()
        nonzeroy = np.array(nonzero[0])
        nonzerox = np.array(nonzero[1])
        margin = 100
        # 在原来曲线浮动范围内的点
        lane_inds = ((nonzerox > (lane_fit[0] * (nonzeroy ** 2) + lane_fit[1] * nonzeroy + lane_fit[2] - margin)) & (
                nonzerox < (lane_fit[0] * (nonzeroy ** 2) + lane_fit[1] * nonzeroy + lane_fit[2] + margin)))

        # 过滤出来符合条件的点
        x = nonzerox[lane_inds]
        y = nonzeroy[lane_inds]

        # 进行2次多项式拟合
        lane_fit = np.polyfit(y, x, 2)

        # 根据拟合的结果生成曲线上的点
        ploty = np.linspace(0, image.shape[0] - 1, image.shape[0])
        lane_fitx = lane_fit[0] * ploty ** 2 + lane_fit[1] * ploty + lane_fit[2]

        return lane_fitx, lane_fit

make_lane

根据拟合的黄白车道线计算出车辆移动的centerX

    def make_lane(self, cv_image, white_fraction, yellow_fraction):
        '''

        :param cv_image: 原图
        :param white_fraction: 掩膜中白色点的个数
        :param yellow_fraction: 掩膜中黄色点的个数
        :return: None
        '''
        # 最终检测出的车道线及绘制的车道中线
        color_warp = np.zeros((cv_image.shape[0], cv_image.shape[1], 3), dtype=np.uint8)
        color_warp_lines = np.zeros((cv_image.shape[0], cv_image.shape[1], 3), dtype=np.uint8)
        # 左右点的y坐标
        ploty = np.linspace(0, cv_image.shape[0] - 1, cv_image.shape[0])
        # ------------------ 绘制黄白车道线 ------------------ #
        if yellow_fraction > 3000:
            # transpose:翻转(行变列 列变行) flipud:逆序
            pts_left = np.array([np.flipud(np.transpose(np.vstack([self.left_fitx, ploty])))])
            cv2.polylines(color_warp_lines, np.int_([pts_left]), isClosed=False, color=(0, 0, 255), thickness=25)
        if white_fraction > 3000:
            pts_right = np.array([np.transpose(np.vstack([self.right_fitx, ploty]))])
            cv2.polylines(color_warp_lines, np.int_([pts_right]), isClosed=False, color=(255, 255, 0), thickness=25)
        # 车道中线是否存在
        self.is_center_x_exist = True
        # ------------------ 计算车辆中间x的核心代码 ------------------ #
        # 车辆行驶中
        if self.reliability_white_line > 50 and self.reliability_yellow_line > 50:
            # 车道线检测两侧均可以正常检测
            if white_fraction > 3000 and yellow_fraction > 3000:
                # 车道中线直接计算平均值即可
                centerx = np.mean([self.left_fitx, self.right_fitx], axis=0)
                # 左右两条车道线
                pts = np.hstack((pts_left, pts_right))
                # 车道中线
                pts_center = np.array([np.transpose(np.vstack([centerx, ploty]))])
                # 绘制车道中线
                cv2.polylines(color_warp_lines, np.int_([pts_center]), isClosed=False, color=(0, 255, 255),
                              thickness=12)
                # 填充两条车道线中间的颜色
                cv2.fillPoly(color_warp, np.int_([pts]), (0, 255, 0))

            if white_fraction > 3000 and yellow_fraction <= 3000:
                # 把拟合的白色曲线x都减去320(向左偏移)
                centerx = np.subtract(self.right_fitx, 320)
                pts_center = np.array([np.transpose(np.vstack([centerx, ploty]))])

                cv2.polylines(color_warp_lines, np.int_([pts_center]), isClosed=False, color=(0, 255, 255),
                              thickness=12)

            if white_fraction <= 3000 and yellow_fraction > 3000:
                # 把拟合的黄色曲线x都加上320(向右偏移)
                centerx = np.add(self.left_fitx, 320)
                pts_center = np.array([np.transpose(np.vstack([centerx, ploty]))])

                cv2.polylines(color_warp_lines, np.int_([pts_center]), isClosed=False, color=(0, 255, 255),
                              thickness=12)
        elif self.reliability_white_line <= 50 and self.reliability_yellow_line > 50:
            # 车辆应该右转而没有转
            centerx = np.add(self.left_fitx, 320)
            pts_center = np.array([np.transpose(np.vstack([centerx, ploty]))])

            cv2.polylines(color_warp_lines, np.int_([pts_center]), isClosed=False, color=(0, 255, 255), thickness=12)
        elif self.reliability_white_line > 50 and self.reliability_yellow_line <= 50:
            # 车辆应该左转而没有转
            centerx = np.subtract(self.right_fitx, 320)
            pts_center = np.array([np.transpose(np.vstack([centerx, ploty]))])

            cv2.polylines(color_warp_lines, np.int_([pts_center]), isClosed=False, color=(0, 255, 255), thickness=12)
        else:
            # 车辆驶入环形车道及将要驶出环形车道黄白交界处
            self.is_center_x_exist = False

        # 把原图和添加的车道线及填充图合并发布
        final = cv2.addWeighted(cv_image, 1, color_warp, 0.2, 0)
        final = cv2.addWeighted(final, 1, color_warp_lines, 1, 0)

        if self.pub_image_type == "compressed":
            if self.is_center_x_exist == True:
                msg_desired_center = Float64()
                msg_desired_center.data = centerx.item(350)
                # 发布centerX给控制节点
                self.pub_lane.publish(msg_desired_center)

            self.pub_image_lane.publish(self.cvBridge.cv2_to_compressed_imgmsg(final, "jpg"))

        elif self.pub_image_type == "raw":
            if self.is_center_x_exist == True:
                msg_desired_center = Float64()
                msg_desired_center.data = centerx.item(350)
                # 发布centerX给控制节点
                self.pub_lane.publish(msg_desired_center)

            self.pub_image_lane.publish(self.cvBridge.cv2_to_imgmsg(final, "bgr8"))

control_lane

turtlebot3_autorace_2020\turtlebot3_autorace_driving\nodes\control_lane

cbFollowLane

订阅detect_lane节点的topic获取centerX,控制小车移动

    def cbFollowLane(self, desired_center):
        '''
        获取detect_lane节点的centerX回调
        :param desired_center: centerX数据
        :return:
        '''
        # centerX数据
        center = desired_center.data
        # 误差(图片宽度是1000)
        error = center - 500
        # pd参数
        Kp = 0.0025
        Kd = 0.007
        # 位置式PID(只使用了p和d)
        angular_z = Kp * error + Kd * (error - self.lastError)
        # 重新设置上一次误差
        self.lastError = error

        twist = Twist()
        # twist.linear.x = min(self.MAX_VEL * ((1 - abs(error) / 500) ** 2.2), 0.05)
        twist.linear.x = min(self.MAX_VEL * ((1 - abs(error) / 500) ** 2.2), 0.2)
        twist.linear.y = 0
        twist.linear.z = 0
        twist.angular.x = 0
        twist.angular.y = 0
        # 如果angular_z为负数(角速度要为正才能移动过去)
        # twist.angular.z = -max(angular_z, -2.0) if angular_z < 0 else -min(angular_z, 2.0)
        twist.angular.z = -angular_z
        self.pub_cmd_vel.publish(twist)

PID控制

https://baijiahao.baidu.com/s?id=1687928966424595013&wfr=spider&for=pc

对自动驾驶来说，控制就是指使用方向盘、油门和刹车，将车驾驶到期望的位置。我们开车在十字路口或拐弯时候，可以凭着直觉和经验来决定拐弯的角度大小，加速的时机，以及是否需要刹车等。我们需要将这种直觉教给计算机。

我们经常把控制算法称之为控制器，PID控制器就是最常见、最基础的控制器之一。在介绍PID控制器之前，我们先了解关于控制的大的分类，根据是否有反馈可以分为开环控制和闭环控制。

开环控制

开环控制，全称开环控制系统（Open Loop Control System），又称为无反馈系统。即系统的输入可以影响输出，但是 输入不受输出影响 的系统。输入到输出的信号是单向传递的。

以下为生活中开环系统的一些例子：

控制系统	输入量	控制器	受控对象	输出量
风扇调速	设置档位	控制电路	风扇电机	风扇转速
红外感应门	人体热辐射	控制电路	自动门电机	门打开
洗衣机	设置洗衣时间	控制电路	水阀&电机	洁净衣服
电饭煲	电源开关	控制电路	锅底加热器	锅内温度
水箱注水	水龙头开关	进水阀门	水流	水箱水位

闭环控制

闭环控制，全称闭环控制系统（Closed Loop Control System）：系统的输出可以通过 反馈回路 对输入造成影响，进而影响控制过程的系统。

常见闭环控制系统场景

• 车道线校正
• 车速控制
• 四轴飞行器高度控制
• 变频空调温控
• 锅炉温控

位置式PID

e(k): 用户设定的值（目标值） - 控制对象的当前的状态值

比例P : e(k)

积分I : ∑e(i) 误差的累加

微分D : e(k) - e(k-1) 这次误差-上次误差

增量式PID

比例P : e(k)-e(k-1) 这次误差-上次误差

积分I : e(i) 误差

微分D : e(k) - 2e(k-1)+e(k-2) 这次误差-2*上次误差+上上次误差

注意:结果只是增量,需要和前面的增量进行累加才能使用