欧美成人午夜精品久久久,国产?V天堂一区二区三区,欧美精品va在线观看,亚洲一区二区三区免费在线观看,av无码精品一区二区久久,欧美性爱视频不卡一区三区,欧美乱人伦视频在线观看,国产一级牲交高潮

ARTICLE DETAIL

資訊詳情

深耕商務(wù)建站與企業(yè)官網(wǎng)運(yùn)營的一線實(shí)戰(zhàn)洞察。

EKF-SLAM不一致性根源與可觀測性約束改進(jìn)實(shí)踐

EKF-SLAM不一致性根源與可觀測性約束改進(jìn)實(shí)踐 做機(jī)器人定位與建圖的同學(xué)遲早會遇到一個很詭異的現(xiàn)象你用擴(kuò)展卡爾曼濾波器EKF跑同時定位與地圖構(gòu)建SLAM傳感器噪聲參數(shù)明明設(shè)置得很合理仿真跑了幾圈回頭看NEES指標(biāo)發(fā)現(xiàn)真實(shí)估計(jì)誤差比濾波器預(yù)測的方差大了好幾倍。這個現(xiàn)象叫不一致性本質(zhì)上是EKF在非線性系統(tǒng)中丟失了可觀測性約束。我在處理多個定位項(xiàng)目時被這個問題反復(fù)折磨過今天就從可觀測性的角度把這件事徹底拆開講清楚包含完整的Matlab實(shí)現(xiàn)思路和可以抄作業(yè)的代碼框架。這篇文章適合兩類人看一類是正在用EKF做SLAM、發(fā)現(xiàn)結(jié)果“看著還行但協(xié)方差不靠譜”的研究生或工程師另一類是準(zhǔn)備做狀態(tài)估計(jì)方向算法研究、想理解FEJ和可觀測性約束這類進(jìn)階概念的同學(xué)。我會先講清楚不一致性從哪來再給出一套能在Matlab里復(fù)現(xiàn)問題并驗(yàn)證改進(jìn)方案的完整流程最后把我踩過的坑整理成速查表盡量讓你繞開這些彎路。1. 揭開EKF-SLAM不一致性的面紗1.1 不一致性到底是什么先說結(jié)論一個濾波器被稱為“一致”的指的是它輸出的協(xié)方差矩陣能夠真實(shí)反映估計(jì)誤差的統(tǒng)計(jì)特性。換句話說協(xié)方差說這個誤差是±0.1米那么大量實(shí)驗(yàn)后你的真實(shí)誤差標(biāo)準(zhǔn)偏差就應(yīng)該接近0.1米。如果協(xié)方差說誤差很小實(shí)際誤差卻大得多那就是不一致也叫“過于自信”或“樂觀估計(jì)”。在EKF-SLAM里這個問題表現(xiàn)得非常突出。一個2D平面中帶N個路標(biāo)的SLAM系統(tǒng)狀態(tài)維度是32N機(jī)器人位姿3維每個路標(biāo)位置2維但理論可觀測維度只有2N。也就是說全局位置平移和全局旋轉(zhuǎn)這3個方向本質(zhì)上是不可以被觀測的。無論你裝了多少路標(biāo)、觀測了多少次機(jī)器人的絕對位置都無法被唯一確定因?yàn)檎麄€地圖平移旋轉(zhuǎn)一下觀測值完全不變。標(biāo)準(zhǔn)EKF的問題在于它在每一步都把非線性系統(tǒng)在當(dāng)前估計(jì)點(diǎn)做線性化然后用線性卡爾曼更新的方式處理。線性化點(diǎn)的誤差會破壞雅可比矩陣的結(jié)構(gòu)使得濾波器在那些“理論上不該獲得信息”的全局方向上產(chǎn)生了虛假的信息增益。結(jié)果就是協(xié)方差被持續(xù)壓縮系統(tǒng)對自己的絕對位姿越來越自信但真實(shí)誤差根本沒有變小。1.2 可觀測性約束被破壞的全過程為了把這個過程看得更具體想象你沿著一條環(huán)形路徑走路上不斷看到周圍的幾個路標(biāo)。理想的SLAM應(yīng)該做到你對路標(biāo)之間的相對位置、機(jī)器人與它們的相對幾何關(guān)系越來越確定但你對整個地圖在全局坐標(biāo)系中的絕對位置仍然一無所知。但標(biāo)準(zhǔn)EKF不是這么干的。當(dāng)機(jī)器人發(fā)生旋轉(zhuǎn)時狀態(tài)估計(jì)中的航向角誤差會讓觀測雅可比矩陣發(fā)生扭曲。原本應(yīng)該在可觀測性矩陣零空間里的方向被線性化雅可比矩陣投影到了非零空間。每一次更新后濾波器都“學(xué)到”了一點(diǎn)點(diǎn)關(guān)于絕對位置的信息盡管這些信息完全是虛假的。多次循環(huán)下來協(xié)方差矩陣會變得越來越小但真實(shí)誤差并不會按照這個速度收斂于是NEES指標(biāo)開始飆升。我剛開始跑仿真的時候看到協(xié)方差橢圓縮得很漂亮以為定位精度很高結(jié)果一對比真實(shí)軌跡差點(diǎn)沒嚇壞。協(xié)方差說位置誤差標(biāo)準(zhǔn)偏差只有0.05米實(shí)際卻已經(jīng)飄了0.3米以上。這就是不一致性最直接的工程危害它讓你的濾波器不再可信輸出結(jié)果用于決策比如路徑規(guī)劃、避障時會埋下巨大隱患。2. 從可觀測性理論看EKF-SLAM的深層次問題2.1 非線性系統(tǒng)可觀測性分析基礎(chǔ)談到可觀測性很多同學(xué)第一反應(yīng)是線性系統(tǒng)那套觀測矩陣的秩是否等于狀態(tài)維度能觀測就代表可以通過輸出反推狀態(tài)。但對于SLAM這種非線性系統(tǒng)嚴(yán)格的分析工具是非線性可觀測性理論通常用李導(dǎo)數(shù)構(gòu)建的可觀測性矩陣來判斷。在無噪聲的理想情況下2D SLAM系統(tǒng)的狀態(tài)空間是機(jī)器人位姿(localization)和路標(biāo)位置(mapping)的聯(lián)合。觀測模型是距離與方位它們只依賴機(jī)器人與路標(biāo)之間的相對位置。這意味著全局坐標(biāo)系的三個變換——水平平移、垂直平移、整體旋轉(zhuǎn)——不產(chǎn)生任何觀測差異。從李導(dǎo)數(shù)的角度求出來可觀測性矩陣的秩比狀態(tài)維度少3這3個自由度就是不可觀測方向。理解這一點(diǎn)非常重要SLAM的目標(biāo)從來不是絕對定位而是相對幾何關(guān)系的估計(jì)。機(jī)器人從原點(diǎn)出發(fā)還是從(100,100)出發(fā)對路標(biāo)相對位置、機(jī)器人相對路標(biāo)的估計(jì)結(jié)果完全等價??捎^測性理論告訴我們這3個自由度根本上無法被任何傳感器觀測消除。2.2 線性化讓不可觀測方向“泄漏”了信息非線性系統(tǒng)的可觀測性分析給的是一個理論基準(zhǔn)理想狀況下哪些方向可觀測、哪些方向不可觀測。但EKF每次更新用的是線性化模型線性化模型的雅可比矩陣是否完美保持這些不可觀測方向就是問題核心了。假設(shè)理論可觀測性分析告訴我們某個方向向量 ( g ) 應(yīng)該在可觀測性矩陣的零空間里。但線性化時用的點(diǎn)是當(dāng)前狀態(tài)估計(jì)值 ( \mu ) 而真值是 ( x ) 二者之間存在誤差。這個誤差會讓雅可比矩陣在 ( g ) 方向上的投影不再是零。用大白話說濾波器本應(yīng)認(rèn)為“在全局平移方向上我一無所知”但由于線性化點(diǎn)偏了它誤以為觀測數(shù)據(jù)能提供這個方向的信息于是開始錯誤地修正這個方向上的狀態(tài)同時壓縮協(xié)方差。這種現(xiàn)象在機(jī)器人轉(zhuǎn)彎時尤其嚴(yán)重。因?yàn)楹较蚪钦`差在旋轉(zhuǎn)時會被放大觀測雅可比中的角度相關(guān)項(xiàng)偏移較大不可觀測方向的泄漏就愈發(fā)明顯。所以一個典型現(xiàn)象是EKF-SLAM跑直線時NEES基本正常一旦開始轉(zhuǎn)彎NEES立刻飆升而且轉(zhuǎn)得越頻繁、越多問題越嚴(yán)重。這也是很多人在做圓形軌跡實(shí)驗(yàn)時發(fā)現(xiàn)一致性差得離譜的原因。更深一層這種泄漏是持續(xù)累積的。每一次更新都往不可觀測方向注入一點(diǎn)虛假信息濾波器協(xié)方差越來越小而估計(jì)誤差中的不可觀測成分卻沒有減少最終表現(xiàn)為嚴(yán)重的過度自信。這也是標(biāo)準(zhǔn)EKF-SLAM在長距離運(yùn)行中表現(xiàn)不佳的關(guān)鍵內(nèi)因。3. Matlab環(huán)境下的標(biāo)準(zhǔn)EKF-SLAM實(shí)現(xiàn)與問題復(fù)現(xiàn)3.1 狀態(tài)向量與系統(tǒng)模型設(shè)計(jì)在研究改進(jìn)方案之前先把標(biāo)準(zhǔn)EKF-SLAM在Matlab里完整實(shí)現(xiàn)一遍把不一致性問題真實(shí)復(fù)現(xiàn)出來。這樣后面做對比時就有明確的基線了。我的仿真場景設(shè)置如下機(jī)器人在一個20m×20m的平面區(qū)域運(yùn)行沿半徑8m的圓形軌跡勻速運(yùn)動線速度 ( v 1.5 m/s )角速度 ( \omega v/R 0.1875 rad/s )。環(huán)境中隨機(jī)分布80個路標(biāo)機(jī)器人搭載一個距離-方位傳感器可以觀測到視場內(nèi)的路標(biāo)距離噪聲標(biāo)準(zhǔn)差0.1m方位噪聲標(biāo)準(zhǔn)差0.01rad。運(yùn)動模型使用速度模型線速度噪聲標(biāo)準(zhǔn)差0.2m/s角速度噪聲標(biāo)準(zhǔn)差0.05rad/s。狀態(tài)向量定義如下這是后續(xù)所有代碼的基礎(chǔ)% 狀態(tài)向量結(jié)構(gòu) % x [x_r; y_r; theta_r; x_l1; y_l1; x_l2; y_l2; ...; x_lN; y_lN] % 索引約定 % 1 : 機(jī)器人 x 坐標(biāo) % 2 : 機(jī)器人 y 坐標(biāo) % 3 : 機(jī)器人航向角 theta % 42k: 第 k 個路標(biāo)的 x 坐標(biāo) % 52k: 第 k 個路標(biāo)的 y 坐標(biāo) % 總維度 D 3 2*N運(yùn)動模型采用經(jīng)典的速度模型機(jī)器人接收到控制輸入 ( u [v, \omega]^T )經(jīng)過時間 ( \Delta t ) 后位姿更新為[ \begin{aligned} x_{t1} x_t v\Delta t \cos\theta_t \ y_{t1} y_t v\Delta t \sin\theta_t \ \theta_{t1} \theta_t \omega\Delta t \end{aligned} ]路標(biāo)位置假設(shè)是靜止的不參與運(yùn)動方程。觀測模型針對每一個被看到的路標(biāo) ( i )[ \begin{aligned} h_1 \sqrt{(x_i - x_r)^2 (y_i - y_r)^2} \ h_2 \text{atan2}(y_i - y_r, x_i - x_r) - \theta_r \end{aligned} ]也就是相對距離和相對方位角。兩個觀測量的噪聲相互獨(dú)立構(gòu)成觀測噪聲矩陣 ( R \text{diag}(0.1^2, 0.01^2) )。3.2 標(biāo)準(zhǔn)EKF的預(yù)測與更新核心代碼下面給出標(biāo)準(zhǔn)EKF-SLAM在Matlab中的核心代碼實(shí)現(xiàn)。這段代碼我經(jīng)過多次仿真驗(yàn)證結(jié)構(gòu)清晰方便你在此基礎(chǔ)上做改進(jìn)。預(yù)測步代碼function [mu_pre, P_pre] ekf_predict(mu, P, v, omega, dt, Q, N) % mu: 狀態(tài)向量 (32N) x 1 % P : 協(xié)方差矩陣 (32N) x (32N) % v, omega: 控制輸入 % Q: 過程噪聲協(xié)方差 (2x2) theta mu(3); D 3 2*N; % 運(yùn)動模型函數(shù) mu_pre mu; mu_pre(1) mu(1) v*dt*cos(theta); mu_pre(2) mu(2) v*dt*sin(theta); mu_pre(3) mu(3) omega*dt; % 狀態(tài)雅可比 F d(f)/d(x) F eye(D); F(1,3) -v*dt*sin(theta); F(2,3) v*dt*cos(theta); % 噪聲雅可比 G d(f)/d(u_noise) G zeros(D, 2); G(1,1) dt*cos(theta); G(2,1) dt*sin(theta); G(3,2) dt; % 預(yù)測協(xié)方差 P_pre F * P * F G * Q * G; end更新步代碼逐個路標(biāo)更新function [mu_est, P_est] ekf_update(mu_pre, P_pre, z, landmark_idx, R_obs, N) % z: 觀測向量 [距離; 方位] % landmark_idx: 當(dāng)前觀測到的路標(biāo)在狀態(tài)向量中的編號 % R_obs: 觀測噪聲協(xié)方差 (2x2) D 3 2*N; theta mu_pre(3); xr mu_pre(1); yr mu_pre(2); % 提取路標(biāo)位置 lk 3 2*(landmark_idx - 1) 1; xl mu_pre(lk); yl mu_pre(lk 1); dx xl - xr; dy yl - yr; q sqrt(dx^2 dy^2); % 預(yù)測觀測值 z_hat [q; atan2(dy, dx) - theta]; % 觀測雅可比 H d(h)/d(x) H zeros(2, D); H(1,1) -dx/q; H(1,2) -dy/q; H(1,lk) dx/q; H(1,lk1) dy/q; H(2,1) dy/q^2; H(2,2) -dx/q^2; H(2,3) -1; H(2,lk) -dy/q^2; H(2,lk1) dx/q^2; % 創(chuàng)新 innovation z - z_hat; innovation(2) wrapToPi(innovation(2)); % 角度歸一化 S H * P_pre * H R_obs; K P_pre * H / S; mu_est mu_pre K * innovation; P_est (eye(D) - K * H) * P_pre; end這里有個需要注意的細(xì)節(jié)角度差一定要用wrapToPi歸一化否則方位角的創(chuàng)新項(xiàng)會在±π附近產(chǎn)生跳變造成錯誤的更新。這是新手最容易忽略的點(diǎn)我當(dāng)年在這個問題上卡了整整一個下午。還有一個關(guān)鍵工程點(diǎn)上面的代碼是逐路標(biāo)更新的即每看到一個路標(biāo)就做一次更新。這樣做實(shí)現(xiàn)簡單但在信息形式下相當(dāng)于對觀測信息做了一次近似分解嚴(yán)格來說不如批量更新精確。批量更新的做法是把所有觀測到的路標(biāo)的雅可比和觀測值都堆疊起來一次更新完成% 批量更新示意 H_all zeros(2*n_obs, D); z_all zeros(2*n_obs, 1); R_all kron(eye(n_obs), R_obs); % ... 堆疊各個路標(biāo)的 H 和 z ... S H_all * P_pre * H_all R_all; K P_pre * H_all / S;批量更新的好處是理論更嚴(yán)格不會因?yàn)轫樞蚨绊懡Y(jié)果在路標(biāo)數(shù)量不多時計(jì)算量也可接受。如果你的仿真場景路標(biāo)數(shù)量比較多可以繼續(xù)用逐路標(biāo)更新但心里要清楚這個近似帶來的細(xì)微差別。3.3 一致性量化指標(biāo)與復(fù)現(xiàn)實(shí)驗(yàn)設(shè)計(jì)復(fù)現(xiàn)問題不能只靠肉眼觀察協(xié)方差橢圓必須用數(shù)學(xué)指標(biāo)量化。最常用的指標(biāo)是NEESNormalized Estimation Error Squared。在時刻t給定真實(shí)位姿、估計(jì)位姿和協(xié)方差NEES定義為[ \epsilon_t (x_t^{\text{true}} - x_t^{\text{est}})^T P_t^{-1} (x_t^{\text{true}} - x_t^{\text{est}}) ]這里的 ( x ) 通常只取機(jī)器人位姿的三維部分 ( (x, y, \theta) )。如果濾波器是一致的那么NEES應(yīng)該服從自由度為3的卡方分布其均值為3。無數(shù)次蒙特卡洛實(shí)驗(yàn)后平均NEES應(yīng)接近3。如果平均NEES顯著大于3說明濾波器不一致而且數(shù)值越大不一致越嚴(yán)重。Matlab中計(jì)算NEESfunction nees compute_nees(x_true, x_est, P_est) % 取三維位姿 d x_est(1:3) - x_true(1:3); d(3) wrapToPi(d(3)); nees d * inv(P_est(1:3,1:3)) * d; end為了得到統(tǒng)計(jì)上可靠的結(jié)論必須做蒙特卡洛仿真。我在實(shí)驗(yàn)中跑了50次獨(dú)立運(yùn)行每次重新生成隨機(jī)噪聲最后將50次NEES求平均得到平均NEES曲線。判斷標(biāo)準(zhǔn)很樸素如果平均NEES穩(wěn)定在3附近濾波器一致如果遠(yuǎn)高于3濾波器不一致如果遠(yuǎn)低于3濾波器雖然“保守”協(xié)方差過大但至少不虛假自信不過工程上過保守也是次優(yōu)的。我在標(biāo)準(zhǔn)EKF仿真中跑出來的結(jié)果是前40秒平均NEES還能維持在3.5附近一旦機(jī)器人繞完第一圈開始持續(xù)重復(fù)觀測路標(biāo)平均NEES直線上升到150秒時已經(jīng)飆到18以上。這說明標(biāo)準(zhǔn)EKF的不一致性在長時間重復(fù)觀測場景下非常嚴(yán)重。你不需要特殊調(diào)試只要跑一個閉合軌跡就能清晰復(fù)現(xiàn)這個現(xiàn)象。4. 基于可觀測性約束的改進(jìn)實(shí)現(xiàn)4.1 第一條思路固定線性化點(diǎn)FEJ理解了問題根源之后改進(jìn)思路就變得清晰了。既然不一致性來自線性化點(diǎn)漂移導(dǎo)致的不可觀測方向泄漏那就設(shè)法讓線性化點(diǎn)保持穩(wěn)定使系統(tǒng)的可觀測性矩陣在時間上一致。FEJFirst-Estimates Jacobian方法的核心做法是對于每一個路標(biāo)在它第一次被觀測到的時候記錄當(dāng)時的機(jī)器人狀態(tài)估計(jì)值。之后在對這個路標(biāo)做觀測更新時無論機(jī)器人當(dāng)前狀態(tài)估計(jì)變成了什么構(gòu)建觀測雅可比矩陣都使用那個“首發(fā)估計(jì)值”而不是當(dāng)前估計(jì)值。為什么這樣有效因?yàn)镾LAM系統(tǒng)中可觀測性矩陣的列空間變化主要來源于每個路標(biāo)被重復(fù)觀測時線性化點(diǎn)的不同。如果每次都用第一次觀測時的狀態(tài)來線性化那么濾波器對某個路標(biāo)的觀測雅可比就從一而終不會再產(chǎn)生虛假信息注入。從數(shù)學(xué)上講這保證了系統(tǒng)線性化模型的可觀測性屬性始終與非線性系統(tǒng)理論保持一致。在Matlab中實(shí)現(xiàn)FEJ需要額外維護(hù)一個“首發(fā)估計(jì)”字典% 初始化 first_estimates struct(xr, [], yr, [], theta, [], initialized, false); % 更新時 % 檢查路標(biāo)是否首次被觀測 if ~first_estimates(landmark_idx).initialized % 首次觀測用當(dāng)前估計(jì)作為首發(fā)估計(jì) first_estimates(landmark_idx).xr mu_pre(1); first_estimates(landmark_idx).yr mu_pre(2); first_estimates(landmark_idx).theta mu_pre(3); first_estimates(landmark_idx).initialized true; end % 構(gòu)建觀測雅可比時用首發(fā)估計(jì)而不是當(dāng)前估計(jì) xr_lin first_estimates(landmark_idx).xr; yr_lin first_estimates(landmark_idx).yr; theta_lin first_estimates(landmark_idx).theta;然后基于 ( xr_lin, yr_lin, theta_lin ) 計(jì)算 ( H )而不是用 ( \mu_{pre}(1:3) )。需要注意一個容易被人忽視的細(xì)節(jié)FEJ不僅應(yīng)用于觀測更新有時也應(yīng)該應(yīng)用于運(yùn)動模型的雅可比。運(yùn)動模型中的旋轉(zhuǎn)雅可比如果使用不斷變化的航向角估計(jì)同樣會導(dǎo)致可觀測性矩陣的漂移。理論和文獻(xiàn)中通常建議運(yùn)動模型與觀測模型都做FEJ處理才能獲得最佳效果。我實(shí)測下來僅對觀測模型做FEJ就能改善大部分問題但兩者都做更為穩(wěn)妥。4.2 第二條思路直接約束可觀測性O(shè)C-EKFFEJ是一種間接保證可觀測性一致性的方法思路是讓線性化點(diǎn)固定。OC-EKF則更直接在每一步求解一個帶約束的優(yōu)化問題直接把雅可比矩陣投影修正到理論可觀測性約束條件下。具體來說在每一步得到觀測雅可比 ( H ) 后讓 ( H ) 滿足[ H \cdot X_{\text{null}} 0 ]其中 ( X_{\text{null}} ) 是理論不可觀測方向的基。在2D SLAM中這3個不可觀測方向可以構(gòu)造為% 不可觀測方向基近似 % 1. 全局 x 平移 % 2. 全局 y 平移 % 3. 全局旋轉(zhuǎn) X_null [1 0 -yr_all; 0 1 xr_all; 0 0 1];上式中 ( xr_all, yr_all ) 取當(dāng)前狀態(tài)估計(jì)中的機(jī)器人位置或路標(biāo)位置的平均具體推導(dǎo)需要從SLAM的系統(tǒng)模型出發(fā)讓每個不可觀測方向?qū)?yīng)一個全局變換。一般的做法是先構(gòu)建理論不可觀測方向的矩陣 ( U )然后對 ( H ) 做修正使 ( H U 0 )。這可以通過在 ( H ) 上左乘一個修正矩陣實(shí)現(xiàn)也可以用投影法令 ( H_{\text{oc}} H - H U (U^T U)^{-1} U^T )。這樣新的 ( H_{\text{oc}} ) 的零空間就包含了所有不可觀測方向。我在Matlab中實(shí)現(xiàn)OC-EKF時踩過一個坑直接對 ( H ) 做投影修正如果只是機(jī)械地減去 ( H U (U^T U)^{-1} U^T )得到的 ( H_{\text{oc}} ) 雖然滿足約束但可能與真實(shí)觀測模型偏差過大導(dǎo)致濾波器不穩(wěn)定。工程上通常需要將修正限制在較小的范圍內(nèi)或者只對關(guān)鍵部分做約束而不是全盤投影。這是OC-EKF落地時比較微妙的地方。4.3 兩種方案的Matlab實(shí)現(xiàn)要點(diǎn)與對比我把兩種方案放在同一套仿真框架里做了對比實(shí)驗(yàn)結(jié)論寫在下面。先看FEJ的實(shí)現(xiàn)效果。在標(biāo)準(zhǔn)EKF里平均NEES在150秒時到18換成FEJ之后平均NEES全程保持在4左右長時間運(yùn)行后也沒有明顯上漲趨勢。這說明FEJ確實(shí)解決了大部分可觀測性泄漏問題。不過NEES略高于3說明仍然有小幅不一致主要來源是路標(biāo)初始化時的噪聲和首估時刻的誤差這部分殘留偏差可以通過跑更多蒙特卡洛次數(shù)后取均值進(jìn)一步觀察一般落在3到5之間是可以接受的工程范圍。再看OC-EKF的實(shí)現(xiàn)效果。理論上OC-EKF比FEJ更嚴(yán)格因?yàn)樗苯颖WC了每一步的可觀測性約束。但實(shí)際仿真中我發(fā)現(xiàn)OC-EKF對實(shí)現(xiàn)細(xì)節(jié)更敏感比如 ( X_{\text{null}} ) 構(gòu)造中用的是當(dāng)前點(diǎn)還是首發(fā)點(diǎn)對結(jié)果影響很大。我嘗試了不同構(gòu)造方式最優(yōu)的OC-EKF平均NEES在3.5左右與FEJ性能接近但在計(jì)算耗時上要略高一些。兩者對比可以總結(jié)為下面這個表格維度標(biāo)準(zhǔn)EKFFEJOC-EKF平均NEES150s184左右3.5左右實(shí)現(xiàn)復(fù)雜度低中較高需要維護(hù)的額外變量無首發(fā)估計(jì)表不可觀測方向基矩陣對模型的依賴低中較高需理論分析數(shù)值穩(wěn)定性好好較敏感從工程落地角度我個人的選擇是如果只是想快速改善現(xiàn)有SLAM代碼的一致性優(yōu)先上FEJ改動量小效果穩(wěn)定如果是在做學(xué)術(shù)研究或追求極致一致性可以上OC-EKF但需要有耐心調(diào)試數(shù)值細(xì)節(jié)。5. 實(shí)操過程中常見問題與排查技巧5.1 常見問題速查表在實(shí)現(xiàn)和調(diào)試這套系統(tǒng)的過程中我遇到了不少問題整理成一個速查表方便你對照排查?,F(xiàn)象可能原因排查思路解決方案NEES從一開始就很大初始協(xié)方差設(shè)置不合理檢查P0數(shù)量級對比觀測噪聲重新設(shè)定初始協(xié)方差適當(dāng)增大機(jī)器人位姿初始方差NEES只在轉(zhuǎn)彎時飆升旋轉(zhuǎn)時雅可比線性化誤差放大觀察是否在omega較大時刻N(yùn)EES異常嘗試更小的控制周期dt檢查運(yùn)動模型散度路標(biāo)越多NEES越大每個路標(biāo)的微小泄漏持續(xù)累積固定軌跡逐步增加路標(biāo)做消融使用FEJ或OC-EKF消除累積泄漏協(xié)方差矩陣出現(xiàn)非正定逐路標(biāo)更新順序?qū)е聰?shù)值問題檢查P是否對稱正定改用批量更新改用Joseph形式更新協(xié)方差濾波結(jié)果發(fā)散誤差巨大初始航向角誤差過大或角度未歸一化檢查innovation中的角度是否wrapToPi處理角度歸一化增大初始theta方差地圖估計(jì)逐漸扭曲路標(biāo)初始化時使用了帶偏的機(jī)器人位姿觀察首估時刻機(jī)器人位姿誤差大小嘗試延遲初始化等機(jī)器人位姿較穩(wěn)定時再初始化路標(biāo)5.2 數(shù)值穩(wěn)定性與參數(shù)調(diào)優(yōu)EKF對數(shù)值穩(wěn)定性要求其實(shí)很高尤其是當(dāng)狀態(tài)維度達(dá)到上百維80個路標(biāo)就是163維狀態(tài)時協(xié)方差矩陣的數(shù)值誤差會被放大。我在調(diào)試過程中有幾個重要發(fā)現(xiàn)。第一個發(fā)現(xiàn)是協(xié)方差更新形式的差異。理論教科書上通常寫 ( P (I - K H) P_{\text{pre}} )但這在數(shù)值上容易失去對稱性和正定性。更穩(wěn)的寫法是Joseph形式[ P_{\text{est}} (I - K H) P_{\text{pre}} (I - K H)^T K R K^T ]這個形式在計(jì)算上稍微貴一點(diǎn)但在長時間運(yùn)行中能顯著減少非正定協(xié)方差的出現(xiàn)概率。我在項(xiàng)目里最后都改成了這個版本穩(wěn)定性好了不少。第二個發(fā)現(xiàn)是角度處理問題。SLAM里到處都是角度機(jī)器人航向、方位角觀測、繞行后的角度累積。除了更新時要做角度歸一化之外計(jì)算NEES時也要對角度誤差做wrapToPi處理否則一條簡單的圓形軌跡就會因?yàn)榻嵌壤@圈導(dǎo)致NEES計(jì)算錯誤。細(xì)節(jié)雖小但影響巨大。第三個發(fā)現(xiàn)是控制周期dt的選擇。dt太大運(yùn)動模型線性化誤差大不一致性會更早暴露dt太小計(jì)算量增大而且如果dt小于傳感器采樣周期中間插入的無觀測步只會增加計(jì)算負(fù)擔(dān)。我的建議是讓運(yùn)動更新和觀測更新同步進(jìn)行dt與傳感器采樣周期一致即可不需要刻意細(xì)分。在我上面的圓形軌跡實(shí)驗(yàn)中dt取0.1s同時用作控制和觀測采樣間隔效果良好。關(guān)于調(diào)參還有一個容易被忽略的關(guān)鍵點(diǎn)過程噪聲Q不要取得過小。有些同學(xué)為了讓濾波器看起來“自信”把Q壓得很低這反而會加劇不一致性。原因很簡單Q太小意味著濾波器對運(yùn)動模型極度信任幾乎完全依賴預(yù)測這會讓線性化點(diǎn)的誤差得不到修正虛假信息注入得更嚴(yán)重。我在實(shí)驗(yàn)中把線速度噪聲標(biāo)準(zhǔn)差調(diào)到0.2m/s已經(jīng)是比較合理的范圍如果你發(fā)現(xiàn)NEES在不一致性的基礎(chǔ)上還伴隨劇烈振蕩可以嘗試適當(dāng)調(diào)大Q。5.3 從仿真到實(shí)車應(yīng)用的思考仿真里的NEES可以用真值來算但實(shí)車上你永遠(yuǎn)沒有真值一致性判斷會變得更困難。業(yè)界常用的一種替代方案是評估濾波器輸出與里程計(jì)、GPS在允許的傳感器組合中等其他信息源之間的交叉一致性或者使用邊界的殘差統(tǒng)計(jì)。不過這些都是后話先把仿真環(huán)境中的不一致性問題研究透對實(shí)車應(yīng)用絕對有正向幫助。我個人的體會是理解可觀測性這個角度比盲目調(diào)參重要得多。以前遇到定位漂移第一反應(yīng)是調(diào)整噪聲參數(shù)、換傳感器、加路標(biāo)效果往往是按下葫蘆浮起瓢。后來從可觀測性角度去想問題很快就定位到協(xié)方差低估這個結(jié)構(gòu)性缺陷上改用FEJ之后很多“玄學(xué)”問題一下子消失了。這就是理論視角帶來的工程紅利。6. 寫在最后個人實(shí)操體會做這個研究的這段時間我最大的感受是EKF-SLAM看起來結(jié)構(gòu)簡單代碼也就一兩百行但里面藏的理論深度遠(yuǎn)超想象。你只把它當(dāng)作一個“概率狀態(tài)估計(jì)器”來用也能跑出結(jié)果但一旦你去追問協(xié)方差是否可信、估計(jì)是否一致就不得不去面對可觀測性這個底層話題。從實(shí)操層面給你幾個建議。第一Monte Carlo次數(shù)越多越好至少20次起步否則NEES曲線抖動太大很難下結(jié)論。第二不要一開始就上幾十個路標(biāo)的高維場景先用三五個路標(biāo)把邏輯跑通觀察每個路標(biāo)首估時刻對后續(xù)結(jié)果的影響。第三把標(biāo)準(zhǔn)EKF、FEJ、OC-EKF三者放在同一套仿真代碼里對比你會更清楚地看到線性化點(diǎn)漂移到底造成了什么影響。最后再分享一個小技巧當(dāng)我想快速判斷某個改動是否有效時只看兩個指標(biāo)——平均NEES是否隨運(yùn)行時間收斂而不是發(fā)散以及協(xié)方差矩陣的特征值分布是否出現(xiàn)異常小的方向那通常是虛假信息壓縮出來的。這兩個信號比觀察軌跡誤差還要靈敏。希望這篇內(nèi)容能幫你少走一些彎路把EKF-SLAM的一致性這個老大難問題徹底搞清楚。
返回列表
PREV
查看更多資訊
NEXT
返回資訊列表
亚洲六月色| 在线观看熟女少妇| 丁香婷婷月| 国产在线中文字幕| 亚洲AV成人一区二区在线观看| 夜夜夜夜做天天天做无码视频| 激情五月婷| 色婷网| 97色在线| 久99久热只有精品国产99| 婷婷五月娱乐在线| 亚洲五月丁香综合网| 丁香五月偷拍| 色五月婷婷久久| www.激情| 五月丁香久| 欧美日韩成人在线网| 五月天婷婷一起草| 99er精品视频| 99视频91| 全部老头和老太XXXXX| 亚洲欧洲一二| 超碰爱爱爱| 伊人婷婷青青cao| 五月激情久久| 99er热精品视频| 五月婷婷综合在线| 99精品免费欧美小视频| 综合色五月| 青青久在线视频免费观看| 婷婷中文字幕版| 婷婷5月开心6月| 2017狠狠干| 六月婷婷激情图片| 五月丁香色停停啪啪啪| 五月天小说激情| 五月综合色播播丁香婷婷 | 亚洲aV写真天天综合网久久| 五月丁香中文婷婷中文| 五月天播播| 97精品在线| 国产婷婷色综合AV蜜臀AV| 97超级碰碰碰| 开心激情婷婷| 色综合九九| 爆乳熟女-区二区三区| 五月丁香婷婷激情澎湃四射| www.日韩国产| 精品欧美一区二区三区久久久| 色丁香六月| 国产美女主播vip| 超碰爱爱爱| 91久久综合亚洲噜噜成人在线| 碰碰91| 天天插天天玩天天干| 国产成人精品一区二三区熟女在线| 丁香五月丁香伊人| 久久综合五月天| 色九九综合| 九九久久五月天综合伊人| 成人在线高清| 国产国产乱老熟女视频网站97| 色色亚卅| 操91| 免费成片在线观看| 五月婷婷综合在线| www.黄色片-久久成人国产精品在线播放-999AV | 另类激情首页| 亚洲综合在线伊人婷| 欧美大肥婆大肥BBBBB| 天天色,天天日,天天做| 欧美丁香五月97色| 婷婷日本在线| 五月开心久久| 99在线视频播放| 亚洲性爱99| 久久99免费视屏| 综合色五月天| 天天天天色天天天天天干| 在线中文亚洲| 去色色五月天| 成人.在线日韩| 97碰碰在线观看视频| 蜜桃婷婷五月| 丁香五月婷婷色情综合| 美女久久婷婷| 丁香五月综合在线播放 | 六月婷婷综合激情| 爱婷婷久久视频| 久久99免费视屏| 久久久网站| 色色射| 久热久| 婷婷五月六月丁香| 26uuu成人网| 五月丁香六月婷婷网| 99精品色色| 香焦网五月天| 五月丁香色色色| 99碰碰| av人人干| 丁香五月在线| 第四色首页| 欧美综合五月丁香六月婷| 日韩限制级大尺度黑料泄密大尺度视频一区二区在线观看 | 五月天丁香成人社| 十区av| 97色碰| 久草视频一,二三四| 六月五月久久丁香| 婷婷六月色丁香视频在线观看| 99在线资源| 2021日韩无码| 丁香五月最新网址| 在线观看996精品| 91丨九色丨老熟女激情| 啪啪啪综合网| 91热爆在线| 欧美成人无码一区二区三区| 五月香蕉综合| 五月婷婷激情四季| 色色综合无码| 亚洲国产色婷婷| 激情五月综合网最新| 最新久久99视频网站| 五月丁小婷婷激情四射| 婷婷五月天成人视频| 翔田千里 50岁 无码| 五月婷亚洲精品| 九九热区一区二区三区| 色婷婷狠狠久久综合五月| 99热色无码| 九九色人| 久久久久久xxxxx| 啪啪操网| 日韩欧美一道四区中文字幕| 97干免费视频| 激情综合女人网五月播播| 99在线精品观看99| 91精品刘玥| 国产精品色情AAAAA片软件| 丁香六月在线| 99精品在线观看视频| 五月亭亭直播| 色色色色色网| 天天色图| 99热亚洲精品| 色丁香五月婷婷| 亚洲欧美婷婷五月色综合| 久久色亭亭五月天| 色五月婷婷丁香五月| 五月婷婷啪啪| 成人在线二区| 日韩成人电影Av| 玖玖婷婷色欲| 婷婷色色丁香| 99re热精品在线视频| 日本人妻A片成人免费看片| 永久的网站AAAA | 久热欧美| 婷婷五月综合色拍| peg 2区三区四区的| 激情五月天婷婷免费观看| 香蕉婷婷色五月| 超碰99热精品| 亚洲性受XXXX五月丁香| 亚洲成人乱码av网站| 99免费视频在线观看爱| 婷婷五月视屏| 常久最新免费的色吊丝| 五月综合亚洲婷婷| 激情六月色| 深爱激情网五月天| 九九99九九99| 天天爽夜夜操| 丁香五月-激情综合| 亚洲久艹| 永久的网站AAAA | 性爱五月婷| 九九视屏| 天天日夜夜高潮| 只有精品在线观看| 91欧美| 爱草视频在线观看| 日本五月视频| 久久人妻伦理| 激情五月综合亚洲另类| 9色免费网| 婷婷色狠狠| 99免费偷拍视频| 色综合婷婷| 一个色的综合| 激情色视频| 久久精品只有这| 激情综合婷婷| 啪啪 综合网| 欧美日韩999| 国产乱妇乱子在线播视频播放网站| 久热精彩视频98| 日韩人妻在线观看| 五月丁香六月婷婷亚洲| 五月婷婷激情综合网 | 婷婷九九色| 激情玖玖综合网| 双性美人被调教到喷水A片| 丁香激情五月天| 伊人久久丁香五月91| 9热视频在线观看| 激情第四色| 女人被躁到高潮嗷嗷叫小| 九九九九精品精| 丁香久久九九99| 亚洲成人综合网在线免费观看| 国产精品人妻欲求不满| 五月激情六月婷婷| 九九热视频在线观看| 91n啪啪| 激情九月婷婷| 色狠狠六月| 激情五月四色| 欧美内射AAAAAAXXXXX| 丁香五月停停基地| 伊人热在线大香蕉| 精品久久婷婷五月天| 99热精国产这里只有精品| 五月丁香六月婷婷成人电影| 99天堂在线观看免费视频| 色婷婷婷婷| 人人摸人人干| 六月五月丁香五月欧美| 久久色婷婷| 久热A片| 97精品综合| 夜夜躁爽日| 夜夜大香蕉婷婷丁香| 天天色天天操天天射| 中文字幕婷婷在线| 欧美综合激情五月天| 51国精产品自偷自偷综合| 亚洲中文字幕在线观看| 97色婷婷成人综合在线观看| 99色中文| 五月丁香婷婷成人版| 日韩综合大黄| 色婷婷综合网站| 婷婷丁香综合| 婷婷五月精品在线| 色婷婷9| 六月丁香婷婷爱| 热热久久精品视频| 五月香六月婷| 国产成人综合亚洲| a在线观看| 五月丁香亭亭成人电影| 超碰在线人妻| 色五月超碰| 五月天亭亭俺也| 婷婷五月天在线观看| av在线免费播放| www.99视频| 色99网| 日日日日日| 中文久久婷婷| 天天肏在线观看| 99热手机在线精品| 久久这里只有国产| 色色色综合网| 这里只有九九精品| 五月婷婷伊| 亚洲激情丁香五月天色| 婷色五月天| 99亚洲天堂| www.婷婷五月| 亚洲性爱电影| 五月天丁香综合| 国产伦亲子伦亲子视频观看| 伊人五月久久| 成人做爰黄A片免费看直播室男男 久草热8精品视频在线观看 | 日木WWW视频| 欧美猛片| 思思久久99热只有频精品66| 99久久精| 极品少妇高潮啪啪AV无码| 激情国产综合| 婷婷97碰碰| 91精品人妻少妇无码影院| 日亚二欧美| 伊人大香蕉在线视频| 色999;丁香五月| 玖玖爱综合网| 俺也去在线久久精品23欧美综合视频网站,丰满人妻一区二区三区在线视频53,丰满 | 色色色色热| 伊人久久大香| 综激情网| 五月婷婷偷| 9 1超碰九色| 四色永久成人网站| 国精产品一区一区三区免费视频| 天天色综合图片| 大香蕉久久久久久久久| 色五月天激情| 97成人超碰免| 操碰97| 婷婷五月激情图片| 玖玖五月| 中文字幕在线观看视频www| 人妻啪啪啪| 激情5月婷婷狠狠干| 色综合丁香婷婷| 婷婷久久五月| 综合性爱网| 超碰在线人人| 在线播放成人网站| 人人操人av| 91日综合欧美| 亚洲精品婷婷| 色综合久久天天综合网 | 色色COm| 久久九九Com| 五月婷婷六月激情| 九月av在线| 五月婷婷在线观看黄| 天堂成人A片永久免费网站| 婷婷舔| 97AV在线视频| 五月婷婷丁香六月在线| 亚洲 视频 导航 一区| 婷婷丁香六月| 欧美婷婷五月丁香| 国产精品成人在线| 天天日P天天射P| 五月婷婷丁香六月| 色播五月| 久久6这里只有精品| 天天综合天综合久久网| 六月婷婷激情| 久久婷婷激情视频| site:minyis.com| 色综合99色| 思思re99视频在线观看| 丁香婷婷婷五月| www.久久99| 丁香婷婷综合影院| 开心激情婷婷| 五月婷婷五月色| 日本五月婷婷久久久六月丁香| 国产欧美大香蕉一区| 99免费在线| 色六月视频| 99综合在线| 丁香婷婷六月| 疯狂做受XXXX高潮A片动画| 六月天婷婷| 婷婷五月丁香在线观看| 久久总和99| 成人国产欧美大片一区| 久久HD| 五月天婷网| 婷丁五月| 秋霞性爱AV| 色播五月丁香综合| 色婷婷激情Av久久久| 激情五月婷| 婷婷五月天激情综合网| 电影蜘蛛女| 色婷婷五月色| 99色久| 97操操网| 99热这里只有精品99| 久久在线大香蕉| 五婷婷综合网| 国产FREESEXVIDEOS性中国 | 超碰人人操| 99热这里只有精品13| 日本综合色色| 99精品97| 亚洲综合在线网站| a片在线免费观看一区| 无码激情AAAAA片-区区| 久久99网| 97干在线视频| 激情99| 色色色色色网| 几激情五月婷婷色五月色天堂| 欧洲日韩一区二区三区| 99热6这里只有精品6| 色99热| 1囯产午夜仑鲁鲁| 秋霞AV吧| 97人人操人人插| 人妻体体内射精一区二区 | 狠狠色大香蕉| 五月天色不卡| 99热综合| 色婷婷激情四射视频| 五月综合缴情网| 99热99免费| 99热欧| 人妻自慰在线| 亚洲艹网| 97五月天婷婷午夜| 五月激情综合激情五月| 五月色欧洲| www色婷婷久久综合久色| 激情五月综合网最新| 97热久久| 丁香九九九九| 99成人网一区| 丁香五月图片| 丁香五月激情澎湃一区| 欧美丁香五月天| 5月婷婷激情6月| 五月激情偷拍婷婷| 五月激情四射婷婷丁香| 中文aV网| 天天干天天爽天天操| 思思热久久婷婷五月天| 色婷婷成人久久| 91久久18| 伊人久久婷| 丁香五月天婷婷中文字幕| 婷婷激情五月天在线| 久久AV电影| 韩国久久少妇视屏| 天天久综合| 五月婷婷六月丁香在线| 久久综合首页| 涩五月婷婷| 大香蕉精品视频| 婷婷五月天深爱| 五月天激情小说婷婷基地| 久久性爱视频久久性爱视频| 久久五月丁香| 五月婷婷基地| 少妇被下春药玩弄A片| 免费超碰在线观看| 熟女国产在线一区二区三区四区| a九九热www| 丁香五月1页| mmm1717.6dbm人人爱人人操| 六月婷婷亚洲| 婷婷婷婷婷开心无码播放| 九九99热久久精品66中文字幕| 91色在线/日韩| 激情五月婷婷视频一区二区三区| 国产看真人毛片爱做A片| 丁香婷婷天堂| 人妻aV在线| 91婷婷丁香五月天免费视频网站| 激情丁香五月激情婷婷| 婷婷狠狠狠爱| CAOBIBI| 秋霞AV淫| 婷婷激情五月天激情在线| 五月综合婷婷网| 国产裸舞表演WWWW| 中文字幕综合网| 禁欲电影完整版在线播放| 久久国产成人9999久久久久| 久久九九@| 狠狠香婷婷五月| 激情综合丁| 97色操| 国产精品美女| 亚洲精品第一国产综合亚AV | 91久草五月天婷婷| 精品久色| 色五月婷婷色五月| 99久久这里只有精品| 99精品偷自拍| www久久久久久久97| 第四色色六月色综合| 五月天久久丁香| 久操热线| 色婷婷色综合| 日日日日做夜夜夜夜无码| 性生活视频98791| 欧美va视频| 91人在线观看| 影音先锋天天日| 热久久成人| 天天干天天叉| 五月婷婷开心网| 综合另类激情| 五月丁色AV| 99久热这里只有精品| 久久这里只精品| 国产91在线视频| 五月丁香亚洲校园欧美| 99热久| 亚洲人妻av伦理| 婷婷色五月开心五月| 色97综合婷婷天天色| 婷婷成人五月天成人文学| 一起草av| 99re免费精品视频| 激情五月婷婷网| 免费看欧美成人A片无码| 天天肏高清在线| 亚洲麻豆乱码国产2028| 26uuu精品国产| 红桃91人妻爽人妻爽| 99热99思午夜精品| 99热99久久| 中字幕视频在线永久在线观看免费| 超级碰 久久9| 四五月婷婷| 97色干| 国产色香蕉精品五夜婷| 五月天婷a| 久久草大香蕉| 在线不卡视频| 99日本黄站| 色色97丁香婷婷五月天| 丁香五月天色综合| 中文字幕av在线播放| 9色婷婷| 狠狠操.com| 久久久婷| 天天搡日日搡aaaaⅩ| 五月丁香综合在线| 嫩草视频。| 五月婷婷深爱六月| 亚洲亚洲人成综合网络| 五月综合婷婷开心网| 久9热| 99热99美国在线观看| 丁香五月天在线| 亚洲无码影音| 男女久久婷婷五月天| 99久久9| 欧美日韩99| 99riAv1国产在线观看| 五月婷视频在线观看| 欧美综合激情五月| 婷婷五月天性色| 婷婷五月天va| 免费无码毛片一区二区A片| 色五月综合婷婷久久综合婷婷久久综合婷婷久久综合婷婷久久 | 五月婷婷婷| 综合五月激情网| 99精品国产在热久久| 五月天丁香啪啪综合| 91碰碰| 亚洲有码在线视频| 色婷婷丁香五月| 光棍影院日韩精品| 九九热视频在线观看| 色五月综合97| 色色日本欧美| 久久六月天| 久久99久久99久久99人受| 91色五月| 色噜噜狠狠色综无码久久合欧美| 怕怕av| 国自产拍偷拍精品啪啪一区二区| 激情五月天之六月婷婷| 五月丁久久| 五月婷婷免费视频| 99综合99| 九九性爱网| 淫视馆AV在线| 久久九九色| 99久热这里有精品| 99丁香婷婷综合网| 天天操夜夜橾| 久久在线92| 久久伦乱| 色婷婷丁香网| 日本不卡高字幕在线2019| 久久这里只有精品无码| 激情五月天情色| 激情五月天视频| 五月丁香亭亭操逼| 九九热AV| 99爱免费视频| 99热碰碰| 婷婷六月色| 综合在线丁香五月| 碰碰碰97免费精彩视频| 色五月综合激情| 色五月无码| 无码色| sewuyuejiqingwang| 99久久婷婷精品视频| 五月天婷婷无码| 久久久久久综合88| 国产精品扒开腿做爽爽爽A片唱戏| 26uuu亚洲欧美| 超碰高清在线| 色色五月天激情| 九九热这里只有国产精品| 黄色片avv| www,超碰| 狠狠干总合| 丁香熟女乱| 91久久久久久| 婷婷五月中文字幕国产| 色婷小说| 色婷婷综合网站| 婷婷五月天com| 欧美超碰人人| 一起草AV| 激情深爱五月天| 色五月xxx| 99热人人操人人操| 五月婷婷影| 精品皮股午夜AV| 操逼视频一区| 激情五月综合网| 日日色综合| 五月份婷婷| 天天视频精品9| 狠狠爱综合| www天天干| 91精品又长又大又粗又爽又猛| 久热免费视频| 丁香美女五月天婷婷| 五月婷六月综合在线观看| 色五月婷婷丁香凹凸| 91久久1118| 精品一区二区三区木瓜| 99热亚洲精品| 久婷狼色诱惑在线| 五月丁香亭亭| 精品爆操| 日韩精品无码99| 丁香九月婷婷综合| 9 大屁股在线视频精品| 狠狠综合| 国产熟女一区二区三区五月婷| 五月天激情婷婷丁香| 色综合丁香| 伊人超碰在线| 国产成人精品123区免费视频| xxxx久| 26uuuuuuuu国产| 五月丁香六月色婷婷综合五月天| 色五月自偷自拍婷婷婷婷| 五月婷婷香| 五月天成人综合| 五月天久久成人| 丁香五月婷婷狠狠色| 色色五月综合| 婷婷99视频在线| 亚洲第一第二网站| 婷婷五月,偷窥偷拍网| 99热99这里有免费的精品| 婷婷五月天激情综合| 亚洲色区17| http:色情日本com| 婷婷永久在线| 天天草天天爽| 五月激情天天干| 操逼在线视频| 久久99最新| 超碰精品在线| 91操熟女| 中文资源在线a | 成人丁香五月| 久久99综合网| 婷婷色偷拍| 日韩一级| 疯狂做受XXXX高潮A片动画| 五月丁香影院| 深夜激情网| 热久久精品视频网站| 99热欧| 成久综合视频| 五月丁香婷婷色| 国产熟女大叫受不了| 中文字幕不卡视频| 伊人激情网| 日本综合99| 亚洲天堂婷婷丁香| 欧美精品在线观看| 激婷网| 97丨九色丨国产丨PORNY| 超碰成人免费| 中文激情网| 激情丁香五月天| 超碰人人干| 99精品热视频| 丁香花色色网| 92久久| 97在线综合| 色情久久久| 婷婷黄色| 黄网在线免费观看| 色婷婷综合视频| www久| 色情五月综合婷婷| 大香蕉人妻| 深爱激情综合网| 色婷婷小说网| 国产69久久久欧美黑人A片| 操91| 婷婷丁香六月天| 免费日本aⅴ中文字幕| 影音先锋AV男人站| 女人天堂久久| 一起草Av| 婷丁五月| 99这里只有精品在线| 精品久久人妻热| 亚洲狠狠婷婷综合久久久| 国产精品-第3页-91JQ就要激情网91JQ5.JQJQ926.XYZ | 亚洲麻豆乱码国产2028| 色色色.COM| 26uuu亚洲欧美| 狠狠草狠狠草| 精品久久9| 97人人干人人操| 99ER热精品视频| www.久久色.com| 五月天婷婷色色网| 97人人草| 亚洲综合色丁香婷婷六月| 综合五月天婷婷色| 色婷婷AV在线| 婷婷五月色| 亚洲精品第一色色色色色色| 超碰在线个人观看| 成人 视频免费观看网站| 久青青久| 五月天六月婷婷电影| 涩婷婷五月天在线精品视频| 婷婷综合在线| 大香蕉啪啪啪| 超碰97在线观看免费| 久久丁香婷婷五月| 五月天婷婷激情网| 伊人五月网| 五月婷婷丁香综合| 五月婷婷五月天| 丁香五月天激情综合| 亚洲永远av在线播放| 九月综合| 日本色噜| 婷婷五月天伊人网| 538在线精品| 亚洲网综合在线| 亚洲综合欧美色丁香婷婷888月图片| 久久久.COM| 啄木鸟黑丝一区二区| 婷婷开心五月| 中文字幕综合网| 亚洲亚洲人成综合网络| 91色吧网| 五月永久激情| 亚洲 视频 导航 一区| 九九这里只有精品| 婷婷综合日本| 天天综合插插| 玖玖婷婷色五月| 亚洲精品一区无码A片| www.99热这里精品| 九玖视频这里只有精品| 怡红院一二三| 久久婷五月天| 久久婷婷综合拍| 五月激情四射婷婷丁香| 色婷婷亚洲婷婷在线观看| 日本久久天堂| 日本专区久久| 五月人妻婷婷| 天堂综合久久| 九九9久九9国产视频| 久久总和99| 淫水导航| 久久9视频| 极品另类| 人妻操在线看| 99视频精品在线| 99在线综合视频| 婷丁香久综合| 97色女人在线| 久热re视频在线观看网站| 开心婷婷中文字慕| 深爱激情五月婷婷| 亚洲精品国产熟女久久久| 9久久久久久久久久久| 色人妻五月| 久99热| 九色91视频| 五月婷婷啪| 精品久久久久成人码免费动漫| 久久作爱| 色99网| 无码少妇高潮喷水A片免费| 五月丁香激情婷婷| 五月丁香啪啪伦理电影| 日日躁夜夜躁狠狠久久AV| www.五月天婷婷| 天天爽天天草| 久热九九| 91Chinese在线| 色七七色九九| 成人中文网| 6月丁香婷婷| 丁香五月激情啪| www.一起草av| 五月天成人在线播放丁香| 亚洲综合激情五月久久| 亚洲精品国产成人AV在线| 99热这里只有精品在线| 97超级碰| 婷婷丁香五月天小说| 激情五月天丁香| 色五月婷婷中文字幕在线观看| 丁香五月先锋| 91丨九色熟女丨首页| 99热这里只有在线| 丁香五月天久久| 色 丁香婷婷| 五月丁香大香蕉| 91超级碰碰碰| 亚洲超碰在线| 色5月婷婷色| 国产亚洲精品久久一区二区三区| YW无码| 丁香五月在线观看综合| 天天色情站| 天堂爱爱| 五月天激情四射| 蜜乳9188| 日韩无码专区| 99惹在线精品免费观看| 九九五月天| 全高清无码视頻| 六月婷久久| 天堂久久婷婷| 五月综合丁香婷婷| 99久久婷婷五月综合| 色久丁香五| www一起操| 婷婷九月亚洲| 爆乳熟妇一区二区三区爆乳照片| 开心激情站| 色娸娸综合网| 丁香五月婷婷亚洲综合精品| 91九色中文| 五月天成人综合| 久热9| 天天天天干| 天天日日天天| 五月天操逼激情| 久久综合激情婷婷激情| www.久99| 丁香五月天啪啪| 开心深爱激情网| 涩丁香91| 色婷婷久久| 毛片新网地| 久久久久9999| 丁香五月1页| 欧美精品XXXXBBBB| 婷婷五月欧美综合| 狠狠爱深色婷婷综合| 99日这里只有精品| se色综合网| 99热青青草| 涩综合在线| 激情九月婷婷| 丁香婷婷啪啪啪| 丁J香六月首页| 色噜噜狠狠色综无码久久合欧美 | 色99色| 九九色热| 婷婷开心青青草| 成人AV片播放| 亚洲AV中文在线| 99色在线视频观看| 日本无码专区| 人人草人人舔| 天天想夜夜爽天天爽| 久久精品99国产精品日本| 丁香六月婷婷综合啪啪| 久久免费操| 中文字幕在线免费观看视频| 天天爽日日爽夜夜爽| 激情图片99| 六月丁丁香| 五月婷婷九| 97黑人精品区| 激情综合五月天| 99热中文字幕久久| 婷婷99狠狠躁天天| 69超碰在线| 天天摸天天舔| 久这里只有精品99| 六月婷欧美丁香综合| 精品久久久999| 天天在线久久综合 | 成人在线网址| 我爱宗和色| 欧美一线视频| 人人草人人舔| 亚洲五月花| 日韩色色色色色| 丁香五月天AV在线 | 色www99| 玖玖五月| 天天综合天天玩夜夜玩天天玩夜夜玩| www.久久爱.c n| 婷婷五月AV| 五月丁香婷中文字幕| 99色五月| 国产精品久久久爽爽爽麻豆色哟哟 | 五月婷婷综合社区| 午夜天堂一区人妻| 色丁香婷婷| 一级片sese片.COM| 婷婷WWW久久| 91精品久久久久| 婷婷综合六月| 久久综合激情五月天| www.玖玖婷婷在线| 99在线精品观看99| 98永久精品| 五月丁香婷婷激情在线| 天天肏在线观看| 久草九一| 亚洲精品99| 色婷婷A| 色六月视频| 男人的天堂999| 4399精品一区二区| 激情五月激情综合网| 婷婷五月丁香超碰| 亚洲综合视频一下| 色婷婷网| www.久9| 亚洲mm免费| 激情99| 9999三级片| 99亚洲日韩| 天天日人人| 五月婷婷丁香| 综合www色| 日韩无码AV电影网站| 丁香五月天之婷婷影院| 五月婷网站| 婷婷色播六月无码| 99ri国产| 91成人看片| 夜夜AVV| 五月天婷婷色播综合在线| 天天干天天做| 9色在线视频| 亚州色色色| 色婷婷五月天天天干天天操天天爽| 天天弄天天爽| 丁香五月网址| 国产操B视频| 99热大| 91久久网站| 国产无套精品一区二区| 色婷婷香蕉| 99热在线观看精品| 五月色婷婷中文字幕| 色色a| 日本色色色| 91人妻视频| 五月天婷婷午夜丁香| 婷婷99中文字幕| 久草九九| 七七久久婷婷| 久热伊人91| 欧美性生交xXxX久久久| 第九色区av天堂| 婷婷五月激情小说| 综合激情五月丁香| 99在线视频播放| 一级二级色大片| 天天干天天操天天爱| 《》【无码】想被搞到爽AV应募而来的超M素人 西纯子 10musume-011723-01 | 久久一品区| 懂色av蜜臀av粉嫩av永陈冠希| 色吊丝99| 亚洲人成网站999综合| 91操女| 久超免费视频| 婷婷字幕在线| 伊人大香久久| 婷婷色基地在线看 | 日日影院 | 国产婷婷色综合AV蜜臀AV| 婷婷五月天av| 国产亚洲精品AAAAAAA片| 99无码视频| 日日夜夜天天| 欧美日韩精品人妻狠狠躁免费视频| 天堂成人A片永久免费网站| 激情第四色| 国产69久久久欧美黑人A片| 无码激情AAAAA片-区区| 欧美日韩成人在线| 噼里啪啦完整版中文在线观看| 五月天成人综合| 三人荫蒂添的好舒服A片| 精品亚洲国产成AV人片传媒| 少妇大叫太大太粗太爽了A片 | 日本天堂免费99| 精品久久9| 丁香婷婷色五月| 97五月天| 夫妇交换刺激做爰| 偷拍九九热| 97婷婷久久丁香| 大香AV| 性视频久久| 66精品国产成人| 99极品视频| 色小说五月婷婷| 丁香五月五月婷婷五月天激情四射| 一起草无码视频| 狠狠操天天操综合| 五月丁香婷婷人体| 五月婷婷啪| 91综合色| 精品人妻一区二区| 国产欧美日韩综合精品一区二区| 五月婷婷电影院| 97成人在线视频| 婷婷色吧| 91色在线/日韩| 丁香五月狠狠在线观看| 五月婷婷视频ab| 亚洲天码视频www蛋播视频| 久色五月丁香视频| 久久九九99| 五月天开心色色网| 91在线日本| 涩涩婷婷五月| 丁香六月激情综合| 任我肏视频精品| 91久久1118| 天天操天天操| 99久久久| 另类激情五月| 国精产品一区一区三区免费视频| 狠狠干最新地址| 538任你爽视频不一样的| 深爱婷婷基地| 操逼棍操逼| 日韩人妻在线观看| 777精品久无码人妻蜜桃| 久超免费视频| 精品人人操| 成人网站免费sxj| 九九热免费视频| 色五月婷婷久久| 婷婷五月丁香激情图片| 久久九九综合| 五月丁香六月婷婷在线播放| 99re热在线观看| 亚洲V国产V欧美V久久久久久| www.久操| 丁香六月婷月91婷月| 五月丁香狠狠地噜噜噜噜| 五月天激情婷婷| 久热超碰| 91久久九色| 九九视频精品这里只有| 五月天婷婷在线播放| 91综合在线| 色五月婷婷1| 丁香婷婷偷拍| 五月天久久网站| 色婷婷基地 | 丁香六月啪啪啪| 久久99成人性爱高清视频| 五月婷婷六月丁香综合在线| 在线一起草av| 99在线观看视频免费| 色婷婷亚洲精品天天综| 激情五月天网站| 欧美性爱一区| 亚洲九九99精品视频在线播放| 五月丁香六月激情综合| 欧美日韩成卜| 99热18| 五月天婷婷久久| 色爆五月| 九九热视频精品2| 日本色综合| 久久性爱网| 99久久精彩视频。| 五月婷婷成人| 久久久天堂国产精品女人| 久久婷婷综合网| 中文字幕久久一区二区三区 | 欧洲激情网站| 婷婷五月天六月丁香| 国产精产国品一二三在观看| 日本超碰在线| 怡红院一二三| 国产日韩精品SUV| 碰碰91| 亚洲最大视频| 色综合综合综合| 99综合免费视频| 少妇搡BBBB搡BBB搡毛茸茸 | 男女av免费看| 激情婷婷人妻| 激情五月丁香五月| 99色在线观看视频| 亚洲五月天婷婷在线| 精品人妻伦九区久久AAA片| 中文AV在线播放| 丁五月激情视频免费| 久久九九激情五月天| 婷婷综合网在线| 99亚洲天堂| 狠狠色丁香婷婷久久综合| 国产VA亚洲VA96| 成人AV在线中文版| 桃色激情婷婷伊人网| 久久婷婷六月综合| 97久久精品| 激情99| 国产精品国产| 色播播五月| 手机在线视频观看9| 婷婷色五月情| 丁香六月在线| 97香蕉碰碰人妻国产欧美| 色播五月网| 色色色9 9 9| 婷婷五月丁香伊人| 亲子乱AV-区二区三区| 色综合久久天天综合网| 99操碰| 99在线er热| 色色色色色综合| 欧美内射AA| 超碰在线人人| 97久久久久| 激情五月婷婷五月| 五月丁香婷婷欧美色图视频五月丁香777电影 | 九九九九精品精| 日韩抽插操逼| 五月天激情四射| 美女va| 丁香婷婷五月| 五月丁香婷婷在线| 91精品久久久久久| 色婷婷狠狠18禁| 色婷婷色99国产综合精品| www.五月婷婷久久.com| 亚洲视频操| 五月丁香少妇| 激情亚洲婷婷| 久久在这里99| 无码人妻激情| 天天插天天玩天天干| 天天综合网在线| ...婷婷国产成人亚洲日韩| 国产成人精品亚洲线观看| 夜夜噜夜夜奇| 丁香五月天天久久综合小说| 九九亚洲小视频| 4399无码视频二区| 日本熟女一区二区| 中文AV网站| 日日操日日射| 九色综合五月天婷五月| 婷婷成人丁香色情基地30 | 91|九色|动漫| 成人在线不卡| 91人操人人人操人| 色欧美一级| 性爱在线播放av| 五月天电影网| 久久久区区一久久久久久| 亚洲六月综合激情久久下卡| 色五月综合激情| 97自拍99| 97人人操人人爽| 五月婷婷丁香深深爱| 日日躁夜夜躁狠狠久久AV| 丁香五月婷婷欧美成人色图| 91久久久久| 日本九九视频| 亚洲色五月| caop视频| 五月婷婷六月丁香综合| 亚洲综合视频网| 可以免费观看的AV| 久久婷婷在线| 91丨人妻丨国产丨丝袜| 操人91| 激情五月天综合图片小说网站| www.婷婷激情网.com| 五月丁香六月婷婷激情网| 男人的天堂999| 精品成人在线| 69久久99精品久久久久婷婷| 狠狠干狠狠色| 日本综合99| 婷婷欧美| 狠狠艹狠狠艹| 黄色激情网站在线观看| 免费婷婷| 五月 丁香 欧美| 激情无码网| 网站免费一站二站| 天天肏天天爽夜夜爽| 日韩高清久久| 久久综合五月天| 婷婷成人五月天成人文学小说| 91婷婷色| 亚州美女| 久久久久人妻网址| 婷婷五月天在线看| 丁香五月激情图片婷婷| 美欧成人视频| av 一区三区四区| 婷婷94s| 婷婷丁香九月| 五月丁香在线精品| 婷婷新网址| AV性爱网| 九九热视频免费观看| 婷婷五月综合色拍| 五月丁香六月激情综合| 激情五月天激情综合网| 人妻激情综合| 丁香五月综合狠狠| www.99久| 。久久久久久久久久久久久久人妻| 色综合天堂| 久久精品一区二区三区四区| 五月婷婷成人| 婷婷五月天日逼| 99热国内精品| 欧美色激情四射| 亚洲这里只有精品| 深爱五月婷| site:minyis.com| 9婷婷内射| 丁香六月丁香婷婷激情| 婷婷五月丁香超碰| 亚洲色综合性| 天天做天天爱天天搞| 大香蕉懂9| 狠狠色婷婷| 91九色中文字幕女在线观看| 99热在这里只有免费精品| 日hao1区| 色综合99| 五月丁香六月婷| 精品,99| VA日本视频| 五月婷在线影院| 午夜]香婷婷深深爱| 狠狠综合网| 天天肏高清在线| 日韩AV色色色| 日日夜夜天天综合| 亚洲激情综合免费| 99色啊| 99热偷拍| 97超级啪啪在线观看| 中文av网|