Tôi phải biết một cánh quạt đa hướng ở đâu, trong một căn phòng hình chữ nhật, thông qua 6 tia laser, 2 trên mỗi trục.

Vấn đề là như thế này:

Đầu vào:

• Phòng: vuông => 10 mét x 10 mét
• 6 vị trí của laser: Cố định trên khung
• 6 hướng của laser: Cố định trên khung
• 6 phép đo của laser
• Đệ tứ từ IMU của bộ điều khiển chuyến bay của tôi (PixHawk).
• Nguồn gốc được tập trung vào tâm trọng lực của đa rôto và được định nghĩa như thể các bức tường vuông góc với mỗi trục (bình thường của tường trong X là (-1,0,0))

Đầu ra:

• Vị trí trong 3D (X, Y, Z)
• Vị trí góc (tứ)

Vì tôi có được vị trí góc của đa rôto, tôi xoay các vị trí và hướng laser qua tứ phân vị, sau đó ngoại suy qua 6 phép đo và tôi có được 3 bức tường. (định hướng của các bức tường là tầm thường, sau đó chỉ một điểm là đủ để xác định vị trí của nó.

Thật tệ, tôi nhận thấy rằng phép đo ngáp (xoay quanh z) từ PixHawk là không đáng tin cậy . Sau đó, tôi nên đo ngáp từ các tia laser, nhưng tôi không thành công để làm điều đó. Sự kiện nếu vấn đề 2D dễ dàng, tôi bị mất trong 3D.

Có ai biết nếu [ Thuật toán để biết vị trí XYZ và bậc bốn từ 6 phép đo ] tồn tại ở đâu đó không? Hoặc cách đúng đắn để đi vào vấn đề này là gì?

Câu hỏi: Làm thế nào tôi có thể có được cái ngáp từ 2 lần đo từ 2 tia laser mà tôi biết vị trí ban đầu, hướng và độ cao và cuộn.

LƯU Ý: Con trỏ màu xanh lá cây là vị trí gốc, Con trỏ màu đỏ là vị trí "cuối cùng", nhưng có thể được xoay quanh vòng tròn màu đỏ (do ngáp).

Giải pháp: Có giải pháp nào khác mà không cần vẽ vectơ không?

Cuối cùng tôi đã có một giải pháp, và đây là nó.

Python, thư viện hình học ROS , numpy

Tóm tắt mã / toán thực tế của tôi:

1) Xoay vị trí và hướng của laser bằng cách cuộn & cao độ. Các axes='sxyz'phương tiện: Trục tĩnh, áp dụng cuộn, cao độ, ngáp.

quaternion_matrix tạo ra một ma trận biến đổi 4 x 4 từ bậc bốn.

laser = (1,1,1,0) # laser position
orientation = (1,0,0,0) # laser orientation

roll, pitch, _ = list(euler_from_quaternion(q, axes='sxyz'))
q = quaternion_from_euler(roll, pitch, 0, axes="sxyz")
laser = numpy.dot(quaternion_matrix(q), laser)
orientation = numpy.dot(quaternion_matrix(q), orientation)

2) Giải pháp đại số: Xoay quanh Z trong chức năng của ngáp

laser       = [-sin(a)*laser[1] + cos(t)*laser[0], 
                cos(t)*laser[1] + sin(t)*laser[0],
                laser[2]]

orientation = [-sin(a)*orientation[1] + cos(t)*orientation[0], 
                cos(t)*orientation[1] + sin(t)*orientation[0],
                orientation[2]]

3) Giải pháp đại số: Phép ngoại suy từ các phép đo trong chức năng của ngáp

Lưu ý quan trọng: Vì phép quay không chia tỷ lệ vectơ, mẫu số của yếu tố K là một hằng số. Sau đó, chúng ta có thể đơn giản hóa nó bằng cách tính trước độ dài của vectơ định hướng.

M = 100 # distance
K = sqrt(M^2 / (orientation[0]^2 + orientation[01]^2 + orientation[1]^2))
PointOnWall = [ K * orientation[0] + laser[0],
                K * orientation[1] + laser[1],
                K * orientation[2] + laser[2]]

4) Giải pháp Algebric: Từ đây, trên hai tia laser, có được các bức tường.

Hai phương trình "PointOnWall" sẽ cung cấp đủ dữ liệu để có được ngáp. Biết đây là một định mức (-1,0,0), tôi có thể tìm thấy 2 mặt phẳng từ hai điểm:

5) Giải pháp đại số: Đo YAW.

Một mặt phẳng khác (Via XMaxima), chúng tôi đã nhận được:

def getYaw(position1, orientation1, measure1, position2, orientation2, measure2):
    length1 = length(orientation1)
    length2 = length(orientation2)
    k1 = measure1/length1
    k2 = measure2/length2
    numerator   = -k2*orientation2[0] + k1*orientation1[0] + position1[0] - position2[0]
    denominator = -k2*orientation2[1] + k1*orientation1[1] + position1[1] - position2[1]
    return atan(numerator/denominator)

Như mong đợi, cuộn và cao độ KHÔNG can thiệp, vì các vị trí và hướng được định hướng.