پیش‌نیازهای علمی و ریاضی AHRS

مقدمه فصل دوم: پیش‌نیازهای علمی و ریاضی

این فصل، مبانی ریاضیاتی مورد نیاز برای درک و پیاده‌سازی سامانه‌های تعیین وضعیت و جهت (Attitude and Heading Reference Systems – $\text{AHRS}$ ) را پایه‌ریزی می‌کند. موفقیت در طراحی یک سیستم ناوبری اینرسی با کارایی بالا، به طور حیاتی به انتخاب صحیح زبان ریاضی برای توصیف وضعیت یک جسم صلب در فضای سه‌بُعدی بستگی دارد. در واقع، ریاضیات حرکت دورانی، زبان مشترک بین اندازه‌گیری‌های سنسورهای اینرسی، مدل‌های فیزیکی و الگوریتم‌های پیشرفته فیلترینگ حالت (مانند فیلتر کالمن) را فراهم می‌آورد. این سفر با مفاهیم سینماتیک دورانی آغاز می‌شود؛ هنری که به توصیف حرکت می‌پردازد، بدون آنکه نیروهای عامل آن را در نظر بگیرد.

در طول تاریخ ناوبری و رباتیک، روش‌های مختلفی برای نمایش وضعیت معرفی شده‌اند که هر کدام دارای مزایا و محدودیت‌های خاصی هستند. زوایای اویلر ( $\text{Euler Angles}$ )، با وجود سادگی و درک شهودی بالا (مانند $\text{Roll}$ , $\text{Pitch}$ , $\text{Yaw}$ )، با مشکل مرگبار قفل گیمبال ( $\text{Gimbal Lock}$ ) مواجه هستند که آن‌ها را برای وسایل نقلیه با مانور بالا کاملاً نامناسب می‌سازد. از سوی دیگر، ماتریس‌های چرخش ( $\text{Rotation Matrices}$ ) یک نمایش کامل و بدون ابهام را فراهم می‌کنند اما با استفاده از نه مؤلفه برای توصیف تنها سه درجه آزادی، منجر به سربار محاسباتی شده و نیاز به عملیات‌های متعامدسازی مجدد دوره‌ای برای حفظ پایداری عددی دارند.

این محدودیت‌ها، نسل جدید سامانه‌های $\text{AHRS}$ را به سمت استفاده از کواترنیون‌ها ( $\text{Quaternions}$ ) سوق داده است. کواترنیون‌ها یک نمایش چهاربُعدی هستند که نه تنها به طور ذاتی از تکینگی‌ها جلوگیری می‌کنند، بلکه کارایی محاسباتی بالاتری را برای ترکیب پیوسته دوران‌ها (فاز پیش‌بینی $\text{EKF}$ ) فراهم می‌آورند. در این فصل، فراتر از معرفی ساده کواترنیون‌ها، بر روی مفهوم نمایش کواترنیونی ضربی ( $\text{Multiplicative Quaternion Representation}$ ) تمرکز ویژه‌ای خواهد شد. این مدل‌سازی پیشرفته برای نمایش خطای وضعیت در فیلتر کالمن توسعه‌یافته ( $\text{MEKF}$ )، تضمین می‌کند که قید یکه کواترنیون در فرآیند تخمین حفظ شده و بهینه‌ترین ساختار برای ادغام با الگوریتم‌های هوشمند و تطبیقی (مانند مدل‌های مارکوف پنهان) فراهم آید.

تسلط بر مفاهیم ارائه شده در این فصل، یک پیش‌نیاز حیاتی برای تمام فصول آتی کتاب است. مدل‌سازی نویز سنسورها (فصل ۳)، طراحی فیلترهای کالمن (فصل ۱۰) و توسعه الگوریتم‌های تطبیقی (فصل ۱۲)، همگی بر بنیان روابط جبری و سینماتیک دورانی که در اینجا معرفی می‌شوند، استوار هستند. بنابراین، درک عمیق این مبانی، کلید بازگشایی پیچیدگی‌های سامانه‌های ناوبری اینرسی مدرن است.

۲.۱ جبر خطی، بردار و ماتریس‌های چرخش

۲.۱.۱ چارچوب‌های مرجع، نگاشت برداری و مبانی سینماتیک دورانی

در قلب هر سامانه ناوبری اینرسی، از جمله $\text{AHRS}$ ، مسئله اصلی، توصیف دقیق و بدون ابهام وضعیت (Attitude) یک جسم صلب است. این توصیف در گرو درک مفاهیم چارچوب‌های مرجع (Reference Frames) و چگونگی تبدیل نمایش برداری یک کمیت از یک چارچوب به دیگری است [Groves, 2008]. یک چارچوب مرجع، مجموعه‌ای از سه بردار یکه متعامد است که یک دستگاه مختصات راست‌گرد ( $\mathbf{i}, \mathbf{j}, \mathbf{k}$ ) را تعریف می‌کند. در ناوبری، دو چارچوب اصلی عبارتند از:

چارچوب بدنه ( $\text{Body Frame}, b$ ): سیستمی که مرکز آن روی مرکز جرم جسم (مثلاً هواپیما یا ربات) قرار دارد و محورهای آن ثابت به بدنه متصل هستند. تمام اندازه‌گیری‌های حسگرهای اینرسی ( $\text{IMU}$ ) مستقیماً در این چارچوب صورت می‌گیرد.
چارچوب ناوبری ( $\text{Navigation Frame}, n$ ): سیستمی که مبدأ آن روی جسم است اما محورهای آن نسبت به زمین ثابت هستند (معمولاً شمال-شرق-پایین یا $\text{NED}$ استفاده می‌شود). این چارچوب، مرجع خروجی $\text{AHRS}$ را فراهم می‌کند.

