1.
JB变量
2.导引控制
q_old_y = atan((pos_Target[1] - y[8]) / (pos_Target[0] - y[7]));
q_old_z = atan((pos_Target[2] - y[9]) / (pos_Target[0] - y[7]));
if (flag4 == 1 && y[0] > T5)
{
q_new_y = atan((pos_Target[1] - y[8]) / (pos_Target[0] - y[7]));
q_new_z = atan((pos_Target[2] - y[9]) / (pos_Target[0] - y[7]));
q_dot_y = (q_new_y - q_old_y) / h;
q_dot_z = (q_new_z - q_old_z) / h;
q_old_y = q_new_y;
q_old_z = q_new_z;
}
if (flag4 == 1 && y[0]>T5)
{
dy[14] = y[15];
dy[15] = q_dot_y (4 / Km) ws * ws - 2 * ws * es * y[15] - ws * ws * y[14];
}
//侧向控制器 y[19] 偏航舵 y[17] y[18] 因为y,x不是套路出牌,所以是负的
if (flag4 == 1 && y[0]>T5)
{
dy[17] = y[18];
dy[18] = -q_dot_z *(10 / Km) * ws * ws - 2 * ws * es * y[18] - ws * ws * y[17];
}
if (y[0] >= T2) //开始控制滚转
{
y[20] = -(-0.1*dy[12] - y[12]); //delta_x 滚转舵增益为负!
}
dy[20] = 0;
具体开始分析:
Km N 纵向弹体增益,导引系数
Ks ws es 舵机增益 舵机频率 舵机阻尼