Saya sedang mengerjakan quadrotor. Saya tahu posisinya - , ke mana saya ingin pergi - posisi target , dan dari sana saya menghitung vektor - vektor satuan yang akan membawa saya ke target saya:b $a$$b$$c$

c = b - a
c = normalize(c)

Karena quadrotor dapat bergerak ke segala arah tanpa rotasi, apa yang saya coba lakukan adalah

1. putar oleh sudut robot yaw$c$
2. membagi menjadi yang komponen$x, y$
3. meneruskannya ke robot sebagai sudut roll and pitch.

Masalahnya adalah bahwa jika menguap adalah 0 ° ± 5 maka ini bekerja, tetapi jika menguap dekat +90 atau -90 gagal dan mengarahkan ke arah yang salah. Pertanyaan saya, apakah saya kehilangan sesuatu yang jelas di sini?

Menerapkan kembali solusi Anda, saya mengerti:

Sudut Antara Vektor

Pertama, Anda ingin sudut antara titik dan - tidak secara khusus vektor satuan. $A$$B$

( via Pemrograman Fx ): $\theta = math.atan2(B_{x}-A_{x}, B_{y}-A_{y})$

Sudut Yaw Kendaraan

Selanjutnya (dan saya menduga ini adalah masalah Anda), Anda perlu mengurangi sudut yaw kendaraan dari dihitung . $\psi$$\theta$

Tajuk vs Yaw

Jika Anda menggunakan kompas untuk "yaw angle" kendaraan Anda, ini juga bisa menjadi kesalahan Anda; heading dan yaw tidak sama . Judul kompas adalah nol di sepanjang sumbu positif , meningkat saat berputar searah jarum jam :$y$

Yaw adalah nol di sepanjang sumbu positif , meningkat saat berputar berlawanan arah jarum jam :$x$

90 derajat tumpang tindih antara pengukuran ini, dikombinasikan dengan menambahkan (bukannya mengurangi) menguap kendaraan dari menguap yang diinginkan, mungkin mengapa hal-hal bekerja ketika target Anda berada di dalam ± 5 ° dan berperilaku buruk di ± 90 °.

Konversi ke Komponen X dan Y

Dari sana, Anda mengatakan bahwa Anda mengubah hasil ini menjadi komponen dan , meneruskannya ke robot sebagai sudut roll dan pitch. Dengan koreksi di atas, Anda harus mendapatkan hasil yang diinginkan pada saat ini. Namun, memetakan komponen-komponen ini secara langsung ke sudut kemiringan mungkin menjadi masalah karena Anda hanya mempertimbangkan perbedaan posisi, dan bukan kecepatan (sebenarnya, momentum) kendaraan.$(\theta-\psi)$$x$$y$

Kontrol PID

Anda mungkin paling baik dilayani dengan menggunakan loop kontrol PID untuk roll dan pitch kendaraan. Yaitu, setelah Anda memperbaiki kode Anda dan dapat mencapai target Anda, tebakan saya adalah Anda akan mulai melakukan overshooting - berosilasi bolak-balik. PID yang disetel dengan benar akan mencegah hal itu terjadi sementara masih memungkinkan Anda untuk mendekati target dengan cepat.

Alih-alih memasukkan dan ke roll and pitch, anggap itu sebagai nilai kesalahan yang diterima roll dan pitch PID sebagai input.$x$$y$

Saya akan menganggap Anda sedang berbicara tentang vektor 3D di sini. Bisakah Anda menyamaratakan normalize()seperti itu? Apakah itu biasa (saya belum pernah melihatnya jadi jika itu, maka berita kepada saya). Kalau tidak, masalah bungkus kompas jelas berlaku untuk masing-masing komponen X dan Y. Mengapa tidak menyebutnya roll dan / atau pitch dan / atau yaw? (Mencampur nomenklatur 3D dan 2D membingungkan pertanyaan).

Normalisasi 2D saya terlihat seperti ini;

int Pilot_QuickestTurnTo(int hdgNow, int hdgNew)
{
    hdgNow = Pilot_Hdg360(hdgNow);
    hdgNew = Pilot_Hdg360(hdgNew);
    if (hdgNow < hdgNew)
        hdgNow += 360;
    int left = hdgNow - hdgNew;
        return (left < 181 ? -left : 360 - left);
}

Jika memang quad, saya menganggap komponen X dan Y Anda benar-benar YAW, Altitude ((X, Y) & Z). Anda harus menangani YAW(X, Y)dalam 2D, dan cukup menjatuhkan atau mendapatkan ketinggian untuk Z (dan sekali lagi itu sebabnya saya curiga menormalkan lebih dari yang Anda miliki sebagai).