มีวิธีใดบ้างในการจัดการกับความล่าช้าของเข็มทิศ (hysteresis ขึ้นอยู่กับอัตรา)?

ฉันมีหุ่นยนต์ที่ใช้ดอกยางโดยมีตัวเข้ารหัสล้อที่มีความแม่นยำต่ำสำหรับการติดตามระยะทางและเข็มทิศอิเล็กทรอนิกส์เพื่อกำหนดหัวเรื่อง เข็มทิศมีความล่าช้าอย่างมีนัยสำคัญ (> 1 วินาที) เมื่อหุ่นยนต์หมุนอย่างรวดเร็วเช่นหลังจากไปถึงจุดอ้างอิงแล้ว - เปลี่ยนตำแหน่งเพื่อชี้ไปยังส่วนหัวใหม่

วิธีการจัดการกับความล่าช้าคืออะไร? ฉันคิดว่าจะใช้การวัดจำนวนมากและทำแบบจำลองการตอบสนองของเข็มทิศ อย่างไรก็ตามดูเหมือนว่าจะเป็นปัญหาเนื่องจากขึ้นอยู่กับอัตราและฉันไม่ทราบอัตราทันที

เป็นวิธีที่เรียบง่าย แต่ช้าฉันมีการหมุนของหุ่นยนต์จนกระทั่งมันชี้ไปในทิศทางที่ถูกต้องจากนั้นทำการเลี้ยวเพิ่มทีละเล็กทีละน้อยด้วยการวัดระยะสั้น ๆ จนกว่ามันจะชี้ไปทางขวา มีวิธีอื่นในการจัดการกับเรื่องนี้หรือไม่?

sensors compass

— ViennaMike
แหล่งที่มา

ความล่าช้าในเข็มทิศเป็นเพราะฟิลเตอร์กรองความถี่ต่ำเพื่อลดเสียงรบกวนความถี่สูง

มีแม่เหล็กที่มีราคาแพงกว่าซึ่งมีสัญญาณรบกวนน้อยกว่าและมีความล่าช้าน้อยกว่า
นอกจากนี้ยังเป็นไปได้ที่จะใช้เครื่องวัดการหมุนวนเพื่อปรับปรุงความแม่นยำ อันที่จริงนี่คือสิ่งที่หน่วยวัดแรงเฉื่อย (IMUs) ทำ สามารถทำได้โดยใช้ตัวกรองคาลมาน การปรับปรุงความแม่นยำช่วยลดความล่าช้าเนื่องจากความแม่นยำที่เพิ่มขึ้นจะลดการพึ่งพาตัวกรองสัญญาณความถี่ต่ำเพื่อลดเสียงรบกวน ตัวกรองคาลมานรวมข้อมูลจากเครื่องวัดสนามแม่เหล็กและไจโรสโคป (ซึ่งวัดอัตราการเปลี่ยนแปลงของส่วนหัว)

หากคุณยึดติดกับเข็มทิศปัจจุบันของคุณมีวิธีแก้ปัญหาที่เป็นไปได้สองทาง (คำเตือนนี่จะเพิ่มขึ้นมากขึ้น แต่ตัวเลือกที่ 1 ควรเข้าถึงได้โดยคนส่วนใหญ่โดยไม่ต้องทำงานมากเกินไป)

คุณสามารถลองยกเลิกตัวกรองได้ สิ่งนี้สามารถขจัดความล่าช้า แต่ยังเพิ่มสัญญาณรบกวนความถี่สูง หลังจากทำสิ่งนี้แล้วคุณสามารถลองควบคุมหุ่นยนต์โดยยึดตามค่าประมาณส่วนหัวใหม่ ในการทำเช่นนี้คุณต้องทดลองเพื่อหาพารามิเตอร์ตัวกรองความถี่ต่ำ ตัวอย่างเช่นในเวลาที่ไม่ต่อเนื่องคุณอาจพบ:

