永安网站建设公司,网页qq登录保护功能,总部在深圳的大企业,专业手机网站制作哪家好文章目录前言整车效果控制思路循迹环岛处理障碍处理关键代码部分差比和以及当前速度计算角速度环速度环环岛处理障碍处理前言
年后就没怎么碰车了#xff0c;到3月中旬换三轮了#xff0c;可算有一点成效了#xff0c;做个记录。
整车效果 三轮负压电磁慢速元素识别控制思…
文章目录前言整车效果控制思路循迹环岛处理障碍处理关键代码部分差比和以及当前速度计算角速度环速度环环岛处理障碍处理前言
年后就没怎么碰车了到3月中旬换三轮了可算有一点成效了做个记录。
整车效果 三轮负压电磁慢速元素识别控制思路
循迹
角速度环以及速度环
差比和的结果与0做差得到的误差作为角速度环的输入。 角速度环的输出输出作为我们期望小车转向的角速度。
简单对差速小车建模以顺时针方向的角速度为正参考方向推导数学公式可得
vLrefvmωL2v_{Lref}v_m\frac{\omega L}{2}vLrefvm2ωL vRrefvm−ωL2v_{Rref}v_m-\frac{\omega L}{2}vRrefvm−2ωL
其中vLrefv_{Lref}vLref是左轮的参考速度vRrefv_{Rref}vRref是右轮的参考速度vmv_mvm是小车整体的运行速度或假想小车中心有一个轮子的运行速度LLL是车模后轮的轮距我测量的D车模的后轮距为0.17mω\omegaω为小车的角速度即角速度环的输出。
得到左右两轮子的参考速度以后经过速度环控制后得到左右电机的占空比并幅值即可。
其中角速度环用PD控制P环节用于放大误差送入矫正环节在这里的三轮车矫正环节即控制两个轮子的速度以矫正误差D环节用于抑制车体的震荡速度环用PI控制P放大误差进行速度调节I用于消除静差尤其是当我们上负压以后I尤为重要。
由于三轮车的运动轨迹完全取决于两个轮子速度之间的关系两个轮子的速度环控制必须做到快速而精准需花时间调节速度环的参数。
电机PID参数整定
环岛处理
检测到圆环预圆环某个电感大于某个阈值到达最佳入环位置强制入环直到陀螺仪大于某个开始园内循迹圆内循迹出环。
障碍处理
识别障碍**(TOF测距小于某个限制)**避开障碍左右轮固定差速直到陀螺仪积分大于某个角度回到赛道左右轮固定差速直到陀螺仪为小于某个角度姿态调整让小车的姿态基本与赛道平齐直到陀螺仪角度在某个范围内或误差在某个范围内
关键代码部分
差比和以及当前速度计算
三电感电磁循迹小车
角速度环
void dir_loop()
{float temp;pid_steer.ek 0 - error;temp pid_steer.KP * pid_steer.ek imu660ra_gyro_z / 65.6;
// 300 * (pid_steer.ek - pid_steer.ek_1); // 参考角速度pid_motor_L.SetValue speed_rate * speed_goal - temp * 0.0875;pid_motor_R.SetValue speed_rate * speed_goal temp * 0.0875;
}速度环
void speed_loop()
{float inc_L, inc_R;// 限制内轮速度if(pid_motor_L.SetValue 0)pid_motor_L.SetValue 0;if(pid_motor_R.SetValue 0)pid_motor_R.SetValue 0;// 外轮速度限制if(pid_motor_L.SetValue 2 * speed_goal * speed_rate)pid_motor_L.SetValue 2 * speed_goal * speed_rate;if(pid_motor_R.SetValue 2 * speed_goal * speed_rate)pid_motor_R.SetValue 2 * speed_goal * speed_rate;inc_L PID_Control_Inc(pid_motor_L, 0);inc_R PID_Control_Inc(pid_motor_R, 0);duty_L inc_L;duty_R inc_R;if(duty_L Motor_UpperLimit)duty_L Motor_UpperLimit;if(duty_L Motor_LowerLimit)duty_L Motor_LowerLimit;if(duty_R Motor_UpperLimit)duty_R Motor_UpperLimit;if(duty_R Motor_LowerLimit)duty_R Motor_LowerLimit;
}环岛处理
void circ_handler()
{static uint16 count_delay_R_circ_pre;static uint16 count_delay;if(flag_L_circ_pre) // 预圆环阶段{count_delay_R_circ_pre;if(count_delay_R_circ_pre 0.5 / speed_goal * 60 * TIM1_ISR_F) //{flag_L_circ_pre 0;flag_L_circ_frc 1; // force to get into circ}}if(flag_L_circ_frc){Angle_circ imu660ra_gyro_z * 0.002 / 65.6;if(Angle_circ -35){flag_L_circ_frc 0;flag_L_circ_in 1;flag_L_circ_out 0;}else{pid_motor_L.SetValue 0 * speed_rate * speed_goal;pid_motor_R.SetValue 2 * speed_rate * speed_goal;}}if(flag_L_circ_in){Angle_circ imu660ra_gyro_z * 0.002 / 65.6;if(Angle_circ -300)flag_L_circ_in 0;}if(flag_L_circ_in 0 flag_L_circ_out 0){count_delay;if(count_delay 5 * TIM1_ISR_F){flag_L_circ_out 1; // 已出环count_delay 0;Angle_circ 0;}}
}障碍处理
void block_handler()
{if(flag_block_detected){pid_motor_L.SetValue 0 * speed_rate * speed_goal;pid_motor_R.SetValue 2 * speed_rate * speed_goal;Angle_block imu660ra_gyro_z * 0.002 / 65.6;}if(Angle_block -45){flag_block_detected 0;flag_block_back 1;}if(flag_block_back){if(Angle_block 50){pid_motor_R.SetValue 0 * speed_rate * speed_goal;pid_motor_L.SetValue 2 * speed_rate * speed_goal;}elseflag_block_adjust 1;if(flag_block_adjust){pid_motor_L.SetValue 0.3 * speed_rate * speed_goal;pid_motor_R.SetValue 1.7 * speed_rate * speed_goal;}Angle_block imu660ra_gyro_z * 0.002 / 65.6;}if(Angle_block 10 Angle_block -10 flag_block_adjust){flag_block_detected 0;flag_block_back 0;flag_block_adjust 0;Angle_block 0;}
}
