我是星云科技团队成员之一，一直负责火箭飞控的开发。我们的第一代矢量火箭飞控是基于Arduino uno和MPU6050的。那时候的算法极其简单，将6050和Arduino通信以后利用map映射控制舵机转过特定的角度，而且这个程序中没有开伞控制，部分代码如下：

C++
开启自动换行复制
  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控制，但是也由于未知原因，会在增量的过程中导致舵机锁死，猜测是增加速度过快导致出现类似“越界”的错误。

C++
开启自动换行复制
    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);
   }

在学习和使用PID控制的过程中，我也简单总结了一些东西，当时也形成了一篇文章，现在也分享出来供大家参考：

为了便于讨论，此时把火箭直接简化成一个围绕原点转动的向量，火箭用黑色向量表示，目标姿态位置用蓝色向量表示

更直接的简化所有模型，我们的目标可以叙述为：如图，现在假设在某直线x=1处有一个质点，现在需要在所有特定时刻给定质点一个加速度（力），使得小球回到原点，并且尽量在原点范围内保持静止，或者在精确范围内振荡。

这与简谐运动有些类似，在简谐运动中，质点的加速度与位移总是成正比，小球会被拉回原点，但因为惯性，小球会继续运动，以至于往复运动，无法在原点稳定。

现在让小球加速度与位移成正比(a=px，p<0，因为沿着位移的反方向)，如图为用Python模拟小球的运动过程，位移时间图象如下：

上面的模拟并不是我们的目的，我们需要的是质点图象在一段时间后平稳的接近x轴或者在x轴的附近振荡。由于希望在原点附近速度接近0，因此让加速度a和速度v的比例也有关系(a=px+dv，d<0，因为希望速度减小，因此给反向的加速度)，再次用Python模拟如下图：

粗略地可以从图中估计出质点在1000*0.01秒时几乎稳定在原点。事实上，如上的模拟过程可以用微分方程求解出，微分方程为：

初始速度v=0，初始位置x=1，代入微分方程，解得的微分方程为：

在Geogebra中画出这个函数的图象，如下图左，模拟如下图右，

比较两图，进行比例缩放后，两条图象吻合。由此，想到直接将位移前的常数和速度前的常数改成变量，也就是

类似于火箭的目的不是与极轴垂直，现在希望小球在并非𝑥=0处稳定，比如说在𝑥=1/2处稳定，此时初始条件发生了变化，依然使用Python进行模拟，绘制的曲线如下：

同样的，图象在大约600 * 0.01s之后在x=0.5处稳定，达到了预期目的。对此依然进行一个微分方程的求解：

现在使情况初始位置改变，即目标位置𝑥𝑟是任意的，而当前位置𝑥≠𝑥𝑟（事实上应该是𝑥不稳定在某个领域，因为在一定的误差内并不影响整体的控制效果），由此微分方程变成了：

由于文章比较长，我截取部分放在这里。总之就是通过一系列的计算导出了PID控制的表达式，最后回归到火箭的控制上，就体现在舵机牵拉矢量喷口摆动角度的变化。

在制作的过程中，将陀螺仪水平放置的时候，舵机角度还是会发生抖动，后来对6050输出的数据进行简单分析发现，6050输出的数据也是在不断发生抖动的，由此我推测可能是6050的数据导致舵机不断抖动，因此开始对6050的输出数据进行处理，这里我选择了卡尔曼滤波算法，算法部分如下：

C++
开启自动换行复制
    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的控制：

C++
开启自动换行复制
eng1_a = Kp * (-angle_x) + Kd * (-Gyro_x);

同时，我也学会了对飞行状态的感知和切换，利用switch case对飞行进行“分段处理”，分为：上电自检和初始化、等待发射、发射、发动机工作、下落开伞。在“发动机工作”段内，我使用了两个开伞条件，第一个是“Z轴加速度反向”，第二个是"XY融合偏转超过某一阈值"。代码块如下：

C++
开启自动换行复制
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_attitude(angle_x, angle_y) == 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效率是真的高

最后简单预告一下，我们的最新一代矢量控制火箭将于寒假期间完成制作