$\hat{θ} (t) = a_{0} θ (t) + a_{1} θ (t - 1) + \dots + a_{k} θ (t - k)$ $\hat\theta(t)=a_0\theta(t)+a_1\theta(t-1)+\cdots+a_k\theta(t-k)$ โดยที่เป็นหัวเรื่องโดยประมาณ (เอาท์พุทเข็มทิศ) ที่ เวลา ,เป็นหัวข้อที่เกิดขึ้นจริง (พื้นดินความจริง) ในขณะที $\hat\theta(t)$ $t$ $\theta$ $t$
คุณสามารถค้นหาพารามิเตอร์โดยทำการทดสอบที่คุณวัดความจริงพื้นฐานโดยใช้วิธีภายนอกอื่น เมื่อรับตัวอย่างคุณมีสมการนี้: $a_i$ $n+k+1$
$[\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}] = [\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}] [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}]$ $\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]$
และคุณสามารถแก้ไขได้โดยการค้นหา: ที่เป็นเมทริกซ์หลอกผกผันของMไม่มีวิธีที่ชัดเจนในการดังนั้นคุณอาจจะเดาได้ สำหรับคะแนนโบนัสสิ่งนี้จะถือว่าเสียงเป็นสีขาวและเป็นอิสระ แต่คุณสามารถทำให้ขาวขึ้นก่อนเพื่อลบอคติและปรับปรุงการประมาณค่าพารามิเตอร์ของคุณ
$[\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] = {[\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}]}^{+} [\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}]$ $\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]^{+}\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]$ $M^+$ $M$ $k$
คุณสามารถแปลงเป็นฟังก์ชันถ่ายโอน (หรือที่เรียกว่า Z-transform ในโดเมนเวลาไม่ต่อเนื่อง):

$\frac{\hat{Θ} (z)}{Θ (z)} = a_{0} + a_{1} z^{- 1} + . . . + a_{k} z^{- k}$ $\frac{\hat\Theta(z)}{\Theta(z)}=a_0+a_1 z^{-1}+...+a_k z^{-k}$
หากต้องการยกเลิกสิ่งนี้เราสามารถใช้อินเวอร์ส (โดยที่เป็นค่าประมาณส่วนหัวใหม่ของเรา): $\bar\theta(t)$

$\frac{\bar{Θ} (z)}{\hat{Θ} (z)} = \frac{1}{a_{0} + a_{1} z^{- 1} + \dots + a_{k} z^{- k}}$ $\frac{\bar\Theta(z)}{\hat\Theta(z)}=\frac{1}{a_0+a_1 z^{-1}+\cdots+a_k z^{-k}}$
การแปลงกลับเป็นโดเมนเวลา:

$a_{0} \bar{θ} (t) + a_{1} \bar{θ} (t - 1) + \dots + a_{k} \bar{θ} (t - k) = \hat{θ} (t)$ $a_0\bar\theta(t)+a_1\bar\theta(t-1)+\cdots+a_k \bar\theta(t-k)=\hat\theta(t)$
$\bar{θ} (t) = \frac{\hat{θ} (t) - a_{1} \bar{θ} (t - 1) - \dots - a_{k} \bar{θ} (t - k)}{a_{0}}$ $\bar\theta(t)=\frac{\hat\theta(t)-a_1\bar\theta(t-1)-\cdots-a_k \bar\theta(t-k)}{a_0}$
จากนั้นเราสามารถใช้เพื่อควบคุมหุ่นยนต์ $\bar\theta$

นี่จะมีเสียงดังมากดังนั้นคุณอาจต้องการที่จะใส่ผ่านตัวกรอง low-pass ก่อนการใช้งาน (แม้ว่าอาจจะมีความล่าช้าน้อยกว่า) $\bar\theta$
การแก้ปัญหาข้างต้นยังคงไม่ใช่วิธีที่ดีที่สุด การประมาณการที่มีเสียงดังอาจไม่มีประโยชน์มาก ถ้าเราใส่สิ่งนี้ลงในสมการของสเปซพื้นที่เราสามารถออกแบบตัวกรองคาลมานและตัวควบคุมผลป้อนกลับแบบเต็มรัฐโดยใช้ LQR (linear quadratic regulator) การรวมกันของตัวกรองคาลมานและตัวควบคุม LQR ยังเป็นที่รู้จักกันในนามของตัวควบคุม LQG (เกาส์เชิงเส้นกำลังสองเชิงเส้น) และใช้การกู้คืนแบบวนรอบเพื่อรับตัวควบคุมที่ดี

เมื่อต้องการทำสิ่งนี้ให้สร้างสมการสถานะพื้นที่ (ไม่ต่อเนื่อง):

$\vec{x}(t)=A\vec{x}(t-1)+B\vec{u}(t-1)$ , $\vec{y}(t)=C\vec{x}(t)$

