本项目使用英飞凌TC264双核MCU开发一套智能车赛道循迹控制系统面向于一张含有二极管三极管电阻电容开关的电路赛道通过视觉识别路口节点检测姿态解算串级pid闭环运动控制在线调整参数与掉点保存进行电路图循迹。采用双核分工架构充分利用硬件资源实现视觉高帧处理与运动控制并行处理1核主要负责摄像头图像采集与上层视觉算法处理包含自适应阈值求解图像二值化赛道边界搜索中线拟合赛道误差加权计算拐点节点识别与路径决策为整车运动控制提供视觉输入源。2核主要负责底层外设和闭环控制包含imu陀螺仪解算编码器速度采样定时器中断时序调度双串级pid算法运算电机差速控制同时承载按键菜单交互flash参数存储等调试功能我主要负责软件。代码主要包括三个板块视觉板块控制板块和调试板块首先是视觉板块//使用大津法在指定行获取阈值 uint8_t Otsus_thresholding_method(uint8* mt9v03x_image,uint16 col,uint16_t row_start,uint16_t row_end){//传入行指针参数 uint32_t histogram[256] {0}; // 灰度直方图 uint32_t pixel_count 0; // 像素总数 uint32_t pixel_sum 0; // 灰度总和 //统计灰度直方图 for(uint16_t row row_start; row row_end;row){ for(uint16_t col_index 0;col_index col;col_index){ //接收此像素点的灰度值 uint8_t gray mt9v03x_image[row*colcol_index]; //统计灰度值个数 histogram[gray]; //统计像素总数 pixel_count; //统计灰度综合 pixel_sumgray; } } //寻找有效灰度范围减小计算量 uint8_t min_gray 0,max_gray 255; for(min_gray 0;min_gray 256 histogram[min_gray] 0;min_gray); for(max_gray 255;max_gray min_gray histogram[max_gray] 0;max_gray--); if(max_gray min_gray || min_gray 1 max_gray){ return min_gray; } //计算最佳阈值 float max_variance 0.0f; //先赋最小值 uint8_t best_threshold min_gray; //背景像素初始化 uint32_t background_count 0; //背景灰度值初始化 uint32_t background_sum 0; for(uint8_t threshold min_gray;threshold max_gray;threshold){ background_count histogram[threshold]; background_sum histogram[threshold]*threshold; //计算前景像素 uint32_t foreground_count pixel_count - background_count; //避免除0错误 if(foreground_count 0){ continue; } //计算前景灰度值 uint32_t foreground_sum pixel_sum - background_sum; // 计算背景和前景的平均灰度 float background_mean (float)background_sum / background_count; float foreground_mean (float)foreground_sum / foreground_count; //类间方差算出前景与背景最大差值 g n0*n1*(u0*u0 - u1*u1) float variance (float)foreground_count*background_count*(background_mean - foreground_mean)*(background_mean - foreground_mean); //算出最大方差记录阈值 if(variancemax_variance){ max_variance variance; best_threshold threshold; } } return best_threshold; } //图像二值化 void image_binarization(uint8 (*mt9v03x_image)[COL]){//传入二维数组首元素地址步长为整行 //大津法自动获取阈值 //若标志位存在则大津法 if(auto_threshold_flag){ //仅首次进入初始化 static uint8 threshold_update_cnt 0; //每20帧更新一次阈值 if(threshold_update_cnt 20){ threshold_update_cnt 0; //分块计算阈值 threshold_far Otsus_thresholding_method(mt9v03x_image[0],COL,BLOCK_FAR_START, BLOCK_FAR_END);//第一个传参是一维数组首元素的地址步长为1个元素 threshold_mid Otsus_thresholding_method(mt9v03x_image[0],COL,BLOCK_MID_START, BLOCK_MID_END); threshold_near Otsus_thresholding_method(mt9v03x_image[0],COL,BLOCK_NEAR_START, BLOCK_NEAR_END); threshold_far (threshold_far THRESHOLD_MIN) ? THRESHOLD_MIN : (threshold_far THRESHOLD_MAX) ? THRESHOLD_MAX : threshold_far; threshold_mid (threshold_mid THRESHOLD_MIN) ? THRESHOLD_MIN : (threshold_mid THRESHOLD_MAX) ? THRESHOLD_MAX : threshold_mid; threshold_near (threshold_near THRESHOLD_MIN) ? THRESHOLD_MIN : (threshold_near THRESHOLD_MAX) ? THRESHOLD_MAX : threshold_near; } } for(uint16_t row 0;row ROW;row){ //判断阈值区间 if(row BLOCK_FAR_END){ threshold threshold_far; }else if(row BLOCK_MID_END){ threshold threshold_mid; }else{ threshold threshold_near; } // } //二值化 for(uint16_t col 0;col COL;col){ bin_image[row][col] (mt9v03x_image[row][col] threshold) ? WHITE : BLACK; /* if(bin_image[row][col]){ //记录白像素点数目 white_point; } */ } } }将一副120*188的灰度图像转化成二值化图像每个像素点都是一个0-255范围的数值0代表黑255代表白通过比较一个像素阈值小于该值则为黑大于该值则为白所以这个阈值很重要影响一副图像的质量确定该阈值我通过大津法来计算并且每20帧更新一次大津法原理即先遍历图像每一个像素点统计每个灰度值出现多少次统计总像素数和灰度综合这样就会形成一个灰度直方图横坐标代表灰度值纵坐标代表灰度值个数接下来从最左侧最小灰度值开始计算背景总像素点和总灰度值以及前景像素点和总灰度值相除从而得出背景和前景平均灰度值通过类间方差公式 方差的平方 背景像素数量*前景像素数量*背景平均灰度 - 前景平均灰度的平方计算类间方差方差越大代表前景背景灰度差异越强逐个比较灰度直方图每个区间之间的类间方差寻找出最大的方差并记录此时灰度阈值即为最优的阈值通过比较该阈值则可以把灰度图像转换为二值化图像//搜线 void search_line(){ //左右边线找到标志位 uint8 l_find_flag 0,r_find_flag 0; //初始化边线数组 memset(l_border,0,sizeof(l_border)); memset(r_border,COL-1,sizeof(r_border)); memset(l_border_final,0,sizeof(l_border)); memset(r_border_final,COL-1,sizeof(r_border)); memset(mid_line,COL/2,sizeof(mid_line)); //从下往上搜 for(int16 row ROW - 1;row 0;row --){ //先找左边线 for(uint8 col 0;col COL - 3;col){ if(bin_image[row][col] BLACK bin_image[row][col1] WHITE bin_image[row][col2] WHITE){ l_find_flag 1; l_border[row] col; l_border_final[row] col; break; } } if(!l_find_flag){ l_border[row] 0; l_border_final[row] 0; } //找右边线 for(uint8 col COL - 1;col 2;col--){ if(bin_image[row][col] BLACK bin_image[row][col-1] WHITE bin_image[row][col-2] WHITE){ r_find_flag 1; r_border[row] col; r_border_final[row] col; break; } } if(!r_find_flag){ r_border[row] COL - 1; r_border_final[row] COL - 1; } //计算中线 mid_line_final[row] (l_border_final[row]r_border_final[row])/2; } }接下来根据二值图像搜索左右边线从下往上逐行扫描左边线从最左边往最右边检测黑 - 黑 - 白跳变点存储到左边线数组中右边线同理从最右边往最左边检测黑 - 黑 - 白跳变点存储到右边线数组中若整行没有判断到此特征将左右边线值置到边界不误导后续误差计算通过左边线 右边线/2算出中线void calc_image_error(){ float error 0 ; static float last_error 0.0f; float weight_sum 0.0f; float error_sum 0.0f; //若边界行存在只计算80行以下区域权重误差 if(border_flag){ // corner_row corner_row ROW-1 ? ROW-1 :corner_row; for(uint8 i 80;i ROW;i){ // draw_line(COL/2,0,COL/2, 80); error_sum (mid_line_final[i] -COL/2)*image_error_weight[i]; weight_sum image_error_weight[i]; } //左右直角前瞻靠前更快转向 }else if(angle_flag_left || angle_flag_right){ for(uint8 i 0;i ROW;i){ error_sum (mid_line_final[i] -COL/2)*image_error_weight_angle[i]; weight_sum image_error_weight[i]; } }else{ //正常扫线权重 for(uint8 i 50;i ROW;i){ error_sum (mid_line_final[i] -COL/2)*image_error_weight[i]; weight_sum image_error_weight[i]; } } //加权平均 error error_sum/weight_sum; //互补滤波 error error*0.9flast_error*0.1f; last_error error; error error -70.0? -70.0 : (error70.0 ? 70.0 :error); final_error error; }根据中线再计算出偏离中线的误差由于有120行中线值不能完全依赖一行或者几行的中线值来判定最终的误差值如果此刻由于光线等原因导致某一行是错误的中线值计算出来的误差会直接影响车体运动轨迹所以每一行应该有一个置信度也就是权重这样的好处不仅仅能提高系统的鲁棒性而且还可以跟据车体运动情况调整这个权重值比如车体运动响应滞后一部分是因为前瞻太近了应该让摄像头更倾向远处图像此时就可以调整远端中线误差的权重值。根据每一行计算的中线值 - 理想中线值 * 权重值最终在进行加权平均算出最终的误差值此刻起始普通寻线视觉部分就完成了但电路赛道多是直角弯并且线比较细只有几行会参与误差计算车体还来不及拐弯就冲出赛道这种情况就需要进行偏置和拟合边线//节点判断 void node_judge(){ switch(node_state){ case NODE_NONE: border_flag 0; if(current_plan-turns[node_count % current_plan - length] GO_STRAIGHT){ find_point(ROW-1,65); }else{ find_point(ROW-1,50); } //先检测目前左右是否有拐点 count_paths_on_rect(bin_image); //左右边界有节点再进行拐点检测 if(path_info.left_exist ! 0 || path_info.right_exist ! 0){ //检测是否有拐点 if(check_point_detected()){ //count_paths_on_rect(bin_image); //执行决策 turn_direction decide_turn_direction(); if(turn_direction ! TURN_NONE){ //改变节点状态 node_state NODE_DETECTED; //节点数1 node_count; } //若没有检测到拐点将进行补线检测左右边界是否有白线 }else if(find_best_border_line_at_col(1, bin_image)){ //若存在将边界点顶底行白线中线偏置抑制检测不到拐点的情况 //corner_row find_best_border_line_at_col(1, bin_image); //draw_line(COL/2,0,COL/2,corner_row); border_flag 1; }else if(find_best_border_line_at_col(COL - 2, bin_image)){ //corner_row find_best_border_line_at_col(COL - 2, bin_image); //draw_line(COL/2,0,COL/2,corner_row); border_flag 1; } } break; case NODE_DETECTED: if(!angle_start_locked){ if(turn_direction TURN_LEFT){ //姿态角清零 yaw_clear_to_zero(); //建立目标姿态角 yaw_targetjudge -yaw_target; } else if(turn_direction TURN_RIGHT){ yaw_clear_to_zero(); yaw_targetjudge yaw_target; } else if(turn_direction GO_STRAIGHT){ straight_start_dist total_dis;//记录当前距离 } angle_start_locked 1; node_state NODE_TURNING; } break; case NODE_TURNING: //----------------------------------------------------------------直行------------------------------------------------------------------------------------------------------ if(turn_direction GO_STRAIGHT){ //测试 gpio_set_level(P33_10, 1); mid_bias(); float travel_distance total_dis - straight_start_dist;//直行行驶距离 //若行驶距离大于阈值且节点数小于3直行结束 if(travel_distance 0.15f){ uint8 count count_paths_on_rect(bin_image);//检测矩形框节点数 if(count 3){//节点数3 reset_node(); //测试 gpio_set_level(P33_10, 0); } } } //--------------------------------------------------------------左直角----------------------------------------------------------------------------------------- else if(turn_direction TURN_LEFT){ //测试 gpio_set_level(P33_10, 1); angle_flag_left 1; //边线左偏置 left_bias(); if(final_yaw yaw_targetjudge){ reset_node(); //左直角标志位清零 angle_flag_left 0; //测试 gpio_set_level(P33_10, 0); } } //--------------------------------------------------------------右直角----------------------------------------------------------------------------------------- else if(turn_direction TURN_RIGHT){ //测试 gpio_set_level(P33_10, 1); angle_flag_right 1; right_bias(); if(final_yaw yaw_targetjudge){ reset_node(); //右直角标志位清零 angle_flag_right 0; gpio_set_level(P33_10, 0); } } break; default: break; } }对当前图像先进行四点矩形框判断也就是检测图像的上下左右边框处有没有白线图像自下往上遍历看是否出现黑-白-白跳变点比如图像最右侧有线则标记右侧边线存在标志位并对节点数进行统计若左或右边线存在则进行角点判断分别检测左右拐点自下往上扫线判断边界数组中前几行变化平稳之后有几行出现突变值如果有记录拐点行数。如果拐点和边界点都存在说明现在需要执行左转/右转/直行中的一个策略我通过一个提前定义好的枚举数组存放当前赛道每个路口需要行走的路线并和当前边界点存在情况进行比对比如左边线存在且当前规划路径是左转那就判定当前车体需要进行左拐处理目前已经告诉车体往哪里拐拐多少也需要进行条件限制左右拐我利用imu陀螺仪解算出的偏航角作为结束条件如果当前规划向左拐则设定目标偏航角为90度转到设定角度继续执行普通巡线逻辑并等待下一个节点到来直行我利用编码器数值进行积分到达指定阈值后继续普通巡线case NODE_DETECTED: if(!angle_start_locked){ if(turn_direction TURN_LEFT){ //姿态角清零 yaw_clear_to_zero(); //建立目标姿态角 yaw_targetjudge -yaw_target; } else if(turn_direction TURN_RIGHT){ yaw_clear_to_zero(); yaw_targetjudge yaw_target; } else if(turn_direction GO_STRAIGHT){ straight_start_dist total_dis;//记录当前距离 } angle_start_locked 1; node_state NODE_TURNING; } break;因为我使用的是摄像头循迹所以车体严格按照摄像头返回的误差控制行进所以该“怎么拐”我们只需改变左右边线值则可以控制左右轮差速转向记录此时的边界行将边界行以上的区域进行使用线性插值进行偏置根据传进来的起始坐标和末尾坐标将这两点中间的值一一映射到这两个坐标的连线上根据规划路径将节点以上的区域合理偏置例如左转将节点行以上的左右边线往左边界偏置直行将左右边线往中间偏置到达设定目标阈值将所有标志位清零进行下一路口处判断---------------------------------------------------控制部分-----------------------------------------------------/* * pid.c * * Created on: 2026年3月6日 * Author: gjl66 */ #include pid.h #include wifi.h static uint8 time_2ms; static uint8 time_5ms; void pid_init(){ time_2ms 0; time_5ms 0; //pid1ms中断 pit_ms_init(CCU60_CH0, 1); } //取绝对值 float my_abs(float value){ if(value 0){ return value; }else{ return -value; } } //速度环参数 float kp1 0.0; float ki1 0.0; int16 max_speed 0; int16 min_speed 0; typedef struct{ //当前pi输出 float current_pwm; //累计pi输出 float cumulative_pwm; //当前偏差 int16 current_error; //上一次误差 int16 pre_error; }SPEED; //创建左右轮速度环中间计算变量结构体指针 SPEED speed_struct_left { .current_pwm 0.0f, .cumulative_pwm 0.0f, .current_error 0, .pre_error 0, }; SPEED* speed_left speed_struct_left; SPEED speed_struct_right { .current_pwm 0.0f, .cumulative_pwm 0.0f, .current_error 0, .pre_error 0, }; SPEED* speed_right speed_struct_right; void speed_parameter_init(){ speed_struct_left.cumulative_pwm 0.0f; speed_struct_left.current_error 0; speed_struct_left.current_pwm 0.0f; speed_struct_left.pre_error 0; speed_struct_right.cumulative_pwm 0.0f; speed_struct_right.current_error 0; speed_struct_right.current_pwm 0.0f; speed_struct_right.pre_error 0; } float speed_control(int16_t practical_speed,int16_t target_speed,SPEED* speed){ //当前偏差 speed -current_error target_speed - practical_speed; //过PI,保存此时刻的误差 speed -current_pwm kp1*(speed-current_error - speed-pre_error)ki1*(speed-current_error); //存上一次偏差 speed-pre_error speed-current_error; //输出值 speed-cumulative_pwm speed-current_pwm; //对输出值限幅 speed-cumulative_pwm speed-cumulative_pwm min_speed? min_speed : (speed-cumulative_pwm max_speed ?max_speed : speed-cumulative_pwm); return speed-cumulative_pwm; } //转向环pid参数 float kp11 0.0; float kd11 0.0; float kp22 0.0; float kd22 0.0; float gkd 0.0; float angle_kp11 0.0; float angle_kd11 0.0; float angle_kp22 0.0; float angle_kd22 0.0; float final_kp11; float final_kd22; //转向环中间计算变量 typedef struct{ float error1; float error2; float error3; float last_error1; }TURN; TURN turn {0}; //转向环 int16 turn_control(){ //分段式pd直角给更快响应 if(angle_flag_left){ final_kp11 angle_kp11; final_kd22 angle_kd11; }else if(angle_flag_right){ final_kp11 angle_kp22; final_kd22 angle_kd22; }else{ final_kp11 kp11; final_kd22 kd22; } turn.error1 final_error; turn.error2 imu660rb_gyro_z; turn.error3 final_kp11*turn.error1 kd11*(turn.error1 - turn.last_error1)kp22*turn.error1*my_abs(turn.error1)-(final_kd22*turn.error2)/100; turn.last_error1 turn.error1; //取整形因为计算的是编码器目标速度 return (int16)turn.error3; } //左右最终pwm值 int16 left_pwm,right_pwm; int16 error_target,target_speed ; /* 速度决策一些参数 */ uint8 first_speed_flag 0; uint8 going_speed_flag 0; //速度决策编码器距离 float judge_speed_dis 0.0; //左右轮经外环处理后的目标速度 int16 target_speed_l,target_speed_r; //逐飞双串思路内环2s外环3s后面有机会试试三串 void pid_control(){ if(time_2ms 2){ time_2ms 0; if(!menu_run_flag){//菜单关闭驱动电机 //软件盲盒在指定岔路口停车 /* if(node_count 44 ){ target_speed 0; } */ if(angle_flag_left || angle_flag_right){ //外轮差速稍大 target_speed_l target_speed (int16)(1.05*error_target); target_speed_r target_speed - error_target; }else{ target_speed_l target_speed error_target; target_speed_r target_speed - error_target; } left_pwm (int16)speed_control(encoder_data_1,target_speed_l,speed_left); right_pwm (int16)speed_control(encoder_data_2,target_speed_r,speed_right); motor_left_set(left_pwm); motor_right_set(right_pwm); } } if(time_5ms3){ time_5ms 0; error_target turn_control(); } }采用双串级pid闭环控制创建两个定时器中断周期时间分别为1ms和3ms两个定时器分别对应内环和外环内环使用增量式pi控制器控制电机达到编码器目标转速每个电机给相同pwm由于齿轮摩擦等外部因素会导致转速不一致所以根据编码器值反馈不断修正当前pwm值控制两轮转速一致使用kp* (当前转速误差 - 上一次转速误差) ki *当前转速误差ki的作用主要调节电机达到目标转速的响应但过大可能会导致超调引入kp乘以计算当前误差减去上一次误差抑制超调现象计算出的最终值累加到pwm中。外环使用位置式pd操作目标编码器转速使得电机实现差速运行kp*当前摄像头最终误差kd*陀螺仪角速度陀螺仪角速度不受任何外界影响直接反映当前车体转向时的角速度而摄像头误差变化项可能会由于曝光噪点等原因导致计算出来的值不准确所以用陀螺仪角速度作为抑制项好过摄像头误差变化项将最终计算值-到左右轮编码器目标转速上内环不断调整pwm到达目标速度从而形成一套闭环控制-----------------------------------------------调试部分-----------------------------------------------------------------typedef struct{ int16_t current;//当前页码 int16_t next;//下一次页码 int16_t enter;//确认 void (*current_display)();//显示 }menu; menu table_display[45] { //主目录 {0,1,6,function_image},//image {1,2,21,function_speed},//spedd_ring {2,3,27,function_turn},//turn_ring {3,4,34,function_path},//path {4,5,0,function_reset},//重置路径 {5,0,36,function_fuya},//负压 {6,7,13,function_image_a2},//手动阈值 {7,8,17,function_image_b2},//自动阈值 {8,9,8,function_image_c2},//目标姿态角 {9,10,9,function_image_d2},//二值图像 {10,11,10,function_image_e2},//灰度图像 {11,12,19,function_image_f2},//元素测试 {12,6,0,function_image_g2},//保存退出 {13,14,13,function_image_a3},//远处阈值 {14,15,14,function_image_b3},//中处阈值 {15,16,15,function_image_c3},//近处阈值 {16,13,6,function_image_d3},//保存退出 {17,18,17,function_image_e3},//自动阈值 {18,17,6,function_image_f3},//退出 {19,20,19,function_image_g3},//元素测试 {20,19,6,function_image_h3},//退出 {21,22,21,function_speed_a2},//target_speed {22,23,22,function_speed_b2},//kp {23,24,23,function_speed_c2},//ki {24,25,24,function_speed_d2},//max_speed {25,26,25,function_speed_e2},//min_speed {26,21,0,function_speed_f2},//save {27,28,27,function_turn_a2},//kp11 {28,29,28,function_turn_b2},//kd22 {29,30,29,function_turn_c2},//angle_kp11 {30,31,30,function_turn_d2},//angle_kd11 {31,32,31,function_turn_e2},//angle_kp22 {32,33,32,function_turn_f2},//angle_kd22 {33,27,0,function_turn_g2},//save {34,35,34,function_path_a2},//选择路线 {35,34,0,function_path_b2},//save {36,37,36,function_fuya_a2},//PWM {37,38,37,function_fuya_b2},//ON {38,39,38,function_fuya_c2},//OFF {39,36,0,function_fuya_d2},//save }; void menu_show_114(){ //next键 if(!gpio_get_level(KEY1)){ ips114_clear(); while(!gpio_get_level(KEY1)); function_index table_display[function_index].next; } //确认键 if(!gpio_get_level(KEY2)){ ips114_clear(); while(!gpio_get_level(KEY2)); //进入到保存参数 if(function_index 12 ||function_index 16 ||function_index 26 ||function_index 33 || function_index 35||function_index 39){ flash_save(); ips114_show_string(20, 60, save successful!); system_delay_ms(500); ips114_clear(); } //重置路径 if(function_index 4){ reset_path(); } //灰度与二值图像切换 if(function_index 9){ binary_image_flag 1; }else if(function_index 10){ binary_image_flag 0; } //操作负压 if(function_index 37){ fuya_motor_set(fuya_value); // fuya_motor_set(4200); ips114_clear(); menu_run_flag 0; function_index 0; return; }else if(function_index 38){ fuya_motor_set(0); } if(function_index 6){//手动阈值 auto_threshold_flag 0; //阈值限幅 //threshold (threshold 90)? 90: (threshold 200) ? 200: threshold; }else if(function_index 7){//自动阈值 auto_threshold_flag 1; } //确认键跳到下一级菜单 function_index table_display[function_index].enter; } table_display[function_index].current_display(); }内外环参数曝光阈值路径更换等工作需要频繁调试每次烧录太麻烦所以引入按键调参菜单解决反复烧录程序调试参数的问题支持支持PID内外环参数图像阈值路线选择等参数在线修改参数修改后可保存Flash,上电自动从页中取值实现脱机调参核心采用菜单页表函数指针的架构。预先定义菜单结构体包含当前页码下一页索引确认跳转索引页面显示函数指针构建全局菜单页表数组数组中每一项对应一个菜单页面预先固化该页面的跳转逻辑与显示回调函数。全局变量function_index作为当前页码索引标记当前停留的菜单页码。封装菜单扫描主函数作为主循环入口4个按键分工key1为下一页切换键按键消抖检测到按下后读取当前页表内next成员赋值给全局变量索引完成同级菜单上下翻页。key2为确定键消抖后执行页面绑定的确认逻辑读取页表enter成员将全局变量索引更新为目标页码实现进入二级/三级子菜单当选中保存页面按下确认调用函数将全部参数写入flash中key3/key4参数增减在对应页面的显示函数中调用调参函数检测增减按键对目标参数做加减并增加上下限限幅防止参数越界每个页面编写独立显示回调函数页面函数内部完成屏幕绘制当前参数数值显示参数增减逻辑绑定并在循环末尾执行当前页面显示函数刷新屏幕内容新增调参页面只需在页表新增条目并编写对应的显示函数可快速添加新的可调参数方便调试开发项目过程中遇到的问题1路口识别后反复触发摄像头会连续采集多帧。车经过一个路口时拐点可能在好几帧中都存在。如果每帧检测到拐点就立即增加节点数预设路线可能提前走到下一项表现为“该左转时却直行转弯方向偶尔不对”代码采用路口状态机分为三个状态1NODE_NONE检测边线拐点并结合预设路径决定方向确认后才将node_count加1并切到NODE_DETECTED2NODE_DETECTED:只初始化一次本次动作。转弯时清零yaw并设置目标角度直行时记录起始里程切换到NODE_TURNING3NODE_TURNING暂不按新路口重复决策只判断本次节点行在哪动态更新中线偏置左右达到目标角度后退出直行超过设定距离后退出切换状态到NODE_NONE进行下一次路口判断有效避免了重复判断拐点和节点导致不按规定路径跑的问题2起步速度突变根据“目标速度 - 编码器反馈”计算电机输出。如果程序运行车还是静止时内环积分项一直在积分累计起步立刻给最高限幅值出现起步翘头和轮胎打滑。此时只降低pid增益可能让正常行驶时的响应也变慢改进方案是在启动时保存用户设定的最终速度把目标速度从低值逐步提高。它由10ms定时中断调用每次增加10达到设定值后停止递增。假如我目标设为500按理想调用周期约0.5秒升到目标值。随后速度环将目标速度与编码器反馈比较分别计算左右轮PWM转向控制还会在此基础上给左右轮不同的目标速度从而实现缓启动