در عمل، یک بردار فیزیکی مانند نیروی مخصوص ( $\mathbf{f}$ ) یا میدان مغناطیسی ( $\mathbf{m}$ ) یک کمیت مستقل از چارچوب است، اما برای محاسبات، باید مؤلفه‌های آن را از نمایش در چارچوب $b$ (اندازه‌گیری‌شده توسط سنسور) به نمایش در چارچوب $n$ (مرجع جهانی) تبدیل کرد. این عمل نگاشت (Mapping) برداری توسط ماتریس چرخش (Rotation Matrix) صورت می‌گیرد [Titterton & Weston, 2004].

۲.۱.۲ ساختار ماتریس چرخش و مفهوم کسینوس‌های جهت

ماتریس چرخش $\mathbf{C}_b^n$ یک ماتریس $3 \times 3$ است که وضعیت (Orientiation) چارچوب $b$ را نسبت به چارچوب $n$ توصیف می‌کند. ماتریس چرخش، از طریق مفهوم کسینوس‌های جهت ( $\text{Direction Cosines}$ ) تعریف می‌شود. هر مؤلفه $(i, j)$ از این ماتریس، کسینوس زاویه بین محور $i$ از چارچوب ناوبری ( $\mathbf{i}_n, \mathbf{j}_n, \mathbf{k}_n$ ) و محور $j$ از چارچوب بدنه ( $\mathbf{i}_b, \mathbf{j}_b, \mathbf{k}_b$ ) را نشان می‌دهد.

به طور رسمی، رابطه تبدیل بردار $\mathbf{r}^b$ از چارچوب بدنه به بردار $\mathbf{r}^n$ در چارچوب ناوبری به صورت زیر است:

$\mathbf{r}^n = \mathbf{C}_b^n \mathbf{r}^b$

و ساختار ماتریس $\mathbf{C}_b^n$ را می‌توان با استفاده از ضرب نقطه‌ای بردارهای یکه دو چارچوب نشان داد [Siciliano et al., 2009]:

$\mathbf{C}_b^n = \begin{bmatrix} \mathbf{i}_n \cdot \mathbf{i}_b & \mathbf{i}_n \cdot \mathbf{j}_b & \mathbf{i}_n \cdot \mathbf{k}_b \\ \mathbf{j}_n \cdot \mathbf{i}_b & \mathbf{j}_n \cdot \mathbf{j}_b & \mathbf{j}_n \cdot \mathbf{k}_b \\ \mathbf{k}_n \cdot \mathbf{i}_b & \mathbf{k}_n \cdot \mathbf{j}_b & \mathbf{k}_n \cdot \mathbf{k}_b \end{bmatrix}$

این ماتریس یک نمایش کامل و بدون ابهام از وضعیت دوران را ارائه می‌دهد.

۲.۱.۳ خواص متعامد، دترمینان واحد، و چالش سربار محاسباتی

ماتریس‌های چرخش، به دلیل نمایش فیزیکی یک دوران صلب، دارای دو خاصیت اساسی جبر خطی هستند که باید همواره حفظ شوند:

خاصیت متعامد (Orthogonality): این ویژگی بیان می‌کند که ماتریس چرخش، طول بردارها و زوایای بین آن‌ها را حفظ می‌کند. از نظر ریاضی، این بدان معناست که حاصل‌ضرب ماتریس در ترانهاده‌اش، ماتریس واحد ( $\mathbf{I}$ ) است و در نتیجه، ماتریس تبدیل معکوس (از $n$ به $b$ ) برابر با ترانهاده ماتریس اصلی است [Noureldin et al., 2013]:
$\mathbf{C} \mathbf{C}^T = \mathbf{I} \quad \Rightarrow \quad \mathbf{C}_n^b = (\mathbf{C}_b^n)^{-1} = (\mathbf{C}_b^n)^T$
دترمینان واحد مثبت: برای تضمین اینکه تبدیل فقط یک دوران است و نه ترکیبی از دوران و انعکاس آینه‌ای، دترمینان ماتریس باید همواره برابر با ۱+ باشد: $\det(\mathbf{C}) = +1$ [Kuipers, 1999].

چالش اصلی ماتریس چرخش، استفاده از ۹ مؤلفه برای توصیف تنها سه درجه آزادی دوران است. این بیش‌ازحد پارامترسازی (Over-parameterization) دو مشکل ایجاد می‌کند: اول، سربار محاسباتی در به‌روزرسانی دینامیک سیستم افزایش می‌یابد؛ دوم، در محیط محاسباتی گسسته و تحت تأثیر نویز حسگرها، خطاهای کوچک عددی می‌توانند باعث شوند که ماتریس خاصیت تعامد خود را به تدریج از دست بدهد ( $\mathbf{C} \mathbf{C}^T \neq \mathbf{I}$ ) [Titterton & Weston, 2004]. برای مقابله با این انحراف، نیاز به اجرای دوره‌ای فرآیندهای نرمال‌سازی مجدد یا متعامدسازی مجدد ( $\text{Re-orthogonalization}$ ) در طول اجرای فیلتر کالمن است که خود این عملیات نیز پیچیده و زمان‌بر است. این ضعف عددی، مهندسان ناوبری را به استفاده از نمایش‌های جایگزین مانند کواترنیون‌ها سوق داده است.

