我是星云科技团队成员之一,一直负责火箭飞控的开发(团队官网:www.nebula001.top)。我们的第一代矢量火箭飞控是基于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效率是真的高