หรือ:
$\vec{x} (t) = [\begin{matrix} θ (t) \\ θ (t - 1) \\ \dots \\ θ (t - k) \end{matrix}] = [\begin{matrix} A_{1} & A_{2} & \dots & 0 & 0 & 0 \\ 1 & 0 & \dots & 0 & 0 & 0 \\ 0 & 1 & \dots & 0 & 0 & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & \dots & 1 & 0 & 0 \\ 0 & 0 & \dots & 0 & 1 & 0 \end{matrix}] \vec{x} (t - 1) + [\begin{matrix} B_{0} \\ B_{1} \\ 0 \\ ⋮ \\ 0 \\ 0 \end{matrix}] \vec{u} (t - 1)$ $\vec{x}(t)=\left[\matrix{\theta(t)\\\theta(t-1)\\\cdots\\\theta(t-k)}\right]= \left[\matrix{ A_1&A_2&\cdots&0&0&0\\ 1&0&\cdots&0&0&0\\ 0&1&\cdots&0&0&0\\ \vdots&\vdots&&\vdots&\vdots&\vdots\\ 0&0&\cdots&1&0&0\\ 0&0&\cdots&0&1&0 }\right] \vec{x}(t-1) + \left[\matrix{B_0\\B_1\\0\\\vdots\\0\\0}\right]\vec{u}(t-1)$
$\vec{y} (t) = [\begin{matrix} \hat{θ} (t) \end{matrix}] = [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] \vec{x} (t)$ $\vec{y}(t)=\left[\matrix{\hat\theta(t)}\right]=\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]\vec{x}(t)$
โดยที่แสดงพลังในมอเตอร์เพื่อหมุนหุ่นยนต์และ , , ,คือจำนวนที่ส่งผลต่อส่วนหัวตามตำแหน่งและความเร็ว (คุณสามารถเลือกค่าที่ไม่เป็นศูนย์ได้ สำหรับองค์ประกอบอื่นของเมทริกซ์และแถวแรกของเมทริกซ์ด้วย) $\vec{u}(t-1)$ $A_0$ $A_1$ $B_0$ $B_1$ $B$ $A$

จากนั้นคุณสามารถสร้างผู้สังเกตการณ์ของคุณ (ตัวกรองคาลมาน) โดยเลือกการประมาณค่าเสียงและสำหรับเสียงในกระบวนการและเสียงการวัด ตัวกรองคาลมานสามารถหาค่าประมาณที่เหมาะสมที่สุดของการกำหนดโดยให้สมมติฐานเกี่ยวกับเสียงรบกวน หลังจากเลือกการประมาณค่าเสียงแล้วการติดตั้งจะขึ้นอยู่กับการติดตั้งโค้ดสำหรับตัวกรองคาลมาน (สมการสามารถพบได้บนวิกิพีเดียดังนั้นฉันจะไม่ข้ามมันไปที่นี่ $Q_o$ $R_o$

หลังจากนั้นคุณสามารถออกแบบตัวควบคุม LQR ได้ในเวลานี้เลือกและแสดงน้ำหนักที่กำหนดให้กับการควบคุมหัวเรื่องและพยายาม จำกัด การใช้ในกรณีนี้คุณอาจเลือกและขวา] สิ่งนี้ทำได้เนื่องจาก LQR ค้นหาคอนโทรลเลอร์ที่ดีที่สุดเพื่อลดฟังก์ชั่นลดต้นทุน: $Q_c$ $R_c$ $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ $R_c = \left[1\right]$ $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$

จากนั้นคุณเพียงแค่ใส่มันผ่านสมการพีชคณิต Riccati ที่ไม่ต่อเนื่อง:

$P = Q + A^{T} (P - P B {(R + B^{T} P B)}^{- 1} B^{T} P) A$ $P = Q + A^T \left( P - P B \left( R + B^T P B \right)^{-1} B^T P \right) A$
และแก้ปัญหาเป็นบวกแน่นอนเมทริกซ์P $P$

ดังนั้นกฎหมายควบคุมของคุณสามารถมอบให้โดย:

$\vec{u} (t) = - K (\vec{x} (t) - {\vec{x}}_{r e f} (t))$ $\vec{u}(t)=-K(\vec{x}(t)-\vec{x}_{ref}(t))$
โดยที่ $K = (R + B^T P B)^{-1}(B^T P A)$