۲.۲ زوایای اویلر و محدودیت $\text{Gimbal Lock}$

۲.۲.۱ توصیف وضعیت با دنباله‌های دوران اویلر و ساختار شهودی

زوایای اویلر ( $\text{Euler Angles}$ )، که شامل زوایای $\text{Roll}$ ( $\phi$ )، $\text{Pitch}$ ( $\theta$ ) و $\text{Yaw}$ ( $\psi$ ) هستند، سنتی‌ترین و از نظر بصری قابل درک‌ترین روش برای توصیف وضعیت (Attitude) یک جسم صلب در فضای سه‌بُعدی محسوب می‌شوند [Titterton & Weston, 2004]. این روش، دوران کلی یک جسم را به عنوان دنباله‌ای از سه دوران تک‌محوره متوالی تعریف می‌کند که هر یک حول یکی از محورهای چارچوب مرجع جسم صورت می‌گیرد. دلیل اصلی استفاده گسترده از این زوایا، سادگی آن‌ها در تفسیر و کاربرد مستقیم آن‌ها در حلقه‌های کنترل پرواز (مانند $\text{PID}$ ) و رابط‌های انسانی است.

اهمیت اصلی در زوایای اویلر، ترتیب اعمال دوران‌ها (Rotation Sequence) است؛ زیرا ضرب ماتریس‌های دوران خاصیت جابه‌جایی ندارد [Siciliano et al., 2009]. برای سامانه‌های ناوبری و هوافضا، متداول‌ترین توالی، $\text{Z-Y-X}$ است که در آن دوران‌ها به ترتیب زیر اعمال می‌شوند: ابتدا $\text{Yaw}$ ( $\psi$ ) حول محور $\text{Z}$ ، سپس $\text{Pitch}$ ( $\theta$ ) حول محور $\text{Y}$ جدید، و در نهایت $\text{Roll}$ ( $\phi$ ) حول محور $\text{X}$ نهایی صورت می‌گیرد.

ماتریس چرخش نهایی ( $\mathbf{C}_b^n$ ) که از طریق این توالی به دست می‌آید، ترکیب ضربی سه ماتریس دوران پایه است:

$\mathbf{C}_b^n (\psi, \theta, \phi) = \mathbf{R}_z(\psi) \mathbf{R}_y(\theta) \mathbf{R}_x(\phi)$

این نمایش، تنها با سه پارامتر، وضعیت کامل یک جسم را توصیف می‌کند و از این منظر، از لحاظ تعداد مؤلفه‌ها، از ماتریس چرخش $3 \times 3$ بهینه‌تر است [Groves, 2008]. با این حال، همان‌طور که در ادامه بررسی می‌شود، این مزیت در برابر یک فاجعه ریاضی در شرایط خاص، اهمیت خود را از دست می‌دهد.

۲.۲.۲ محدودیت تکینگی و پدیده $\text{Gimbal Lock}$

با وجود تمام مزایای شهودی، زوایای اویلر دارای یک تکینگی (Singularity) ذاتی هستند که استفاده مستقیم از آن‌ها را در معادلات سینماتیک برای $\text{AHRS}$ های با مانور بالا، غیرممکن می‌سازد. این پدیده به عنوان **قفل گیمبال ( $\text{Gimbal Lock}$ )** شناخته می‌شود و در هر دنباله دوران متوالی‌ای که دو محور چرخش با یکدیگر هم‌راستا شوند، رخ می‌دهد [Kuipers, 1999].

برای توالی استاندارد $\text{Z-Y-X}$ ، این مشکل زمانی رخ می‌دهد که زاویه $\text{Pitch}$ ( $\theta$ ) به مقدار $\pm 90^{\circ}$ (یعنی جسم کاملاً عمود بر زمین) برسد. در این وضعیت، محور $\text{Roll}$ (دوران سوم) و محور $\text{Yaw}$ (دوران اول) عملاً منطبق یا موازی می‌شوند. این هم‌خطی باعث می‌شود که سیستم یک درجه آزادی چرخشی را از دست بدهد و امکان تمایز بین چرخش حول محور $\text{Yaw}$ و $\text{Roll}$ وجود نداشته باشد. فضا از توصیف سه‌بُعدی به دو بُعد کاهش می‌یابد.

از دیدگاه ریاضی و در چارچوب $\text{AHRS}$ ، این پدیده از طریق ماتریس سینماتیک اویلر ( $\mathbf{T}$ ) که سرعت زاویه‌ای ژیروسکوپ ( $\boldsymbol{\omega}$ ) را به آهنگ تغییر زوایای اویلر ( $\dot{\boldsymbol{\alpha}}$ ) تبدیل می‌کند، مشهود است:

$\dot{\boldsymbol{\alpha}} = \mathbf{T}(\boldsymbol{\alpha}) \boldsymbol{\omega}$

