Pengantar Teknik Kontrol: Pengamat Keadaan dan Filter Kalman Linier

Contents — find the section you need

Robot jarang membaca status yang dibutuhkannya secara langsung. Encoder mungkin menampilkan posisi tetapi bukan kecepatan; IMU melaporkan percepatan dan laju sudut dengan bias; lokalisasi kamera datang terlambat atau menghilang. Pengamat status menggunakan model plant dan selisih antara output yang diprediksi dan yang diukur untuk memperkirakan status tersembunyi. Filter Kalman linier (KF) adalah pengamat standar yang membawa kovariansi kesalahan dan menggunakan asumsi noise proses dan pengukuran untuk memutuskan seberapa besar kepercayaan pada prediksi versus pengukuran.

Ide kuncinya bukanlah "sensor rata-rata." Yang penting adalah menyatakan waktu fisik mana yang diwakili oleh suatu data, model kesalahan apa yang diasumsikan, dan apa yang terjadi ketika sensor tidak sepakat. Estimator yang secara yakin salah akan mengarahkan PID, LQR, dan MPC ke arah yang salah. Untuk batasan waktu dan eksekusi ROS 2, baca ROS 2 Primer; untuk integrasi sensor yang lebih luas, baca Sensor Fusion Primer.

Kesimpulan Praktis

Pengamat memperbarui $\hat{x}$ dari prediksi model dan inovasi. Keadaan yang tidak dapat diamati tidak dapat direkonstruksi secara unik hanya dengan memilih filter yang lebih cerdas.
KF membawa kovariansi kesalahan keadaan $P$ dan menggunakan kovariansi proses $Q$ dan kovariansi pengukuran $R$ untuk menghitung penguatan Kalman $K$ .
$Q,R$ adalah asumsi tentang kesalahan model dan sensor, bukan "tombol akurasi" yang sembarangan. Validasi asumsi tersebut dengan inovasi, kovariansi, stempel waktu, dan data kesalahan.
Jika estimasi sudah usang, kovariansi berlebihan, inovasi anomali, atau transformasi gagal, berikan keadaan yang terdegradasi ke kontrol dan kurangi atau hentikan gerakan sesuai dengan bahaya.

Pengamat ruang keadaan dan keterobservasian

Untuk sistem linier diskrit,

x_{k+1}=A_dx_k+B_du_k+w_k,\qquad z_k=Cx_k+v_k

$x$ adalah keadaan sebenarnya, $u$ adalah input yang diketahui, $z$ adalah pengukuran, $w$ adalah gangguan proses, dan $v$ adalah derau pengukuran. Pengamat Luenberger memiliki bentuk

\hat{x}_{k+1}=A_d\hat{x}_k+B_du_k+L(z_k-C\hat{x}_k)

Istilah terakhir adalah inovasi. Gain pengamat yang tinggi $L$ mengikuti pengukuran dengan cepat tetapi mengirimkan lebih banyak derau; $L$ yang rendah mempercayai model lebih lama tetapi mengoreksi gangguan dengan lambat.

Diagram: Duskcoil, konseptual dan bukan terukur. Estimasi dan kovariansi adalah kuantitas model internal, bukan pengamatan sensor langsung.

Matriks observabilitas $\mathcal O=[C^T,(CA)^T,\ldots]^T$ harus memiliki rank penuh untuk merekonstruksi setiap keadaan dalam kasus linier ideal. Pengukuran posisi saja dari integrator ganda dapat membuat kecepatan dapat diamati melalui riwayat waktu, tetapi laju sampling, kuantisasi, penundaan, dan dropout menentukan apakah estimasi yang dihasilkan bermanfaat. “Teramati” bukanlah pengganti persyaratan latensi dan akurasi maksimum.

Prediksi dan pembaruan Kalman

Dengan asumsi Gaussian putih rata-rata nol $w\sim\mathcal N(0,Q)$ dan $v\sim\mathcal N(0,R)$ , priornya adalah

\hat{x}_k^-=A_d\hat{x}_{k-1}^++B_du_{k-1},\qquad P_k^-=A_dP_{k-1}^+A_d^T+Q

$P$ adalah ketidakpastian kesalahan estimasi, bukan sekadar varians keadaan. Selama gangguan pengukuran, propagasi model biasanya membuat $P^-$ tumbuh melalui $Q$ .

