Python xoay vector theo quaternion

Trong hướng dẫn này, tôi sẽ chỉ cho bạn cách chuyển đổi một quaternion thành ma trận xoay ba chiều. Ở cuối bài viết này, tôi đã cung cấp mã Python để thực hiện chuyển đổi

Mục lục

Một Quaternion là gì?

Quaternion là một trong một số cách toán học để biểu diễn hướng và góc quay của một vật thể trong không gian ba chiều. Một cách khác là sử dụng ma trận xoay dựa trên góc Euler như tôi đã làm trong bài đăng này và bài đăng này [tôi. e. cuộn, cao độ và ngáp], cũng như ảnh bìa của hướng dẫn này

Các bậc bốn thường được sử dụng thay cho ma trận xoay góc Euler vì “so với ma trận xoay chúng nhỏ gọn hơn, ổn định hơn về số lượng và hiệu quả hơn” [Nguồn. Wikipedia]

Lưu ý rằng một quaternion chỉ mô tả chuyển động quay của một khung tọa độ [i. e. một số đối tượng trong không gian 3D] về một trục tùy ý, nhưng nó không cho bạn biết bất cứ điều gì về vị trí của đối tượng đó

Việc sử dụng Quaternions trong Robotics

Quaternions là phương pháp mặc định để biểu thị hướng và góc quay trong ROS, nền tảng phổ biến nhất để phát triển phần mềm rô-bốt

Trong chế tạo rô-bốt, chúng tôi luôn cố gắng xoay mọi thứ. Ví dụ, chúng ta có thể quan sát một đối tượng trong camera. Để cánh tay robot gắp được vật thể ta cần xoay hệ quy chiếu camera sang hệ quy chiếu robot để robot “biết” vị trí của vật thể trong hệ tọa độ của chính nó.

Sau khi hoàn tất quá trình xoay từ tọa độ pixel camera sang tọa độ khung cơ sở rô-bốt, cánh tay rô-bốt có thể di chuyển động cơ của nó đến các góc thích hợp để nhặt vật thể

Làm thế nào để Đại diện Quaternions

Đệ tứ là một phần mở rộng của. Tuy nhiên thay vì hai giá trị [e. g. a + bi hoặc x + yi…tương tự] đại diện cho một điểm [hoặc vectơ], chúng ta có bốn giá trị [a, b, c, d]

q = a + bi + cj + dk

Hình dung một điểm [a, b] dưới dạng một số phức trên sơ đồ Argand hai chiều. Nguồn. Wikipedia

Bốn giá trị trong một quaternion bao gồm một vectơ vô hướng và một vectơ đơn vị 3 phần tử

Thay vì a, b, c và d, bạn sẽ thường thấy

q = w + xi + yj + zk hoặc q = q0 + q1i + q2j + q3k

• q0 là một giá trị vô hướng đại diện cho một góc quay
• q1, q2, q3 tương ứng với một trục quay mà xung quanh đó thực hiện góc quay

Các cách khác bạn có thể viết một quaternion như sau

• q = [q0, q1, q2, q3]
• q = [q0, q] = q0 + q

Điều thú vị về bậc bốn là chúng hoạt động giống như số phức. Trong hai chiều, bạn có thể xoay một vectơ bằng phép nhân số phức. Bạn có thể làm tương tự với quaternions. Toán học phức tạp hơn với bốn thuật ngữ thay vì hai, nhưng nguyên tắc là như nhau

Chúng ta hãy xem một ví dụ hai chiều về phép nhân số phức để bạn có thể hiểu khái niệm nhân các số ảo [phức] để xoay một vectơ. Quaternions thêm một vài biến để mở rộng khái niệm này để thể hiện phép quay trong không gian 3D

Ví dụ 2D

Giả sử chúng ta có một vectơ trên mặt phẳng 2D với các thông số kỹ thuật sau. [x = 3, y = 1]

Vectơ này có thể được biểu diễn dưới dạng số phức như

3 + tôi [e. g. sử dụng dạng x + yi của số phức]

Hãy xoay vectơ này 45 độ [tức là π/4 tính bằng radian]