هنگامی که $\theta = \pm 90^{\circ}$ ، ماتریس $\mathbf{T}$ به یک ماتریس تکین (Singular Matrix) تبدیل شده و وارون‌پذیر نیست. این بدان معناست که هیچ راهی برای محاسبه نرخ تغییر زوایای اویلر ( $\dot{\boldsymbol{\alpha}}$ ) از روی اندازه‌گیری ژیروسکوپ ( $\boldsymbol{\omega}$ ) وجود ندارد. در نتیجه، الگوریتم $\text{AHRS}$ قادر به ادامه‌ی انتگرال‌گیری و تخمین وضعیت نیست و این موضوع، سیستم‌های ناوبری را مجبور به استفاده از کواترنیون‌ها به عنوان فرمت داخلی خود برای نمایش وضعیت کرده است تا از این مشکل مهلک اجتناب شود [Noureldin et al., 2013].

۲.۳ کواترنیون‌ها و مزیت‌های عددی

۲.۳.۱ ساختار جبری کواترنیون و نمایش دوران صلب

کواترنیون‌ها ( $\mathbf{q}$ ) یک سیستم عددی چهار بُعدی هستند که در سال ۱۸۴۳ توسط ویلیام روآن همیلتون کشف شدند و اکنون به عنوان راه‌حل استاندارد برای نمایش دوران در فیزیک، رباتیک و ناوبری اینرسی مدرن شناخته می‌شوند [Kuipers, 1999]. یک کواترنیون $\mathbf{q}$ از چهار مؤلفه تشکیل شده است: یک بخش نرده‌ای (Scalar) ( $q_0$ ) و یک بخش برداری ( $\mathbf{q}_v$ ) سه بُعدی:

$\mathbf{q} = q_0 + q_1 \mathbf{i} + q_2 \mathbf{j} + q_3 \mathbf{k} = [q_0, \mathbf{q}_v^T]^T$

که در آن $\mathbf{i}$ , $\mathbf{j}$ , $\mathbf{k}$ یک پایه تعمیم‌یافته با قوانین ضرب غیرجابه‌جایی خاص ( $\mathbf{i}\mathbf{j} = \mathbf{k}$ , $\mathbf{j}\mathbf{i} = -\mathbf{k}$ , …) را دنبال می‌کنند.

بر اساس قضیه دوران اویلر، هر دوران در فضای سه‌بُعدی را می‌توان با یک دوران واحد حول یک محور ثابت مشخص کرد. کواترنیون‌ها به‌طور مستقیم این قضیه را با استفاده از کواترنیون یکه (Unit Quaternion) نمایش می‌دهند. کواترنیون یکه $\mathbf{q}$ که دوران حول محور $\mathbf{u}$ (با $\|\mathbf{u}\|=1$ ) و با زاویه $\alpha$ را توصیف می‌کند، به صورت زیر تعریف می‌شود [Kuipers, 1999]:

$\mathbf{q} = \begin{bmatrix} \cos \left( \frac{\alpha}{2} \right) \\ \mathbf{u} \sin \left( \frac{\alpha}{2} \right) \end{bmatrix}$

برای اینکه کواترنیون یک وضعیت فیزیکی معتبر را نشان دهد، باید دارای نرم یکه باشد: $\|\mathbf{q}\| = \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2} = 1$ . این قید ریاضی، ساختار کواترنیون‌ها را در الگوریتم‌های تخمین $\text{AHRS}$ حفظ می‌کند.

۲.۳.۲ برتری عددی: حذف $\text{Gimbal Lock}$ و کارایی محاسباتی

مزایای عددی کواترنیون‌ها آن‌ها را به بهترین انتخاب برای نمایش داخلی وضعیت در سامانه‌های $\text{AHRS}$ تبدیل کرده است:

حذف تکینگی (No Singularities): برخلاف زوایای اویلر که در $\text{Pitch} = \pm 90^{\circ}$ دچار قفل گیمبال می‌شوند، کواترنیون‌ها یک نمایش چهاربُعدی را فراهم می‌کنند که عاری از هرگونه نقطه تکین فیزیکی یا ریاضی است [Siciliano et al., 2009]. این امر پایداری الگوریتم‌های ناوبری را در تمام مانورهای ممکن و شدید تضمین می‌کند، که برای وسایل نقلیه با تحرک بالا حیاتی است.
کارایی محاسباتی در ترکیب دوران‌ها: فرآیند به‌روزرسانی وضعیت در $\text{AHRS}$ (انتگرال‌گیری از ژیروسکوپ) مستلزم ترکیب پیوسته دوران‌ها است. ترکیب دو کواترنیون ( $\mathbf{q}_a$ و $\mathbf{q}_b$ ) از طریق ضرب کواترنیون ( $\mathbf{q}_c = \mathbf{q}_a \otimes \mathbf{q}_b$ ) انجام می‌شود. این عمل، از نظر محاسباتی، به شکل قابل توجهی سریع‌تر از ضرب دو ماتریس چرخش $3 \times 3$ (که شامل ۲۷ ضرب و ۱۸ جمع است) عمل می‌کند. این کارایی برای سیستم‌های بلادرنگ ( $\text{Real-Time}$ ) که با نرخ‌های بالای داده ( $100$ تا $1000$ هرتز) سروکار دارند، کلیدی است [Groves, 2008].
بهینگی پارامتری: کواترنیون‌ها تنها با چهار مؤلفه و یک قید (نرم یکه) دوران را توصیف می‌کنند. این تعداد مؤلفه، از ۹ مؤلفه ماتریس چرخش کمتر است و از لحاظ عددی بهینه‌تر از آن است. هرچند کواترنیون‌ها هنوز بیش‌ازحد پارامترسازی شده‌اند (چهار مؤلفه برای سه درجه آزادی)، اما پایداری و سهولت حفظ قید یکه در فیلتر کالمن ضربی (MEKF) آن‌ها را به انتخاب برتر تبدیل کرده است [Noureldin et al., 2013].

