前言

年后就没怎么碰车了，到3月中旬换三轮了，可算有一点成效了，做个记录。

整车效果

控制思路

循迹

角速度环以及速度环

差比和的结果与0做差得到的误差作为角速度环的输入。
角速度环的输出输出作为我们期望小车转向的角速度。

简单对差速小车建模，以顺时针方向的角速度为正参考方向，推导数学公式可得

$v_{Lref}=v_m+\frac{\omega L}{2}$
$v_{Rref}=v_m-\frac{\omega L}{2}$

其中$v_{Lref}$是左轮的参考速度，$v_{Rref}$是右轮的参考速度，$v_m$是小车整体的运行速度，或假想小车中心有一个轮子的运行速度，$L$是车模后轮的轮距，我测量的D车模的后轮距为0.17m，$\omega$为小车的角速度，即角速度环的输出。

得到左右两轮子的参考速度以后，经过速度环控制后得到左右电机的占空比并幅值即可。

其中角速度环用PD控制，P环节用于放大误差送入矫正环节（在这里的三轮车矫正环节即控制两个轮子的速度以矫正误差），D环节用于抑制车体的震荡；速度环用PI控制，P放大误差进行速度调节，I用于消除静差，尤其是当我们上负压以后，I尤为重要。

由于三轮车的运动轨迹完全取决于两个轮子速度之间的关系，两个轮子的速度环控制必须做到快速而精准，需花时间调节速度环的参数。

电机PID参数整定

环岛处理

1. 检测到圆环（预圆环）（某个电感大于某个阈值）
2. 到达最佳入环位置（强制入环，直到陀螺仪大于某个，开始园内循迹）
3. 圆内循迹
4. 出环。

障碍处理

1. 识别障碍**(TOF测距小于某个限制)**
2. 避开障碍（左右轮固定差速，直到陀螺仪积分大于某个角度）
3. 回到赛道（左右轮固定差速，直到陀螺仪为小于某个角度）
4. 姿态调整（让小车的姿态基本与赛道平齐，直到陀螺仪角度在某个范围内或误差在某个范围内）

关键代码部分

差比和以及当前速度计算

三电感电磁循迹小车

角速度环

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;}
}