Để xoay 45 độ, chúng ta nhân số với

cos[π/4] + sin[π/4]i [công thức De Moivre]

Vì vậy, chúng ta có phương tiện sqrt [“lấy căn bậc hai của”]

[1/sqrt[2]+ i/sqrt[2]] * [3 + i] = sqrt[2] + 2sqrt[2]i

Và kể từ khi

sqrt[2] = 1. 414

vectơ mới của chúng tôi là

[x = 1. 414, y = 4. 242]

Như tôi đã đề cập trước đó, phép toán nhân các bậc bốn thực với nhau phức tạp hơn thế này, nhưng nguyên tắc thì giống nhau. Nhân một hướng [được biểu thị là một bậc bốn] với một phép quay [được biểu thị là một bậc bốn] để có được hướng mới

Chuyển đổi một Quaternion thành Ma trận Xoay

Cho trước một quaternion, bạn có thể tìm ma trận xoay ba chiều tương ứng bằng công thức sau

Tôi cũng bối rối rằng tôi không thể tìm thấy một hàm bình phương độ lớn cho các vectơ. Tôi đoán nếu tôi tự chấm vectơ thì có thể có một chút tối ưu hóa ở đó hoặc chỉ cần bỏ thao tác ra bằng tay?

Nhưng, nếu vẫn còn một chức năng chính thức ở đó, tôi sẽ biết ơn nếu biết về nó

Trong bài đăng này, tôi sẽ tìm hiểu một số điều cơ bản về quaternion và cung cấp cách triển khai đơn giản trong python để trực quan hóa các phép quay. Tuy nhiên, chúng tôi sẽ không thực hiện bất kỳ dẫn xuất nào từ các nguyên tắc đầu tiên vì nó khá toán học. Thay vào đó, chúng tôi sẽ sử dụng các công thức hiện có để xây dựng mã của mình

Kiến thức cơ bản về bậc bốn

Quaternion cung cấp cho chúng ta cách quay một điểm quanh một trục xác định theo một góc xác định. Nếu bạn mới bắt đầu với chủ đề xoay 3d, bạn sẽ thường nghe mọi người nói rằng “hãy sử dụng quaternion vì nó sẽ có bất kỳ vấn đề nào về khóa gimbal”. Điều này đúng, nhưng điều tương tự cũng áp dụng cho ma trận xoay. Ma trận xoay không gặp sự cố khóa gimbal. Trên thực tế, nó không có ý nghĩa gì khi nói rằng. Sự cố khóa gimbal xảy ra khi bạn sử dụng Góc Euler, đơn giản là một bộ gồm 3 phép quay nguyên tố để cho phép bạn mô tả bất kỳ hướng nào trong không gian 3D. Khi xác định thái độ, chúng ta thường hình dung một vòng quay 3D là sự kết hợp giữa yaw, pitch và roll. Đây là các góc Euler nên chúng dễ gặp sự cố khóa gimbal, bất kể bạn có sử dụng quaternion hay không

Một quaternion được đặt tên như vậy vì có tổng cộng 4 thành phần. Nếu $q$ là một bậc bốn, thì

$q=q_0+q_1\tilde{\imath}+q_2\tilde{\jmath}+q_3\tilde{k}$

Bạn có thể coi bậc bốn như một phần mở rộng của số phức trong đó thay vì 1 số thực và 1 số ảo, giờ đây bạn có 1 số thực và 3 số ảo. Một cách khác để ký hiệu một quaternion là như vậy

$q=\begin{bmatrix} q_0 \\ \tilde{\textbf{q}} \end{bmatrix} = \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3\end{bmatrix}$

Tuy nhiên, lưu ý rằng một số tác giả viết phần tưởng tượng trước phần thực. Ví dụ,

$q=q_0\tilde{\imath}+q_1\tilde{\jmath}+q_2\tilde{k}+q_3 = \begin{bmatrix} \tilde{\textbf{q}} \\ q_3 \end{bmatrix}

