دوستان كمي تكلم كنيد :)
پروژه رو تحويل دادم و عملا ديگه نمي تونم روي الگوريتم تست بزنم، از چيزايي كه تو ذهنم مونده براتون مي گم.
توي اين 2-3 روزي كه باهاش درگير بودم چيزاي جديدي فهميدم كه به اين شرحه كه مي گم.
وقتي از 6Dof اش استفاده مي كنيم ، همه چيز خوبه . yaw هم محاسبه مي شه اما بعدا متوجه دريفتش مي شيم. اگر لرزش بهش وارد بشه ، yaw اش خيلي قاطي مي كنه و توي روبات اين لرزش وجود دارهه.
وقتي در هر كدوم از جهت هاي X , Y , Z سنسور رو با شتاب حركت بديم ، شاهد بالا پايين شدن مقادير Roll و Pitch هستيم كه نبايد اتفاق بيفته و اين مشكل رو شتاب سنج ايجاد مي كنه. سرعت يكنواخت شتابش 0 هست و شتابسنج خرابكاري نمي كنه.
وقتي از 9DOF اش استفاده مي كنيم ، چرخش yaw روي roll , pitch اثر جدي مي ذارهه و مثلا roll , pitch اي كه بايد 0 باشن الان يكيشون -1 و يكيشون 3 ! جالب اينه كه جبران هم نمي شن و در همونجا مي مونن!
اما سرعت پاسخگويي هر 2 ( هم 6dof , هم 9dof ) تا اينجاي كار خوبه.
yaw مشكلي ندارهه ، اما roll , pitch مشكل جدي دارن!
مي شه از شتاب سنج ميانگين گيري كرد ( فيلتر پايين گذر ) در اين صورت مشكلات ياد شدهه اصلاح مي شه ، اما اين باعث مي شه اطلاعات 3 محور شتاب سنج از orthogonal بودن خارج بشه. همينطور سرعت پاسخگويي رو كم مي كنه. هم سرعت اصلاح roll , pitch از ميزان انحراف ، و هم تعيين مقدار yaw .
خوب پس ميانگين گيري هم راه حل خوبي براي اين الگوريتم نيست.
من زمان كمي داشتم و مي خواستم روي پروژه تستش كنم و نتيجه رو ببينم، پردازنده اي هم كه استفاده مي كردم قدرت كافي رو داشت و نگران بهينه نبودن الگوريتم نبودم! فقط مي خواستم اصلاح بشه و نتيجه ي بهتري ببينم!
پس تابع 6dof و 9dof رو با هم استفاده كردم تا از نقاط قوت همديگه استفاده كنن.
6Dof در roll و pitch خوب بود و در yaw افتضاح.
9ِDof در yaw خوب بود و در roll , pitch افتضاح.
خوب بريم سراغ كد:
کد php:
MadgwickAHRSupdate(gyro[0]*DEG2RAD, gyro[1]*DEG2RAD ,gyro[2]*DEG2RAD , HADXL[0], HADXL[1], HADXL[2], X_MAG, Y_MAG, Z_MAG);
getRollPitchYaw(&roll_p,&pitch_p,&yaw);
MadgwickAHRSupdateIMU(gyro[0]*DEG2RAD,gyro[1]*DEG2RAD,gyro[2]*DEG2RAD,HADXL[0],HADXL[1],HADXL[2]);
getRollPitchYaw(&roll,&pitch,&yaw_p);
roll_p , pitch_p , yaw_p اطلاعاتي درش ذخيرهه مي شه كه صحيح نيستن و به كارمون نمي ياد.
roll , pitch , yaw حاويه اطلاعات صحيح هستن.
خوب هنوز مشكل حركت در راستاهاي x,y,z هست ، به دليل تاثير شتاب حركت و ذات شتاب سنج.
با انتخواب ضريب Beta زير 0.1 تا حدي مي زان خطا كم مي شه.
در تكان هاي ريز شديد هم عمليات زير موثر هست كه بايد در الگوريتم اعمالش كنيد.
تابع زير رو:
کد php:
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalize accelerometer measurement
recipNorm = InvSqrt_Opt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
HADXL_P[0] = ax;
HADXL_P[1] = ay;
HADXL_P[2] = az;
}
به اين تغيير بديد ، براي هر 2 تابع:
کد php:
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalize accelerometer measurement
recipNorm = InvSqrt_Opt(ax * ax + ay * ay + az * az);
acc_weight = 1-(2 * abst(1-recipNorm));
if((acc_weight >= 0) && (acc_weight <= 1)) /*{ acc_weight = 0; acc_div = 0; }*/
{
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
HADXL_P[0] = ax;
HADXL_P[1] = ay;
HADXL_P[2] = az;
}
اين قسمت هم كه در پست قبلي گفتم ، كه به اين شكل تغيير بديد.
کد php:
q0 += qDot1 * (interval/1000.0f);
q1 += qDot2 * (interval/1000.0f);
q2 += qDot3 * (interval/1000.0f);
q3 += qDot4 * (interval/1000.0f);
البته هنوز اشكالات زيادي بعد از اصلاحاتي كه گفتم دارهه، اما همين هم اميدوار كننده هست ... :)
در كل اين الگوريتم رو توصيه نمي كنم ، مخصوصا براي كواد و ... .
يه ويدئو هم براي پيادهه سازيه اصلاحات فوق مي ذارم. امروز صبح ضبط كردم ، هنوز سر همش نكردم:)
صفحه اصلی
انجمن
جستجو
فهرست اعضا
افزونه های فایرفاکس
» [پروژه] انتشار نسخه 1.0 PRO-IMU (متن باز + منطبق با پروتکل دايناميکسل)
تشکر شده توسط :
