本帖最后由 岁月如歌 于 2023-12-13 13:47 编辑
软件方案设计运动部分总体设计软件框图如下图所示，利用简单的菜单对不同题目的不同功能进行选择，分别运行不同题目后，系统会回到原点准备进行下一个项目的测试。
2.1运动目标控制——坐标映射在本装置中需通过 PWM 驱动舵机转动，实现红色激光的目标控制与绿色激光的自动追踪。考虑到需完成上下、左右这两个维度的方向运动，因此采用两个舵机构成而自由度的云台，通过两个舵机的协调，可以将最上方的云台转到任意目标位置。由于云台的坐标为球面坐标，目标控制与追踪的平面为二维平面，如图 2.1 所示，因此需要完成球面角度、坐标转化。 球面角度坐标转化过程如下，取最上方舵机的机械中心作为坐标圆点建立空间坐标系，在进行目标控制和自动追踪过程中，激光的运动轨迹本质上是一个圆，而根据机械结构可得到，圆的半径为 H 以及云台与坐标轴的夹角a、b。 考虑到设备复位，故坐标圆点与屏幕中心位置在同一平面内，方便后续的建模分析。假设激光在屏幕上的坐标为 A，屏幕上距离激光的距离为 k。坐标转化求解原理示意如图 2.2 所示。
根据图 2.2 结合几何映射关系可得 A 点坐标，首先确定中间变量值 l，求解式如下式所示。
求得 l 的值为
![U77HW~JO09TW$IY]~HKLHNI.png](data/attachment/forum/202310/13/103258xh8uxlz2g3noyhul.png "U77HW~JO09TW$IY]~HKLHNI.png")
之后求解 A 点的(y,z)的坐标，公式如下所示
![7CS1CPL6LWBD~M_]TG{LCO7.png](data/attachment/forum/202310/13/103318eg70572chs2s0ak3.png "7CS1CPL6LWBD~M_]TG{LCO7.png")
可得屏幕上任意点的坐标为 A 如下式 2.3 所示，
2.2追踪性能分析与计算
增量式PID算法的相关分析与计算
增量式PID算法公式： u(k) = u(k-1) + Kp * (e(k) - e(k-1)) + Ki * e(k) + Kd * (e(k) - 2 *e(k-1) + e(k-2)) 其中，u(k)是控制器在时刻k的输出值； u(k-1)是控制器在上一时刻k-1的输出值； e(k)是当前时刻k的误差，即设定值与实际值之间的差异； e(k-1)是上一时刻k-1的误差；e(k-2)是上两个时刻k-2的误差； Kp是比例系数，用于调整比例控制的作用程度； Ki是积分系数，用于调整积分控制的作用程度； Kd是微分系数，用于调整微分控制的作用程度。 通过调整比例系数Kp、积分系数Ki和微分系数Kd的大小，可以对控制器的响应速度、稳定性和抗干扰能力进行调节，以达到期望的控制效果 #include"headfile.h"
Pid_Inc Right; Pid_Inc Left;
floatP_SET=0.11; //0.1 floatI_SET=0.07;//0.06 floatD_SET=0;//40 //100 //5 uint8 cam_flag =0; uint8cam_temp_flag = 0; floatblack_angle_temp[9] = {0}; float err[9] ={0};
Pid_Inc Right; Pid_Inc Left;
voidPid_Inc_Init() {
Right.P = P_SET; //p参数 Right.I = I_SET; //i参数 Right.D = D_SET; //d参数
Right.Err = 0.0; //偏差值 Right.Err_Prev = 0.0; //定义上上个偏差值 Right.Err_Last = 0.0; //定义上一个偏差值 Right.Set_Speed = 0; //设定的速度 Right.Actual_Speed = 0.0; //定义实际速度 Right.Out = 0.0; //电机输出 Right.increment = 0.0; //定义增量 Right.proportion = 0.0; //PID比例项 Right.integration = 0.0; //PID积分项 Right.differential = 0.0; //PID微分项
Left.P = P_SET; //p参数 Left.I = I_SET; //i参数 Left.D = D_SET; //d参数
Left.Err = 0.0; //偏差值 Left.Err_Prev = 0.0; //定义上上个偏差值 Left.Err_Last = 0.0; //定义上一个偏差值 Left.Set_Speed = 0; //设定的速度 Left.Actual_Speed = 0.0; //定义实际速度 Left.Out = 0.0; //电机输出 Left.increment = 0.0; //定义增量 Left.proportion = 0.0; //PID比例项 Left.integration = 0.0; //PID积分项 Left.differential = 0.0; //PID微分项
}
//增量式PID中被控制量一直都是偏差error，并不是直接控制速度，控制的目的是让偏差趋近于0。
void Pid_Deal(Pid_Inc*pid) { pid->Err = pid->Set_Speed -pid->Actual_Speed; //偏差 = 期望 - 实际
pid->proportion = pid->P *(pid->Err - pid->Err_Last); //比例
pid->integration = pid->I *pid->Err; //积分
pid->differential = pid->D *(pid->Err - 2*pid->Err_Last + pid->Err_Prev); //微分
pid->increment = pid->proportion +pid->integration + pid->differential; //增量 = 比例 + 积分 + 微分
pid->Out += pid->increment; //输出
pid->Err_Prev = pid->Err_Last; //上上次偏差赋值
pid->Err_Last = pid->Err; //上次偏差赋值 }
voidcamera_control() //摄像头控制黑线循迹 {
if(cam_flag == 0) //角度手动补偿 { black_angle_temp[1]=black_angle[7]+err[1]; black_angle_temp[2]=black_angle[8]+err[2];//2 black_angle_temp[3]=black_angle[5]+err[3];//-2 black_angle_temp[4]=black_angle[6]+err[4];//2 black_angle_temp[5]=black_angle[3]+err[5];//-2 black_angle_temp[6]=black_angle[4]+err[6];//-2 black_angle_temp[7]=black_angle[1]+err[7];//2 black_angle_temp[8]=black_angle[2]+err[8];//-2
| 
发表于 2023-10-12 09:41:04
发表于 2023-10-13 10:16:17
![58TIU))G])[MIK}[G%PGLHE.png](data/attachment/forum/202312/13/150626udyy6uewy6vjg60j.png "58TIU))G])[MIK}[G%PGLHE.png")