Bạn thường có thể biết họ sử dụng thứ tự nào bằng cách kiểm tra xem biến nào được in đậm [hoặc in nghiêng]. Biến được đánh dấu thường là thành phần ảo và biến không được đánh dấu [thường có chỉ số dưới là số] là phần thực. Đối với hướng dẫn này, chúng tôi sẽ sử dụng định nghĩa đầu tiên trong đó phần thực có trước

Liên hợp của một bậc bốn, $q=q_0+q_1\tilde{\imath}+q_2\tilde{\jmath}+q_3\tilde{k}$, được định nghĩa như sau

$q^*=q_0-q_1\tilde{\imath}-q_2\tilde{\jmath}-q_3\tilde{k}$

Đừng lo nó dùng để làm gì vì bạn sẽ biết ngay dưới đây

Nerxt, độ lớn của một quaternion được xác định theo kiểu tương tự như vectơ

$. q. =q_0^2+q_1^2+q_2^2+q_3^2$

Do đó, một quaternion đơn vị có thể được định nghĩa theo cách sau

$\Large{Uq=\frac{q}{. q. }=\frac{q_0}{. q. }+\frac{q_1}{. q. }\tilde{\imath}+\frac{q_2}{. q. }\tilde{\jmath}+\frac{q_3}{. q. }\tilde{k}}$

trong đó $Uq$ đề cập đến một bậc bốn đơn vị

Tiếp tục, bên dưới là định nghĩa cho phép cộng và phép trừ bậc bốn [chỉ cần thay thế “+” bằng “-“]

$p+q=\begin{bmatrix} p_0+q_0 \\ p_1+q_1 \\ p_2+q_2 \\ p_3+q_3\end{bmatrix}$

Tiếp theo là các định nghĩa của phần ảo

$\tilde{\imath}\tilde{\imath}=\tilde{\jmath}\tilde{\jmath}=\tilde{k}\tilde{k}=-1$

$\tilde{\imath}\tilde{\jmath}=\tilde{k}=-\tilde{\jmath}\tilde{\imath}$

$\tilde{\jmath}\tilde{k}=\tilde{\imath}=-\tilde{k}\tilde{\jmath}$

$\tilde{k}\tilde{\imath}=\tilde{\jmath}=-\tilde{\imath}\tilde{k}$

Với điều này, sau đó bạn sẽ có thể thực hiện phép nhân bậc bốn. Điều này được thực hiện giống như khi bạn nhân một đại số 4 biến với một đại số 4 biến khác. Đạo hàm khá dài nên tôi sẽ bỏ qua nó và chỉ đăng câu trả lời

$q{\otimes}p=\begin{bmatrix} q_0p_0-q_1p_1-q_2p_2-q_3p_3 \\ q_1p_0+q_0p_1-q_3p_2+q_2p_3 \\ q_2p_0+q_3p_1+q_0p_2-q_1p_3 \\ q_3p_0-q_2p_1+q_1p_2+qend

Phép nhân trên cũng có thể viết dưới dạng phép nhân ma trận như sau

$q{\otimes}p=Qp=\begin{bmatrix} q_0 & -q_1 & -q_2 & -q_3 \\ q_1 & q_0 & -q_3 & q_2 \\ q_2 & q_3 & q_0 & -q_1 \\ q_3 & -

Bây giờ, cuối cùng chúng ta đã sẵn sàng nói về phép quay. Nếu chúng ta định nghĩa một quaternion theo cách sau

$q=\begin{bmatrix} cos[\theta/2] \\ \tilde{\textbf{u}}sin[\theta/2] \end{bmatrix}$

Sau đó,

$r’=q{\otimes}r{\otimes}q^*$