Untuk pengukuran yang tiba, hitung inovasi $\nu_k=z_k-C\hat{x}_k^-$ dan kovariansinya

S_k=CP_k^-C^T+R

lalu perbarui dengan

K_k=P_k^-C^TS_k^{-1},\qquad \hat{x}_k^+=\hat{x}_k^-+K_k\nu_k,

P_k^+=(I-K_kC)P_k^-

Presisi terbatas dapat merusak simetri atau semidefinit positif dari $P$ . Bentuk Joseph, $P^+=(I-KC)P^-(I-KC)^T+KRK^T$ , seringkali lebih disukai; menyelesaikan sistem linier terfaktorisasi seringkali lebih aman daripada secara eksplisit membentuk invers. Matriks kovarians $P$ harus diperiksa setelah setiap pembaruan.

Memilih Q dan R

$R$ harus mencerminkan kesalahan terukur dalam kondisi getaran, suhu, pencahayaan, jangkauan, percobaan ulang paket, dan kuantisasi yang sebenarnya—bukan hanya angka akurasi katalog. $Q$ mewakili seberapa besar model dapat salah dalam satu periode: percepatan yang tidak diketahui, bias random walk, perubahan muatan, atau dinamika yang tidak dimodelkan. $Q$ yang kecil terlalu mempercayai model; $Q$ yang berlebihan mengikuti pengukuran yang bising.

Contoh numerik: memperkirakan kecepatan dari pengukuran posisi

Untuk keadaan kecepatan konstan satu dimensi $x=[p\ v]^T$ dengan periode sampel $T$ ,

A_d=\begin{bmatrix}1&T\\0&1\end{bmatrix},\quad B_d=\begin{bmatrix}T^2/2\\T\end{bmatrix},\quad C=\begin{bmatrix}1&0\end{bmatrix}

Hanya posisi yang diukur. Sebagai pembaruan konkret tanpa satuan, misalkan

T=1

\hat{x}^-=[0\ 0]^T

P^-=[[2,1],[1,1]]

z=1

R=1

, dan

C=[1\ 0]

. Maka

S=3

K=[2/3\ 1/3]^T

, dan

\hat{x}^+=[2/3\ 1/3]^T

; kovariansi posteriornya adalah

P^+=(I-KC)P^-=[[2/3,1/3],[1/3,2/3]]

. Ini mengilustrasikan satu pembaruan dan bukan klaim akurasi sensor. Nilai awal

P_{vv}

yang besar menunjukkan ketidakpastian tentang kecepatan. Inovasi posisi berulang menyimpulkan perkiraan kecepatan

\hat v

; penurunan jumlah data membuat prediksi tetap berjalan tetapi meningkatkan ketidakpastiannya. Pengontrol yang membutuhkan kecepatan harus memeriksa

P_{vv}

, usia pengukuran, dan status residual, kemudian memperlambat atau berhenti ketika melebihi nilai yang ditentukan. amplop.

Penambahan IMU membantu melacak percepatan jangka pendek, tetapi menghilangkan bias dari keadaan memungkinkan kesalahan integrasi terakumulasi. Pengamatan kamera atau LiDAR dapat membatasi posisi absolut ketika dikaitkan dengan peta, penanda, atau referensi lain yang diketahui; pelacakan relatif saja tidak menjamin posisi global. GNSS juga dikondisikan oleh visibilitas satelit, multipath, laju pembaruan, latensi, dan penyelarasan bingkai. Untuk pengamatan sikap atau jarak nonlinier, EKF atau UKF dapat digunakan, tetapi kekhawatiran KF linier tentang $Q,R$ , kovariansi pengukuran $R$ , waktu, outlier, dan observabilitas tetap ada.

Inovasi kuadrat yang dinormalisasi $\nu^TS^{-1}\nu$ membantu mendeteksi pengukuran yang tidak konsisten dengan distribusi yang diprediksi. Nilai yang besar mungkin berarti outlier, bingkai yang salah, kesalahan stempel waktu, kesalahan sensor, atau ketidaksesuaian model. Pembatasan datum bukanlah alat untuk menyembunyikan kesalahan: itu harus memberikan diagnosis dan mode degradasi yang aman.