۲.۴ ماتریس چرخش و روابط آن با کواترنیون

۲.۴.۱ تبدیل کواترنیون به ماتریس چرخش ( $\mathbf{C}$ ) برای نگاشت برداری

همان‌طور که در زیرفصل‌های ۲.۲ و ۲.۳ تشریح شد، کواترنیون‌ها ( $\mathbf{q}$ ) به عنوان نمایش بهینه و عاری از تکینگی برای تکامل (Evolution) وضعیت در طول زمان استفاده می‌شوند، در حالی که ماتریس‌های چرخش ( $\mathbf{C}$ ) به عنوان عملگرهای خطی برای تبدیل (Transformation) مختصات بردارها (مانند شتاب اندازه‌گیری شده یا میدان مغناطیسی زمین) از یک چارچوب مرجع به چارچوب دیگر به کار می‌روند. در نتیجه، در مرحله به‌روزرسانی (Update Phase) فیلتر کالمن ( $\text{EKF}$ )، کواترنیون وضعیت تخمین‌زده شده ( $\hat{\mathbf{q}}$ ) باید به ماتریس چرخش متناظر تبدیل شود تا امکان مقایسه بین اندازه‌گیری‌های حسگر و مقادیر مرجع مورد انتظار فراهم گردد. این نیاز به تبدیل، یک پل ریاضی حیاتی بین دو فرمت نمایش دوران ایجاد می‌کند.

کواترنیون یکه $\mathbf{q} = [q_0, q_1, q_2, q_3]^T$ که وضعیت را از چارچوب بدنه ( $b$ ) به چارچوب ناوبری ( $n$ ) توصیف می‌کند، به ماتریس چرخش $\mathbf{C}_b^n$ با استفاده از یک تابع غیرخطی تبدیل می‌شود. این تبدیل، تضمین می‌کند که ماتریس چرخش حاصل، به صورت ذاتی متعامد (Orthogonal) باشد، بدون آنکه نیاز به اجرای فرآیندهای پیچیده نرمال‌سازی مجدد داشته باشیم. روابط جبری بین مؤلفه‌های کواترنیون و عناصر ماتریس $\mathbf{C}_b^n$ به صورت زیر تعریف می‌شوند:

$\mathbf{C}_b^n(\mathbf{q}) = \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2 - q_0q_3) & 2(q_1q_3 + q_0q_2) \\ 2(q_1q_2 + q_0q_3) & q_0^2-q_1^2+q_2^2-q_3^2 & 2(q_2q_3 - q_0q_1) \\ 2(q_1q_3 - q_0q_2) & 2(q_2q_3 + q_0q_1) & q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix}$

این ماتریس $3 \times 3$ سپس برای تبدیل بردار نیروی گرانش مرجع ( $\mathbf{g}^n$ ) از چارچوب ناوبری به چارچوب بدنه ( $\mathbf{g}^b$ ) استفاده می‌شود: $\mathbf{g}^b = (\mathbf{C}_b^n)^T \mathbf{g}^n$ . این بردار گرانش تبدیل‌شده، همان مقداری است که انتظار می‌رود شتاب‌سنج در حالت سکون اندازه‌گیری کند و اساس تصحیح $\text{Roll}$ و $\text{Pitch}$ در $\text{AHRS}$ را تشکیل می‌دهد.

۲.۴.۲ تبدیل ماتریس چرخش به کواترنیون و حل ابهام ۱۸۰ درجه‌ای

تبدیل معکوس، یعنی از ماتریس چرخش به کواترنیون، اگرچه از نظر ریاضی امکان‌پذیر است، اما معمولاً در طول فرآیند فیلترینگ $\text{EKF}$ استفاده نمی‌شود و کاربرد اصلی آن در مرحله مقداردهی اولیه (Initialization) سیستم $\text{AHRS}$ است. در این مرحله، وضعیت اولیه جسم ممکن است توسط اندازه‌گیری‌های استاتیک شتاب‌سنج و مغناطیس‌سنج به صورت یک ماتریس چرخش اولیه ( $\mathbf{C}_0$ ) تقریب زده شود، و سپس این ماتریس به کواترنیون اولیه ( $\mathbf{q}_0$ ) تبدیل می‌گردد تا فیلتر با یک حالت کواترنیونی آغاز به کار کند.

برای انجام این تبدیل، معمولاً از عناصر قطری ماتریس چرخش استفاده می‌شود. با این حال، همان‌طور که در زیرفصل ۲.۳ اشاره شد، کواترنیون‌ها دارای یک ابهام دوگانه (Double-Cover) هستند؛ به این معنی که کواترنیون $\mathbf{q}$ و کواترنیون منفی آن ( $-\mathbf{q}$ ) هر دو یک وضعیت فیزیکی یکسان را توصیف می‌کنند.

یکی از فرمول‌های متداول برای محاسبه مؤلفه نرده‌ای ( $\mathbf{q}_0$ ) از طریق رَد (Trace) ماتریس چرخش ( $\mathbf{C}$ ) استفاده می‌کند:

