请问一下有发射视频吗,想看看
2021年的时候,我刚刚进入高三,也刚刚开始接触火箭圈子,从那时起便想自己制作一枚搭载控制系统的火箭,在深思熟虑中,我们选择了一种叫做“矢量”的控制方法。本文中的代码和结构文档均已开源,供各位爱好者交流互鉴
第一阶段:
最初我们刚刚进入高三,利用课余时间进行绘图、设计,由于彼时的我们不清楚SW、CAD等相对专业的建模软件,故采用了手工绘图的方法。最早的图纸就像下面这样,非常的业余:
由于本人主要负责电控部分,所以我分享一下最开始的全部代码(代码已在Github开源,大家需要自取)。最初的控制系统采用了Arduino UNO和MPU6050姿态传感器,只有一个最简单的map映射,软件中没有控制开伞部分的程序,完整代码如下:
#include <Adafruit_MPU6050.h> #include <Adafruit_Sensor.h> #include <Wire.h> #include <Servo.h> Adafruit_MPU6050 mpu; int x = 0; int y = 0; int z = 0; Servo servo1; Servo servo2; Servo servo3; Servo servo4; int value = 0; void setup(void) { Serial.begin(115200); // 尝试初始化 if (!mpu.begin()) { Serial.println("Failed to find MPU6050 chip"); while (1) { delay(10); } } Serial.println("MPU6050 Found!"); // 将加速度计范围设置为 +-8G mpu.setAccelerometerRange(MPU6050_RANGE_8_G); // 将陀螺仪范围设置为 +- 500 度/秒 mpu.setGyroRange(MPU6050_RANGE_500_DEG); // 将滤波器带宽设置为 21 Hz mpu.setFilterBandwidth(MPU6050_BAND_21_HZ); delay(100); servo1.attach(8); //servo2.attach(6); //servo3.attach(5); servo4.attach(6); servo1.write(90); servo2.write(0); servo3.write(0); servo4.write(90); } void loop() { /* 使用读数获取新的传感器事件 */ 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); } }
当然,这份代码只是在理论上实现了所谓的控制,实测的时候观察到6050传感器的抖动和舵机修正角度的过大,导致火箭呈现“S”形上升。修正效果并不理想,而且此时的火箭结构还仅限于纸筒,未形成完整的图纸。全部代码和开源图纸我也放在这里:
本文所有涉及到的火箭发动机燃料均为KNSB
第二阶段:
为了解决第一版的问题,于是在后面版本的程序迭代中,我引入了卡尔曼滤波算法以及简单的增量PID算法,代码我先放在下面:
#include <Adafruit_Sensor.h> #include <Wire.h> #include <I2Cdev.h> #include <Servo.h> #include <MPU6050.h> #include <Math.h> MPU6050 accelgyro; unsigned long now, lastTime = 0; float dt; //微分时间 int16_t ax, ay, az, gx, gy, gz; //加速度计陀螺仪原始数据 float aax=0, aay=0,aaz=0, agx=0, agy=0, agz=0; //角度变量 long axo = 0, ayo = 0, azo = 0; //加速度计偏移量 long gxo = 0, gyo = 0, gzo = 0; //陀螺仪偏移量 float pi = 3.1415926; float AcceRatio = 16384.0; //加速度计比例系数 float GyroRatio = 131.0; //陀螺仪比例系数 uint8_t n_sample = 8; //加速度计滤波算法采样个数 float aaxs[8] = {0}, aays[8] = {0}, aazs[8] = {0}; //x,y轴采样队列 long aax_sum, aay_sum,aaz_sum; //x,y轴采样和 float a_x[10]={0}, a_y[10]={0},a_z[10]={0} ,g_x[10]={0} ,g_y[10]={0},g_z[10]={0}; //加速度计协方差计算队列 float Px=1, Rx, Kx, Sx, Vx, Qx; //x轴卡尔曼变量 float Py=1, Ry, Ky, Sy, Vy, Qy; //y轴卡尔曼变量 float Pz=1, Rz, Kz, Sz, Vz, Qz; //z轴卡尔曼变量 float num1=1; float num2=1; float num3=1; float num4=1; Servo servo1; Servo servo2; Servo servo_pin_2; //开伞舵机 /** * 矢量pid部分 */ double kp = 0.3,ki = 0.15,kd = 0.1; /* Kp:比例增益,是调适参数; Ki:积分增益,也是调适参数; Kd:微分增益,也是调适参数; 取增量pid 采用恒定的采样周期 T,一旦确定Kp、Ki、Kd参数,只要使用前后三次测量的偏差值,就可以由增量式PID公式求出控制量。 现在设置的参数是我瞎蒙的,不一定准确 */ double sumerror_x,sumerror_y;//x轴和y轴总计误差 double lasterror_x,lasterror_y;//上一时刻误差值 double output_x,output_y;//累加修正结果 void setup() { servo1.attach(8); servo2.attach(6); servo_pin_2.attach(9); servo1.write(90); servo2.write(90); servo_pin_2.write( 180 ); Wire.begin(); Serial.begin(115200); accelgyro.initialize(); //初始化 unsigned short times = 200; //采样次数 for(int i=0;i<times;i++) { accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值 axo += ax; ayo += ay; azo += az; //采样和 gxo += gx; gyo += gy; gzo += gz; } axo /= times; ayo /= times; azo /= times; //计算加速度计偏移 gxo /= times; gyo /= times; gzo /= times; //计算陀螺仪偏移 } void loop() { unsigned long now = millis(); //当前时间(ms) dt = (now - lastTime) / 1000.0; //微分时间(s) lastTime = now; //上一次采样时间(ms) accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //读取六轴原始数值 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轴的夹角 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; // 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; /** * 矢量控制部分 */ 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) { //servo2.write(num3*agy); 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) { //servo2.write(num4*(90-agy)); 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); Serial.print("开伞agx>70"); while(1) { } } if(agy>70)//开伞角度 { //digitalWrite(8,1); servo_pin_2.write( 0 ); delay(2000); Serial.print("开伞agy>70"); while(1) { } } if(agy<-70)//开伞角度 { //digitalWrite(8,1); servo_pin_2.write( 0 ); delay(2000); Serial.print("开伞agy<-70"); while(1) { } } if(agx<-70)//开伞角度 { //digitalWrite(8,1); servo_pin_2.write( 0 ); delay(2000); Serial.print("开伞agx<-70"); while(1) { } } // Serial.print(agx);Serial.print(","); // Serial.print(agy);Serial.print(","); // Serial.print(agz);Serial.println(); }
而针对滤波算法,当时我找到了一段描述,大致是这样的:卡尔曼滤波是一种高效的自回归滤波器,它能在存在诸多不确定性情况的组合信息中估计动态系统的状态,是一种强大的、通用性极强的工具。卡尔曼滤波算法采用递推方式实现实时在线计算,在目标跟踪、模式识别、导航等领域有着广泛应用。对于EKF,在一个动态系统中,将目标状态方程表示为:
\begin{array}{c} X(t)=F(t)·X(t-1)+w(t) \end{array}
量测方程表示为:
\begin{array}{c} Z(t)=H(t)·X(t)+v(t) \end{array}
其中:X(t)为t时刻系统状态向量;F(t)为t时刻系统状态转移矩阵;Z(t)为t时刻系统量测向量;H(t)为t时刻系统量测转移矩阵;w(t)为过程噪声;v(t)为观测噪声。
对于火箭姿态解算问题,建立基于姿态四元数的状态向量:
\begin{array}{c} X(t)=[q_{0} q_{1} q_{2} q_{3} b_{gx} b_{gy} b_{gz} ]^{T} \end{array}
其中,\begin{array}{c} q=[q_{0} q_{1} q_{2} q_{3} ]^{T} \end{array}为表示姿态信息的四元数,\begin{array}{c} b=[b_{gx} b_{gy} b_{gz} ]^{T} \end{array}为陀螺仪角速度测量偏差。
这枚火箭的控制逻辑如图所示:
PID算法部分在我之前的帖子中也有提及,在此不多赘述。此时的火箭结构部分我们换用了桁架蒙皮结构,发动机也从最开始的PPR机换成了碳纤维机,增强了稳定性的同时也易于装配。
(1)牵拉部分
我们使用了舵机原装的拉杆,并自己利用金属3D打印制作了一段特制的长拉杆,装配时,将舵机原装的拉杆和特制的长拉杆利用小螺丝进行安装,此时在螺丝孔位置,二者可以相对转动。长的拉杆再与固定在矢量喷口上的金属环以相同的方式相连,从而达到了传动的目的。舵机连杆和拉杆结构如下图所示。
(2)开伞装置及原理
为保证降落伞能顺利打开,我们选择将火箭的头锥分为两半设计,一半是固定在头锥底座上的,另一半则可以分离,中间用连杆和舵机相连,头锥固定一侧的内部结构如下图所示。正常情况下,舵机会旋转到0度的地方,将可活动头锥固定住,当Seeeduino XIAO开发板解算出火箭的偏移角度超过了70度时,舵机会立刻旋转到180度的位置,将头锥中可活动的部分推出,此时放置在头锥内的降落伞会展开,完成开伞动作。
(3)发动机装配细节
发动机部分外壳采用碳纤维管制作,内部用PVC管做隔热层,前后两个法兰盘、喷口、固定矢量喷管用的支架以及矢量喷管均采用金属3D打印制作,堵头部分则利用酚醛浇筑制成,并利用O型圈做密封处理,防止漏气发生爆炸。发动机内部药柱采用中空设计,从而增加燃烧的速率。发动机前后用法兰盘和螺杆夹紧,做进一步的固定,提高发动机的稳定性。发动机结构如下图所示(未安装螺杆时)
(4)发动机舱结构设计
为确保发动机不会从舱内滑落,我们在火箭尾部设计了小卡扣,可以保证发动机在任何情况下都会固定在发动机舱内,发动机舱的上方隔板上有四个圆孔,便于特制的拉杆通过;相应的,法兰盘上也有孔位,以确保矢量系统的正常运行。发动机舱的外部有尾翼卡环,用来固定尾翼、优化火箭的气动外形。前后法兰盘结构、发动机舱上下部、尾翼卡环如下图所示。为确保发动机不会从舱内滑落,我们在火箭尾部设计了小卡扣,可以保证发动机在任何情况下都会固定在发动机舱内;发动机舱的上方隔板上有四个圆孔,便于特制的拉杆通过;相应的,法兰盘上也有孔位,以确保矢量系统的正常运行。发动机舱的外部有尾翼卡环,用来固定尾翼、优化火箭的气动外形。
前法兰盘结构
后法兰盘结构
发动机舱上部
发动机舱下部(内部用一圈卡扣卡住发动机,防止发动机滑落)
尾翼卡环(尾翼固定环)
(5)飞控支架设计
支架共分为四层,从下往上分别是:矢量舵机层、飞控电源层、飞控PCB层、惯导模块层。其中矢量舵机层用来存放控制矢量喷管的舵机;飞控电源层用来存放锂电池,锂电池同时给飞控和惯导模块供电,飞控PCB层用来存放PCB板,最上面的惯导模块层存放用来监控火箭姿态的IMU模块,该IMU模块加装无线串口后支持远程实时传输。飞控支架如图所示。
(6)矢量喷口部分设计
喷口分为固定部分和可摆动部分,固定部分内部采取拉瓦尔喷管结构,具有扩张段结构,能有效增大推力。可摆动部分在拉杆的牵拉下旋转来改变尾焰喷射方向,从而实现矢量控制。喷口部分结构如图所示。
固定喷口
喷口可摆动部分
这时候我们也使用了3D打印的方法,制作了完整的火箭模型,并完成了组装和测试,火箭总装如下图所示
这时候火箭的雏形已经具备,在细节方面还有待优化,比如开伞部分和尾翼部分。
第三阶段:
在这个阶段,我基本重写了整个项目的软件部分,进一步优化控制流程和控制思路,除了像上一篇帖子提到的“利用switch case对飞行进行“分段处理”,分为:上电自检和初始化、等待发射、发射、发动机工作、下落开伞”以外,还学会了调整舵臂安装误差、软限位和地面站数据传输等。主体部分代码如下:
void setup() { #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE Wire.begin(); Wire.setClock(400000); // 400kHz的I2C #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE Fastwire::setup(400, true); #endif Servo1.attach(8); Servo2.attach(6); Servo_KS.attach(9); Servo1.write(90); Servo2.write(90); Servo_KS.write(Servo_KS_ANGLE1); Serial.begin(115200); //while (!Serial); //当串口监视打开以后程序运行,方便调试,上箭之前记得注释掉 Serial.println(F("加载I2C设备...")); mpu.initialize(); pinMode(INTERRUPT_PIN, INPUT); Serial.println(F("测试设备连接中...")); Serial.println(mpu.testConnection() ? F("MPU6050连接成功") : F("MPU6050连接失败")); Serial.println(F("调用 DMP...")); devStatus = mpu.dmpInitialize(); mpu.setXGyroOffset(220); mpu.setYGyroOffset(76); mpu.setZGyroOffset(-85); mpu.setZAccelOffset(1788); if (devStatus == 0) { mpu.CalibrateAccel(6); mpu.CalibrateGyro(6); mpu.PrintActiveOffsets(); //打开DMP Serial.println(F("Enabling DMP...")); mpu.setDMPEnabled(true); Serial.print(F("Enabling interrupt detection (Arduino external interrupt ")); Serial.print(digitalPinToInterrupt(INTERRUPT_PIN)); Serial.println(F(")...")); attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); mpuIntStatus = mpu.getIntStatus(); Serial.println(F("DMP ready! Waiting for first interrupt...")); dmpReady = true; packetSize = mpu.dmpGetFIFOPacketSize(); } else { // ERROR! // 1 = 初始内存加载失败 // 2 = DMP 配置更新失败 Serial.print(F("DMP配置失败 (代码 ")); Serial.print(devStatus); Serial.println(F(")")); } } void loop() { // put your main code here, to run repeatedly: if (!dmpReady) return; // 从FIFO读取包 if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { #ifdef OUTPUT_READABLE_YAWPITCHROLL mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); mpu.dmpGetQuaternion(&q, fifoBuffer); mpu.dmpGetAccel(&aa, fifoBuffer); mpu.dmpGetGravity(&gravity, &q); mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); acc_z=aaReal.z; angle_x=ypr[2] * 180/M_PI; angle_y=ypr[1] * 180/M_PI; angle_z=ypr[0] * 180/M_PI; angle_x_ano = ypr[2] * 180/M_PI; angle_y_ano = ypr[1] * 180/M_PI; angle_z_ano = ypr[0] * 180/M_PI; Send_Data_Attitude(angle_x_ano, angle_y_ano, angle_z_ano); //发送欧拉角 #endif PID_control(); sample_valid = 0; sample_ready = 1; sample_cnt = sample_cnt + 1; }
这一代的航电部分,我们选择自行绘制PCB板,主控芯片选择了Atmega328p,USB转TTL芯片选择的是CH340N(之前帖子中提到的无法识别设备的问题已经解决,原因就是Type-c接口焊接不牢固,后面我用烙铁挨个引脚上锡加焊就解决了,感谢各位大佬的帮助),供电板上面的电压转换芯片选择的是74HC244D,实测可以带动三个舵机,四个舵机稍有勉强。
在这一代的结构部分,我们进一步完善了开伞的机械结构和尾翼的外形,新的开伞结构如下图所示:
舵机触发后,会将滑块和头锥的活动部分向上推出,头锥就会打开,完成开伞动作,如下图所示:
火箭的总装效果如下图所示。
然后我们进行了发射测试,并顺利完成开伞回收(开伞瞬间被发动机喷出的烟给挡住了,这里用一张正在降落阶段的照片代替。发射场地是一个废弃的飞机场,面积足够大,可能是由于长焦的缘故好像离民房很近)。
总结一下,在这个项目中,我学到了很多知识,也了解了很多未曾涉猎的领域。在实现梦想的道路上,感谢所有团队成员的辛勤付出,也感谢所有朋友对我们的制导帮助。希望在未来的日子里,我们继续前行在追梦的道路上。以上便是我和我的团队成员探索、追梦的全过程,有些地方用到的术语可能不准确,也希望各位批评指正,再次感谢各位的帮助。
[修改于 6个月25天前 - 2024/02/15 23:03:59]
自己写卡尔曼,精度恐怕问题不小吧
线性化处理后的模型会存在较大的截断误差,感觉以328p这种小芯片的算力跑不了UT变换,所以性价比更高的是用自带滤波的陀螺仪。(记得捷特智能的jy61p精度不错,价格也还好。)
发动机的侧向推力有测么,视频看不清。
做做尾翼性价比更高,矢量发动机大可不必。
自己写卡尔曼,精度恐怕问题不小吧线性化处理后的模型会存在较大的截断误差,感觉以328p这种小芯片的算...
其实自己搓卡尔曼的目标更多的事解决Z轴漂移,实测也有一定的优化,不过也会飘,只是飘的不那么离谱了而已,这个代码实测能跑。
我们后面想用jy901b,里面也自带滤波算法。
发动机侧向推力没有测试过,我们没有矢量试车台,下一步我们会转向鸭翼控制,非常感谢老哥的指导
发动机固定全压螺栓会不会过细。给你看一下我的,不过太粗也不好我这个M8 180mm好像碳纤维管57*55喷口堵头简单3D打印了一个厚度7mm单单这些应该达到了700g。为开源精神致敬!加油
发动机固定全压螺栓会不会过细。给你看一下我的,不过太粗也不好我这个M8 180mm好像碳纤维管57*...
你这个法兰盘和喷口是什么材质,酚醛的嘛,感觉酚醛不太结实呢
我们后面会改良发动机,不过目前的首要目标变成减重了,还是感谢你的支持
你这个法兰盘和喷口是什么材质,酚醛的嘛,感觉酚醛不太结实呢我们后面会改良发动机,不过目前的首要目标变...
不是酚醛树脂,图片上的是PLA(3D打印)无法使用的,之后是上45钢要考虑散热问题。耐高温的好像有钛合金吧但是极贵
想请教lz个问题,已有尾翼的气动制导为什么还需要矢量喷口呢,是否二者取其一即可?
时段 | 个数 |
---|---|
{{f.startingTime}}点 - {{f.endTime}}点 | {{f.fileCount}} |
200字以内,仅用于支线交流,主线讨论请采用回复功能。