创新实训工作记录十二（自动巡航4）

class LanePidCal():
    def __init__(self, cfg_pid_y=None, cfg_pid_angle=None):
        # y_out_limit = 0.7
        # self.pid_y = PID(5, 0, 0)
        # self.pid_y.setpoint = 0
        # self.pid_y.output_limits = (-y_out_limit, y_out_limit)
        print(cfg_pid_y)
        print(cfg_pid_angle)
        self.pid_y = PID(**cfg_pid_y)
        print(self.pid_y)

        angle_out_limit = 1.5
        self.pid_angle = PID(3, 0, 0)
        self.pid_angle.setpoint = 0
        self.pid_angle.output_limits = (-angle_out_limit, angle_out_limit)
    
    def get_out(self, error_y, error_angle):
        pid_y_out = self.pid_y(error_y)
        pid_angle_out = self.pid_angle(error_angle)
        return pid_y_out, pid_angle_out

这里是对应的巡航模型需要的车道定位：

LanePidCal 类

此类专为车道跟随任务设计，其中需要控制横向位移（y轴）和角度定向。

• 初始化：构造函数接受两个可选参数，分别为横向移动的PID配置（cfg_pid_y）和角度定向的PID配置（cfg_pid_angle）。它初始化了两个PID控制器：一个用于y轴移动（pid_y），另一个用于角度调整（pid_angle）。这些控制器的配置可以通过参数传入或在类内部设置默认值。

• 输出计算：get_out 方法根据提供的 error_y（横向误差）和 error_angle（角度误差）计算两个PID的输出。该方法返回必要的控制信号，以纠正横向位移和朝向目标轨迹的方向。

机器人摄像头侧面定位：

        # 此时设置相对初始位置
        self.set_pos_relative()
        find_tar = False
        # 类别, id, 置信度, 归一化bbox[x_c, y_c, w, h]
        tar_cls, tar_id, tar_label, tar_score, tar_bbox = pt_tar[0], pt_tar[1], pt_tar[2], pt_tar[3], pt_tar[4:]
        flag_location = False
        while True:
            if self._stop_flag:
                return
            _pos_x, _pos_y, _pos_omage = self.get_pos_relative() # 用来计算距离

            if abs(_pos_x) > dis_out or abs(_pos_y) > dis_out:
                if not find_tar:
                    logger.info("task location dis out")
                    break
            img_side = self.cap_side.read()
            dets_ret = infer(img_side)
            # dets_ret = self.mot_hum(img_side)
            # cv2.imshow("side", img_side)
            # cv2.waitKey(1)
            # 进行排序，此处排列按照由近及远的顺序
            dets_ret.sort(key=lambda x: (x[4]-tar_bbox[0])**2 + (x[5]-tar_bbox[1])**2)
            # print(dets_ret)
            # 找到最近对应的类别，类别存在第一个位置
            det = self.get_list_by_val(dets_ret, 2, tar_label)
            # 如果没有，就重新获取
            if det is not None:
                find_tar = True
                # 结果分解
                det_cls, det_id, det_label, det_score, det_bbox = det[0], det[1], det[2], det[3], det[4:]
                # 计算偏差, 并进行偏差转换为输入pid的输入值
                bbox_error = ((np.array(det_bbox) - np.array(tar_bbox)) * error_adjust).tolist()
                # 离得远时. ywh值进行滤波为0，最终仅使用了w的值
                if abs(bbox_error[0]) > 0.1:
                    bbox_error[1] = 0
                    bbox_error[2] = 0
                    bbox_error[3] = 0
                out_x = pid_x(bbox_error[0])
                # out_y = pid_y(bbox_error[1])
                out_y = pid_w(bbox_error[2])
                # 检测偏差值连续小于阈值时，跳出循环
                # print(bbox_error)
                flag_x = x_count(abs(bbox_error[0]) < 0.02)
                
                flag_y = y_count(abs(bbox_error[2]) < 0.025)
                if flag_x:
                    out_x = 0
                if flag_y:
                    out_y = 0
                if flag_x and flag_y:
                    print("location ok")
                    flag_location = True
                    break
                
                # print("error_x:{:.2}, error_y:{:.2}, out_x:{:.2}, out_y:{:2}".format(bbox_error[0], bbox_error[2], out_x, out_y))
            else:
                x_count(False)
                y_count(False)
            self.mecanum_wheel(out_x, out_y, 0)
        # 停止
        self.mecanum_wheel(0, 0, 0)
        return flag_location

初始化和设置

• 设置相对初始位置：通过 set_pos_relative 方法初始化机器人的当前位置为基准点。
• 目标初始化：从 pt_tar 参数中解析目标的类别、ID、置信度和归一化边界框。
• 寻找目标标志：初始化 find_tar 为 False，用于标记是否找到目标。

循环处理

• 循环直至停止：如果检测到 _stop_flag 为真，则终止循环。
• 距离阈值检查：如果机器人相对于起始位置的位移超出了设定的 dis_out，且未找到目标，则记录日志并退出循环。
• 图像获取与处理：
  • 从侧面摄像头获取图像。
  • 调用 infer 函数对图像进行处理，获取检测结果。
  • 根据目标与当前检测框的距离差异对检测结果进行排序。

目标匹配与PID控制