$q_0 = \frac{1}{2} \sqrt{1 + C_{11} + C_{22} + C_{33}}$

سپس، سایر مؤلفه‌ها از عناصر خارج از قطر محاسبه می‌شوند. در عمل، برای جلوگیری از عدم قطعیت عددی ناشی از تقسیم بر صفر (به ویژه زمانی که $\mathbf{q}_0$ نزدیک به صفر است)، معمولاً از الگوریتم‌هایی استفاده می‌شود که بزرگترین عنصر قطری ماتریس را شناسایی کرده و محاسبه را بر اساس آن عنصر آغاز می‌کنند. این تضمین می‌کند که پارامتر تخمین‌زده شده دارای بزرگترین مقدار و کمترین نویز عددی است. این روابط دوگانه بین ماتریس چرخش و کواترنیون، زیربنای انعطاف‌پذیری ریاضی در طراحی الگوریتم‌های $\text{AHRS}$ را تشکیل می‌دهند.

۲.۵ نمایش کواترنیونی ضربی ( $\text{Multiplicative Quaternion}$ ) ← افزوده از مقاله $\text{HMM}$ -based $\text{EKF}$

۲.۵.۱ مزیت $\text{Multiplicative Quaternion}$ بر نمایش جمعی در $\text{EKF}$

مدل‌سازی خطای وضعیت (Attitude Error) در فیلتر کالمن توسعه‌یافته ( $\text{EKF}$ ) برای سامانه‌های $\text{AHRS}$ ، از مهم‌ترین چالش‌های طراحی الگوریتم است. از آنجایی که کواترنیون‌ها ( $\mathbf{q}$ ) نمایش‌دهنده وضعیت هستند، یک نمایش صرفاً جمعی (Additive) برای خطا، یعنی $\mathbf{q}_{\text{true}} = \hat{\mathbf{q}} + \delta\mathbf{q}$ ، با یک مشکل ذاتی روبرو است: نقض قید یکه ( $\|\mathbf{q}\|=1$ ) [Tong et al., 2018]. کواترنیون حاصل از این جمع ممکن است نرمی متفاوت از یک داشته باشد که پایداری عددی فیلتر را به خطر می‌اندازد و نیازمند عملیات نرمال‌سازی مکرر و پرهزینه است.

برای حل این چالش، رویکرد نمایش کواترنیونی ضربی ( $\text{Multiplicative Quaternion Representation}$ ) در ساختار فیلتر کالمن توسعه‌یافته ضربی ( $\text{MEKF}$ – Multiplicative EKF) معرفی شد. در این مدل، فرض می‌شود که خطای وضعیت، یک دوران خطای کوچک ( $\mathbf{q}_{\text{error}}$ ) است که به صورت ضربی با وضعیت تخمین‌زده شده ( $\hat{\mathbf{q}}$ ) ترکیب می‌شود:

$\mathbf{q}_{\text{true}} = \mathbf{q}_{\text{error}} \otimes \hat{\mathbf{q}}$

این نمایش ضربی، مزیت‌های عددی چشمگیری دارد. اولاً، با مدل‌سازی خطا به عنوان یک دوران کوچک، تضمین می‌کند که ساختار واحد کواترنیون حفظ می‌شود. ثانیاً، دینامیک خطای ضربی از نظر ریاضی ساختار ساده‌تری نسبت به دینامیک خطای جمعی دارد که خطی‌سازی ماتریس یعقوبی ( $\mathbf{F}$ ) را در فاز پیش‌بینی $\text{EKF}$ تسهیل می‌کند [Groves, 2008]. این مدل به صورت غیرمستقیم، تنها خطای وضعیت را تخمین می‌زند و نه خود وضعیت را، که باعث می‌شود ابعاد و پیچیدگی ماتریس کوواریانس خطا ( $\mathbf{P}$ ) کاهش یابد.

۲.۵.۲ نمایش برداری خطای ضربی ( $\delta\boldsymbol{\alpha}$ ) و ادغام با مدل‌های سازگار

اگرچه خطای وضعیت به صورت کواترنیون ضربی ( $\mathbf{q}_{\text{error}}$ ) مدل می‌شود، اما برای استفاده در چارچوب خطی فیلتر کالمن، باید آن را به یک فضای حالت خطی نگاشت کرد. این کار با تبدیل کواترنیون خطای واحد به یک بردار خطای زاویه‌ای کوچک سه‌بُعدی ( $\delta\boldsymbol{\alpha}$ ) انجام می‌شود. با فرض اینکه خطا کوچک است (که فرض اساسی در $\text{EKF}$ است)، کواترنیون خطا به صورت یک تقریب خطی ساده‌تر درمی‌آید:

$\mathbf{q}_{\text{error}} \approx \begin{bmatrix} 1 \\ \frac{1}{2}\delta\boldsymbol{\alpha} \end{bmatrix}$

که در آن $\delta\boldsymbol{\alpha} = [\delta\alpha_x, \delta\alpha_y, \delta\alpha_z]^T$ بردار خطای زاویه‌ای کوچک حول محورهای مختصات است. این بردار سه‌بُعدی به عنوان مولفه‌های وضعیت خطا در بردار حالت نهایی $\text{MEKF}$ استفاده می‌شود و ابعاد ماتریس کوواریانس خطا ( $\mathbf{P}$ ) را برای بخش وضعیت از $4 \times 4$ به $3 \times 3$ کاهش می‌دهد [Kang et al., 2016].

