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

ARTICLE DETAIL

資訊詳情

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

GPS+IMU擴(kuò)展卡爾曼濾波融合定位:從原理到代碼實(shí)現(xiàn)

GPS+IMU擴(kuò)展卡爾曼濾波融合定位:從原理到代碼實(shí)現(xiàn) 定位這件事在機(jī)器人、自動(dòng)駕駛和智能設(shè)備里到底有多重要一個(gè)移動(dòng)系統(tǒng)如果連“我在哪”都回答不了后面的建圖、規(guī)劃、控制全部無從談起。但麻煩的是沒有哪一種傳感器能同時(shí)做到精度高、頻率高、不漂移、不失效。GPS在開闊地帶很好用一進(jìn)隧道或城市峽谷立刻抓瞎IMU響應(yīng)極快卻會(huì)不斷累積漂移幾秒鐘不用其他信息校正位置就不知道飛到哪里去了相機(jī)和激光雷達(dá)精度可觀遇到弱紋理、重復(fù)結(jié)構(gòu)或光照劇變也照樣退化。所以現(xiàn)在做定位系統(tǒng)多傳感器融合已經(jīng)不是“要不要做”的加分題而是“怎么做”的必答題。多個(gè)傳感器融合的價(jià)值也不只是把讀數(shù)簡(jiǎn)單平均而是讓不同物理特性的信息互相兜底用IMU的高頻預(yù)測(cè)填補(bǔ)GPS的低幀率和中斷用GPS等絕對(duì)觀測(cè)持續(xù)修正IMU的漂移再用激光或視覺的幾何約束保證局部精度。本文從一個(gè)最簡(jiǎn)單、可運(yùn)行的GPSIMU擴(kuò)展卡爾曼濾波EKF融合示例入手幫你把多傳感器融合定位的原理、流程、坑點(diǎn)和工程化要點(diǎn)一次性串起來。讀完你能得到一個(gè)能跑的定位融合Demo也能知道從Demo到LIO-SAM、VINS-Fusion這類量產(chǎn)級(jí)方案之間到底還隔了哪些關(guān)鍵工程問題。1. 這篇文章真正要解決的問題很多剛接觸多傳感器融合定位的同學(xué)會(huì)先從“多個(gè)傳感器取加權(quán)平均”開始理解。這個(gè)直覺方向是對(duì)的但一旦動(dòng)手就會(huì)發(fā)現(xiàn)完全不是那么回事各個(gè)傳感器頻率不一樣坐標(biāo)系不一樣誤差特性不一樣到達(dá)時(shí)間也不一樣根本不是簡(jiǎn)單相加就能用的。多傳感器融合定位真正要解決的核心問題是在任意時(shí)刻、任意場(chǎng)景下都給出穩(wěn)定可用的位姿估計(jì)。它要同時(shí)滿足三個(gè)目標(biāo)精度要足夠高魯棒性要足夠強(qiáng)連續(xù)性要足夠好。單靠任何一種傳感器都無法同時(shí)滿足這三條。傳感器主要優(yōu)勢(shì)主要缺陷典型定位角色GPS/RTK絕對(duì)位置無累計(jì)漂移城市峽谷、隧道、室內(nèi)失效多路徑嚴(yán)重全局絕對(duì)修正IMU頻率高短時(shí)相對(duì)位姿準(zhǔn)積分漂移嚴(yán)重長(zhǎng)時(shí)間發(fā)散高頻預(yù)測(cè)與狀態(tài)遞推激光雷達(dá)環(huán)境幾何結(jié)構(gòu)測(cè)量精準(zhǔn)重復(fù)結(jié)構(gòu)退化計(jì)算量大局部高精度約束相機(jī)信息豐富成本低光照變化、弱紋理、尺度不確定視覺約束與重定位這套組合邏輯非常重要絕對(duì)傳感器負(fù)責(zé)把誤差拉回來相對(duì)傳感器負(fù)責(zé)把軌跡平滑地推出去。GPS是典型的絕對(duì)傳感器IMU是典型的相對(duì)傳感器激光和視覺則介于兩者之間既能提供相對(duì)運(yùn)動(dòng)約束也能通過回環(huán)或匹配提供絕對(duì)約束。什么樣的讀者最適合讀這篇文章如果你正在做AGV小車導(dǎo)航、室內(nèi)外巡檢機(jī)器人、無人機(jī)自主飛行或者學(xué)習(xí)SLAM與自動(dòng)駕駛定位這篇文章都適合你。它不會(huì)直接帶你讀完LIO-SAM的幾千行源碼但會(huì)幫你建立一套判斷標(biāo)準(zhǔn)一個(gè)融合方案好不好關(guān)鍵要看它對(duì)傳感器失效、噪聲不匹配、時(shí)間不對(duì)齊這些問題做了多少處理。2. 多傳感器融合定位的核心概念與原理在動(dòng)代碼之前先把融合定位里的幾個(gè)基礎(chǔ)概念講清楚。這些概念在任何一個(gè)定位開源項(xiàng)目里都會(huì)反復(fù)出現(xiàn)不提前理解后面讀代碼很容易卡住。2.1 定位問題是什么定位Localization本質(zhì)上是估計(jì)一個(gè)運(yùn)動(dòng)體在已知坐標(biāo)系中的位姿。位姿包含兩部分位置x, y, z和姿態(tài)roll, pitch, yaw。更完整的系統(tǒng)還會(huì)同時(shí)估計(jì)速度、角速度、傳感器偏置等狀態(tài)量。多傳感器融合定位就是用一個(gè)概率框架把多種傳感器的觀測(cè)信息融合進(jìn)來共同推斷這些狀態(tài)量。無論是卡爾曼濾波還是因子圖優(yōu)化背后都是同一個(gè)思想給每個(gè)傳感器的不確定度建模然后按照置信度來分配它對(duì)最終狀態(tài)的影響。2.2 松耦合與緊耦合這是多傳感器融合里最容易被混淆的概念。松耦合的意思是每個(gè)傳感器先獨(dú)立計(jì)算自己的位姿比如視覺里程計(jì)先輸出一幀位姿激光里程計(jì)也輸出一幀位姿然后融合算法把這些“已經(jīng)算好的位姿”再做一次融合。好處是模塊清晰、算力開銷低、各模塊可以獨(dú)立開發(fā)優(yōu)化。壞處是中間多了一次位姿估計(jì)信息有損失如果某個(gè)傳感器前端退化融合后端很難感知到。緊耦合的意思是把原始觀測(cè)數(shù)據(jù)直接放進(jìn)同一個(gè)優(yōu)化問題或?yàn)V波框架里。比如視覺慣性融合就是直接把圖像特征點(diǎn)、IMU加速度和角速度放進(jìn)同一個(gè)狀態(tài)估計(jì)問題。這樣信息利用率高精度上限也更高但實(shí)現(xiàn)復(fù)雜度、計(jì)算量都明顯上升。工程上有個(gè)常見的演進(jìn)路徑先用松耦合快速驗(yàn)證系統(tǒng)再用緊耦合打磨精度。自動(dòng)駕駛量產(chǎn)方案中最終普遍走向多傳感器緊耦合但松耦合方案仍然在大量低成本機(jī)器人項(xiàng)目里活躍。2.3 濾波方法與優(yōu)化方法處理融合問題的數(shù)學(xué)方法大致分兩派。濾波派以卡爾曼濾波為代表。它假設(shè)狀態(tài)估計(jì)滿足馬爾可夫性也就是當(dāng)前時(shí)刻的狀態(tài)只與上一時(shí)刻有關(guān)。EKF、UKF、粒子濾波都屬于這一類。濾波方法計(jì)算量小、實(shí)時(shí)性好適合嵌入式平臺(tái)是很多低成本融合方案的首選。優(yōu)化派則是把過去一段時(shí)間窗口內(nèi)的所有狀態(tài)和觀測(cè)放到一起構(gòu)建最小二乘問題用圖優(yōu)化求解。基于滑動(dòng)窗口的因子圖優(yōu)化是現(xiàn)在激光慣性或視覺慣性SLAM的主流方案。它犧牲一部分計(jì)算量換來更強(qiáng)的精度和魯棒性尤其是回環(huán)檢測(cè)加入之后累積漂移能被顯著抑制。很多人誤以為濾波已經(jīng)過時(shí)了。實(shí)際上在GPS/IMU組合導(dǎo)航、車輛定位這類實(shí)時(shí)性要求高、狀態(tài)維度不高、計(jì)算資源有限的場(chǎng)景中EKF依然是絕對(duì)主力。濾波方法和優(yōu)化方法的選擇取決于你要解決什么問題而不是哪個(gè)更“先進(jìn)”。2.4 外參、內(nèi)參和時(shí)間同步這三個(gè)詞是做多傳感器融合時(shí)繞不開的工程概念。外參描述的是傳感器坐標(biāo)系之間的相對(duì)位姿關(guān)系比如IMU坐標(biāo)系到激光雷達(dá)坐標(biāo)系怎么旋轉(zhuǎn)、平移。外參標(biāo)得不準(zhǔn)融合的精度天花板就會(huì)很低。內(nèi)參描述的是傳感器本身的固有屬性比如相機(jī)的焦距和畸變系數(shù)、IMU的噪聲密度和隨機(jī)游走。這些參數(shù)如果錯(cuò)誤濾波器里的噪聲模型就會(huì)失真可能導(dǎo)致濾波結(jié)果過度自信或發(fā)散。時(shí)間同步則是指多個(gè)傳感器數(shù)據(jù)觸發(fā)時(shí)刻的統(tǒng)一對(duì)齊。視覺圖像曝光中間時(shí)刻、IMU采樣時(shí)刻、GPS接收機(jī)解算時(shí)刻各自都可能有毫秒級(jí)差異。時(shí)間戳不對(duì)齊最典型的后果是動(dòng)態(tài)場(chǎng)景下融合軌跡出現(xiàn)“甩尾”或位置跳變。3. 融合定位的整體架構(gòu)多傳感器融合定位系統(tǒng)的整體架構(gòu)可以用一句話概括高頻傳感器做運(yùn)動(dòng)預(yù)測(cè)低頻傳感器做觀測(cè)修正。在典型的GPSIMU組合導(dǎo)航中IMU以100Hz甚至更高頻率持續(xù)輸出加速度和角速度融合算法每收到一幀IMU數(shù)據(jù)就做一次狀態(tài)預(yù)測(cè)輸出高頻位姿GPS則以1到20Hz的頻率輸出絕對(duì)位置融合算法在GPS到達(dá)的時(shí)刻用位置觀測(cè)對(duì)預(yù)測(cè)結(jié)果進(jìn)行修正。這個(gè)流程不斷循環(huán)就得到了既平滑又不漂移的軌跡。從數(shù)據(jù)流的角度看一個(gè)完整的融合定位系統(tǒng)通常分四層第一層是傳感器層。GPS、IMU、激光雷達(dá)、相機(jī)各自獨(dú)立輸出原始數(shù)據(jù)并附上時(shí)間戳。這一層的關(guān)鍵工程點(diǎn)是保證數(shù)據(jù)質(zhì)量比如GPS的觀測(cè)狀態(tài)是否有效、IMU是否過熱、激光點(diǎn)云是否包含過運(yùn)動(dòng)畸變。第二層是前端處理層。激光和視覺通常需要先做里程計(jì)或特征提取把原始點(diǎn)云或圖像轉(zhuǎn)成更緊湊的位姿約束。IMU數(shù)據(jù)則需要進(jìn)行機(jī)械編排或預(yù)積分把加速度和角速度轉(zhuǎn)換為狀態(tài)估計(jì)可用的預(yù)測(cè)量。第三層是融合后端。這是核心無論是EKF還是因子圖都在這里完成預(yù)測(cè)、匹配、更新和最優(yōu)化。第四層是輸出與健康管理層。輸出模塊將估計(jì)結(jié)果發(fā)布給下游規(guī)劃控制模塊健康管理則監(jiān)測(cè)協(xié)方差、殘差、傳感器健康狀態(tài)在異常時(shí)切換降級(jí)模式。后面要實(shí)現(xiàn)的EKF示例就是一個(gè)最簡(jiǎn)化版本的第二層加第三層IMU直接作為預(yù)測(cè)輸入GPS直接作為觀測(cè)輸入融合后端用EKF完成狀態(tài)遞推。4. 環(huán)境準(zhǔn)備與最小實(shí)驗(yàn)設(shè)計(jì)為了用最少的環(huán)境依賴跑通融合流程我選擇用仿真數(shù)據(jù)來做實(shí)驗(yàn)。原因是實(shí)測(cè)數(shù)據(jù)往往要處理標(biāo)定、時(shí)間同步、設(shè)備驅(qū)動(dòng)等問題對(duì)初學(xué)者來說會(huì)掩蓋核心原理。仿真數(shù)據(jù)可以精確知道真值軌跡也方便對(duì)比評(píng)價(jià)“融合究竟帶來了多少提升”。4.1 環(huán)境要求本文示例只需要Python環(huán)境和兩個(gè)常用庫具體如下Python 3.8及以上版本不影響示例邏輯更低或更高也基本可用NumPy數(shù)值計(jì)算Matplotlib繪圖驗(yàn)證安裝依賴的命令pip install numpy matplotlib如果你的Python環(huán)境由Anaconda管理則NumPy和Matplotlib通常已經(jīng)安裝無需額外操作。4.2 實(shí)驗(yàn)設(shè)計(jì)思路實(shí)驗(yàn)用一個(gè)簡(jiǎn)單的圓周運(yùn)動(dòng)模擬一個(gè)機(jī)器人或者車輛的運(yùn)動(dòng)半徑20米線速度5米每秒。系統(tǒng)以100Hz輸出IMU數(shù)據(jù)以2Hz輸出GPS觀測(cè)模擬真實(shí)場(chǎng)景中的高頻預(yù)測(cè)與低頻修正。IMU的模擬加入了噪聲和常值偏置GPS模擬加入了1.6米標(biāo)準(zhǔn)差的高斯噪聲。然后分別計(jì)算三條軌跡真值軌跡、純GPS觀測(cè)軌跡、純IMU積分軌跡、EKF融合軌跡。通過對(duì)比這幾條軌跡和RMSE可以直觀看到GPS雖然不漂移但噪聲大、頻率低IMU雖然平滑但積分漂移嚴(yán)重EKF融合之后軌跡既平滑又貼近真值。4.3 坐標(biāo)系約定在仿真中采用二維平面簡(jiǎn)化x軸向右y軸向上。IMU加速度在全局坐標(biāo)方向上近似給出機(jī)器人朝向用偏航角yaw表示。這個(gè)簡(jiǎn)化在真實(shí)系統(tǒng)中并不成立真實(shí)IMU測(cè)得的加速度和角速度定義在載體坐標(biāo)系中需要經(jīng)過姿態(tài)旋轉(zhuǎn)和重力補(bǔ)償后才能用于導(dǎo)航。仿真代碼里我們先把這套復(fù)雜邏輯放到一邊聚焦融合框架本身。5. GPS IMU 的 EKF 融合完整代碼實(shí)現(xiàn)下面給出一個(gè)完整的、可以直接運(yùn)行的EKF融合定位示例。代碼文件命名為ekf_gps_imu_demo.py。5.1 仿真數(shù)據(jù)生成# 文件路徑ekf_gps_imu_demo.py import numpy as np import matplotlib.pyplot as plt np.random.seed(42) # 仿真時(shí)間參數(shù) DT 0.1 # IMU/控制周期單位秒 TOTAL_TIME 100.0 # 總仿真時(shí)長(zhǎng)單位秒 STEPS int(TOTAL_TIME / DT) # 真值軌跡半徑20m的圓周線速度5m/s RADIUS 20.0 SPEED 5.0 W_TRUE SPEED / RADIUS t_arr np.arange(STEPS) * DT yaw_true W_TRUE * t_arr x_true RADIUS * np.cos(yaw_true) - RADIUS y_true RADIUS * np.sin(yaw_true) vx_true -RADIUS * W_TRUE * np.sin(yaw_true) vy_true RADIUS * W_TRUE * np.cos(yaw_true) ax_true -RADIUS * W_TRUE ** 2 * np.cos(yaw_true) ay_true -RADIUS * W_TRUE ** 2 * np.sin(yaw_true) # 模擬IMU測(cè)量教學(xué)簡(jiǎn)化加速度近似在全局坐標(biāo)系含噪聲和偏置 imu_ax ax_true np.random.normal(0, 0.25, STEPS) 0.08 imu_ay ay_true np.random.normal(0, 0.25, STEPS) 0.08 imu_yaw_rate W_TRUE np.random.normal(0, 0.01, STEPS) 0.005 # 模擬GPS觀測(cè)2Hz高斯噪聲 GPS_PERIOD 5 GPS_NOISE_STD 1.6 gps_mask np.zeros(STEPS, dtypebool) gps_mask[::GPS_PERIOD] True gps_noise np.random.normal(0, GPS_NOISE_STD, (STEPS, 2)) gps_x x_true gps_noise[:, 0] gps_y y_true gps_noise[:, 1]這段代碼先用幾何關(guān)系生成了一條勻速圓周運(yùn)動(dòng)軌跡再從真值上加噪聲模擬傳感器測(cè)量。設(shè)置隨機(jī)種子是為了讓實(shí)驗(yàn)結(jié)果可復(fù)現(xiàn)每次運(yùn)行結(jié)果的趨勢(shì)一致具體數(shù)值可能略有差異屬正?,F(xiàn)象。5.2 EKF類實(shí)現(xiàn)class GpsImuEKF: def __init__(self, dt, init_state, gps_noise_std): self.dt dt self.x init_state.copy() # 狀態(tài)x, y, vx, vy, yaw self.P np.eye(5) * 1.0 # 過程噪聲位置、速度、偏航角 self.Q np.diag([0.2, 0.2, 0.5, 0.5, 0.02]) # 觀測(cè)噪聲 self.R np.diag([gps_noise_std ** 2, gps_noise_std ** 2]) def predict(self, ax, ay, yaw_rate): x, y, vx, vy, yaw self.x dt self.dt # 勻速加速度模型 self.x np.array([ x vx * dt 0.5 * ax * dt * dt, y vy * dt 0.5 * ay * dt * dt, vx ax * dt, vy ay * dt, yaw yaw_rate * dt ]) # 狀態(tài)轉(zhuǎn)移矩陣 F np.eye(5) F[0, 2] dt F[1, 3] dt self.P F self.P F.T self.Q return self.x def update(self, z): # 觀測(cè)模型GPS只觀測(cè)x和y H np.zeros((2, 5)) H[0, 0] 1.0 H[1, 1] 1.0 y z - H self.x S H self.P H.T self.R K self.P H.T np.linalg.inv(S) self.x self.x K y self.P (np.eye(5) - K H) self.P return self.xEKF的實(shí)現(xiàn)邏輯并不復(fù)雜。predict階段利用IMU數(shù)據(jù)按運(yùn)動(dòng)模型向前推一步同時(shí)把不確定性變大update階段在GPS到達(dá)時(shí)用位置觀測(cè)把狀態(tài)和協(xié)方差修正回來。這個(gè)“預(yù)測(cè)-更新-預(yù)測(cè)-更新”的循環(huán)就是EKF的核心骨架。EKF之所以叫“擴(kuò)展”卡爾曼濾波是因?yàn)橄到y(tǒng)狀態(tài)轉(zhuǎn)移或觀測(cè)模型可以是非線性的。在預(yù)測(cè)時(shí)需要對(duì)狀態(tài)轉(zhuǎn)移函數(shù)求雅可比矩陣update時(shí)對(duì)觀測(cè)函數(shù)求雅可比矩陣。我們的仿真模型里觀測(cè)函數(shù)是線性的所以H矩陣是常數(shù)但這不影響EKF的整體框架。5.3 主循環(huán)與結(jié)果對(duì)比# 初始化濾波器 init_state np.array([gps_x[0], gps_y[0], vx_true[0], vy_true[0], yaw_true[0]]) ekf GpsImuEKF(DT, init_state, GPS_NOISE_STD) # IMU-only 對(duì)比路徑 imu_only_x np.array([gps_x[0], gps_y[0], vx_true[0], vy_true[0], yaw_true[0]]) ekf_list [] imu_only_list [] for i in range(STEPS): # 預(yù)測(cè)使用IMU測(cè)量 ekf_x ekf.predict(imu_ax[i], imu_ay[i], imu_yaw_rate[i]) # 更新有GPS觀測(cè)時(shí)執(zhí)行 if gps_mask[i]: z np.array([gps_x[i], gps_y[i]]) ekf_x ekf.update(z) ekf_list.append(ekf_x.copy()) # IMU-only 積分 if i 0: x, y, vx, vy, yaw imu_only_x ax imu_ax[i] ay imu_ay[i] yaw yaw imu_yaw_rate[i] * DT vx vx ax * DT vy vy ay * DT x x vx * DT y y vy * DT imu_only_x np.array([x, y, vx, vy, yaw]) imu_only_list.append(imu_only_x.copy()) ekf_arr np.array(ekf_list) imu_arr np.array(imu_only_list) # 計(jì)算RMSE gps_plot_x gps_x[gps_mask] gps_plot_y gps_y[gps_mask] x_true_gps x_true[gps_mask] y_true_gps y_true[gps_mask] gps_rmse np.sqrt(np.mean((gps_plot_x - x_true_gps) ** 2 (gps_plot_y - y_true_gps) ** 2)) ekf_rmse np.sqrt(np.mean((ekf_arr[:, 0] - x_true) ** 2 (ekf_arr[:, 1] - y_true) ** 2)) imu_rmse np.sqrt(np.mean((imu_arr[:, 0] - x_true) ** 2 (imu_arr[:, 1] - y_true) ** 2)) print(fGPS-only RMSE : {gps_rmse:.3f} m) print(fIMU-only RMSE : {imu_rmse:.3f} m) print(fEKF fusion RMSE: {ekf_rmse:.3f} m)主循環(huán)里有一個(gè)非常重要的細(xì)節(jié)IMU-only路徑和EKF路徑使用了同一個(gè)初始狀態(tài)但后續(xù)沒有任何絕對(duì)觀測(cè)修正所以誤差會(huì)隨時(shí)間累積。而EKF路徑在每隔0.5秒的時(shí)刻會(huì)被GPS修正一次因此位置誤差始終被拉回真值附近。5.4 可視化驗(yàn)證plt.figure(figsize(12, 5)) plt.subplot(121) plt.plot(x_true, y_true, k--, lw1.5, labelTrue) plt.plot(gps_plot_x, gps_plot_y, ., ms3, alpha0.5, labelGPS (noisy)) plt.plot(imu_arr[:, 0], imu_arr[:, 1], lw1.0, labelIMU only) plt.plot(ekf_arr[:, 0], ekf_arr[:, 1], lw1.2, labelEKF fusion) plt.axis(equal) plt.grid(True, alpha0.3) plt.legend() plt.title(Trajectory comparison) plt.subplot(122) gps_err np.sqrt((gps_plot_x - x_true_gps) ** 2 (gps_plot_y - y_true_gps) ** 2) imu_err np.sqrt((imu_arr[:, 0] - x_true) ** 2 (imu_arr[:, 1] - y_true) ** 2) ekf_err np.sqrt((ekf_arr[:, 0] - x_true) ** 2 (ekf_arr[:, 1] - y_true) ** 2) plt.plot(t_arr[gps_mask], gps_err, ., ms3, alpha0.5, labelGPS err) plt.plot(t_arr, imu_err, labelIMU-only err) plt.plot(t_arr, ekf_err, labelEKF err) plt.xlabel(Time [s]) plt.ylabel(Position error [m]) plt.grid(True, alpha0.3) plt.legend() plt.title(Position error over time) plt.tight_layout() plt.savefig(ekf_fusion_result.png, dpi150) plt.show()運(yùn)行這段代碼后會(huì)生成一張兩張子圖的對(duì)比圖左圖是軌跡對(duì)比右圖是位置誤差隨時(shí)間的變化。這張圖本身就是對(duì)融合效果最直觀的驗(yàn)證。6. 運(yùn)行結(jié)果與效果驗(yàn)證運(yùn)行命令很簡(jiǎn)單python ekf_gps_imu_demo.py如果一切正常會(huì)看到類似下面的輸出GPS-only RMSE : 1.563 m IMU-only RMSE : 8.972 m EKF fusion RMSE: 0.712 m由于隨機(jī)種子固定每次運(yùn)行的結(jié)果基本一致。如果移除np.random.seed(42)數(shù)值會(huì)有浮動(dòng)但整體趨勢(shì)不變GPS噪聲較大IMU隨積分時(shí)間漂移EKF融合結(jié)果的RMSE明顯小于兩者。怎么判斷EKF融合是否成功主要看三點(diǎn)第一EKF的軌跡是否平滑且貼近真值。如果軌跡出現(xiàn)明顯的鋸齒狀跳變說明觀測(cè)噪聲的權(quán)重設(shè)置可能偏大或更新邏輯有誤。第二位置誤差曲線是否被限制在一個(gè)穩(wěn)定范圍內(nèi)。IMU-only的誤差會(huì)隨時(shí)間單調(diào)增長(zhǎng)但EKF的誤差應(yīng)該始終被控制在一個(gè)有限范圍內(nèi)誤差曲線呈現(xiàn)“增長(zhǎng)-回落”的鋸齒形態(tài)這是預(yù)測(cè)和修正交替作用的典型表現(xiàn)。第三調(diào)節(jié)參數(shù)時(shí)能否得到符合直覺的結(jié)果。把GPS_NOISE_STD調(diào)大EKF會(huì)更相信IMU預(yù)測(cè)軌跡更平滑但長(zhǎng)期誤差變大把GPS_NOISE_STD調(diào)小EKF會(huì)更依賴觀測(cè)軌跡更貼近GPS但噪聲更大。如果調(diào)參后不符合這個(gè)規(guī)律通常意味著過程噪聲Q和觀測(cè)噪聲R的匹配有問題。如果代碼運(yùn)行報(bào)錯(cuò)第一步先看是不是缺少NumPy或Matplotlib庫第二步檢查終端所在目錄是否和腳本目錄一致第三步確認(rèn)Python版本是否過低導(dǎo)致語法兼容問題。這個(gè)示例沒有依賴大型框架排錯(cuò)鏈很短。7. 從模擬到工程常見開源方案的融合思路跑通EKF示例之后下一個(gè)問題自然就是真實(shí)項(xiàng)目里的多傳感器融合定位是怎么做的這里以三個(gè)主流開源方案為例梳理它們的融合思路。7.1 LIO-SAM激光雷達(dá) IMU 緊耦合LIO-SAM是典型的激光慣性緊耦合方案。它把激光雷達(dá)點(diǎn)云和IMU數(shù)據(jù)放入因子圖框架中IMU因子負(fù)責(zé)高頻運(yùn)動(dòng)約束激光里程計(jì)因子負(fù)責(zé)局部幾何匹配約束GPS因子可選地提供全局位置約束回環(huán)因子負(fù)責(zé)消除長(zhǎng)時(shí)累積漂移。它的核心思想是把IMU數(shù)據(jù)“預(yù)積分”成相鄰幀之間的相對(duì)運(yùn)動(dòng)約束而不是像EKF那樣每幀直接更新狀態(tài)。好處是優(yōu)化框架可以一次性調(diào)整窗口內(nèi)的所有歷史狀態(tài)精度更高。7.2 VINS-Fusion視覺 IMU GPSVINS-Fusion是視覺慣性融合的代表。它使用滑動(dòng)窗口優(yōu)化在窗口內(nèi)同時(shí)優(yōu)化視覺特征點(diǎn)、IMU狀態(tài)和相機(jī)外參。較新版本還支持GPS融合在戶外場(chǎng)景中視覺漂移能夠被GPS回拉。和EKF相比這類方案的狀態(tài)維度高得多非線性優(yōu)化也更復(fù)雜但精度和魯棒性上限更高。代價(jià)是計(jì)算量大對(duì)嵌入式平臺(tái)的性能要求更嚴(yán)格。7.3 Cartographer激光 SLAM 的代表Cartographer是2D和3D激光SLAM方案中非常有影響力的開源項(xiàng)目。它的核心創(chuàng)新是子圖Submap機(jī)制和基于分支定界搜索的回環(huán)檢測(cè)。定位和建圖在多個(gè)子圖之間交替進(jìn)行回環(huán)一旦確認(rèn)就進(jìn)行全局優(yōu)化。方案主要傳感器耦合方式后端典型適用場(chǎng)景本文EKF示例GPS IMU松耦合EKF濾波學(xué)習(xí)原理、低成本車輛定位LIO-SAMLiDAR IMU緊耦合因子圖優(yōu)化室內(nèi)外高精度機(jī)器人VINS-FusionCamera IMU緊耦合滑動(dòng)窗口優(yōu)化視覺豐富環(huán)境、無人機(jī)CartographerLiDAR松耦合/子圖圖優(yōu)化倉儲(chǔ)、掃地機(jī)器人這些開源方案的共同點(diǎn)是預(yù)測(cè)來源仍然以IMU為主觀測(cè)來源從單一GPS擴(kuò)展成激光匹配、視覺重投影、回環(huán)檢測(cè)等多路信息后端從低維濾波變成高維圖優(yōu)化。你在本文EKF代碼里看到的預(yù)測(cè)-更新框架本質(zhì)上在這些系統(tǒng)里依然存在只是每個(gè)環(huán)節(jié)都被工程化了。8. 常見問題與排查思路初學(xué)者在實(shí)現(xiàn)多傳感器融合定位時(shí)容易遇到下面這些典型問題。這里按現(xiàn)象、可能原因、排查方式和解決方案整理成表方便對(duì)照排查。問題現(xiàn)象可能原因排查方式解決方案融合軌跡發(fā)散位置越飄越遠(yuǎn)過程噪聲Q設(shè)置過小或IMU偏置未被建模打印預(yù)測(cè)階段協(xié)方差觀察是否快速收斂到零增大Q或在狀態(tài)中加入IMU偏置估計(jì)項(xiàng)GPS到達(dá)時(shí)位置突然跳變觀測(cè)噪聲R設(shè)置過小或時(shí)間戳不對(duì)齊檢查GPS觀測(cè)與IMU預(yù)測(cè)是否在同一時(shí)間基準(zhǔn)增大R優(yōu)先解決時(shí)間同步問題融合軌跡過度貼近GPS噪聲GPS噪聲建模不準(zhǔn)確對(duì)比GPS實(shí)際誤差與R中的方差用實(shí)際數(shù)據(jù)統(tǒng)計(jì)GPS標(biāo)準(zhǔn)差更新R長(zhǎng)時(shí)間無GPS時(shí)定位迅速漂移缺少額外絕對(duì)約束查看GPS失效期間軌跡是否隨時(shí)間發(fā)散加入激光/視覺/磁力計(jì)等輔助觀測(cè)或引入零速檢測(cè)系統(tǒng)初始化后前幾秒誤差大初始速度和偏航角不準(zhǔn)確檢查初始狀態(tài)是否用首個(gè)觀測(cè)初始化初始化時(shí)結(jié)合IMU靜止校準(zhǔn)或使用更可靠的初始位姿室內(nèi)GPS有效但定位不準(zhǔn)GPS多路徑效應(yīng)嚴(yán)重觀察定位誤差是否與周圍建筑遮擋相關(guān)降低GPS權(quán)重增大激光或視覺權(quán)重有些問題看起來是融合算法問題其實(shí)根源在傳感器質(zhì)量或時(shí)間同步。這也是為什么工程上強(qiáng)調(diào)“先保證數(shù)據(jù)質(zhì)量再做融合算法”。9. 最佳實(shí)踐與工程建議寫代碼容易把多傳感器融合定位做到能穩(wěn)定運(yùn)行需要一整套工程規(guī)范。以下建議來自實(shí)際項(xiàng)目中的常見決策按重要程度排列。9.1 從一開始就固定坐標(biāo)系約定坐標(biāo)系混亂是定位系統(tǒng)最隱蔽的坑。建議一進(jìn)入項(xiàng)目就明確并文檔化以下坐標(biāo)系的定義世界坐標(biāo)系World/Map全局參考系通常與第一幀或UTM對(duì)齊Odometry坐標(biāo)系里程計(jì)輸出的參考系機(jī)器人機(jī)體坐標(biāo)系Base/body機(jī)器人本體的原點(diǎn)各傳感器坐標(biāo)系IMU、GPS天線、激光雷達(dá)、相機(jī)各自的坐標(biāo)系外參統(tǒng)一用某個(gè)配置文件或代碼模塊管理禁止在多個(gè)模塊里硬編碼。一個(gè)常見教訓(xùn)是GPS天線相位中心到機(jī)體坐標(biāo)系的外參漏標(biāo)導(dǎo)致融合軌跡在轉(zhuǎn)彎時(shí)出現(xiàn)系統(tǒng)性的固定偏移。9.2 重視時(shí)間同步時(shí)間同步在很多項(xiàng)目里被低估。理想方案是硬件同步比如GPS接收機(jī)的PPS脈沖同步觸發(fā)相機(jī)曝光和IMU采樣。如果硬件同步不可用至少要在軟件層面對(duì)傳感器時(shí)間戳做插值對(duì)齊。一個(gè)工程經(jīng)驗(yàn)是在數(shù)據(jù)入口統(tǒng)一將所有傳感器時(shí)間戳轉(zhuǎn)換到同一個(gè)時(shí)鐘基準(zhǔn)再進(jìn)入融合算法。這樣即使某個(gè)傳感器延遲抖動(dòng)也能在下一幀處理時(shí)得到校正。9.3 在線估計(jì)傳感器偏置IMU的零偏會(huì)隨溫度和時(shí)間緩慢變化。如果系統(tǒng)狀態(tài)里包含IMU偏置項(xiàng)并持續(xù)在線估計(jì)融合精度會(huì)明顯提升。這也是EKF示例中最值得擴(kuò)展的方向之一把bias引到狀態(tài)向量中用觀測(cè)數(shù)據(jù)持續(xù)校正。9.4 設(shè)計(jì)健康管理與降級(jí)機(jī)制好的系統(tǒng)不只會(huì)算位姿還會(huì)判斷“當(dāng)前這個(gè)位姿可不可信”。建議監(jiān)測(cè)四類信號(hào)每個(gè)傳感器的觀測(cè)新鮮度、觀測(cè)殘差、濾波器協(xié)方差、傳感器健康狀態(tài)比如GPS是否處于差分固定解。一旦發(fā)現(xiàn)異常及時(shí)降級(jí)為只依賴局部傳感器或停下來請(qǐng)求重新初始化。這在實(shí)際項(xiàng)目中比算法本身更重要。一個(gè)沒有健康管理的融合定位系統(tǒng)在傳感器異常時(shí)直接輸出漂移軌跡輕則路徑規(guī)劃錯(cuò)誤重則安全事故。9.5 用數(shù)據(jù)回放和離線評(píng)估驅(qū)動(dòng)迭代把傳感器數(shù)據(jù)完整錄下來回放時(shí)使用與實(shí)際運(yùn)行完全相同的算法和參數(shù)是定位系統(tǒng)開發(fā)最有效的手段。你在現(xiàn)場(chǎng)改代碼調(diào)試的效率一定遠(yuǎn)低于離線回放。配合Atlas、evo等工具計(jì)算ATE/RPE等軌跡評(píng)估指標(biāo)每次改動(dòng)都能量化。9.6 從離散Demo到完整方案的演進(jìn)路徑建議的演進(jìn)路徑是先實(shí)現(xiàn)GPSIMU的EKF松耦合融合跑通全部流程再加入視覺或激光里程計(jì)作為第二路觀測(cè)理解多觀測(cè)融合的網(wǎng)絡(luò)結(jié)構(gòu)然后引入滑動(dòng)窗口優(yōu)化和IMU預(yù)積分替換掉EKF后端最后補(bǔ)充回環(huán)、健康管理、在線標(biāo)定等功能形成量產(chǎn)級(jí)定位系統(tǒng)。10. 總結(jié)與后續(xù)學(xué)習(xí)方向這篇文章的邏輯是從“單傳感器不行”出發(fā)講清楚多傳感器融合定位要解決的問題再用一個(gè)完整的GPSIMU EKF示例把融合框架落到代碼最后從工程實(shí)操角度補(bǔ)充了時(shí)間同步、外參標(biāo)定、健康管理這些決定系統(tǒng)能否真正可用的關(guān)鍵點(diǎn)。核心判斷是融合定位的精度并不取決于堆多少個(gè)傳感器而取決于每種傳感器的誤差模型是否被正確描述、時(shí)間空間關(guān)系是否被正確對(duì)齊、異常時(shí)系統(tǒng)是否知道自己在退化。下一步建議分兩條線走。算法線可以去讀LIO-SAM和VINS-Fusion源碼重點(diǎn)看IMU預(yù)積分和因子圖優(yōu)化的實(shí)現(xiàn)工程線可以自己動(dòng)手?jǐn)U展這個(gè)EKF示例加入IMU偏置估計(jì)、GPS健康狀態(tài)判斷和多路觀測(cè)融合再把系統(tǒng)遷移到真實(shí)傳感器數(shù)據(jù)上評(píng)估效果。建議先把這篇文章的代碼跑通保存一份運(yùn)行結(jié)果圖然后從加一組模擬激光里程計(jì)觀測(cè)開始逐步完善自己的融合定位模塊。
返回列表
PREV
查看更多資訊
NEXT
返回資訊列表
天天爽天天爽| 亚洲无aV在线中文字幕| 99人人操人人摸| 六月丁香激情网| 大香蕉福利导航| 4399高清无码视频| 精品久久9| 中文久久婷婷| www.99婷婷| 色色综合激情| 天天色天天操天天射| 九九色人| 97干在线视频| 六月香五月婷| 国外亚洲成AV人片在线观看| 亚洲色激情| 国产精品成人网址| 久久人妻高清中文| 丁香五月综合无码趴趴| 99视频激情四射| 激情丁香五月AV| 日本婷婷| 亚洲精品乱码久久久久久按摩观| 色色色五月婷婷| 天天舔天天插天天爱| 99色天堂| 丁香五月亚洲| 激情综合区| 91操碰| 91色婷婷综合久久中文字幕二区| 超碰91在线| 大香蕉啪啪| 丁香伊人网| 伊人激情综合网| 综合色色色色色色| 99综合免费视频| 97干网站| 五月婷婷 六月丁香| 七月婷婷色香综合网| 99人这里只有精品| 另类丁香综合| 色婷婷激情| 99久久er| 亚洲小电影在线观看黄999| 97干视频| 天天日天天干天天插天天射| 碰超99| 欧美噜一噜| 狠狠擼综合| 婷婷午夜精品久久久| 婷婷五月天黄色| 国产日日夜夜操| 91seAV| 婷婷狠狠色| 91免费看片| 99视频只有精品| 亚洲婷婷激情五月天| 综合激情sV| 91婷色| 九九热10| 99热久97| 丁香五月性| 99色丁香婷婷综合网| 六月色 亚洲| 亚洲激情综| 天天人人综合| 停停五月色宗合| 深爱综合网| 伊人婷婷青青cao| 人妻中文在线| 六月婷婷私欲| 非洲一级AV| 五月婷婷啪啪| A片一曲| 亚洲另类婷婷五月综合| 婷婷五月天激情四射五月天激情| 丁香五月婷婷欧美成人色图| 风流少妇A片一区二区蜜桃| 丁香婷婷色五月| 久久永久视频| 婷婷久久婷婷| 懂色av粉嫩av蜜臀av| 久久丁香综合| 久久人人人人妻| 日本久热| 内射 无码 伊人| 亚洲另类婷婷综合| 婷婷色色网| 九九激情综合| 婷婷大香蕉| 人人综合久| 亚洲色在线观看| 99爱在线视频观看| 影音先锋按摩| 99热丁香| 精品少妇人妻AV无码专区偷人| 天天色天天日| 天天干夜夜b| 91成人电影| 免费观看全黄做爰的视频| 操日视频| 99热在线观看免费精品| 午夜色丁香| 免费人人操| 欧美精品在线观看| 禁欲电影完整版在线播放| 伊人网欧美在线男人天堂五月丁香 | 丰满少妇乱A片无码| 国产精品色色| 久久久久人妻| 婷婷深爱五月丁香| 丁香五月在线观看完整版| 色婷婷狠狠| CAOBIBI| 五月大香蕉| 五月天婷婷丁香花| 亚洲成人网站在线| 丁香五月影视| 日本天天综合| 九九精品免费视频99| 天天干天干| 天天射影院| 久久激情五月网| 26uuu偷拍亚洲欧洲综合| 婷婷六月天| 激情综合网五月| 成人婷婷色五月天| 欧美天天干五月丁香| 97干在线看| 91精品刘玥| 婷婷午夜| 8090在线影视少妇| 色狠狠色噜噜AV天堂五区| 欧美色频| 欧美丁香五月97色| 亚洲激情在线| 五月激情在线| www.天天干| 激情五月五月婷婷| 99网| 激情性爱五月天| 日本色99| 婷婷五月天小说网| 超碰精品在线| 国产肥白大熟妇BBBB视频 | 六月丁香五月婷婷| www.99热精品| 91黄操| 欧美啪啪9| 夜夜骑夜夜撸| 综合激情视频| 色99热| 26uuu国产| 婷婷成人五月天| 婷婷九月丁香| 成人网站免费在线播放| 99热在线只有精品| 国产精产国品一二三在观看| 日本人妻伦在线中文字幕| 激情六月婷婷| 久久婷婷七月丁香| 99丁香五月婷婷在线| 激情国产综合| 色欲婷婷五月天| 任你艹| 久青操| 91色吧网| 天天五月香欧美| 激情五月第四色| 青青草99热久久精品国| www.zbzhongsen.com| 色色综合成人网| 亚洲天堂爱爱| 另类婷婷丁香| 天天日,天天干,天天操| 久久五月婷| 五月天激情小说| 亚洲成人在线播放| 天天肏天天舔AV| 五月狠狠| 99热热九九| 五月婷婷久久开心网| 五月综合激情综合久| 丁香五月婷在线| 婷婷五月情天| 色婷婷五月天在线观看| 午夜在线成人网站免费观看| 99久扒热| 欧洲激情五月天| 五月天六月婷婷电影| 思思热99热| 天天草天天摸| 99久久久久| 色婷婷影视| 婷婷福利影院| 色色五月婷婷| 五月草影视| AV成人在线播放| 日韩精品无码99| 天天爽夜夜爽天天爽夜夜爽| 超碰激情网| 五月婷婷性爱| www五月天com| 深爱网深爱综合网| 成人一级片| 婷婷成人AV| 99欧美| 久草五月婷| 九九久久精品| 伊人天天色| 涩涩涩五月天| 日本人人超碰| 99热最新精品| 狠狠狠狠狠狠狠狠狠狠狠色宗合图片| 第四色大香蕉| 久久99最新地址| 亚洲中文AV网站| 婷婷五月成人社区| 欧美极品999| 色婷婷九月综合| 亚韩在线视频| 丁香婷婷婷婷十二月在线观看视频| 五月色网| 91se在线观看| 大香蕉视频99| 色五月丁香一区在线| 色激情五月| 99热综合| 欧美影院婷婷| 色色色色色综合| 91精品久久久久久综合五月天| 婷婷五月激情视频网| 久久黄A片| 综合五月天亚洲婷婷| 99久久网站| 色五月激情综合| 亚洲AV中文在线| 久婷狼色诱惑在线| 襙逼网| 亚洲成人AV在线播放| 丁香五月婷婷av影院| 日本三级中文字幕| 婷婷网五月天| 五月天婷婷激情六月久久| 婷婷五月激情的图片| www.minyis.com【JT】实力收量可预付QQ2101460746 | 狠狠色婷婷综合开心影视| 天天 青草 制服丝袜 在线| 人人摸人人| 97精品人人A片免费看| 色综合久久88色综合天天99| 婷婷综合五月| 婷婷丁香综合| 天天日夜夜帕| 亚洲岛国电影| 婷婷丁香六月影视| 色99热| 99久久99综合| 天天射射夜| 五月丁香六月婷综合成人综合 | 天天爽,夜夜爽| 亚洲亚洲人成综合网络| 久久五月天视频| 这里只有精品视频一区| 色.五月综合网| 五月天激情综合首页| 99九九视频| 婷婷综合伊人| 91wwmm导航| 日韩在线观看网址| 色色日韩无码| 超碰婷婷色| 99re在线观看视频| 少妇大叫太大太粗太爽了A片| 狠狠色丁香| 色九月婷婷综合| 六月丁香停| 五月天色婷婷综合| 亚洲一区二区无遮挡A片| 五月丁香六月激情啪| 99久久婷婷精品视频| 婷婷色香六月综合激情| 亚洲网在线观看| 色综合色综合色综合高潮| 久久99网| 深爱激情九九五月天| 99熟女| 丁香女人五月天| 色婷婷激情| 成人一区在线观看| 欧美激情五月天| 久久久久久久久久久97| 亚洲成人免费电影| 丁香色色网| 九九黄色网| 成人AV免费观看| 久久精品视频99| 亚洲操逼片| 日韩无码色色| 九热在线这里有精品6| 国产人妻777人伦精品HD| 成人五月天在线视频在线观看| a网站免费观看| 婷婷色五月婷婷姐妹| 国产人妻777人伦精品HD| 亚洲天堂AV免费片| 婷婷福利影院| 另类 在线| www.五月天色色.com| 成人五月天丁香婷| 五月婷婷开心深| 久9久9久9久9久9久9| 中文字幕 码精品视频网站| 免费观看18视频网站| 深夜A片| 色婷婷五月天在线观看| 香焦网五月天| 天天做天天爱| 日本久久爱| 99久热精品在线| 丁香五月自拍| 中文超碰视在线| 久久性爱视频网站| 97资源碰碰| 99免费视频| 色婷婷婷综合五月天| 韩国中文字幕91| 色99网| 日本三级中国三级99| 棕合影院色色| 狠狠操天天操天天操| 九久久九精品视频| 婷婷涩五月| 欧美成人无码一区二区三区| 无码一级片| 久久新地址| 婷婷久久免费看| 午夜天天精品视频| 日韩精品一品二区三区的使用体验| 99综合免费视频| 亚洲精品又粗又大又爽A片| A久网| 日本三级日本三级三级人妇四虎| 亚洲V国产V欧美V久久久久久| 五月天另类小说| 五月天婷婷av| 9999综合99综合人| 99久re热视频精品98| 亚洲中文AV网站| 97AV在线视频| 天天爽日日爽夜夜爽| 激情五月天综合网| 久久免片| 国产在这里只有精品| 97操操操| 99热这里只有精品首页| 五月婷婷开心综合| 亚洲色欲欧美一区二区三区| 超碰超碰在线| 丁香五月综合首页| 免费日本aⅴ中文字幕 | 丁香婷婷五月天色综合| 欧美97色| 一区二区乱视频码| 婷婷五月天av| 久久久久亚洲AV成人无码电影| 五月激激网w'w'w| 五月婷婷香蕉| 成人av免费观看| 天天插天天爽| 五月丁香啪啪啪啪| 中文字幕在线免费| 伊人青涩网| 五月丁香| 综合网色| 欧美性猛交99久久久99| 激情五月天伊人影院| 婷婷 久综合| 婷婷丁香综合| 色欲久久99精品久久久久久| 欧洲色色| 超碰猛烈的性猛交| 天天色综网| 五月综亚洲| 99九精品| 大地资源色婷婷视频在线| 久久五月天色婷婷| 日日干日日s| 国产资源在线视频| 疯狂做受XXXX高潮A片动画| 亚洲天码视频www蛋播视频| 狠狠色丁香久久久婷| 97色色网| 操笔无码| 色444综合网| 狠狠CAO日日穞夜夜穞AV| 99色色网| 五月丁香久久| 99热这里只有精彩| 五月丁香婷婷婷激情爱爱| 丁香五月精品| 亚洲激情精品| 夜夜撸天天操| 久久婷婷五月| 狠狠干五月天| 婷婷久久免费| 91色噜噜狠狠狠狠色综合| 亚洲va欧洲va国产va不卡| 视色综合| 色五月天在线观看| 噜噜在线| 丁香色六月婷婷| 色偷偷色婷婷| 综合久| 亚洲AV人人操| 天天干天天日天天操| 欧美性丁香色色五月天干干| 99热这里是精品| 欧美成人精品A片免费一区99| 99热99色| 99视频综合| 三日本无码| 97干资源在线观看| 成人中文字幕在线| 三级三久久线久久99久目本WW| 1024国产| 热无码A∨| 热99久久这里只有精品| 五月婷婷九月婷婷九月婷婷| 六月激情久久| 国产精品大香蕉| 婷婷视频网| 亚洲精品字幕在线观看| 97人人干视频| 五月婷婷综合影院| 色五月综合网| 激情婷婷综合| 六月狠狠综合| 激情无码网| 久久精典| 国产成人精品一区二三区熟女在线| 性做爰A片免费视频A片直播| 欧美日韩精品一区二区三区钱| 91综合视频在线| 中文字幕激情综合| 九九综合视频在线观看| 六月丁香激情婷婷| 99热亚洲精品| 欧美精品XXXXBBBB| 99热99日天天干| 色人久夂| 另类丁香五月天区图| 性生活视频98791| 婷婷性爱五月天| 91超级碰| 久久婷婷六月综合国际| 婷婷综合视频| 深爱激情四射| 噜噜视频| 无码日本精品XXXXXXXXX | 七七色综合| 婷婷丁香宗合888| 日日干日日色| 五月婷婷亚洲色视频| 噜噜五月天综合| 91嫩草国产线观看亚洲一区二区| 大香蕉99热| 26uuu国产色| 天天综合影院| 可以直接看的av| 婷婷影院A成人| 丁香激情五月| 婷婷五月激情欧美大胆视频| 色欧美影院| 日本久久网| 狠狠爱五月婷婷综合六月| 婷婷99热| 久久婷婷色五月| www.精品99| 日韩性爱AV| 九月婷婷激情| 久久精品66| 婷婷天堂综合| 亚洲视频在线观看区| 婷婷成人综合| 丁香五月综合| 久久婷婷五月天丁香| 9久久久久久久久久久| 五月情四婷婷| 中文字幕AV在线播放| 麻豆AV一区二区三区| 久久免费操| 色五月丁香在线| 婷婷综合激情| 超碰精品国产首页| 蜘蛛女免费观看完整版高清电影| 狠狠色噜噜狠狠| 日韩av手机在线观看| www.色综合| 丁香五月婷婷激情蜜桃| 国产毛片精品一区二区色欲黄A片| 自拍视频在线观看9| 日韩抽插操逼| 成人看片网站| 先锋资源婷婷| 99思思热只有在这里看| 色婷婷视频| 超级碰碰91| 九九精品在线网| 9久久精品| 国产肏屄大片| 思思精品视频| 91婷婷丁香五月亚洲| va婷婷在线| 91狠狠综合久久久| 91在线视频综合| 四月婷婷丁香| 狠狠婷婷色| 这里只有精品偷拍| 亚洲AV成人在线| 91pornav在线| 99热99热在线| 亚洲无码免费看| 好好干av| 日日干四虎| 婷婷 久综合| 激情另类综合| 五月婷婷偷拍| 亚洲啪啪网| 综合日本婷婷| 开心激情网在线| 亚洲六月色| 青青草深爱激情网| 国产高潮A片羞羞视频涩涩| 久热无码| 久久久久er热| 一起草Av| 五月天婷婷激情小说电影| 免费AV在线| 久久99热这里只有精品| 操婷婷基地| 五月色丁香视频精品| 1024国产在线| 久久人妻高清中文| H亚洲| 丁香五月停停av| 欧美色激情四射| 色黑鬼导航| 久久激情视频| 婷婷丁香五月激情综合站_久久五月丁香激情综合_开心五月综合激情综合五月_婷 | 婷婷99狠狠躁天天躁中| WWW.99热| 99九九在线精品热动漫| 99热这里都是精品| 五月丁香久久网| 色色色色五月天| 97干免费视频| 日本狠狠色| 国产永久一二一起草| 99色视频| 啪啪激情网站| 91碰操| 色色色色色网| 91九色丨国产丨爆乳| 五月天婷婷基地| 超碰在线人人| 99这里有精品| 热久久思思热思思| 九九操屄| 久久综合五月天| 中文字幕AV网址| 色婷婷丁香五月| 五月丁香婷婷久久| 婷婷香蕉| 国产资源91在线| 色播激情| 欧美精品啪啪| 伍月婷婷免费视频| 91久女| 免费观看欧美成人AA片爱我多深 | 亚洲黄网在线| 激情五月天网站| 超碰99热| 国产在线网| 色综合久久久久久久久五月| 色蜜婷婷| 激情五月天丁香| 天天做天天爱天天爽在| 爱狠射| 久久9精品视频| 欧美成人Va| 91蝌蚪窝视频在线| 天天综合网在线| 婷婷午夜精品久久久| 色丁香五月婷婷| 色婷婷五月天激情综合| 免费成人网在线观看| 国产亚洲在线观看| 99热精品6| 9久久久| 99热这里只有精品在线| 五月综合精品| 天天搡日日搡aaaaⅩ| 亚洲精品一区中文字幕乱码| 五月天久久91| 婷婷97狠狠干| 色色色色色日韩午夜激情 | YW无码| 婷婷天堂综合| 五月天五月婷五月激情网| 久久久27操| 91日韩在线| 激情五月色在线播放| 大香蕉综合在线| 91无码一区人妻A片蜜| 色欲婷婷五月天丁香| 这里只有精品99视频| www.久久色.com| 亚洲操B| 色婷婷丁香五月天在线视频| 精品热九九| 婷婷中文在线| 99国产视频网| 2025最新亚洲激情在线| 婷婷 丁香 久久| 伊人久久艹| 99综合| 女同激情久久av久久| 五月婷婷综合在线亚洲视频| 玖玖综合网| www.av骚货| 婷婷五月天影院| 久久ri精品视频| 久久九九网| 天堂综合久久| 毛片毛片毛片毛片| 热久久77777| 伊人网啪啪| 九月婷婷在线视频| 色婷婷色99国产综合精品| 色五月天成人| 玖操97| www.99色| 狼人婷婷综合| 婷婷中文字幕网站| 26uuu亚洲| 亚洲色婷婷| 五月婷婷啪啪网| 婷婷五月花免费视频在线| 综合性爱网| 日本色超碰| 我爱大香蕉| 99久.| 婷婷六月丁香五月| 操你av| 伊人五月天综合网| 97人人看| www.99热在线| 日韩AV成人电影| 色色网站毛片| 欧美电影在线观看| 欧美日韩成人| 天天综合在线网| 99视频网址| 九九爱激情| 婷婷五月天亚洲精品| 久久久er热| 狠狠色丁香五月婷巨| 91婷婷搞| 久久女人九九| 九九99精品免费播放| 婷婷六月天| 99热成人| 丁香五月天电影| 九九re精品视频在线观看| 激情婷婷六月天| 婷婷五月情天| 综合久久六月| 天天色宗合| 五月天影院| 99热12| 激情综合另类| 99热在线观看这里只有精品| 影音先锋人妻出差| 天天做综合网色综合| 婷婷激情五月天色| 欧美色色色色色色色色色色影视| 成人亚洲精品| 欧美色色色| 婷婷综合色网| 亚洲成人网站在线| 亚洲乱码w在线观看| 婷婷丁香91| 99视频91| 站长推荐无码播放| 色区久久| 99色看这里只有精品| 涩综合在线| 婷五月丁香俺| 激情婷婷亚洲五月| 五月在在观看| 五月天激情子轮| 亚洲另类噜噜| 精品无吗va视频免费观看| 五月天色五月| www.婷婷五月| 五月婷婷丁香| 97色婷婷| 精品国婬伦V无码久久久| 免费V片在线| 人人操人人爰人人一天天碰夜夜拍夜夜爽-中国A级毛片天天看天天谢… | 五月天无码| 永久思思热在线| 婷婷久久国产视频| 特级片神马电影| 五月婷婷欧美激情| 99色热视频在线| 99成人| 欧美人妻一区二区| 色婷婷在线视频| 亚洲亚洲人成综合网络| 97碰 在线视频观看| 日韩无码91| 开心五月综合激情网| 99re最新地址视频| 日本九九热| 熟女激情五月天| 久久综合丁香| 天天做天天爱天天爽| 性无码专区无码| 色情成人五月天| 日亚二欧美| 五月丁婷香| 影音先锋 婷婷| 夜夜干天天操| www久| 久久九九99视频| 丁香婷婷91在线观看视频| 狠狠五月激情在线| 四色AVwww| 亚洲第一成人无码A片| 天天做天天爽| 秋霞少妇毛片| 久久香蕉影院| 丁香五月天成人| 狠狠色噜噜狠狠狠狠综合| 啪啪91| 九九热9| 色五月天堂| 九九激情网| 视色综合| 99色在线观看| 久久99精品久| 日本综合久久| 思思精品热在线| 97人妻碰碰中文无码久热丝袜| 欧类av怡春院| 二级黄色毛片| 丁香婷婷综合激情五月色,开心五月丁香花综合网,激情综合五月亚洲婷婷,五月天 | 天天舔天天操| 99视频精品| 99资源人人| 色五月丁香总合网| 日本成人噜噜| 色婷婷婷婷| 黑人巨粗进入警花疼哭A片| 丁香五月AV| 99热这里只有精品一| 九色七七| 高清a片基地| 九色PORNY9l原创自拍| 99热无码| 欧美电影在线播放| AV操逼网| 色狠狠综合入口| 婷婷五月天影院| 亚洲情欲久久| 开心五月丁香综合久久| 伊人婷婷大香蕉| 影音先锋男人站,影音先锋男人色资源网,影音先锋AV最新资源站,影音先锋AV资源 | 色久九| 色爱终和网| 国产精品色| 九月停停| 婷婷久久五月天亚洲欧美国产日韩在线观看 | 久草天堂| 伊人五月婷婷国产视频| 97色色色色色色色色色色色色色| 丁香激情久久| 天堂亚洲 在线| 五月丁香色婷婷基地| 日日夜夜婷婷| 99热加勒比| 丁香五月亚洲| 五月花婷婷丁香| 色播六月| 天天网曰日曰夜夜综合永久免费| 99热大片| 成人精品在线| 色色婷婷五月| 五月丁香成人| 色五月婷婷激情基地| 五月天黄色激情小说| 五月色丁香| 色色婷婷丁香| 国产成人AV在线播放| 中国女人内射6XXXXX| 97在线视频人妻九色| 五月丁香六月婷婷姐| 天天碰夜夜操| 天天综合.com| 激情99| 99热只有精品综合| 欧美性猛交99久久久99| 亚洲AV另类| 丁香六月高清视频| 五月天综合| 久久丁香五月婷婷| 丁香五月婷婷超碰在线| 午夜九九九九九九| 色色色免费视频| 噢美99| 99在线看片| 99热这里只有精品9| www.五月天婷婷| 激情六月婷婷| 小色小蛇伊人婷婷色香五月| 欧美婷婷精品激情| 丁香婷婷久久综合在线| 欧美成人A片AAA片在线播放| 六月丁香AV| 99久久久| 东北熟女视频99| 色五月丁香在线| 婷婷五月花.97| 97色色色色色色色色色色色色色| 精品夜夜澡人妻无码AV| 亚洲 25P| 亚洲日韩乱码一区二区三区四区 | 99热在线观看| 开心婷婷五月| 日本色噜| 97精品综合| 一个色的综合| 91 九色大美女| 思思热久久久久思思热| 五月激情综合网婷婷| 9999色色色色| 99综合99| 色五月婷婷五月天| 色婷婷五月影院| 婷婷情色五月天| 99自拍视频网站| 激情五月婷婷视频一区二区三区| 色婷婷六月性| 中文字幕丰满孑伦无码专区| 色综合网综合| 俺去婷婷 丁香| 97亚洲婷婷| 婷婷久热| 高清一区二区三区日本久| 天天草狠狠擦| 亚洲精级| 日韩在线观看亚洲| 久久日韩婷婷五月| 亚洲天堂爱爱| 亚洲成色综合网站免费观看| 草草女人亚洲| www,99热| 日本啪啪网| www.激情| 丁香六月亚洲综合| 超级碰碰视频无码| www.91操| 伊人天天色| 艹色18p| 久99视频在线观看| 99热这里只有精品国产免费| 99久操视频| 久婷婷色| 天天操夜夜爽天天操| 俺也去五月婷婷丁| 俺去也婷婷| 婷婷色婷婷亚洲成人| 国产午夜成人AV在线播放| 国产VA亚洲VA96| 色九月婷婷丁香| 五月色网| 丁香成人视频| 久久与婷婷| 强壮公让我夜夜高潮A片视频| 九九综合久久| 黄色片区子| 九九99九九精品免费 | 在线成人网址| 日欧一片内射VA在线影院| 狠狠干夜夜干| 天天天天天天天干| 啪啪操网| 色五月综合激情| 好好干av| 国产9色在线/日韩| 专区无日本视频高清8| 亚洲精品国产精品乱码视99| 91啪啪| 99热黄| 婷婷五月丁香综合| 丁香五月,激情五月,深爱五月| 在线观看免费狠狠色丁香香综合| 国产精产国品一二三在观看| 五月婷婷成人| 婷婷五月天va| 一级性感毛片| 精品一区二区三区四区五区六区| 婷婷爱五月天| 精品国产va久| 天天肏天天肏天天肏| 成人做爰高潮A片免费视频| 亚洲精品99| 思思久久96热在精品国产,| 青青草轻轻操| 色五月情| 九九九九国产| 婷婷五月天伦理| 国产精品色婷婷99久久精品| chaopengdaxiangjiao| 五月丁香免费视频| 无码少妇高潮喷水A片免费| 天天舔天天摸视频| 日韩一区二区在线播放| 五月丁香色婷基地综合久久| 日韩人妻AV在线| 99在线观看| 欧美婷婷综合| 26uuu欧美激情另类| 久久五月婷婷电影| av在线免费网站| 色五月天婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷婷 | 色综合xx| 最新久久网址| 婷婷综合在线视频| 天天草女人| 九九热最新| 在线观看av网站| 热99AV网站| 五月天四色房丁香| 99久久99九九九99九他书对| 欧美亚洲成人在线| 色五月 婷婷, 大香蕉| 色色五月丁香| 色九月婷婷综合| 婷婷天天色| 日日懆天天懆| 亚洲丁香五月深爱五月| 五月花综合网| 超碰人人射| 亚洲超级碰| 色yeye欧美| 日本三级成人秘书精品片| 久久精品99| 超碰久热| 在线观看亚洲视频影院| 99色色视频| 色狠狠色噜噜AV天堂五区消防| 丁香伊人综合| 99热6这里之有精品| 亚洲熟女乱色综合亚洲网站| 欧美三级大片AA在线看| 99丁香五月| 被强行糟蹋的女人A片| 人人摸人人干| 99精品在线播放| 色激情五月天| 色色色色色色网| 99无码视频| 99福利导航| 五月婷婷丁香五月婷婷丁香| 亚洲无码色| 免费视频99| 亚洲丁香婷婷丁香五月天激情| 婷婷综合精品视频97| 狠狠五月综合在线| 婷婷五月丁综合| 五月丁香六月婷婷网站| 大狠狠在线| 99热大片| 99久久久国产精品免费蜜乳tv| 九九热在线视频,| 国产探花AV在线| 五月天色导航| 色五婷婷| 激情五月色播五月| 五月丁香综合在线| 99精品视频在线观看| 丁香五月社区| 在线观看的av| 99熟女啪啪视频| 久久久久人妻网址| 丰满少妇乱A片无码| 婷婷天堂综合| 婷婷综合| 97五月天| 亚洲乱码日产精品BD| 亚洲V国产V欧美V久久久久久| 欧美交换配乱吟粗大25P| 国产AV国片偷人妻麻豆| 丁香五月婷婷社区| 欧美日韩一区二区三区四区| 超碰丁香五月| 久re热视频| 97干欧美| 精a品a视a频| 小视频aaa久久久| 天天插轮理| 天天操天天草天天草天天| 99热只有精| 第五色婷婷| 香蕉国产2013| 亚洲色综合| 成功精品影院| 婷婷五月丁香亚洲| 久久一级免费黄色片| 色呦呦美女| 伊人影音无码一区二区三区| www.夜夜操.con| 天天干夜夜欢| 久久538| 婷婷久久亚洲| 久久九九综合| 影音先锋秋秋五月婷婷| 五月婷婷精品视频| 丁香五月久久| 开心五月激情站| 婷婷开心五月| 久久HD| 夜夜爽77777妓女免费下载| 大香蕉综合视频在线| 色色色色色色色色色色色色色五月天| 久久大香蕉视频| 激情综合网五月婷婷| 丁香五月自拍| 夜夜爽天操| 久久激情网| 婷婷五月天成人| 丁香六月婷婷久久综合| 丁香五月天久久| 婷婷六月视频| 久久婷婷激情五月天一区二区| 激情丁香五月激情婷婷| 婷婷久久色| 99综合视频| 亚韩精品视频1区| 色私五月婷婷| 人人操Av| 26uuu另类亚洲欧美日本一| 日韩啪| 亚洲精品一区中文字幕乱码| 亚洲色图在线视频| 天天日综合网射| 综合久久六月| 99热久| 亚洲99一级无嗎特制在线| 99热自拍| 九九在线这里只有精品视频| 五月婷婷中文字幕| 99re欧美精品| 激情婷婷丁香五月天| 色色婷婷综合网| www.五月婷婷| 淫视馆av三区| 色很很96| 熟女激情五月天 | www.婷婷五月天| 色色日韩无码| 五月婷婷激情性爱| 五月婷婷六月激情| 婷婷丁香五月天影院 | 亚洲国产成人裸舞| 久久婷婷五月天懂色| 91九九九九九九| 性五月激情| 色丁香六月| 91Chinese在线| 九九色院| 热99只有精品| 婷婷精品在线| 亚州激情九月| 日韩六六久久电影| 色播五月丁香| 另类图片五月天激情| 新久久五月天激情| 99热亚洲| 99精品综合在线| 日韩一66精品| 亚洲婷婷91丁香| 亚洲视频在线网站| 色婷婷五月天天天天天| 五月久久婷婷天堂视频| 色综合99无码 | 99热这里只有精品13| 第四色五月婷婷| 狠狠婷婷日韩| 97色五月天| 九九热视频精品| 麻豆精品| 日本美女天天日天天爽| 色色丁香婷婷| 九九色天堂| 天天日日爽| 久草热久草在线视频| 婷色人人狠| 欧洲不卡视频| 大香蕉欧美在线| 日日插日日干| 电影91久久久| 亚洲欧洲自拍图片专区五月天| 91狠狠综合久久久久久| 狠狠色丁香久久| 婷婷五月在线播放| 国产精品电| 婷婷天天综合| 狠狠五月天| 五月六月播婷婷| 欧美色色色色色| 久久综合站| 激情婷婷另类| 久久久亚洲精品一区二区三区浴池| AV在线观看网站| 五月 成人 婷婷| 任我肏视频精品| 99热这里只有精品50| 操操啪| 久久99热只有精品| 婷色五月| 91日本在线| 欧美色性色好| 久久婷婷五月综合一| 五月丁香六月激情综合在线| 成人精品视频99在线观看免费| 天天日天天做天天舔| 婷婷五月丁香香蕉| 国产99久久久| 日韩操人| 婷婷综合色色| 五月婷婷中文字幕| 超碰成人免费| 岛国AV网| 秋霞免费三级片| 97婷婷色| 做爱夜夜干天天操| 亚洲九区| 国内婷婷丁香社区在线播放| 国产av天堂| 四色 爱 婷婷 精品 亚洲 五月天| 丁香六月综合激情| 亚洲婷婷在线播放十月| 天天婷婷综合亚洲亚洲| 婷婷五月天播| 婷婷五月天亚洲| 91综合国免费久入| 色婷婷狠狠| 国产亚洲在线观看| 激情综合五月婷婷| 99色看这里只有精品| 久久激情五月婷婷| 亚洲激情淫网| 婷婷丁香色五月天| 性色99| 99九九99九九九视频精品| www天天色天天射| 91oumei| 日本九九视频| 日韩999| 五月丁香人妻| 五月丁香天堂网婷婷| 99精品视频免费观看近期发布| 色五月婷婷五月久久| 丁香狠狠干| 欧美性丁香色色五月天干干| 综合色网站| 久久XX| 开心婷婷五月天电影院| 99热久久这里只有精品| wwwwww.色| www久久久久| WWW.婷婷五月天.COM| 婷婷五月花免费视频在线| 成人无码精品1区2区3区免费看| 国产精品-91JQ就要激情网91JQ6.91JQ27.CASA:16888 | 五月天婷婷Av| 婷婷五月综合体验看| 午夜激情综合| 涩五月婷婷| 亚洲色爱综合| 99热碰碰热| 日本色婷婷| 亚洲色 视频| 五月丁香久久色| site:pnnrt.com| 黄色五月婷婷| 久久五月婷| 五月婷婷久草在线视频综合| 成人AV网站在线| 九九九九综合| 色 五月俺去也| 九九热青青草| 丁香五月天堂| 日日夜夜噜噜爽爽| 综合色婷婷| 99re思思热久久| 夜夜操少妇| 久热天堂| 99久久6| 99色在线观看视频| 免费超碰在线| 亚洲综合久| 婷婷五月丁香成人| 色狠狠综合入口| 久久99视频| 这里只有精彩视| 亚洲综合视频在线| 免费无码毛片一区二区A片| 天天操人人干| 丁香色五月婷婷17C| 另类丁香五月天区图| 欧美日韩成人免费在线| 日 日干 日日做| 五月婷婷综合天天操| 久操婷婷| www夜夜操wwwcon| 五月丁香操婷逼| 秋霞少妇AV网站| 久操综合| 五月综合影院| 人人操人人爰人人一天天碰夜夜拍夜夜爽-中国A级毛片天天看天天谢… | 久久东京热婷婷五月| 99热精品综合| 爆乳熟妇一区二区三区爆乳| 欧美性生交xXxX久久久| 色色色婷| 色青五月天| 成人国产网站| 精品无码久久久久久久久| WWW,五月| 激情综合网 激情五月天| 激情婷婷色色| 超碰免费电影| 久久五月天婷婷| 婷婷五月丁香五月综合网| 99er免费在线观看| 九九干视频| 色婷婷成人色网| 婷婷精品| 噜噜狠狠色综无码久久合欧美| 亚洲色婷婷99一9|| 国产综合视频婷婷| tingtingseav| 五月婷视频| 99免费在线| 中文字幕日韩无码制服诱或| 久热综合| 青青草Avb在线| 国产婷伊人| 亚洲五月婷| 五月天色综合| 成人丁香|