• 匹配目标：从排序后的检测结果中找到与目标标签匹配的最近对象。
• 计算误差并调整：如果找到目标，根据目标框与预设框的差异计算误差，调整误差值后使用PID控制器计算x和y轴上的调整值。
• 连续误差检查：使用计数器检查PID输出是否连续多次低于阈值，若是则认为位置校正完成。

控制执行与反馈

• 移动控制：根据PID输出控制机器人移动。
• 终止移动：在完成位置校正后，停止机器人的移动。
• 返回定位标志：返回 flag_location，标示是否成功定位到目标。

    def lane_base(self, speed, end_fuction, stop=STOP_PARAM):
        while True:
            if self._stop_flag:
                return
            image = self.cap_front.read()
            error_y, error_angle = self.crusie(image)
            y_speed, angle_speed = self.lane_pid.get_out(-error_y, -error_angle)
            # speed_dy, angle_speed = process(image)
            self.mecanum_wheel(speed, y_speed, angle_speed)
            if end_fuction():
                break
        if stop:
            self.stop()

    def lane_det_base(self, speed, end_fuction, stop=STOP_PARAM):
        y_speed = 0
        angle_speed = 0
        while True:
            image = self.cap_front.read()
            dets_ret = self.front_det(image)
            # 此处检测简单不需要排序
            # dets_ret.sort(key=lambda x: x[4]**2 + (x[5])**2)
            if len(dets_ret)>0:
                det = dets_ret[0]
                det_cls, det_id, det_label, det_score, det_bbox = det[0], det[1], det[2], det[3], det[4:]
                error_y = det_bbox[0]
                dis_x = 1 - det_bbox[1]
                if end_fuction(dis_x):
                    break
                error_angle = error_y/dis_x
                y_speed, angle_speed = self.det_pid.get_out(error_y, error_angle)
            self.mecanum_wheel(speed, y_speed, angle_speed)
            if end_fuction(0):
                break
        if stop:
            self.stop()
            
    def lane_det_time(self, speed, time_dur, stop=STOP_PARAM):
        time_end = time.time() + time_dur
        end_fuction = lambda x: time.time() > time_end
        self.lane_det_base(speed, end_fuction, stop=stop)

    def lane_det_dis2pt(self, speed, dis_end, stop=STOP_PARAM):
        # lambda定义endfunction
        end_fuction = lambda x: x < dis_end and x != 0
        self.lane_det_base(speed, end_fuction, stop=stop)

    def lane_time(self, speed, time_dur, stop=STOP_PARAM):
        time_end = time.time() + time_dur
        end_fuction = lambda: time.time() > time_end
        self.lane_base(speed, end_fuction, stop=stop)
    
    # 巡航一段路程
    def lane_dis(self, speed, dis_end, stop=STOP_PARAM):
        # lambda重新endfunction
        end_fuction = lambda: self.get_dis_traveled() > dis_end
        self.lane_base(speed, end_fuction, stop=stop)

    def lane_dis_offset(self, speed, dis_hold, stop=STOP_PARAM):
        dis_start = self.get_dis_traveled()
        dis_stop = dis_start + dis_hold
        self.lane_dis(speed, dis_stop, stop=stop)

这段代码包括几个方法，主要用于控制一个装备了麦克纳姆轮的机器人或自动驾驶车辆，以实现基于图像的车道保持和目标跟踪功能。每个方法都设计用于不同的导航需求，包括时间控制、距离控制和目标检测。以下是对每个方法的详细解释：

lane_base 方法

此方法用于车道保持，通过调整机器人的速度和方向以保持在车道中。

• 参数:

  • speed：基本前进速度。
  • end_fuction：函数，用来判断何时停止调整。
  • stop：布尔值，指示完成后是否停止。

• 功能流程:

  • 持续读取前方摄像头的图像。
  • 使用 crusie 方法分析图像，得到y轴误差和角度误差。
  • 利用 lane_pid 控制器计算基于这些误差的y轴速度和角速度调整值。
  • 调用 mecanum_wheel 方法应用这些速度，以实现精确的车道跟踪。
  • 检查 end_fuction 来决定是否终止调整。
  • 如设置，则调用 stop 方法来停止机器人。

lane_det_base 方法

此方法用于基于图像的目标检测和跟踪。

• 功能流程:
  • 持续读取前方摄像头图像。
  • 使用 front_det 方法进行目标检测。
  • 如果检测到目标，从检测结果中提取第一个目标，并计算其位置误差和距离。
  • 利用 det_pid 控制器计算调整值，以对准目标。
  • 继续调用 mecanum_wheel 控制机器人的移动。
  • 根据 end_fuction 判断何时停止调整。

lane_det_time 方法

此方法为基于时间的控制，设置了一段时间内执行目标检测和调整。

• 功能流程:
  • 定义一个基于时间的结束函数 end_fuction。
  • 调用 lane_det_base 方法执行基于时间的调整。

lane_det_dis2pt 方法

此方法为基于距离的控制，设置了到达一定距离时停止调整。

• 功能流程:
  • 定义一个基于距离的结束函数 end_fuction。
  • 调用 lane_det_base 方法执行基于距离的调整。

lane_time 方法

此方法用于在指定时间内进行基本的车道保持，不涉及目标检测。

• 功能流程:
  • 定义一个基于时间的结束函数。
  • 调用 lane_base 方法执行基于时间的车道保持。