IMU/INS representatif

Kamera kedalaman representatif

Gambar: Xsens MTi-G (Kallap85, CC BY-SA 4.0) dan Intel RealSense D435 (Marc Auledas, CC BY-SA 4.0), Wikimedia Commons. Ini adalah sensor representatif, bukan bukti terukur atau kombinasi yang direkomendasikan sistem.

Menyediakan PID, LQR, dan MPC

PID dapat menggunakan umpan balik terfilter mentah, tetapi suku turunannya dan loop kecepatan internal seringkali mendapat manfaat dari estimasi. LQR mengasumsikan multistate $\hat{x}$ ; di bawah kondisi prinsip pemisahan, gain LQR dan estimator Kalman membentuk arsitektur LQG. Saturasi, penundaan, dan nonlinieritas masih dapat menghancurkan ketahanan dunia nyata. MPC juga bergantung pada estimasi keadaan awal dan gangguan yang tepat waktu; pengoptimalan dari estimasi usang atau kovariansi yang salah kecil menciptakan rencana yang sempurna untuk dunia yang salah.

Tujuan	Kontrol tipikal	Output estimator yang dibutuhkan	Respons kegagalan
Loop kecepatan/suhu tunggal	PID	pengukuran terfilter / estimasi kecepatan	hentikan integral pada kehilangan sensor
Stabilisasi multistate lokal	LQR/LQG	tersinkronisasi $\hat{x}$ dan validitas	jangan terlalu mempercayai kovariansi yang berkembang
Jalur/alokasi terbatas	MPC	keadaan, penundaan, gangguan, kepercayaan	perlambat atau berhenti pada keadaan usang/kesalahan solver

Pengaturan waktu robot dan operasi aman kesalahan

Di ROS 2, bedakan waktu pengukuran dari waktu kedatangan pesan. Memperbarui data saat ini dengan data kamera yang terlambat seolah-olah itu adalah data saat ini akan menarik robot ke masa lalunya. Pilih dan dokumentasikan kebijakan: putar ulang/sebarkan kembali riwayat, tolak data yang tertunda, atau pisahkan odometri tingkat rendah dari koreksi peta yang lebih lambat. Transformasi transport dan koordinat ROS 2 yang dijelaskan dalam ROS 2 Primer dan Sensor Fusion Primer adalah prasyarat di luar persamaan filter.

Batas waktu monitor, batas kovariansi, kegagalan gerbang inovasi berulang, bias yang menyimpang, Transformasi yang tidak tersedia, dan keterlambatan tenggat waktu. Jika semua pengukuran hilang, jangan jalankan tanpa batas hanya berdasarkan prediksi. Pilih pembatasan kecepatan, penghentian, redundansi, atau serah terima operator berdasarkan aplikasi. Reset estimator dapat melompati status, sehingga referensi pengontrol, status integral, dan output perlu ditangani tanpa gangguan.

Daftar Periksa Implementasi

Definisikan status, pengukuran, input, frame, waktu pengukuran, dan waktu penerimaan.
Kuantifikasi observabilitas ditambah akurasi yang dibutuhkan dan latensi maksimum untuk setiap status.
Catat bukti untuk $Q,R,P_0$ dari spesifikasi, data, dan pengujian.
Pantau simetri/kepastian $P$ , inovasi, gerbang, dan waktu habis.
Suntikkan kasus dropout, outlier, relokalisasi, dan perubahan bias.
Definisikan apa yang diterima PID/LQR/MPC ketika kualitas estimasi menurun dan verifikasi tindakan keselamatan independen.

Referensi

[Kalman, 1960: A Pendekatan Baru untuk Masalah Penyaringan dan Prediksi Linier
MIT Underactuated Robotics: LQR
Pengantar ROS 2, Pengantar Fusi Sensor

Periksa pemahaman Anda

Apa yang terjadi ketika suatu pengukuran lebih dipercaya?

Dengan ketidakpastian prediksi yang sama, kovariansi pengukuran yang lebih rendah menggeser pembaruan ke arah pengukuran. Kepercayaan yang tidak realistis dapat membuat estimasi mengikuti noise.

Try it in the browser

Lab Change the conditions in a browser experimentMenjalankan filter Kalman di Python: noise, data yang hilang, dan outlier