ในที่สุดการทำเช่นนี้จะไม่ได้ผลดีมากและมีแนวโน้มที่จะไม่เสถียรเนื่องจากเสียงดัง ที่จริงนั่นหมายถึงตัวเลือกที่ 1 อาจไม่ทำงานเว้นแต่คุณจะใส่ผ่านตัวกรอง low-pass (แม้ว่าจะไม่จำเป็นต้องใช้เวลาหน่วงเวลานานก็ตาม) นี่เป็นเพราะในขณะที่ LQR รับประกันความเสถียรทันทีที่คุณใช้ตัวกรอง Kalman การรับประกันจะหายไป $\bar\theta$

ในการแก้ไขปัญหานี้เราใช้เทคนิคการถ่ายโอนแบบวนรอบซึ่งคุณปรับตัวกรองคาลมานแล้วเลือกโดยที่เป็นเมทริกซ์ต้นฉบับของคุณปรับเพื่อให้ตัวกรองคาลมานเหมาะสมที่สุด . คือเมทริกซ์สมมาตรเชิงบวกที่แน่นอนใด ๆ ซึ่งคุณสามารถเลือกได้ว่าเป็นเมทริกซ์เอกลักษณ์ ( ) จากนั้นเพียงแค่เลือกเกลาQคอนโทรลเลอร์ที่เป็นผลลัพธ์ควรมีเสถียรภาพมากขึ้นเมื่อแม้ว่าเมทริกซ์จะไม่ถูกปรับซึ่งหมายความว่ามันจะเหมาะสมที่สุดน้อยกว่า $Q_o = Q_0 + q^2BVB^T$ $Q_0$ $Q$ $V$ $V=I$ $q$ $q \rightarrow \infty$ $Q_o$

ดังนั้นคุณเพียงเพิ่มจนกว่ามันจะเสถียร อีกวิธีหนึ่งที่คุณสามารถทำให้เสถียรคือเพิ่ม (หรือลด ) เพื่อทำให้คอนโทรลเลอร์ LQR ช้าลง $q$ $R_c$ $Q_c$

แนวคิดในบทความนี้มีความก้าวหน้าค่อนข้างมาก แต่ถ้าคุณต้องการแก้ปัญหาอย่างสมการ Riccati คุณสามารถใช้ MATLAB หรือซอฟต์แวร์อื่นเพื่อทำสิ่งนี้ อาจมีห้องสมุดที่ใช้ตัวกรองคาลมานอยู่แล้ว (ฉันเชื่อว่า MATLAB ทำเช่นนี้ด้วย)

สำหรับแอปพลิเคชันแบบฝังคุณอาจต้องใช้ตัวกรองคาลมานเองแม้ว่าอาจมีการติดตั้ง C ก็ตาม

— ronalchn
แหล่งที่มา

ขอบคุณสำหรับคำตอบที่ยอดเยี่ยมและในเชิงลึก ฉันทำตามส่วนสำคัญของการแก้ปัญหาแรกของคุณและฉันแน่ใจว่าฉันสามารถทำงานได้ อย่างที่สองที่คุณพูดมีความท้าทายมากขึ้นและฉันจะต้องทำงานเพื่อดูว่าฉันสามารถติดตามได้ทั้งหมดหรือไม่

— ViennaMike

ไจโรคือคำตอบง่ายๆ ฉันได้ยินมาเสมอวงแหวนสำหรับการวัดระยะสั้นเข็มทิศสำหรับความยาว และตัวกรองคอลแมนหนึ่งถ้วยที่แนบสนิทกันระหว่างเวลาส่วนใหญ่สองส่วน ราคาของบอร์ด gyro / acc 6DOF น้อยกว่า $ 20 วันนี้ราคาถูกเกินไปที่จะไม่ใช้

ครั้งหนึ่งผมทำงานผ่านตัวกรอง Kallman ของคนอื่น และทำให้มันทำงานได้ ตัวกรองคอลแมนเป็นวิธีการที่มากกว่าการใช้งานจริงและในกรณีไจโรผลลัพธ์สุดท้ายไม่จำเป็นต้องใช้เมทริกซ์คณิตศาสตร์ มันทำให้รหัสง่ายขึ้นมาก

— Spiked3
แหล่งที่มา