این نمایش خطی خطا، امکان ادغام آسان با الگوریتم‌های هوشمند مانند $\text{HMM}$ (Hidden Markov Model) یا $\text{Markov Chain}$ را فراهم می‌کند [Tong et al., 2018]. این مدل‌های هوشمند می‌توانند حالت دینامیکی سیستم (مانند سکون، حرکت آهسته، مانور شدید) را تشخیص دهند و سپس از این اطلاعات برای تطبیق ماتریس کوواریانس نویز فرآیند ( $\mathbf{Q}$ ) در فضای خطی $\delta\boldsymbol{\alpha}$ استفاده کنند. این تطبیق خودکار پارامترهای نویز (Adaptive Estimation)، به‌ویژه برای مقابله با نویز متغیر حسگرهای $\text{MEMS}$ در محیط‌های عملی، ضروری است و دقت نهایی سیستم $\text{AHRS}$ را به شکل چشمگیری بهبود می‌بخشد. در نهایت، پس از تخمین $\delta\boldsymbol{\alpha}$ توسط فیلتر، این بردار به $\hat{\mathbf{q}}_{\text{error}}$ تبدیل شده و به صورت ضربی در $\hat{\mathbf{q}}$ اعمال می‌شود تا وضعیت تصحیح گردد.

۲.۶ روابط کواترنیونی در فیلتر کالمن توسعه‌یافته

۲.۶.۱ معادلات سینماتیک کواترنیونی و فاز پیش‌بینی در $\text{EKF}$

استفاده از کواترنیون‌ها ( $\mathbf{q}$ ) در فیلتر کالمن توسعه‌یافته ( $\text{EKF}$ ) برای $\text{AHRS}$ ، مستلزم مدل‌سازی دقیق دینامیک وضعیت است. از آنجایی که کواترنیون‌ها یک کمیت چهاربُعدی غیرخطی را نمایش می‌دهند، نرخ تکامل وضعیت با استفاده از معادله سینماتیک کواترنیونی بیان می‌شود [Groves, 2008]. این معادله، نرخ تغییر کواترنیون وضعیت ( $\dot{\mathbf{q}}$ ) را به‌طور مستقیم به سرعت زاویه‌ای اندازه‌گیری شده توسط ژیروسکوپ ( $\boldsymbol{\omega}$ ) مرتبط می‌کند.

معادله سینماتیک در زمان پیوسته که زیربنای فاز پیش‌بینی (Prediction Phase) فیلتر کالمن را تشکیل می‌دهد، به صورت زیر تعریف می‌شود:

$\dot{\mathbf{q}}(t) = \frac{1}{2} \mathbf{q}(t) \otimes \boldsymbol{\omega}_q(t) \quad \text{یا} \quad \dot{\mathbf{q}}(t) = \frac{1}{2} \mathbf{\Omega}(\boldsymbol{\omega}) \mathbf{q}(t)$

که در آن $\boldsymbol{\omega}_q = [0, \omega_x, \omega_y, \omega_z]^T$ نمایش کواترنیونی بردار سرعت زاویه‌ای ( $\boldsymbol{\omega}$ ) و $\mathbf{\Omega}(\boldsymbol{\omega})$ ماتریس ضرب متقاطع کواترنیونی است. این ماتریس برای یک بردار سرعت زاویه‌ای $\boldsymbol{\omega}$ به شکل زیر به دست می‌آید [Noureldin et al., 2013]:

$\mathbf{\Omega}(\boldsymbol{\omega}) = \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix}$

برای پیاده‌سازی این معادله در $\text{EKF}$ ، باید آن را از زمان پیوسته به زمان گسسته تبدیل کرد. این گسسته‌سازی (Discretization) با استفاده از روش‌هایی مانند تقریب مرتبه اول یا سری تیلور حول زمان قبلی انجام می‌شود: $\hat{\mathbf{q}}_k^- = \mathbf{f}(\hat{\mathbf{q}}_{k-1}^+, \boldsymbol{\omega}_k)$ . این فرآیند به‌روزرسانی وضعیت، اطمینان می‌دهد که وضعیت تخمین‌زده شده ( $\hat{\mathbf{q}}$ ) در برابر رانش ژیروسکوپ به درستی مقاومت کند و قید یکه آن به طور ضمنی در ساختار کواترنیونی حفظ شود [Titterton & Weston, 2004].

۲.۶.۲ ماتریس یعقوبی انتقال حالت خطا ( $\mathbf{\Phi}_k$ ) و کوواریانس نویز

در $\text{EKF}$ ، برخلاف تخمین خود وضعیت ( $\mathbf{q}$ )، آنچه فیلتر محاسبه می‌کند، خطای وضعیت ( $\delta\mathbf{x}$ ) است. برای یک سیستم $\text{AHRS}$ با مدل $\text{MEKF}$ که از کواترنیون ضربی استفاده می‌کند، بردار حالت خطا معمولاً شامل خطای زاویه‌ای سه‌بُعدی ( $\delta\boldsymbol{\alpha}$ ) و خطای بایاس ژیروسکوپ ( $\delta\mathbf{b}$ ) است: $\delta\mathbf{x} = [\delta\boldsymbol{\alpha}^T, \delta\mathbf{b}^T]^T$ .

