本项目为电赛控制题简易自行瞄准装置是一套移动底盘二维云台激光打靶一体化控制系统实现小车黑线循迹行驶过程中动态持续锁靶。系统采用双主控架构MSPM0主控负责小车黑线循迹灰度传感器 编码器 PID实现底盘循迹运动视觉单元负责靶心识别并把数据传给云台主控云台主控经过图像预处理得到靶心像素偏差根据陀螺仪和摄像头数据控制云台在小车行驶过程中锁定靶心我负责项目中的软件项目代码分为3个模块底盘控制云台控制和摄像头识别----------------------------------------------------底盘控制-----------------------------------------------------------------//循迹板初始化 void track_init(){ gpio_init(B8, GPI, GPIO_HIGH, GPI_PULL_UP); gpio_init(B9, GPI, GPIO_HIGH, GPI_PULL_UP); gpio_init(B12, GPI, GPIO_HIGH, GPI_PULL_UP); gpio_init(B13, GPI, GPIO_HIGH, GPI_PULL_UP); gpio_init(B4, GPI, GPIO_HIGH, GPI_PULL_UP); gpio_init(B5, GPI, GPIO_HIGH, GPI_PULL_UP); gpio_init(A1, GPI, GPIO_HIGH, GPI_PULL_UP); gpio_init(A0, GPI, GPIO_HIGH, GPI_PULL_UP); //初始化定时器中断 pit_ms_init(PIT_TIM_G6, 5, track_pit_handler, NULL); } //本次误差和上一次误差 float track_error 0.0; float track_last_error 0.0; float track_arigin_error 0.0; //左右直角标志位 uint8_t left_angle_flag ,right_angle_flag; void number_track_test(){ uint8_t status[8] {0}; uint8_t cnt 0; status[0] gpio_get_level(B8); status[1] gpio_get_level(B9); status[2] gpio_get_level(B12); status[3] gpio_get_level(B13); status[4] gpio_get_level(B4); status[5] gpio_get_level(B5); status[6] gpio_get_level(A1); status[7] gpio_get_level(A0); for(int i 0;i 8;i){ if(status[i] 1){ cnt; } } //左四灯全亮 if(status[0] 1status[1] 1status[2] 1status[3] 1){ left_angle_flag 1; } //右四灯全亮 if(status[4] 1status[5] 1status[6] 1status[7] 1){ right_angle_flag 1; } // for(int i 0;i 8;i){ // if(status[i] 0){ // cnt; // } // } // // // //左四灯全亮 // if(status[0] 0status[1] 0status[2] 0){ // left_angle_flag 1; // // } // //右四灯全亮 // if(status[5] 0status[6] 0status[7] 0){ // right_angle_flag 1; // // } track_arigin_error (status[0]*5status[1]*4status[2]*2status[3] - status[4] - status[5]*2 - status[6]*4 - status[7]*5)/cnt; if(cnt 0){ track_arigin_error track_last_error; } //track_error track_arigin_error * 1 (track_arigin_error - track_last_error) * 1; track_error track_arigin_error * read_buf[5] (track_arigin_error - track_last_error) * read_buf[6]; track_last_error track_arigin_error; // //ips200_show_int(0 , 16*10,final_error,2); // for(int i 0;i 8;i){ // ips200_show_int(i*10, 16*15,status[i],1); // } }配置8路GPIO为上拉输入模式用于采集8路灰度电平状态传感器检测到黑线输出0/白线输出1读取8个GPIO的电平存入状态数组遍历统计有效触发传感器数量cnt,加权误差计算越靠近边缘的权重值越大左正右负状态数组8个状态乘响应权重值/权重总值计算出误差若有效触发传感器数量为0很可能车身已经出界沿用上次误差值防止误差突变导致车辆失控在10ms定时器中断中不断更新误差值#define MAX_SPEED 4000 #define MIN_SPEED -2000 //PID初始化 void pid_pit_handler(); void pid_init(){ pit_ms_init(PIT_TIM_A1, 5, pid_pit_handler, NULL); } //右轮速度环 typedef struct{ //当前pi输出 int16_t current_pwm; //累计pi输出 int16_t cumulative_pwm; //当前偏差 int16_t current_error; //上一次误差 int16_t pre_error; }SPEED_RIGHT; SPEED_RIGHT speed_struct_right {0}; SPEED_RIGHT *speed_right speed_struct_right; SPEED_RIGHT speed_struct_left {0}; SPEED_RIGHT *speed_left speed_struct_left; int16_t speed_control(int16_t practical_speed,int16_t target_speed,SPEED_RIGHT* speed){ //当前偏差 speed -current_error target_speed - practical_speed; //过PI,保存此时刻的误差 speed -current_pwm read_buf[0]*(speed-current_error - speed-pre_error)read_buf[1]*(speed-current_error); //存上一次偏差 speed-pre_error speed-current_error; //输出值 speed-cumulative_pwm speed-current_pwm; //对输出值限幅 speed-cumulative_pwm speed-cumulative_pwm -(int16_t)read_buf[4]? -(int16_t)read_buf[4] : (speed-cumulative_pwm (int16_t)read_buf[3] ?(int16_t)read_buf[3] : speed-cumulative_pwm); return speed-cumulative_pwm; } void pid_control(){ float final_error track_error; if(left_angle_flag 1){ final_error left_angle_ring_control( read_buf[9]); } if(right_angle_flag 1){ final_error right_angle_ring_control( read_buf[9]); } // int16_t target_right read_buf[2]final_error; // int16_t target_left read_buf[2]-final_error; int16_t target_right read_buf[2]-final_error; int16_t target_left read_buf[2]final_error; //int16_t target_right 2000; //int16_t target_left 2000; //对输出值限幅 target_right target_right -(int16_t)read_buf[4]? -(int16_t)read_buf[4] : (target_right (int16_t)read_buf[3] ?(int16_t)read_buf[3] : target_right); target_left target_left -(int16_t)read_buf[4]? -(int16_t)read_buf[4] : (target_left (int16_t)read_buf[3] ?(int16_t)read_buf[3] : target_left); //int16_t pwm_right speed_control(count_right,target_right,speed_right); right_motor_go(target_right); //int16_t pwm_left speed_control(count_left,target_left,speed_left); left_motor_go(target_left); } void pid_pit_handler(){ pid_control(); }控制方案采用电机固定PWM -转向环误差来差速控制轮转向转向环接收循迹板最终误差经过PD控制后最后作用于电机PWM底盘只是低速运行这里只采用转向环闭环保证承载云台稳定即可在一个5MS定时器中断中执行-----------------------------------------------------------云台控制-------------------------------------------------------整个流程主要使用串口来进行通信与控制串口1RX用来接收摄像头数据串口1TX用来控制y轴电机串口2TX控制x轴电机串口3RX接收陀螺仪数据//使能步进电机 void Emm_V5_En_Control(uint8_t addr, bool state, bool snF) { uint8_t cmd[6] {0}; // 装载命令 cmd[0] addr; // 地址 cmd[1] 0xF3; // 功能码 cmd[2] 0xAB; // 辅助码 cmd[3] (uint8_t)state; // 使能状态 cmd[4] snF; // 多机同步运动标志 cmd[5] 0x6B; // 校验字节 // 发送命令 if(addr 1){ HAL_UART_Transmit(huart1, cmd, sizeof(cmd), HAL_MAX_DELAY); } if(addr 2){ HAL_UART_Transmit(huart2, cmd, sizeof(cmd), HAL_MAX_DELAY); } //延时1ms HAL_Delay(1); } //闭环步进电机位置模式 void Emm_V5_Pos_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, uint32_t clk, bool raF, bool snF) { uint8_t cmd[13] {0}; //Emm_V5_Pos_Control(1, 0, 1000, 0, 3200, 0, 0);地址方向速度加速度脉冲数3200个脉冲转一圈相对/绝对运动多机同步标志 // 装载命令 cmd[0] addr; // 地址 cmd[1] 0xFD; // 功能码 cmd[2] dir; // 方向 cmd[3] (uint8_t)(vel 8); // 速度(RPM)高8位字节 cmd[4] (uint8_t)(vel 0); // 速度(RPM)低8位字节 cmd[5] acc; // 加速度注意0是直接启动 cmd[6] (uint8_t)(clk 24); // 脉冲数(bit24 - bit31) cmd[7] (uint8_t)(clk 16); // 脉冲数(bit16 - bit23) cmd[8] (uint8_t)(clk 8); // 脉冲数(bit8 - bit15) cmd[9] (uint8_t)(clk 0); // 脉冲数(bit0 - bit7 ) cmd[10] raF; // 相位/绝对标志false为相对运动true为绝对值运动 cmd[11] snF; // 多机同步运动标志false为不启用true为启用 cmd[12] 0x6B; // 校验字节 // cmd[13] \n; // 发送命令 //下面电机2发给串口2上面电机1发给串口3 if(addr 2){ HAL_UART_Transmit(huart2, cmd, sizeof(cmd), HAL_MAX_DELAY); } if(addr 1){ HAL_UART_Transmit(huart1, cmd, sizeof(cmd), HAL_MAX_DELAY); } } //闭环步进电机速度模式 void Emm_V5_Vel_Control(uint8_t addr, uint8_t dir, uint16_t vel, uint8_t acc, bool snF) { //Emm_V5_Vel_Control(1, 0, 1000, 10, 0);地址方向速度加速度多机同步标志 HAL_Delay(10); uint8_t cmd[8] {0}; // 装载命令 cmd[0] addr; // 地址 cmd[1] 0xF6; // 功能码 cmd[2] dir; // 方向 cmd[3] (uint8_t)(vel 8); // 速度(RPM)高8位字节 cmd[4] (uint8_t)(vel 0); // 速度(RPM)低8位字节 cmd[5] acc; // 加速度注意0是直接启动 cmd[6] snF; // 多机同步运动标志 cmd[7] 0x6B; // 校验字节 // 发送命令 HAL_UART_Transmit(huart2, cmd, sizeof(cmd), HAL_MAX_DELAY); } void Emm_V5_Origin_Set_O(uint8_t addr, bool svF) { uint8_t cmd[5] {0}; // 装载命令 cmd[0] addr; // 地址 cmd[1] 0x93; // 功能码 cmd[2] 0x88; // 辅助码 cmd[3] svF; // 是否存储标志false为不存储true为存储 cmd[4] 0x6B; // 校验字节 // 发送命令 HAL_UART_Transmit(huart2, cmd, sizeof(cmd), HAL_MAX_DELAY); } //触发回零 void Emm_V5_Origin_Trigger_Return(uint8_t addr, uint8_t o_mode, bool snF) { //延时10ms HAL_Delay(10); //设置单圈回零位置 //Emm_V5_Origin_Set_O(1,0); uint8_t cmd[5] {0}; // 装载命令 cmd[0] addr; // 地址 cmd[1] 0x9A; // 功能码 cmd[2] o_mode; // 回零模式0为单圈就近回零1为单圈方向回零2为多圈无限位碰撞回零3为多圈有限位开关回零 cmd[3] snF; // 多机同步运动标志false为不启用true为启用 cmd[4] 0x6B; // 校验字节 // 发送命令 HAL_UART_Transmit(huart2, cmd, sizeof(cmd), HAL_MAX_DELAY); } //电机立即停止 void Emm_V5_Stop_Now(uint8_t addr, bool snF) { uint8_t cmd[5] {0}; // 装载命令 cmd[0] addr; // 地址 cmd[1] 0xFE; // 功能码 cmd[2] 0x98; // 辅助码 cmd[3] snF; // 多机同步运动标志 cmd[4] 0x6B; // 校验字节 // 发送命令 HAL_UART_Transmit(huart2, cmd, sizeof(cmd), HAL_MAX_DELAY); }电机通过串口发送命令来控制先封装电机位置模式使能停止回零的串口帧主要用到位置模式控制电机帧格式为[地址][功能码][方向][速度高字节][速度低字节][加速度][四字节脉冲数][相对/绝对标志][多机同步标志][校验字节]通过外部传参地址和脉冲数来控制电机运动if(huart huart3){ Usart3_Data *buf222; //printf(%d\n,Usart3_Data); //只接收以帧头为0x590x53开头的有效数据或已经开始接收的帧 if(((last_rsnum 0x59) (Usart3_Data 0x53))||Count 0){ //刚检测到帧头 if(Count 0){ rx_buf[0] 0x59; rx_buf[1] 0x53; Count 1; }else{ //开始接收帧 rx_buf[Count] Usart3_Data; } Count;//个数值1 } //1帧结束 if(Count 67){ Count 0; calc_checksum(rx_buf2,63,check_sum); CRC_num rx_buf[65]; if(check_sum CRC_num){ //printf(successful\n); } //读取所有子单元总长度 payload_len rx_buf[4]; //指针偏移量初始化为5第一个子单元起始位置帧头2TID2总长度1 pos 5; while(payload_len 0){ //结构体指针指向当前子单元的起始位置 payload (payload_data_t*)(rx_bufpos); //校验当前子单元并解析数据到g_output_info ret check_data_len_by_id(payload-data_id, payload-data_len, (unsigned char *)payload 2); if(ret (unsigned char)0x01){ //更新指针偏移量跳过当前子单元数据长度ID长度 pos payload-data_len sizeof(payload_data_t); //更新所有未处理子单元长度 payload_len - payload-data_len sizeof(payload_data_t); }else{ //跳过当前非法字节 pos; //总长度减1减少未解析的长度 payload_len--; } } } //用于检测帧头 last_rsnum Usart3_Data; }首先先实现底盘在旋转过程中云台保持原角度不跟着旋转所以要使用陀螺仪实现这一特性用串口3接收陀螺仪传感器数据该传感器帧格式为[帧头0x59 0x53][事务ID][载荷总长度][多个TLV数据包][校验和][填充位]共67字节配置串口3为接收1字节中断先检测帧头用一变量备份上一字节数据当上一字节是0x59当前字节是0x53,识别帧头后续不断自增存入数据缓冲区中当接收67字节整帧收满时先将索引清零从缓冲区第三位开始取63字节计算校验和和接收到的校验和对比校验成功从第6字节有效数据ID 数据长度 数据开始解析数据由于中断不能长时间停留所以我只根据ID号解析出我需用的偏航角一帧结束重新使能串口接收中断接收下一帧数据----------------------------------------------------------摄像头识别--------------------------------------------------------class FrameDetector: def __init__(self, min_area, max_area): self.min_area min_area self.max_area max_area # 四点排序把轮廓四个顶点固定顺序左上、右上、右下、左下 def _order_points(self, pts): rect np.zeros((4, 2), dtypefloat32) s pts.sum(axis1) rect[0] pts[np.argmin(s)] #xy最小 →左上 rect[2] pts[np.argmax(s)] #xy最大 →右下 diff np.diff(pts, axis1) rect[1] pts[np.argmin(diff)] #y-x最小 →右上 rect[3] pts[np.argmax(diff)] #y-x最大 →左下 return rect def detect_frames(self, frame): #高斯模糊降噪 blurred cv2.GaussianBlur(frame, (5, 5), 0) gray cv2.cvtColor(blurred, cv2.COLOR_BGR2GRAY) #转灰度 #二值化反向大于阈值变黑小于变白 _, mask cv2.threshold(gray, 100, 255, cv2.THRESH_BINARY_INV) #寻找最外层轮廓 contours, _ cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) frames [] for contour in contours: area cv2.contourArea(contour) #过滤面积过小/过大的轮廓 if not (self.min_area area self.max_area): continue peri cv2.arcLength(contour, True) #轮廓周长 #多边形逼近把曲线轮廓简化成少量顶点 approx cv2.approxPolyDP(contour, 0.02 * peri, True) #如果逼近后刚好4个顶点 →判定是矩形 if len(approx) 4: rect self._order_points(approx.reshape(4, 2)) frames.append(rect) return frames #YOLOv5检测矩形框 from maix import camera, display, image, nn, app, comm, uart, pinmap, time,pwm,gpio import struct, os from enum import KEEP from struct import pack report_on True APP_CMD_DETECT_RES 0x02 #串口设备 device /dev/ttyS0 serial0 uart.UART(device, 460800) # 协议定义 和STM32接收端匹配 FRAME_HEAD bytes([0xAA]) #帧头 FRAME_TAIL bytes([0x55]) #帧尾 # 包结构0xAA x(2B) y(2B) w(2B) h(2B) checksum(1B) 0x55 # 一共11字节checksum 前面10个字节累加 0xFF model_path model_241908.mud if not os.path.exists(model_path): model_path /root/models/maixhub/241908/model_241908.mud detector nn.YOLOv5(modelmodel_path) cam camera.Camera(detector.input_width(), detector.input_height(), detector.input_format()) dis display.Display() p comm.CommProtocol(buff_size 1024) while not app.need_exit(): img cam.read() objs detector.detect(img, conf_th 0.5, iou_th 0.45) for obj in objs: img.draw_rect(obj.x, obj.y, obj.w, obj.h, color image.COLOR_RED) msg f{detector.labels[obj.class_id]}: {obj.score:.2f} x obj.x y obj.y w obj.w h obj.h data x, y, w, h print(data) # 1.打包4个uint16小端 x,y,w,h 共8字节 data_bytes struct.pack(HHHH, x, y, w, h) # 2.拼接帧头构成前10字节0xAA x y w h pkg_10bytes FRAME_HEAD data_bytes #3.计算累加校验和前10个字节全部相加取低8位 checksum 0 for b in pkg_10bytes: checksum b checksum checksum 0xFF #4.拼接校验字节 帧尾0x55组成完整11字节数据包 full_pkg pkg_10bytes bytes([checksum]) FRAME_TAIL print(full_pkg.hex()) serial0.write(full_pkg) img.draw_string(obj.x, obj.y, msg, color image.COLOR_RED) fps time.fps() dis.show(img)起初我使用的方案是传统的opencv识别对图像进行高斯模糊降噪转灰度二值化寻找最外层轮廓根据远近实测限定轮廓的面积过滤掉过大/过小的轮廓再将剩余的轮廓进行多边形逼近返回当前轮廓存在的顶点若顶点数为4判定为矩形目标标记此轮廓在主循环调用此函数若检测到至少1个矩形常常是因为复杂环境所影响取最大的矩形作为目标如果倾斜方向检测到矩形框会导致计算到的中心点有偏差所以将检测到的矩形框四点透视变换到100*100的正方形取5050中心坐标点在逆透视到原图像该点即为当前中心点坐标传送给主控单片机但是此方案最终运行处理时间只有10帧也就是100ms处理1帧图像整个系统滞后性严重底盘在转弯时直接脱靶改善方案使用YOLOv5目标检测向训练平台发送一套图像训练集CNN卷积提取特征低于0.5的目标直接丢弃保留每个目标唯一的最优框提取目标框中的左上角x,y坐标和框的宽高度将帧头数据校验码帧尾进行二进制打包以小端模式传送给主控stm32运行帧率可达60-70帧远远高于opencv帧率if(huart huart1) { uint8_t ch *buf111; static uint8_t count_camera 0; uint8_t camera_buff[11] {0}; // 11字节接收缓存 if(count_camera 0) { // 寻找帧头 0xAA if(ch 0xAA) { camera_buff[0] ch; count_camera 1; } } else { camera_buff[count_camera] ch; // 收满11字节一整包 if(count_camera 11) { // 校验和计算 uint8_t sum 0; for(uint8_t i 0; i 10; i) { sum camera_buff[i]; } uint8_t recv_sum camera_buff[9]; uint8_t frame_tail camera_buff[10]; // 校验和正确 帧尾是0x55才解析 if( (sum recv_sum) (frame_tail 0x55) ) { // camera_buff[0]0xAA // [1]xL [2]xH | [3]yL [4]yH | [5]wL [6]wH | [7]hL [8]hH location.x ((uint16_t)camera_buff[2] 8) | camera_buff[1]; location.y ((uint16_t)camera_buff[4] 8) | camera_buff[3]; location.w ((uint16_t)camera_buff[6] 8) | camera_buff[5]; location.h ((uint16_t)camera_buff[8] 8) | camera_buff[7]; // 坐标范围校验 if(location.x 320 location.x 0 location.y 240 location.y 0 location.w 320 location.w 0 location.h 240 location.h 0) { // 计算图像中心误差 camera_error_x center_point[0] - (location.x (float)(location.w / 2.0f)); camera_error_y center_point[1] - (location.y (float)(location.h / 2.0f)); } scan_flag 1; // 检测到目标关闭扫描进入跟踪 } // 无论校验成功失败接收计数器清零等待下一帧头 count_camera 0; } } }使用串口1接收摄像头数据我封装的帧格式为[帧头][2字节x坐标][2字节y坐标][2字节w宽][两字节h高][校验码][帧尾]从帧头开始接收接受满11字节时判断校验码和帧尾正确进行解析高字节左移8位或低字节解析出x,y,w,h通过中心x坐标-当前x坐标矩形框宽度/2中心y坐标 - 当前y坐标 矩形框高度/2计算出当前x轴y轴的误差清空索引准备接收下一帧中断void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)//溢出中断回调函数 { if(htim-Instance TIM1) { //检索到矩形框 if(scan_flag 1){ pid_control_x_internal(target_yaw-g_continuous_yaw); motor_task_x(incremental_error); } else{ //开启检索模式 //摄像头误差清零防止增量式累计 new_error[1] 0.0; pid_control_x_internal(target_yaw-g_continuous_yaw); target_yaw target_yaw0.1*scan_dir; if(target_yaw-target_yaw1 90){ scan_dir -1; }else if(target_yaw-target_yaw1 -90){ scan_dir 1; } motor_task_x(incremental_error); } } if(htim-Instance TIM2){ //若检索到矩形框 if(scan_flag 1){ //外环修改内环目标姿态角值 pid_control_x_external(camera_error_x); target_yawnew_error[2]; pid_control_y(camera_error_y); //控制下步进电机 motor_task_y(new_error[0]); } } }我使用双环PID控制云台打靶内环使用陀螺仪保持目标角度外环使用摄像头误差修正目标角度陀螺仪波特率为460800bit/s换算出来传输一帧数据大概需要670/460800 1.45ms所以内环配置为2ms定时器中断通过目标角度 - 当前角度过PI运算使用绝对模式累加到电机脉冲数上增量式的一个好处就是只要有误差就会一直累积直到误差为零因为如果云台离靶纸很远的情况下稍微一点小误差就会使激光偏离靶心肉眼可见的距离如果使用位置式就会有静态误差导致打靶效果不佳我的摄像头帧率为70帧也就是1帧14ms外环配置为15ms通过接收到的摄像头误差经过位置式PD运算改变目标角度的值使其锁定靶心内环不断修正其到达目标角度从而形成一套闭环系统若某一时刻摄像头丢帧则开启检索模式只有内环起作用内环不断改变当前目标角度使其在当前角度的基础上在-60° - 60°循环移动直到识别到靶纸关闭检索模式重新开始锁定靶心