本文介绍的是我自己的火箭飞控开发历程，有任何不妥之处请大家批评指正。
ps：我本人是学化工的，整个开发涉及到的技术栈和本人专业无关，部分用词可能不准确，也劳烦各位帮忙指出······
我作为星云科技团队成员之一，一直负责火箭飞控的开发。我们的第一代矢量火箭飞控是基于Arduino uno和MPU6050的。那时候的算法极其简单，将6050和Arduino通信以后利用map映射控制舵机转过特定的角度，而且这个程序中没有开伞控制，部分代码如下：
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
x = a.acceleration.x;
y = a.acceleration.y;
z = a.acceleration.z;
Serial.print(x);Serial.print(" ");
Serial.println(y);Serial.print(" ");
Serial.println(z);Serial.print("\n");
if (x < 10 && x > 0 && y < 4 && y > -4){
Serial.println("up");
value = map(x, 0, 10, 0, 180);
servo1.write(value);
Serial.print(value);
}
else if (x > -10 && x < 0 && y < 4 && y > -4){ //2
Serial.println("down");
value = map(x, -10, 0, 180, 0);
servo1.write(180-value);
Serial.print(value);
}
if (y < 10 && y > 0 && x < 4 && x > -4){ //3
Serial.println("Right");
value = map(y, 0, 10, 0, 180);
servo4.write(180-value);
Serial.print(value);
}
else if (y > -10 && y < 0 && x < 4 && x > -4){
Serial.println("left");
value = map(y, -10, 0, 180, 0);
servo4.write(value);
Serial.print(value);
}这份代码在星云科技官网已经开源，大家可以自行下载调试。但是这个代码也存在一些（很多）问题，包括但不限于舵机抖动、修正角度过大等，后面我更新了一下技术栈，引入PID控制算法，我们在第二阶段的迭代中将Arduino uno更换成了Seeeduino Xiao。Seeeduino Xiao的不足之处是可用引脚较少，不能支撑起太多的舵机输出。优点就是体积较小，采用的微控制器相比Atmega328P来说性能更好一些。
在这里我仅展示PID部分，这份代码中采用增量PID控制，但是也由于未知原因，会在增量的过程中导致舵机锁死，猜测是增加速度过快导致出现类似“越界”的错误。
float accx = ax / AcceRatio; //x轴加速度
float accy = ay / AcceRatio; //y轴加速度
float accz = az / AcceRatio; //z轴加速度
aax = atan(accy / accz) * (-180) / pi; //y轴对于z轴的夹角
aay = atan(accx / accz) * 180 / pi; //x轴对于z轴的夹角
aaz = atan(accz / accy) * 180 / pi; //z轴对于y轴的夹角
float gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度
float gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度
float gyroz = - (gz-gzo) / GyroRatio * dt; //z轴角速度
agx += gyrox; //x轴角速度积分
agy += gyroy; //y轴角速度积分
agz += gyroz;
if(agx > 0)
{
Serial.print("up ");
Serial.print(agx);
Serial.println("度");
sumerror_x += agx;
output_x = kp*agx + ki*sumerror_x + kd*(lasterror_x - agx);
lasterror_x = agx;
agx += output_x;
servo1.write(agx);
}
if(agx < 0)
{
Serial.print("down ");
Serial.print(agx);
Serial.println("度");
sumerror_x += agx;
output_x = kp*agx + ki*sumerror_x + kd*(lasterror_x - agx);
lasterror_x = agx;
agx += output_x;
servo1.write(agx);
//servo1.write(num2*(90-agx));
}
if(agy > 0)
{
Serial.print("left ");
Serial.print(agy);
Serial.println("度");
sumerror_y += agy;
output_y = kp*agy + ki*sumerror_y + kd*(lasterror_y - agy);
lasterror_y = agy;
agy += output_y;
servo2.write(agy);
}
if(agy < 0)
{
Serial.print("right ");
Serial.print(agy);
Serial.println("度");
sumerror_y += agy;
output_y = kp*agy + ki*sumerror_y + kd*(lasterror_y - agy);
lasterror_y = agy;
agy += output_y;
servo2.write(agy);
}
if(agx>70)//开伞角度，后面省略，免得文章太长
{
//digitalWrite(8,1);
servo_pin_2.write( 0 );
delay(2000);
}在制作的过程中，将陀螺仪水平放置的时候，舵机角度还是会发生抖动，后来对6050输出的数据进行简单分析发现，6050输出的数据也是在不断发生抖动的，由此我推测可能是6050的数据导致舵机不断抖动，因此开始对6050的输出数据进行处理，这里我选择了卡尔曼滤波算法，算法部分如下：
aax_sum = 0; // 对于加速度计原始数据的滑动加权滤波算法
aay_sum = 0;
aaz_sum = 0;
for(int i=1;i<n_sample;i++)
{
aaxs[i-1] = aaxs[i];
aax_sum += aaxs[i] * i;
aays[i-1] = aays[i];
aay_sum += aays[i] * i;
aazs[i-1] = aazs[i];
aaz_sum += aazs[i] * i;
}
aaxs[n_sample-1] = aax;
aax_sum += aax * n_sample;
aax = (aax_sum / (11*n_sample/2.0)) * 9 / 7.0; //角度调幅至0-90°
aays[n_sample-1] = aay; //此处用实验取得合适的系数
aay_sum += aay * n_sample; //本例系数为9/7
aay = (aay_sum / (11*n_sample/2.0)) * 9 / 7.0;
aazs[n_sample-1] = aaz;
aaz_sum += aaz * n_sample;
aaz = (aaz_sum / (11*n_sample/2.0)) * 9 / 7.0;
float gyrox = - (gx-gxo) / GyroRatio * dt; //x轴角速度
float gyroy = - (gy-gyo) / GyroRatio * dt; //y轴角速度
float gyroz = - (gz-gzo) / GyroRatio * dt; //z轴角速度
agx += gyrox; //x轴角速度积分
agy += gyroy; //y轴角速度积分
agz += gyroz;
/* 卡尔曼滤波算法部分 */
Sx = 0; Rx = 0;
Sy = 0; Ry = 0;
Sz = 0; Rz = 0;
for(int i=1;i<10;i++)
{ //测量值平均值运算
a_x[i-1] = a_x[i]; //即加速度平均值
Sx += a_x[i];
a_y[i-1] = a_y[i];
Sy += a_y[i];
a_z[i-1] = a_z[i];
Sz += a_z[i];
}
a_x[9] = aax;
Sx += aax;
Sx /= 10; //x轴加速度平均值
a_y[9] = aay;
Sy += aay;
Sy /= 10; //y轴加速度平均值
a_z[9] = aaz;
Sz += aaz;
Sz /= 10;
for(int i=0;i<10;i++)
{
Rx += sq(a_x[i] - Sx);
Ry += sq(a_y[i] - Sy);
Rz += sq(a_z[i] - Sz);
}
Rx = Rx / 9; //得到方差
Ry = Ry / 9;
Rz = Rz / 9;
Px = Px + 0.0025;
Kx = Px / (Px + Rx); //计算卡尔曼增益
agx = agx + Kx * (aax - agx); //陀螺仪角度与加速度计速度叠加
Px = (1 - Kx) * Px; //更新p值
Py = Py + 0.0025;
Ky = Py / (Py + Ry);
agy = agy + Ky * (aay - agy);
Py = (1 - Ky) * Py;
Pz = Pz + 0.0025;
Kz = Pz / (Pz + Rz);
agz = agz + Kz * (aaz - agz);
Pz = (1 - Kz) * Pz;在这里我可以说，这些算法我学的很艰难，毕竟当时没有线性代数基础，所以代码中存在各种当时的我难以解决的问题，这个滤波算法并不是很完善，如果哪里存在错误也请大家多多批评指教。
在这一代的硬件上，我们抛弃了之前采用的“飞杜邦线”策略，我绘制了简单的PCB板，在这里大家也不难看出这个PCB上存在至少一处错误：6050采用了3.3V的供电（正常6050应该是5V供电）。其余地方应该就是信号线过细等，如果还有其他我没注意到的问题也请大家批评指正。
我们目前正在推进的最新一代矢量火箭抛弃了开发板和6050模块，我重新绘制了全新的PCB，这里主控芯片采用Atmega328P，姿态传感器从MPU6050换成维特智能的JY901B，JY901B内置了滤波算法和气压高度计，也省去了我重写卡尔曼滤波的过程。而且这次的飞控新增了硬件自检、飞行日志记录、mos管控制二级点火（为以后迭代做准备）、GPS定位和Zigbee双向通信的功能（为以后的航线规划做准备）。PCB如下图所示：
大家也不难看出这次的PCB上有两个排线座，这个排线座是用于和供电板连接的，为了避免由于电流过大导致主控板烧毁，我将主控板和供电板分开。主控板上的六针接口用于和GPS、Zigbee通信。供电板如下图所示：
在供电的PCB上，大家应该不难看到哪个巨大的蜂鸣器，蜂鸣器就是用来做上电自检的，自检通过后，利用mos管导通控制蜂鸣器发出滴滴声。
这次绘制PCB的过程中，我认识到了模数分地的重要性，简单概括就是：模拟电源参考模拟地，数字电源参考数字地，非必要不采用隔层参考，信号线也是如此，不能跨区域（模数）走线，比如数字信号下方在投影上不能有模拟地。布局时将用于模拟信号的器件与数字信号的器件分开，然后从ad芯片中间一刀切!模拟信号铺模拟地，模拟地/模拟电源与数字电源通过电感/磁珠单点连接。
同时我也总结出了其他的一些问题，也形成了文档，等后续我将文档完善后会上传到论坛。
这一次的控制算法中，我抛弃了积分控制，采用PD控制的方法，对三轴欧拉角分别进行控制，下面代码是对pitch的控制：
eng1a = Kp * (-anglex) + Kd * (-Gyrox);同时，我也学会了对飞行状态的感知和切换，利用switch case对飞行进行“分段处理”，分为：上电自检和初始化、等待发射、发射、发动机工作、下落开伞。在“发动机工作”段内，我使用了两个开伞条件，第一个是“Z轴加速度反向”，第二个是”XY融合偏转超过某一阈值”。代码块如下：
case S_1ST_ENGINE_WORK: //第一级发动机工作, 为二级留冗余
{
if (acc_z==LAUNCH_ACC_Z)//开伞条件1:加速度反向
{
delay(3500);//惯性飞行时间
state_change(S_1ST_FALL, RECORD_LOG);
}
else if (judige_1st_angle(anglex, angley) == 2)//开伞条件2:判断XY轴合偏转角度
{
state_change(S_1ST_FALL, RECORD_LOG);
}
break;
}如果大家有什么更好的火箭开伞条件，也请多多指教，开伞这个地方还是越保险越好。
同时，为了更直观看到火箭的姿态，我采用了匿名飞控地面站，利用匿名飞控的协议实现姿态、高度等的检测。
在制作的过程中，我也踩了很多坑，在这里我还是想说一下，避免更多刚刚接触火箭飞控的朋友踩坑：
1、千万不要用开发板的5V或者3.3V给其他电子元件供电，我上面的Seeeduino Xiao那块就是个反例，如果其他电子元件功率比较大，可能会导致开发板重启，后果将是不可估量的。
2、舵机和点火头的耗电量比较大，建议把舵机、电火头都单独拉出一路供电，利用5V稳压芯片的输出给舵机供电，这样也能避免殃及其他元件。
3、一定不要用太高电压的电池供电，比如5V的接口接上7V多的电池，真的有概率会冒烟。
4、JY901B不要采用邮票贴的焊接方法，多次风枪加热会损坏内置的气压高度计。
5、丝印一定要充分，连接器的接口定义、板子名字、日期、版本号以及各种补充说明等等，性质就和代码的注释一样！！！如果焊接和画板子的不是同一个人的话一定要注意！
6、板子的边缘不能放置器件，板子边缘的器件在使用中容易被碰掉，特别是晶振，更不能放板子边缘，除了上述的原因，还会造成EMI问题，而且晶振离芯片尽量近，且晶振下尽量不走线，铺地网络铜皮。多处使用的时钟使用树形时钟树方式布线。
7、PCB布局时，依照模块化布局的方式，优先把模块内部的布局最小化和最优化（过孔、连线、铜皮都弄好）且所有电源和地信号都尽量打孔扇出，这样在后期走线时会有意识的避开这些孔，PCB上的信号走线尽量不换层，也就是说尽量减少过孔。
8、电源和地的管脚要就近过孔，过孔和管脚之间的引线越短越好，因为它们会导致电感的增加。同时电源和地的引线要尽可能粗，以减少阻抗。
9、0402封装基本就是手焊的极限了（对我来说是这样），强烈建议用更大的元器件，降低焊接的难度。
10、3.3V一般是主电源，直接铺电源层，通过过孔很容易布通全局电源网络。
5V一般可能是电源输入，只需要在一小块区域内铺铜。且尽量粗(能多粗就多粗，越粗越好！！）
11、推荐用Vscode做开发，Code配合Copilot效率是真的高
最后简单预告一下，我们的最新一代矢量控制火箭将于寒假期间完成制作
Related Posts
星云科技“台风哨兵”系统正式发布
今天，我们实现了一个来自2021年的梦想——星云科技矢量火箭制作流程及开源计划