در فاز پیش‌بینی، ماتریس کوواریانس خطا ( $\mathbf{P}$ ) باید با استفاده از ماتریس انتقال حالت خطا ( $\mathbf{\Phi}_k$ ) و ماتریس کوواریانس نویز فرآیند ( $\mathbf{Q}_k$ ) به‌روزرسانی شود:

$\mathbf{P}_k^- = \mathbf{\Phi}_{k-1} \mathbf{P}_{k-1}^+ \mathbf{\Phi}_{k-1}^T + \mathbf{Q}_{k-1}$

ماتریس انتقال حالت خطا ( $\mathbf{\Phi}_k$ )، که یک ماتریس یعقوبی (Jacobian) از دینامیک خطا است، تکامل خطاهای قبلی به خطاهای فعلی را توصیف می‌کند. این ماتریس برای یک $\text{AHRS}$ شامل ماتریس ضدتقارن بردار چرخش (Skew-Symmetric) است که نشان می‌دهد چگونه خطاهای زاویه‌ای و بایاس در طول دوران با یکدیگر ترکیب و انباشته می‌شوند [Groves, 2008].

از طرف دیگر، ماتریس $\mathbf{Q}_k$ (ماتریس کوواریانس نویز فرآیند) نمایش‌دهنده نویز ژیروسکوپ است و مستقیماً بر اساس پارامترهای آماری $\text{ARW}$ و $\text{Bias Instability}$ (که در فصل ۳ مدل‌سازی می‌شوند) تعیین می‌گردد. همانطور که در مقالات پیشرفته ( $\text{AEKF}$ مبتنی بر $\text{HMM}$ ) تأکید شده است، تعیین دقیق و تطبیقی این ماتریس $\mathbf{Q}$ برای مقابله با نویز متغیر ژیروسکوپ در شرایط دینامیکی، حیاتی است و دقت نهایی $\text{AHRS}$ را تعیین می‌کند [Tong et al., 2018; Kang et al., 2016].

۲.۶.۳ فاز به‌روزرسانی و تصحیح ضربی وضعیت

پس از فاز پیش‌بینی و دریافت اندازه‌گیری‌های تصحیحی از شتاب‌سنج و مغناطیس‌سنج، فیلتر کالمن خطای وضعیت ( $\delta\hat{\boldsymbol{\alpha}}$ ) را در فاز به‌روزرسانی محاسبه می‌کند. این مرحله نهایی برای تصحیح وضعیت تخمین‌زده شده ( $\hat{\mathbf{q}}_k^-$ ) حیاتی است و باید بدون نقض قید یکه کواترنیون انجام شود.

در $\text{MEKF}$ ، خطای زاویه‌ای تخمین‌زده شده ( $\delta\hat{\boldsymbol{\alpha}}$ ) ابتدا به یک کواترنیون خطای ضربی سه‌بُعدی $\hat{\mathbf{q}}_{\text{error}}$ نگاشت می‌شود. این عمل به سادگی و به صورت خطی انجام می‌گیرد:

$\hat{\mathbf{q}}_{\text{error}} \approx \begin{bmatrix} 1 \\ \frac{1}{2}\delta\hat{\boldsymbol{\alpha}} \end{bmatrix}$

سپس، وضعیت تصحیح شده نهایی ( $\hat{\mathbf{q}}_k^+$ ) با اعمال این خطای تخمین‌زده شده به‌صورت ضرب کواترنیون به وضعیت پیش‌بینی شده، به دست می‌آید:

$\hat{\mathbf{q}}_k^+ = \hat{\mathbf{q}}_{\text{error}} \otimes \hat{\mathbf{q}}_k^-$

این روش ضربی تضمین می‌کند که کواترنیون نهایی تصحیح شده به قید یکه بسیار نزدیک باقی بماند و پایداری بلندمدت فیلتر حفظ شود. این روابط کواترنیونی در قلب هر سیستم $\text{AHRS}$ مدرن قرار دارند و کارایی و پایداری آن را در مانورهای شدید تضمین می‌کنند.

منابع مورد استفاده در فصل ۲: پیش‌نیازهای علمی و ریاضی

کتاب‌ها:

Groves, P. D. (2008). Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Artech House.
Kuipers, J. B. (1999). Quaternions and Rotation Sequences: A Primer with Applications to Orbits, Aerospace, and Virtual Reality. Princeton University Press.
Noureldin, A., Karamat, T. B., & Georgy, J. (2013). Fundamentals of Inertial Navigation, Satellite-based Positioning and their Integration. Springer.
Siciliano, B., Sciavicco, L., Villani, L., & Oriolo, G. (2009). Robotics: Modelling, Planning and Control. Springer.
Titterton, D. H., & Weston, J. L. (2004). Strapdown Inertial Navigation Technology (2nd ed.). The Institution of Electrical Engineers.

مقالات ژورنالی:

Kang, C. W., Kim, H. J., & Park, C. G. (2016). A Human Motion Tracking Algorithm Using Adaptive EKF Based on Markov Chain. IEEE Sensors Journal, 16(24), 8953–8960.
Tong, X., Li, Z., Han, G., Liu, N., Su, Y., Ning, J., & Yang, F. (2018). Adaptive EKF Based on HMM Recognizer for Attitude Estimation Using MEMS MARG Sensors. IEEE Sensors Journal, 18(8), 3299–3310.