Создание собственной системы стабилизации
ну вопервых, напряжение на аналоговых выходах - индикатор измеряемого параметра который можно померять и перевести в понятные для математики цифирки, а вот дальше считать, считать и считать
гироскопы:
gyronoXadc = analogRead(gX); //считали данные с выхода гироскопа
gyroXadc = (gyronoXadc * FI) + (gyroXadc * (1.0 - FI)); // ну тут слегка фильтронули
gyroXrate = (gyroXadc-gyroZeroX)*2.4; //2.4 это =(Aref/1023)/0.67 0.67-чувствительность гироскопа mV/deg/sec собственно угловая скорость= (показания - нулевыепоказания)*2.4
gyroXangle=gyroXangle+gyroXrate*dtime/1000; // собственно интегрирование угол = угол предыдущий + угловая скорость * делта t (время между двумя цыклами расчёта)
акселерометр:
R = sqrt(pow(accXval,2)+pow(accYval,2)+pow(accZval,2));//расчёт длинны вектора)
accXangle = acos(accXval/R)*RAD_TO_DEG-90; // расчёт угла между вектором и осью X акселерометра arccos(проекция вектора ускорения свободного падения/ длинна вектора)* перевод из радиан в градусы
accYangle = acos(accYval/R)*RAD_TO_DEG-90;
о как раз в программном лажу обнаружил пойду переделывать, а то блин если оба угла с акселерометра равны нулю то все расчёты летят к чертям …
Слабо гироскопы местами перепутать😁
так а как у Вас реализована защита от наличия линейных ускорений?Они же будут “портить” угол.
Основные расчёты берутся именно с гироскопов, а для коррекции показания с акселерометра именно через альфа-бета фильтр,
compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle));
compAngleY = (0.98*(compAngleY+(gyroYrate*dtime/1000)))+(0.02*(accYangle));
его ещё комплиментарным называют,
если же имеются именно линейные ускорения это даже хорошо допустим аппарат смещается по какой-то оси без крена и тангажа, в любом случае на данное смещение нужно противодействие в данном случае опять же изменение угла которого на самом деле то и нет, тоесть для противостояния порыву ветра аппарат же надо наклонить в противоположном направлении, как-то так, всё равно бьюсь над съёмом показаний ненравятся они мне (именно сами показания)…
надо наверно гироскопы запитать всё-таки 5ю вольтами и переделать общение по i2c с акселерометром
о а нафига я два раза угод с гир расчитываю😵
попробова считать углы с акселя через atan2
accXangle = atan2(accZval,accXval)*RAD_TO_DEG+135;
accYangle = atan2(accZval,accYval)*RAD_TO_DEG;
почему-то так только получается
похоже аксель я доканал - глючит
чёт с переменными нето знак я всётаки гдето теряю
Ну вот последнее на чём остановился, по крайней мере углы правдоподобные
accXangle = -atan2(-accXval,-accZval)*RAD_TO_DEG;
accYangle = -atan2(-accYval,-accZval)*RAD_TO_DEG;
теперь буду каким-то образом прошивку добивать (управление, шымы, настройки и т.п.)…
а теперь представьте, что коптер резко начинает изменять высоту.Что будет с углом от акселя?
перевернётся, я уже модуль ввожу для Z, остаётся только -90 +90 сейчас воюю с ним
У меня оказывается и стол шатается и переворачивается даже😵
Сейчас решаю проблему
либо коэффициент силы трения лыж с асфальтьм слишком высокий, либо коэффициент моего интеллекта слишком низкий😁
покрайней мере уже в чужом коде разбиратся начал…
Мне кажется разумным следующий аглоритм.
1.Проверяем что сумма ускорений по всем осям=g.
2.Если так, то обнуляем ошибку интегрирования от гироскопов.
Вопрос лишь в вероятности ситуации, когда сумма ускорений будет g , но коптер будет ускоряться…надо посчитать…
Вопрос лишь в вероятности ситуации, когда сумма ускорений будет g , но коптер будет ускоряться…надо посчитать…
Падать боком както так, или крутится волчком и снижатся…
я вот чё немогу понять:
i2c_ADXL345_getRawADC();
accADC[PITCH] = ((rawADC_ADXL345[1]<<8) | rawADC_ADXL345[0]);
accADC[ROLL] = - ((rawADC_ADXL345[3]<<8) | rawADC_ADXL345[2]);
accADC[YAW] = - ((rawADC_ADXL345[5]<<8) | rawADC_ADXL345[4]);
это из вия все переменные rawADC unsigned как там чё получается
а углы както так
Axz = atan2(RxEst,RzEst) + a;
Ayz = atan2(RyEst,RzEst) + b;
где a и b это получается угловые скорости (если я правильно понял) с гир…
интересно следующее - я могу отключить в акселе g как факт тогда он меряет ускорения по осям без учёта g вот если бы наоборот😁
Подскажите как получается горизонт (вертикаль), что то не могу понять.
а вот с аркосинусами и вектором беда - в какойто момент времени просто отказывается считать углы уже через альфа бета фильтр, и непойму почему, вектор отказывается считать хотя расчёты ускорений идут…
вертикаль
всмысле по курсу? пока никак нечем отследить кроме гироскопа - только если по угловой скорости пока как в КУКе, а так магнитометр нужен
а если хе крен и тангаж то atan2 возврашает значение угла между горизонтом и осью по которой считаеи X или Y
Не раньше было как: выставка горизонта на земле т.е. ноль(сейчас инициализация), потом от этого ноля и плясали. Вопрос, если случайным образом подбросить объект с гироскопами и акселерометрами (не делая перед этим инициализацию), мы будем знать где низ?
не так колибровка то зачем, я ж с ней столько времени угрохал на третьей странице как раз на земле нулевые показания от них и пляшу
А за горизонт гироскопы отвечают? То есть мы запоминаем их значение когда объект на земле выставлен в горизонт?
да
А можно качественно сравнить точность акселерометров и точность гироскопов, может вместо гироскопов использовать шесть акселерометров? Это позволит избавиться от дрейфа гироскопа, но будет накапливаться ошибка интегрирования.
интегрируем как раз угловую скорость от гир дабы получить угол, а с акселерометрами тоже не всё так просто, хотя есть проект квадрик с магнетометром трёхосевым и акселерометром
#include <Wire.h> //Library for I2C implementation using analog pins 4 (SDA) and 5
#include <MegaServo.h>
#define NBR_SERVOS 4 // the maximum number of servos (up to 12 on Arduino Diecimil
#define FIRST_SERVO_PIN 6 // define motor pins on the Arduino board
#define SECOND_SERVO_PIN 9
#define THIRD_SERVO_PIN 10
#define FOURTH_SERVO_PIN 11
MegaServo Servos[NBR_SERVOS] ; // max servos
//Accelerometer variables
float ax,ay,az; //Acceleration values in m/s^2 for all three axis
const float as=0.00488; //Arduino analog resolution = 5Volts/1024 (10bits)
const float zg=1.63; //Sensor zero g bias = Supply Voltage/2 =3.3/2 (V)
const float sR=0.3; //Sensor resolution (V/g)
//Compass variables
int compassAddress = 0x42 >*> 1; // compass I2C slave adress: 0x42
// because the wire.h library only uses the 7 bits most significant bits
// a shift necessary is to get the most significant bits.
int psi = 0; // Compass heading = yaw (direct readings from the compass in degrees)
float psirad; // Compass heading = yaw in radians
//Euler angles Roll and pitch
float phi; // (rad)
float theta; // (rad)
//Kalman filter data
const float A[6][6] = {
{1.0000,0.0000,0.0000,-0.5985,-0.0000,0.0000},
{0.0000,1.0000,-0.0000,-0.0000,-0.5985,0.0000},
{-0.0000,-0.0000,1.0000,-0.0000,-0.0000,-0.0479},
{0.0500,0.0000,0.0000,0.3384,-0.0000,-0.0000},
{0.0000,0.0500,-0.0000,-0.0000,0.3384,0.0000},
{-0.0000,0.0000,0.0500,0.0000,0.0000,0.9147}
};//(Kalman state-space matrix Ak)
const float B[6][10] = {
{-0.0000,-0.0188,-0.0000,0.0188,0.0000,0.0302,-0.0000,0.0031,-0.0000,-0.0000},
{0.0194,0.0006,-0.0194,-0.0006,-0.0302,-0.0000,-0.0000,-0.0000,0.0031,-0.0000},
{0.0009,-0.0009,0.0009,-0.0009,-0.0000,0.0000,0.0000,0.0000,0.0000,0.0479},
{-0.0000,0.0002,-0.0000,-0.0002,0.0000,0.0334,-0.0000,0.0034,-0.0000,0.0000},
{0.0005,0.0006,-0.0005,-0.0006,-0.0334,-0.0000,-0.0000,-0.0000,0.0034,-0.0000},
{0.0000,-0.0000,0.0000,-0.0000,0.0000,0.0000,0.0000,0.0000,-0.0000,0.0853}
}; //(Kalman state-space matrix Bk)
float x[6] = {0,0,0,0,0,0}; //state vector: Roll speed, Pitch Speed, Yaw speed, Roll angle, Pitch angle, Yaw angle
float u[10] = {0,0,0,0,0,0,0,0,0,0}; //input vector: w1 (motor 1 speed),w2,w3,w4,ax,ay,az,phi,theta,yaw
float At[6] = {0,0,0,0,0,0}; //temp variable
float Bt[6] = {0,0,0,-0.01,-0.04,0}; //temp variable
//LQR gain matrix
const float K[4][6] = {
{0.0000,7.1030,20.4163,0.0000,18.2839,14.4659},
{-7.1030,0.0000,-20.4163,-18.2839,0.0000,-14.4659},
{0.0000,-7.1030,20.4163,0.0000,-18.2839,14.4659},
{7.1030,0.0000,-20.4163,18.2839,0.0000,-14.4659}
};
float w1,w2,w3,w4; //Motor inputs from LQR controller (rad.s^-1)
float w0; //Throttle to the motors (from joystick)
//Motor inputs for PWM signal
float wc1=0;
float wc2=0;
float wc3=0;
float wc4=0;
//PWM signals
int p1 = 200;
int p2 = 200;
int p3 = 200;
int p4 = 200;
//Reference
float yr[6]={0,0,0,0,0,0};
void setup()
{
Wire.begin();
Serial.begin(9600); // Initialize serial communications with setup
Servos[0].attach( FIRST_SERVO_PIN, 800, 2200); //Motor 1 - North - Clockwise rotation
Servos[1].attach( SECOND_SERVO_PIN, 800, 2200); //Motor 2 - East - Counterclockwise rotation
Servos[2].attach( THIRD_SERVO_PIN, 800, 2200); //Motor 3 - South - Clockwise rotation
Servos[3].attach( FOURTH_SERVO_PIN, 800, 2200); // Motor 4 - West - Counterclockwise rotation
//Put the motors in full stop
Servos[0].write(0);
Servos[1].write(0);
Servos[2].write(0);
Servos[3].write(0);
}
void loop()
{
// get the most recent acceleration values for all three axis
ax = ((analogRead(0)*as-zg)/sR)*9.81; //acceleration x-axis (m.s^-2)
ay = ((analogRead(1)*as-zg)/sR)*9.81; //acceleration y-axis (m.s^-2)
az = ((analogRead(2)*as-zg)/sR)*9.81; //acceleration z-axis (m.s^-2)
ax=-ax; //This correction allows the accelerometer to provide positive acceleration in the x-axis direction
// Compass task 1: connect to the HMC6352 sensor
Wire.beginTransmission(compassAddress);
Wire.send(’A’); // The ascii character "A" tells the compass sensor to send data
Wire.endTransmission();
// Compass task 2: wait for data processing
delay(50); // Compass datasheet says we need to wait at least 6000 microsegundos (0.006s) for data processing in the sensor
//We can also take this opotunity to implement the sampling time (20Hz)
// Compass task 3: Request heading
Wire.requestFrom(compassAddress, 2); // request 2 bytes of data
// Compass task 4: Get heading data
if(2 <= Wire.available()) // if 2 bytes are available
{
//16bit numbers can be broken in two 8 bit chunks. The first is called high byte and the second low byte
psi = Wire.receive(); // get high byte
psi = psi <*< 8; // shift high byte
psi += Wire.receive(); // get low byte
psi /= 10; // comment this line if you wish to get the heading in decidegrees instead of degrees
}
psirad = deg2rad((float)psi); // Map the compass reading from 0 to 359 degrees to -pi radians to pi radians
phi=atan(ay/az); //Calculate roll angle from the accelerations provided by the accelerometer (rad)
theta=-atan(ax/az); //Calculate pitch angle from the accelerations provided by the accelerometer (rad)
//Perform Kalman filtering
At[0]=A[0][0]*x[0]+A[0][1]*x[1]+A[0][2]*x[2]+A[0][3]*x[3]+A[0][4]*x[4]+A[0][5]*x[5];
At[1]=A[1][0]*x[0]+A[1][1]*x[1]+A[1][2]*x[2]+A[1][3]*x[3]+A[1][4]*x[4]+A[1][5]*x[5];
At[2]=A[2][0]*x[0]+A[2][1]*x[1]+A[2][2]*x[2]+A[2][3]*x[3]+A[2][4]*x[4]+A[2][5]*x[5];
At[3]=A[3][0]*x[0]+A[3][1]*x[1]+A[3][2]*x[2]+A[3][3]*x[3]+A[3][4]*x[4]+A[3][5]*x[5];
At[4]=A[4][0]*x[0]+A[4][1]*x[1]+A[4][2]*x[2]+A[4][3]*x[3]+A[4][4]*x[4]+A[4][5]*x[5];
At[5]=A[5][0]*x[0]+A[5][1]*x[1]+A[5][2]*x[2]+A[5][3]*x[3]+A[5][4]*x[4]+A[5][5]*x[5];
//Get throttle from the joystick
if (Serial.available() > 0)
{
// read the incoming byte:
w0 = map((float)Serial.read(), 0, 100, 0, 500);
w0=constrain(w0,0,500); //Limit maximum velocity to 600 rad/s
Serial.flush();
}
else
{
w0 = 0;
}
//place known inputs and measurements in the input vector u
u[0]=wc1-w0;
u[1]=wc2-w0;
u[2]=wc3-w0;
u[3]=wc4-w0;
u[4]=ax-yr[0];
u[5]=ay-yr[1];
u[6]=az-yr[2];
u[7]=phi-yr[3];
u[8]=theta-yr[4];
u[9]=psirad-yr[5];
Bt[0]=B[0][0]*u[0]+B[0][1]*u[1]+B[0][2]*u[2]+B[0][3]*u[3]+B[0][4]*u[4];
Bt[0]=Bt[0]+B[0][5]*u[5]+B[0][6]*u[6]+B[0][7]*u[7]+B[0][8]*u[8]+B[0][9]*u[9];
Bt[1]=B[1][0]*u[0]+B[1][1]*u[1]+B[1][2]*u[2]+B[1][3]*u[3]+B[1][4]*u[4]
Bt[1]=Bt[1]+B[1][5]*u[5]+B[1][6]*u[6]+B[1][7]*u[7]+B[1][8]*u[8]+B[1][9]*u[9];
Bt[2]=B[2][0]*u[0]+B[2][1]*u[1]+B[2][2]*u[2]+B[2][3]*u[3]+B[2][4]*u[4]
Bt[2]=Bt[2]+B[2][5]*u[5]+B[2][6]*u[6]+B[2][7]*u[7]+B[2][8]*u[8]+B[2][9]*u[9];
Bt[3]=B[3][0]*u[0]+B[3][1]*u[1]+B[3][2]*u[2]+B[3][3]*u[3]+B[3][4]*u[4]
Bt[3]=Bt[3]+B[3][5]*u[5]+B[3][6]*u[6]+B[3][7]*u[7]+B[3][8]*u[8]+B[3][9]*u[9];
Bt[4]=B[4][0]*u[0]+B[4][1]*u[1]+B[4][2]*u[2]+B[4][3]*u[3]+B[4][4]*u[4]
Bt[4]=Bt[4]+B[4][5]*u[5]+B[4][6]*u[6]+B[4][7]*u[7]+B[4][8]*u[8]+B[4][9]*u[9];
Bt[5]=B[5][0]*u[0]+B[5][1]*u[1]+B[5][2]*u[2]+B[5][3]*u[3]+B[5][4]*u[4]
Bt[5]=Bt[5]+B[5][5]*u[5]+B[5][6]*u[6]+B[5][7]*u[7]+B[5][8]*u[8]+B[5][9]*u[9];
//Get estimated states x(k+1)=A.x(k)+B.u(k)
x[0]=At[0]+Bt[0];
x[1]=At[1]+Bt[1];
x[2]=At[2]+Bt[2];
x[3]=At[3]+Bt[3];
x[4]=At[4]+Bt[4];
x[5]=At[5]+Bt[5];
//Controlo LQR
w1=K[0][0]*x[0]+K[0][1]*x[1]+K[0][2]*x[2]+K[0][3]*x[3]+K[0][4]*x[4]+K[0][5]*x[5];
w2=K[1][0]*x[0]+K[1][1]*x[1]+K[1][2]*x[2]+K[1][3]*x[3]+K[1][4]*x[4]+K[1][5]*x[5];
w3=K[2][0]*x[0]+K[2][1]*x[1]+K[2][2]*x[2]+K[2][3]*x[3]+K[2][4]*x[4]+K[2][5]*x[5];
w4=K[3][0]*x[0]+K[3][1]*x[1]+K[3][2]*x[2]+K[3][3]*x[3]+K[3][4]*x[4]+K[3][5]*x[5];
//Adjust speeds with the throttle
wc1=w0-w1;
wc2=w0-w2;
wc3=w0-w3;
wc4=w0-w4;
//Calculate and send pulse width to the motors
p1=(int)((wc1/2.0276)+1252); // (speed of motor 1 (rad/s) / transfer function gain) + Dead-zone pulse width
p2=(int)((wc2/1.8693)+1262);
p3=(int)((wc3/2.0018)+1255);
p4=(int)((wc4/1.9986)+1255);
if(w0==0) //Forçar paragem
{
p1=400;
p2=400;
p3=400;
p4=400;
}
Servos[0].write(p1);
Servos[1].write(p2);
Servos[2].write(p3);
Servos[3].write(p4);
// output states
Serial.print(x[0]);
Serial.print(" ");
Serial.print(x[1]);
Serial.print(" ");
Serial.print(x[2]);
Serial.print(" ");
Serial.print(x[3]);
Serial.print(" ");
Serial.print(x[4]);
Serial.print(" ");
Serial.print(x[5]);
Serial.println("");
}
float deg2rad (float x) //function that receives an angle between 0 and 359 degrees and resturns the same angle between -pi
and pi radians
{
if(x>=0 && x<=180)
{
x=-x*3.14/180;
}
else
{
x = (360-x)*3.14/180;
}
return x;
}
непомню уже где нашол…
compAngleX = (0.98*(compAngleX+(gyroXrate*dtime/1000)))+(0.02*(accXangle)); здесь хочу какимто образом сделать коэффициент(бета) изменяющийся в зависимости от показаний акселерометра, а то при уменьшении угловой скорости угол тоже уменьшается, где-то до 70 гр нормально а если платку поставить вертикально тоеть 90 гр угол покажет где-то 60гр…
Ну вот, добрался всётаки до камеры, фотика нет так что качество не ахти