[发明专利]一种沿索道飞行器装置控制系统

摘要

本发明公开了一种沿索道飞行器装置控制系统。本发明采用沿索道飞行器控制装置，可以模拟实现针对飞机迫降的各类情况，之后采集飞机的各类数据，对真实的飞机迫降提出有针对性的意见，接口灵活方便，易于控制；具有良好的可维护性和扩展性，较好的经济性，制造方便，成本低。

主权项

1.一种沿索道飞行器装置控制系统，其特征在于，所述沿索道飞行器装置控制系统包括：飞行数据采集模块、PID控制模块，并通过锂电池及电调为系统供电；所述飞行数据采集模块包括：U‑blox GPS，用于获取当前GPS数据，通过串口发送给主控芯片；L3G4200D陀螺仪，用于采集xyz三个方向的角加速度，通过I2C总线发送给主控芯片；ADXL345加速度计，用于采集xyz三个方向的加速度，通过I2C总线发送给主控芯片；HMC5883L地磁计，用于采集xyz三个方向的地磁信号，通过I2C总线发送给主控芯片；BMP085气压计，用于采集当前所处高度的气压，通过I2C总线发送给主控芯片，同时换算成高度值；主控芯片将接收到的三轴加速度、三轴地磁信号、三轴角加速度、气压和GPS数据打包后，通过无线串口发送到远程pc端的PID控制模块进行后续处理；所述PID控制模块包括：PID速度控制器及PID角度控制器；所述PID速度控制器：首先将速度设定为一个起始值r(t)，然后测得当前速度值y(t)与设定速度r(t)的误差值e(t),再通过计算得到下次周期输出到电机的速度，公式：其中：e(t)——误差值，u(t)——输出值,Kp——比例系数，Ti——积分时间常数，Td——微分时间常数，t——当前时间；经过一个比例环节、积分环节、微分环节后，获得当前速度控制量，既PWM信号的输出量，然后读取光电编码器传回的当前速度值，与设定速度比较后，再经过比例环节、积分环节、微分环节，获得下一轮的速度控制量，如此反复，以实现速度的动态调节；模拟飞机以1：15的比例来模拟真实的飞机降落过程；所述PID角度控制器：利用三轴加速度、三轴地磁信号、三轴角加速度测得的地磁信号、角加速度数据，通过卡尔曼滤波算法,建立飞行姿态模型；状态方程为：x_k+1＝Ax_k+Bu_k+w_k；输出方程：y_k＝Cx_k+z_k；姿态模型：a_k+1＝a_k+(t_k‑b_k)dt；其中A，B，C均为矩阵，A，B是系统参数，C＝(1 0)，k是时间系数，x_k称为系统状态，u_k是控制向量，y_k是所测量的输出，是受到噪声z_k干扰的系统状态x_k的函数；w_k和z_k表示噪声，其中，变量w_k称为进程噪声，z_k称为测量噪声，它们都是向量，因此包含多个元素；x_k中包含系统当前状态的所有资讯，但不能被直接测量，因此，要测量向量y_k；a_k是飞行姿态角，即俯仰角或横滚角，b_k是陀螺仪的误差，t_k是陀螺仪输出的角速度数据，dt是采样时间间隔；具体的实现步骤为：(1)读取当前L3G4200D陀螺仪数据u；(2)由L3G4200D陀螺仪数据更新系统的状态预测X_est＝AX_sta+Bu；X_sta为当前系统状态；(3)读取由ADXL345加速度传感器计算的转角数据y；(4)计算测量过程的更新Inn＝y‑CXest；(5)计算协方差s＝CPC+S_z；其中；Sz表示过程噪声；(6)计算卡尔曼增益K＝APC′s^‑1；其中K是卡尔曼滤波增益的值，s^‑1是s的逆；(7)更新X_sta＝X_est+K*Inn；(8)计算预测误差的协方差P＝APA‑K*CPA+S_w,其中,S_w为进程噪声协方差矩阵,即S_w＝E(ee^T)，e是单位矩阵，e^T是单位矩阵的转置矩阵；(9)在到达下一个采样时刻时，从步骤(1)开始重复本流程；从而计算出飞行装置的飞行姿态，即姿态角，与初始设定的姿态角进行对比，经过比例环节、积分环节、微分环节后获得下一次云台的三个PWM信号，从而控制云台三个方向的转动。