đề cập đến một phép quay của vectơ r, $\theta$ độ quanh vectơ $\tilde{\textbf{u}}$. $r’$ do đó là vectơ quay. Ở trên một lần nữa có thể được viết dưới dạng phép nhân ma trận thay vì phép nhân bậc bốn. Toán học tẻ nhạt nên tôi sẽ đăng lại kết quả một lần nữa. Bạn luôn có thể thử tự mình tìm ra nó nếu bạn không tin [. Dù sao, ma trận tương đương ở trên là

$r'=Cr$       ——————–     [1]

ở đâu

$C=\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-

Dạng ma trận này rất quan trọng vì nó cho phép chúng ta so sánh với ma trận xoay bắt nguồn từ Euler Angles để xác định thái độ [nghịch, cao độ, lăn] của đối tượng

Cuối cùng là phương trình thú vị nhất của tất cả

$\dot{q}=\frac{1}{2}q{\otimes}w$

ở đâu

$w=0+w_1\tilde{\imath}+w_2\tilde{\jmath}+w_3\tilde{k}$

$w_1, w_2, w_3$ lần lượt là vận tốc góc theo phương x, y và z. Phương trình này cho chúng ta một cách để sử dụng các giá trị trực tiếp từ máy đo con quay hồi chuyển của chúng ta để biến nó thành chuyển động quay. Một lần nữa, sẽ trực quan hơn khi làm việc trong ma trận, vì vậy chúng tôi sẽ chuyển đổi ở trên thành ma trận từ. Lưu ý rằng cùng một phương trình có thể được biểu thị theo 2 cách khác nhau ở đây

$\dot{q}=\frac{1}{2}S[w]q=\frac{1}{2}S[q]w$       ——————–     [2]

ở đâu

$\begin{align}S[w]&=\begin{bmatrix} 0 & -w_1 & -w_2 & -w_3 \\ w_1 & 0 & w_3 & -w_2 \\ w_2 & -w_3 & 0 & w_1 \\ w_3

Bây giờ, chúng ta hãy thử phương pháp xoay này với khối hình chữ nhật Pygame mà chúng ta đã tạo trong phần trước

Kiểm tra Phép quay Quaternion trong Pygame

Đây là mã mẫu cho phần này. Nó được xây dựng dựa trên bài viết trước nên nếu bạn đang muốn hiểu code thì nên đọc lại bài viết trước

• Python Mã xoay bậc bốn đơn giản

Mã BoardDisplay tham chiếu mã Wireframe và mã Wireframe tham chiếu mã Quaternion. Để cho Pycharm biết nơi nó có thể tìm thấy tất cả các tệp có liên quan, bạn sẽ cần đánh dấu thư mục chứa tất cả các tệp là nguồn gốc. Bạn có thể thực hiện việc này bằng cách nhấp chuột phải vào thư mục trong Pycharm, chọn “Đánh dấu thư mục là”, sau đó chọn “nguồn gốc”. Nếu bạn làm đúng, thư mục sẽ được đánh dấu màu xanh lam trong Pycharm

Mục đích của phần này là chuyển vận tốc góc thành chuyển động quay của vật. Để kiểm tra mọi thứ, chúng ta sẽ sử dụng vận tốc góc không đổi làm đầu vào và xem một vật thể sẽ quay như thế nào trong Pygame. Chúng ta có thể sử dụng phương trình [1] và [2] từ trên để đạt được điều này nhưng chúng ở dạng thời gian liên tục nên chúng ta phải rời rạc hóa nó trước

Để rời rạc hóa phương trình [2], chúng ta sẽ thực hiện phép biến đổi chuỗi Taylor bậc nhất. Điều này có thể được thực hiện khá đơn giản như được hiển thị

$\dot{q}=\frac{1}{2}S[w]q$

$[q_{k+1}-q_k] / dt=\frac{1}{2}S[w_k]q_k$

$q_{k+1}=\frac{dt}{2}S[w_k]q_k+q_k$

trong đó $dt$ là thời gian giữa mẫu $k+1$ và $k$

Điều tương tự cũng có thể được áp dụng cho dạng khác trong phương trình [2]. Làm như vậy sẽ mang lại kết quả này

$q_{k+1}=\frac{dt}{2}S[q_k]w_k+q_k$

Bạn có thể thấy rằng trong mã Quaternion, đây thực sự là phương trình mà tôi đã thực hiện trong phương thức xoay

class Quaternion:
    def __init__[self]:
        self.q = np.array[[1, 0, 0, 0]]  # Initial state of the quaternion

    def rotate[self, w, dt]:
        q = self.q
        Sq = np.array[[[-q[1], -q[2], -q[3]],
                       [q[0], -q[3], q[2]],
                       [q[3], q[0], -q[1]],
                       [-q[2], q[1], q[0]]]]
        self.q = np.matmul[dt/2 * Sq, np.array[w].transpose[]] + q

Bây giờ tôi gặp một số vấn đề trong việc hiểu $q$ thực sự đại diện cho cái gì vì đôi khi chúng có thể ở dạng vectơ [phần vô hướng là 0]. Ở đây, $q$ đại diện cho quaternion ánh xạ hướng ban đầu của đối tượng với hướng hiện tại của đối tượng. Như vậy, khi tôi khởi tạo $q$ là [1, 0, 0, 0], thực ra tôi đang ngụ ý rằng hướng bắt đầu của đối tượng là hướng của đối tượng trong Pygame. Chúng tôi thực sự không có đối tượng vật lý để kiểm tra ngay bây giờ nên nghe có vẻ hơi kỳ lạ nhưng khi chúng tôi chuyển sang phần tiếp theo, nó sẽ trở nên rõ ràng hơn. Nó chỉ có nghĩa là đối tượng trong đời thực có cùng hướng với đối tượng được hiển thị trong Pygame tại thời điểm 0

Xác định các góc Euler

Tiếp theo, chúng ta cần một cách để xác định các Góc Euler vì chúng dễ xử lý hơn. Phương trình [1] cung cấp cho chúng ta một ma trận xoay bằng cách sử dụng quaternion, nhưng chúng ta thực sự có thể xác định ma trận xoay tương tự bằng cách sử dụng các góc Euler [ngoại trừ các điểm kỳ dị gặp phải trong quá trình Khóa Gimbal]. Chúng tôi sẽ sử dụng quy ước Yaw, Pitch, Roll, vì vậy hãy lưu ý rằng nếu bạn sử dụng bất kỳ thứ tự nào khác, bạn sẽ nhận được một kết quả khác

Độ lệch dương được định nghĩa là xoay ngược chiều kim đồng hồ quanh trục z. Ma trận xoay cho góc nghiêng của $\alpha$ là

$R_z[\alpha]=\begin{bmatrix} \cos[\alpha] && -\sin[\alpha] && 0 \\ \sin[\alpha] && \cos[\alpha] && 0 \\ 0 && 0

Cao độ dương được định nghĩa là xoay ngược chiều kim đồng hồ quanh trục y. Ma trận xoay cho góc tung độ $\beta$ là

$R_y[\beta]=\begin{bmatrix} \cos[\beta] && 0 && \sin[\beta] \\ 0 && 1 && 0 \\-\sin[\beta] && 0 && \cos[\

Cuộn dương được định nghĩa là quay ngược chiều kim đồng hồ quanh trục x. Ma trận xoay cho góc cuộn của $\gamma$ là

$R_x[\gamma]=\begin{bmatrix} 1 && 0 && 0 \\ 0 && \cos[\gamma] && -\sin[\gamma] \\0 && \sin[\gamma] && \cos[\

Bất kỳ phép quay nào cũng có thể được phân tách thành 3 phép quay nguyên tố này, do đó, phép quay cuộn, nghiêng, xoay [lưu ý thứ tự quay ở đây] có thể được mô tả bằng ma trận sau

$\begin{align}R[\alpha,\beta,\gamma] &= R_z[\alpha]R_y[\beta]R_x[\gamma] \\ &= \begin{bmatrix} \cos[\alpha]\

Hãy lưu ý rằng phương trình trên chỉ áp dụng cho một vòng quay thực hiện cuộn trước, cao độ, sau đó xoay vòng cuối cùng. Nếu thứ tự bị thay đổi, bạn sẽ có một ma trận xoay khác. Bây giờ, nếu ma trận xoay bậc bốn là đúng, tất nhiên là đúng như vậy vì nó đã được rất nhiều người trên thế giới sử dụng, thì chúng ta có thể sử dụng ma trận xoay roll, pitch, yaw ở trên để lấy các góc Euler tương ứng từ bậc bốn

Từ phương trình [1], hãy cung cấp cho mỗi ô trong ma trận $C$ một chỉ số

$C=\begin{bmatrix} C_{00} && C_{01} && C_{02} \\ C_{10} && C_{11} && C_{12} \\ C_{20} && C_{21} &&

Sau đó, có thể giải quyết các góc cuộn, cao độ, nghiêng

$\begin{align}C_{20}&=-\sin[\beta] \\ \beta&=\sin^{-1}[-C_{20}]\qquad       ——————–     [3]

Do đó, cao độ được đưa ra bởi phương trình trên [3]

TRƯỜNG HỢP 1

Nếu $\cos[\beta]\neq0$ ,

$\begin{align}\frac{C_{10}}{C_{00}} &= \frac{\sin[\alpha]\cos[\beta]}{\cos[\alpha]\cos[\beta

$\begin{align}\frac{C_{21}}{C_{22}} &= \frac{\cos[\beta]\sin[\gamma]}{\cos[\beta]\cos[\gamma

Do đó, góc nghiêng được cho bởi phương trình [4] và góc cuộn được cho bởi phương trình [5]

$\begin{align}yaw&=\tan^{-1}\left[\frac{C_{10}}{C_{00}}\right] \\ pitch &=\sin^{-1}[-C_

Tuy nhiên, nếu $\cos[\beta]=0$ thì xảy ra sự cố khóa Gimbal. Với hệ tọa độ của chúng tôi, điều này xảy ra khi góc nghiêng là 90 hoặc -90 độ.

trường hợp 2

Đối với trường hợp góc nghiêng là 90 độ, ma trận xoay đơn giản hóa thành

$R[\alpha,\beta,\gamma] = \begin{bmatrix} 0 && \cos[\alpha]\sin[\gamma]-\sin[\alpha]\cos[\gamma] && \cos[\

Áp dụng một số danh tính lượng giác và chúng ta có thể nhận được những điều sau đây

$R[\alpha,\beta,\gamma] = \begin{bmatrix} 0 && \sin[\gamma-\alpha] && \cos[\alpha-\gamma] \\ 0 &&\cos[\alpha-\

Trong tình huống này, cả yaw và roll đều đề cập đến cùng một chuyển động quay, vì vậy chúng tôi thường chỉ định một trong số đó là 0 và tính toán cái còn lại. Đối với tôi, tôi đã chỉ định yaw là 0 và tính toán cuộn nhưng bạn luôn có thể làm theo cách khác. Vì vậy,

$R[\alpha,\beta,\gamma] = \begin{bmatrix} 0 && \sin[\gamma] && \cos[-\gamma] \\ 0 &&\cos[-\gamma] && \sin[-

Bây giờ chúng ta có thể xác định góc cuộn với

$roll = \tan^{-1}\left[\frac{C_{01}}{C_{02}}\right]$

Nếu bạn hỏi tại sao tôi lại sử dụng $C_{01} và C_{02}$ khi có thể nhận được câu trả lời chỉ với một ô bất kỳ, thì đó là vì tôi muốn dựa vào hàm arctan2 để tự động tìm

TRƯỜNG HỢP 3

Đối với trường hợp góc nghiêng -90 độ, ma trận xoay đơn giản hóa thành

$R[\alpha,\beta,\gamma] = \begin{bmatrix} 0 && -\cos[\alpha]\sin[\gamma]-\sin[\alpha]\cos[\gamma] && -\cos

Một lần nữa, đơn giản hóa với các đẳng thức lượng giác,

$R[\alpha,\beta,\gamma] = \begin{bmatrix} 0 && -\sin[\alpha+\gamma] && -\cos[\alpha+\gamma] \\ 0 && -\cos[\alpha+\

Tôi đã chỉ định yaw bằng 0 để phương trình trên đơn giản hóa hơn nữa thành

$R[\alpha,\beta,\gamma] = \begin{bmatrix} 0 && -\sin[\gamma] && -\cos[\gamma] \\ 0 && -\cos[\gamma] &&-\sin

Bây giờ chúng ta có thể xác định góc cuộn với

$roll = \tan^{-1}\left[\frac{-C_{01}}{-C_{02}}\right]$

Và cuối cùng chúng ta đã hoàn thành. Tất cả những điều trên có thể được viết bằng mã python như sau

def getRotMat[q]:
    c00 = q[0] ** 2 + q[1] ** 2 - q[2] ** 2 - q[3] ** 2
    c01 = 2 * [q[1] * q[2] - q[0] * q[3]]
    c02 = 2 * [q[1] * q[3] + q[0] * q[2]]
    c10 = 2 * [q[1] * q[2] + q[0] * q[3]]
    c11 = q[0] ** 2 - q[1] ** 2 + q[2] ** 2 - q[3] ** 2
    c12 = 2 * [q[2] * q[3] - q[0] * q[1]]
    c20 = 2 * [q[1] * q[3] - q[0] * q[2]]
    c21 = 2 * [q[2] * q[3] + q[0] * q[1]]
    c22 = q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2

    rotMat = np.array[[[c00, c01, c02], [c10, c11, c12], [c20, c21, c22]]]
    return rotMat

def getEulerAngles[q]:
    m = getRotMat[q]
    test = -m[2, 0]
    if test > 0.99999:
        yaw = 0
        pitch = np.pi / 2
        roll = np.arctan2[m[0, 1], m[0, 2]]
    elif test < -0.99999:
        yaw = 0
        pitch = -np.pi / 2
        roll = np.arctan2[-m[0, 1], -m[0, 2]]
    else:
        yaw = np.arctan2[m[1, 0], m[0, 0]]
        pitch = np.arcsin[-m[2, 0]]
        roll = np.arctan2[m[2, 1], m[2, 2]]

    yaw = rad2deg[yaw]
    pitch = rad2deg[pitch]
    roll = rad2deg[roll]

    return yaw, pitch, roll

Được rồi, những gì còn lại là giao diện giữa lớp Quaternion và mã thực tế, điều này không thú vị lắm nên tôi sẽ bỏ qua nó. Nếu bạn đang thắc mắc về phương thức convertToComputerFrame, thì nó thực sự dành cho phần tiếp theo và tôi chỉ lười xóa nó. Về cơ bản, trục của cảm biến vật lý nằm ở hướng khác với trục trong Pygame nên cần phải chuyển đổi sao cho chuyển động quay của cảm biến có thể được phản ánh chính xác trong Pygame. Nếu bạn có bất kỳ câu hỏi nào khác, vui lòng viết trong phần bình luận bên dưới

Dưới đây là cách chương trình sẽ trông như thế nào khi nó chạy

Thực hiện các phép quay với dữ liệu con quay hồi chuyển MEMS

Bây giờ chúng tôi cuối cùng đã sẵn sàng để tương tác với cảm biến Con quay hồi chuyển của chúng tôi. Tôi đã sử dụng cảm biến Pololu MinIMU-9 v3 cho con quay hồi chuyển [đã ngừng sản xuất] và Arduino UNO để đọc dữ liệu từ cảm biến và truyền dữ liệu đó vào máy tính. Đây là mã nguồn cho các thành phần có liên quan

• Mã Arduino
• Mã Python

Thư mục “AccelGyro” trong Mã Arduino thực chất là một thư viện cho cảm biến pololu MinIMU-9 v3, vì vậy bạn cần đặt nó trong thư mục thư viện của Arduino để nó hoạt động. Nếu bạn có cùng một mô hình, bạn có thể sử dụng mã như hiện tại. Nếu cảm biến của bạn thuộc kiểu máy khác hoặc bạn đang sử dụng một loại vi điều khiển khác, tất cả những gì bạn phải làm là tuân theo định dạng mà dữ liệu được gửi đến máy tính và Mã Python vẫn hoạt động tốt. Đây là định dạng mà tôi đã sử dụng

sendToPC[&gyroData.X, &gyroData.Y, &gyroData.Z, 
         &accelData.X, &accelData.Y, &accelData.Z,
         &magData.X, &magData.Y, &magData.Z];

-------------------------------------------------------------

void sendToPC[int* data1, int* data2, int* data3, 
              int* data4, int* data5, int* data6, 
              int* data7, int* data8, int* data9]
{
  byte* byteData1 = [byte*][data1];
  byte* byteData2 = [byte*][data2];
  byte* byteData3 = [byte*][data3];
  byte* byteData4 = [byte*][data4];
  byte* byteData5 = [byte*][data5];
  byte* byteData6 = [byte*][data6];
  byte* byteData7 = [byte*][data7];
  byte* byteData8 = [byte*][data8];
  byte* byteData9 = [byte*][data9];
  byte buf[18] = {byteData1[0], byteData1[1],
                 byteData2[0], byteData2[1],
                 byteData3[0], byteData3[1],
                 byteData4[0], byteData4[1],
                 byteData5[0], byteData5[1],
                 byteData6[0], byteData6[1],
                 byteData7[0], byteData7[1],
                 byteData8[0], byteData8[1],
                 byteData9[0], byteData9[1]};
  Serial.write[buf, 18];
}

Về cơ bản, dữ liệu được gửi theo thứ tự sau. gyro_X, gyro_Y, gyro_Z, accelerometer_X, accelerometer_Y, accelerometer_Z, magnetometer_X, magnetometer_Y, magnetometer_Z và mỗi biến là một số nguyên 2 byte

Đối với Mã Python, bạn phải đánh dấu thư mục chứa tất cả các tệp là “Nguồn gốc” để IDE biết tìm tệp ở đâu. Bạn có thể thực hiện việc này bằng cách nhấp chuột phải vào thư mục trong Pycharm, chọn “Đánh dấu thư mục là”, sau đó chọn “nguồn gốc”. Nếu bạn làm đúng, thư mục sẽ được đánh dấu màu xanh lam trong Pycharm. readSensor_naive là bản chuyển thể từ một trong những dự án trước đây của tôi, vì vậy nếu bạn muốn hiểu nó, vui lòng xem bài đăng này. Tôi đã thay đổi một số tham số [và cả tên tham số nữa] nhưng nó vẫn gần giống nhau. Nếu bạn có bất kỳ câu hỏi nào về nó, đừng ngần ngại viết trong phần bình luận bên dưới

Tuy nhiên, một điểm cuối cùng, trong phương thức getSerialData cho mã python, bạn sẽ thấy một số hằng số ngẫu nhiên như từ phần trích xuất bên dưới

def getSerialData[self]:
    privateData = self.rawData[:]
    for i in range[self.numParams]:
        data = privateData[[i*self.dataNumBytes]:[self.dataNumBytes + i*self.dataNumBytes]]
        value,  = struct.unpack[self.dataType, data]
        if i == 0:
            value = [[value * 0.00875] - 0.464874541896] / 180.0 * np.pi
        elif i == 1:
            value = [[value * 0.00875] - 9.04805461852] / 180.0 * np.pi
        elif i == 2:
            value = [[value * 0.00875] + 0.23642053973] / 180.0 * np.pi
        elif i == 3:
            value = [value * 0.061] - 48.9882695319
        elif i == 4:
            value = [value * 0.061] - 58.9882695319
        elif i == 5:
            value = [value * 0.061] - 75.9732905214
        elif i == 6:
            value = value * 0.080
        elif i == 7:
            value = value * 0.080
        elif i == 8:
            value = value * 0.080
        self.data[i] = value
    return self.data

Đây thực sự là giá trị sai lệch của cảm biến mà tôi đã xác định được thông qua việc thu thập dữ liệu khi cảm biến đứng yên. Nó rất có thể sẽ khác với cảm biến của bạn, vì vậy hãy chắc chắn hiệu chỉnh nó trước khi sử dụng. Nếu bạn xem kỹ readSensor_naive. py, thực sự có mã để bạn xuất kết quả đọc cảm biến sang CSV. Bạn có thể sử dụng chúng để lấy danh sách các giá trị của đầu ra cảm biến để xác định giá trị sai lệch của nó

Ngoài ra, lý do tại sao tôi dán nhãn mã là "ngây thơ" là vì nó không tính đến tiếng ồn của con quay hồi chuyển. Điều này làm cho hướng được tính toán dần dần biến mất mà không có phương tiện xác định hướng thực sự. Sẽ dễ hiểu hơn nếu bạn xem video demo bên dưới