翼伞自主归航控制器及半实物仿真系统设计与实现.docx

翼伞自主归航控制器及半实物仿真系统设计与实现
随着科技的不断发展，无人机技术得到了广泛应用，其中翼伞自主归航控制器也是相当重要的一种。本篇论文旨在探讨翼伞自主归航控制器及半实物仿真系统的设计及实现。
一、翼伞自主归航控制器的设计
首先，为了实现翼伞的自主归航，需要对其进行控制器的设计。翼伞自主归航控制器的任务是在飞行过程中对翼伞的速度、姿态、高度及方向进行控制，从而使翼伞能够完成预定的任务。我们采用PID控制算法来控制翼伞的运动。PID控制器包括比例控制、积分控制和微分控制三部分，所以对翼伞的航向控制、俯仰控制和滚转控制进行PID控制设计。
翼伞的航向控制设计如下：设期望航向为ac，翼伞当前航向为h，定义姿态变化率为c；则此时，可以得出如下公式：
u1=kp1*（h-ac）+ki1*∫（h-ac）dt+kd1*（dc/dt）
其中，kp1、ki1和kd1为PID控制算法中的比例控制、积分控制、微分控制变量。根据翼伞系统的设计参数，kp1=10，ki1=0.5，kd1=1.5。代码实现如下：
floatget_err(floata,floatb){//计算误差
returnfmod((a-b)+360,360);
}
void_flight_control(){
floaterr_angle=get_err(g_data.target_yaw,g_data.imu.yaw);
g_data.voltage=(g_data.ctrl_mode==CTRL_MANUAL?g_data.voltage:voltage_filter(g_data.voltage));
floatkp=5;
floatkd=3;
floatKi=0.0;//idontuseit
floatdt=(g_data.last_ctrl_time>0?((float)(g_data.loop_usec-g_data.last_ctrl_time)/1e6f):0);
floaterr_speed=g_data.target_speed-g_data.speed;
floatk1=g_data.pids[PID_YAW].kp;
floatk2=g_data.pids[PID_YAW].ki*Ki;
floatk3=g_data.pids[PID_YAW].kd*kd/dt;
floatoutput_yaw=k1*err_angle+k2*g_data.yaw_error_i+k3*(err_angle-g_data.yaw_error_d/dt);
g_data.yaw_error_i=g_data.yaw_error_i+err_angle*dt;
g_data.yaw_error_d=err_angle;
g_data.roll_ctrl_out=constrain_int16(output_roll,-320,320);
g_data.pitch_ctrl_out=constrain_int16(output_pitch,-320,320);
}
翼伞的俯仰控制和滚转控制同理，代码实现如下：
floatk1=g_data.pids[PID_ROLL].kp;
floatk2=g_data.pids[PID_ROLL].ki*Ki;
floatk3=g_data.pids[PID_ROLL].kd*kd/dt;
floatoutput_roll=k1*err_roll+k2*g_data.roll_error_i+k3*(err_roll-g_data.roll_error_d/dt);
g_data.roll_error_i=g_data.roll_error_i+err_roll*dt;
g_data.roll_error_d=err_roll;
floatk4=g_data.pids[PID_PITCH].kp;
floatk5=g_data.pids[PID_PITCH].ki*Ki;
floatk6=g_data.pids[PID_PITCH].kd*kd/dt;
floatoutput_pitch=k4*err_pitch+k5*g_data.pitch_error_i+k6*(err_pitch-g_data.pitch_error_d/dt);
g_data.pitch_error_i=g_data.pitch_error_i+err_pitch*dt;
g_data.pitch_error_d=err_pitch;
二、半实物仿真系统的实现
为了验证翼伞自主归航控制器的有效性，在实际情况之前，我们需要设计一个半实物仿真系统，来验证控制器的有效性和稳定性。
半实物仿真系统需要包括仿真环境、传感器和动力系统以及实时数据处理等模块。仿真环境通过U