欧美成人午夜精品久久久,国产?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
返回資訊列表
久久五月天精品视频| 67194线路二在线观看| 久久丁香五月| 国产无套精品一区二区| 激情五月婷婷网| 91丨九色丨大屁股| 婷婷六月天| 五月丁香啪啪啪综合网| 乱精品一区字幕二区| 久久婷五月| 91人操人人人操人| 8区视频在线| 超级碰碰碰97免费| 亚洲乱码日产精品BD| 久久开心五月婷婷| 中文字幕在线播放视频| 婷婷五月综合激情| 五月婷婷日本| 抽插特写| 4399成人黄A片| 五月婷婷影| 伊人综合网4| 五月婷婷很很色| 亚洲99热| 天天舔天天爽| 香蕉大综综综合久久| 亚洲久久婷婷| 9 7总站超级碰免费视频| 9l视频自拍九色9l视频自拍九色9l社区| 婷婷五月天在线综合导航| AV色婷婷| 婷五月天| 婷婷五月天亚洲综合网| 精品一二三区久久AAA片 | 婷婷久久五月| 综合久久9| 久久999久久999久久999久久| 久久婷婷网址| 婷婷免费无马| 99久视频| 激情都市五月天| 五月婷婷激情网| 五月婷婷婷丁香播| 色婷婷狠狠久久综合五月| 91在线看片| 大香蕉久| 色情五月天首页| 婷婷伊人五月天| 久久总和99| 大香线蕉伊人| 97操碰在线视频| 五月婷婷丁香婷婷| 超碰在线看| 另类小说五月天综合| 婷婷六月五月天综合| 国产成人网址| 色婷婷六月激情| 六月婷婷五月丁香首页| 五月丁香成人| 玖热精品综合视频| 2014天天爽| 婷婷五月丁香五月丁香| 秋霞学生妹一二级| 伍月激情天| 亭亭社区五月天| 国产26uuu| www.9797国产| 97在线精品| 亚洲熟妇AV乱码在线观看| 我要射综合| 亚洲啪啪自拍| 欧美私人家庭影院| 婷婷深爱五月| 色色亚洲| 袁子仪视频观看| 丁香五月中文字幕| 67久久| 色五月婷婷、老熟女| 激情色情五月天| 婷婷六月激情在线视频| 熟妇人妻中文字幕无码老熟妇| 精品亚洲国产成AV人片传媒| 91男人资源站| XX色综合| 思思w99| 99精在线| 日日夜夜狠狠| 黄网在线免费| 久色五月天| 五月丁香好婷婷姑娘综合网| 婷婷五月骚厕所| 久9热在线视频| 亚洲性爱AV在线| 五月天婷婷基地综合网| 天天肏天天插| 色色丁香婷婷| 99爽视频| 欧美啪啪五月天| 熟女人妻一区二区三区免费看| 九九久热| 丁香色六月婷婷| 丁香五月天激情综合| 欧美色色色色色| 六月激情婷婷| 91碰在线| 丁香五月婷久久| 久久92| 婷婷五月六月丁香综合| 《》【无码】想被搞到爽AV应募而来的超M素人 西纯子 10musume-011723-01 | 91丨九色丨熟女高潮| 久久A极片| 欧美激情中文字幕| 79色色免费| 久久久精品人妻| 狠狠九九婷婷韩| 婷婷舔| 五月丁香婷婷六月| 亚洲人妻AV| 久久婷婷综| 激情九月综合| 永久AⅤ1| 久操福利| 五月婷婷六月天| 久久久五月天| 大香蕉久久| 五月丁香啪| 中文字幕婷婷五月天在线观看| cc精品国产性传播| 日本AAAAAAAAAAAAAA片| 99久热在线精品| 91色综合| 人人爱操| 精品五月视频婷婷在线观看| 五月天婷婷色色网| 久热精品在看| 色丁香五月| 婷婷丁香综合在线| 拳交大逼| 色婷婷五月天无码视频| 婷婷综合网伊人| 久久久婷| 操人无码| 激情综合丁香| 91熟妇大香蕉| 成人短视频在线| 91色色色| 色婷婷色婷婷五月| 丁香五月天婷婷中文字幕| 五月婷婷AV| 婷婷五月天综合网| 天天xxxxxx天天日| 9999久久久久| 4399欧美另类视频| 二级黄色毛片| 综合激情婷婷| 五月丁香色| 色五月综合激情| 超碰人人在线| 99热思思在线观看| 成人αV视频免费观看| 激情色情五月天| 97人人干。| 99精品网| 99视频色在线观看| 日本熟妇乱妇熟色A片蜜桃| 五月丁香六月综合情在线观看| 五月婷婷中文字幕| 伊人五月人妻精品| 色综合久久99色| AA片在线观看视频在线播放| 996黄色片| 色欲影香| 天天网站天天爽| 99热在线这里| 天天操比比| 手机免费福利视频| 婷婷色色播五月天| 亚洲AV无码一区二| 国产精品爽爽久久久久久| 99ri精品视频在线观看| 97在线视频观看| 99热99思午夜精品| 色五月丁香五| 综合性爱网| 狼人狠狠操| 超碰AV在线| 久久婷婷丁香六月天| 亚洲无码www| 五月天久久婷婷| 日韩艹比| 性色人人爽| 婷婷9月天| www色婷婷| 99.色| 午夜亚洲国产精品av一区二区| 久久性爱视频| 五月丁香成人网| 九九家庭影院| 久久九九中文字幕| 国产成人一区二区三区在线观看| 亚洲美女网Va| 99色在线观看| 日本女天天爽| 中文精品在| 色色五月婷婷狠狠| 91欧美| 嫩草视频观看| 中文字幕,综合,91| 五月婷婷激情网| 欧美日韩大黄| 色婷婷XXXXX| 九月色婷婷综合| 91婷婷丁香五月亚洲| 五月婷婷香| 婷婷六月综合基地| 丁香激情网| 开心五月深爱五月丁香五月激情五月| 综合久久丁香婷婷,五月婷婷六月丁香,开心激情综合网,六月丁香在线观看,婷婷丁 | 伊人五月天久久| 狠狠CAO日日穞夜夜穞AV| 91avse| 日日操夜夜撸| 天天搞天天爽| 五月天成人小说| 大香蕉99| 亚洲久久日| 日韩精品VIP| 色欲天天综合网| 亚洲天堂99| 亚洲A片成人无码久久精品青桔| 超碰人人操| 五月婷婷丁香狠狠撸久久| 大波美女VA网站| 五月丁香啪啪啪免费看| 天天干天天 亚洲| 五月丁香婷婷啪啪综合网| tingting五月天亚洲| 秋霞午夜理论| 色五月激情基地| 色五月色综合| 欧美国产一区二区三区| 99这里只有精品视频| 这里只有精品免费视频| 久久99热这里只有精品| 色色99| 五月丁香六月激情在线| 色色五月天网站| 99色婷婷视频| 午夜成人片400| 9视频在线成人网站| EEUSS鲁片一区二区三区| 天天日日夜夜| 精品热九九| 久久9RE热视频精品98| 99热99思午夜精品| 欧美婷婷精品激| 久色网址| 天天射天天射一道本日本社区 | 婷婷五月天成人小说| 99成人无码| 九九色综合| 久久99久久99久久99人受| 色在线99| 五月成人天| 国产成人av在线播放| 99啪啪| 一起操 91N.com| AA丁香综合激情| 99热无码| 五月天激情小说| 五月丁香六月激情| 26UUU一区二区| 婷婷五月亚洲一本在线丁香| 婷婷激情综合网| 狠狠色婷婷丁香五月| 日本一道久久| 老师的粉嫩小又紧水又多A片视频 粉嫩AV久久一区二区三区 | 人妻性爱| 4399在线日本A片| 666555。COm毛片| 日韩国产在线免费观看| 五月丁香欧美在线| 国产无套精品一区二区| 丰满老熟妇BBBBB搡BBB| 狠狠干狠狠操狠狠爱| 五月天婷婷色色| 99综合激情久久精品久久| 午夜色婷婷| 综合另类激情| 婷婷激情综合无月| 国产真实乱对白精彩| 亚洲成人在线在线| 五月天婷婷色色| 九色视频91| 综合色图区| 大香蕉婷婷丁香天堂AV| 九九99九九99九九99视频网| 久久久精品色色色| 欧美性爱5月天天天看| 大香蕉伊人久久| 狠狠88综合久久久久噜噜噜| 精品国产乱码久久久久夜深人妻| 99啪| 五月花激情| 99在线观看精彩视频| 日日干夜夜干| 伊人丁香婷婷东京| 日韩性爱无码| caop在线视频| 色婷婷最爱五月| 人妻久久久久久久| 激情五月天婷婷丁香| 激情婷婷护士激情| 思思热视频在线观看| 色五月色图| 99在线精品在线视频| 国产在线aaa片一区二区99| 五月色色色| 免费观看的av| 色yeye色综合| 99在线国| 欧美五月停| 五月天色婷婷小说| 99综合网| 婷婷综合网在线| 九九热视频99| 欧美激情五月天在线观看| 久9精品视频在线| 99免费视频网| 99欧美| 色五月综合网| 99精品综合| www.国产亚洲69ty.久久久久久久久久久久| 五月婷A V在线| 激情五月婷| 婷婷第六色| 九九色黄色| 五月丁香六月欧美综合网站| 亚洲天天综合| 色婷婷婷av| 婷婷六久久| 热久久91| 天天干一干| 久热这里只有精品99re,久热这里只有精品7 | 色婷久九| 婷婷深爱五月天在线| 五月天久久网站| 思思re最新视频| 第五色婷婷| 蜜臀综合久草| 久色大| 99久久久| 4438全国最大视频成人网站在线观看| 极品人妻VideOssS人妻| 色五月婷婷中文字幕在线观看| 婷婷五月综合在线| 久久人妻伦理| 天天日天天舔天天摸| 综合久久丁香婷婷,五月婷婷六月丁香,开心激情综合网,六月丁香在线观看,婷婷丁 | 激情碰碰碰| 五月丁香无码| 色色婷| 玖玖婷婷色五月| 五月天六月婷婷| 久久伊人日日夜夜| 99精品在线观看| 激情五月天色婷婷综合| 99精品网址| 丁香久久久| www.婷婷,com| 丁香五月婷婷色综合基地| 玖玖99精品视频| 久久人妻少妇嫩草AV| AV网在线观看| 综合色婷婷| av线电影| 色婷婷基地| 色婷婷久久综合| 99这里有精品视频3| 91中文在线| 激情综合五月| 婷婷五月俺要去| 激情婷婷五月黑人| 新伍月婷婷| 色婷婷五月天久久| 五月婷婷色综图片| 99热都是精品| 婷婷五月天天爽| 欧美成人性爱网| 婷婷成人五月天| 丁香六月色婷婷| 午夜婷婷| 黄急一级视频| 嫩草AV久久伊人妇女超级A| 97人人看| www.99热精品| 狠狠搞狠狠操| 99小视频网站| 亚洲婷婷丁香五月视频| 色综久久久| 综合啪啪| 丁香五月婷婷啪啪啪| 婷婷久久免费| 米奇影视资源婷婷狠狠色激情欧美五月丁香 | www.色五月.com| 日本久久99| 91操色| www.婷婷| 久久久久九九九九视屏小说88| 五月丁香综合| 91人操人人人操人| 久久五月天激情| 日韩AV片| 五月激情久久综合网| 色五月超碰| 丁香五月天操B| 91婷婷色 | 激情五月天啪啪视频| 91九色欧美| site:901-07.com| 激情综合色播| 久久AAAA片一区二区| 午夜日日| 亚洲色婷婷视频| 人人操AV| 美女黄频aⅴ视频| 丁香婷婷少妇| 深爱激情六月天| 五月婷婷先锋| 亚洲天天免费| 久久久无码精品成人A片小说| 99热这里只有精品首页| 精品亚洲日韩99欧美片| 丁香五月婷婷综合视频| 青草青青草| 这里只有精品视频一区| 九色91国产| 色操综合| 色五月五月丁香| 婷婷丁香五月亚洲17cao| 九伊人网| 桃色成人网| 91丨九色丨首页| 色色九区| 97碰超级人人看| se色99| 久久久久久五月天| 色五月天丁香婷婷| 综合激情婷婷| 99爱视频在线| 最近免费中文字幕大全高清大全1| 五月天成人在线播放| 亚洲网在线观看| 久久HD| 九九色综合| WWW久久99久久99久久| 日韩成人电影在线播放| 日本久久超碰| 亚洲视频操| 97在线视频人妻九色| 婷婷丁香色情五月天| 五月婷婷先锋| 色丁香五月| 九月av在线| 五月激情综合网| 色婷婷五月天成人网| caobi四区| 99精品成人无码A片观看金桔| 日韩成人网站精品久久大全| 碰超99| 色就色94欧美setu| 婷婷在线观看五月天在线视频| 丁香五月婷婷无码AV| 99热这里只有精品中文字幕| 婷婷永久在线| 99热老司机| AV无码免费| 9l视频自拍9l九色9l成人| 97日本在线| 岛国AV网站| 92久操视频| 在线成人视频免费| 國語久久婷| 激情綜合W W W,激情五月天| 99色色热| 丁香五月婷婷啪啪视频| A1片久久久| 亚洲美女网Va| 五月天激情丁香| 欧美电影在线播放| 亚洲激情视频在线观看| 丁香五月在线| 国产精品久久久久久喷浆| 五月色色色| 99热只有这里有精品| 欧美性色A片免费免费观看的| 99男人天堂| 国精产品一区二区三区| 激情综合五月丁香| 99色热视频| 丁香五月综合AV在线| 中文字幕丰满孑伦无码专区| 激情五月天黄色小说| 五月天综合在线| 久久99人人| 婷婷五月丁香婷婷| 丁香五月性| 九月婷婷综合| 久草婷婷| 性色婷婷| 91丨九色丨熟女| 丁香五月激情网| 丁香五月六月婷婷综合| 日本99视频精品免费播放| 97色色色色色| www.com任你艹| 久久丁香五月| 免费无码毛片一区二区A片| 天天做天天爱高潮片| 99综合久久| 日本97在线视频| 天天干一干| www.AV在线| 五月久久婷婷成人网| 激情四射亚洲| 香蕉AV777XXX色综合一区| 婷婷激情五月呦呦| 五月天影院| 狠狠色噜噜狠狠狠777奇米| 开心五月综合| 丁香六月成人网| 丁香五月天婷婷激情| 99色嘟嘟精品网站| 中文字幕乱轮| 98永久精品| 婷综合| 久久99精品九九久久久婷婷| 色色色色色色色色综合网| 五月色婷婷在线观看| 五月婷婷色五月| 丁香六月色香蕉视频| 黑人糟蹋人妻HD中文字幕| 丁香五月婷婷激情视频播放| www.金莲av| 五月六月婷| 五月婷啪啪| 99久久网站| 亚洲综合婷婷五月| 国产偷人爽久久久久久老妇APP | 五月天六月婷| 五月婷婷久久久久| 深爱五月婷婷| 97婷婷五月| 九九综舍久久| 国产片XXXXA片国语对白| 五月天综合影院| 九九婷婷综合| 最近中文字幕2019视频1| 六月五月婷婷| 色私五月婷婷| 中文字幕在线aⅴ免费观看| 开心深爱激情网| 久久综合婷婷| 久久精品66| 激情五月天综合网| 日日操夜夜擼| 9久精品视频| 欧美性丁香色色五月天| 亚洲、欧美、国产另类笫二区| 一本大道嫩草AV无码专区| 99久久极情精品一区| 五月激情视频| 99玖玖精品| www.丁香五月| 79精品视频| 亚洲性受XXXX五月丁香| 色播五月丁香综合| 免费看欧美成人A片无码| 婷婷六月激情在线视频| 极品色丁香| 色婷婷丁香社综合| 色狠狠色噜噜AV天堂五区| 激情五月天综合网| 很很干在线视频| 亚洲AV综合在线观看| www,99热在线观看| 色玖玖综合| 五月深爱网| 99在线观看这里都是精品| 色情婷婷。| 九 九九九AV| 欧美激情性做爰免费视频| 任你爽免费视频| 天天操天天日天天爽| 激情床戏| www.综合久久.com| 大香伊人婷婷影院| 1024成人免费看| 国产AV一区二区三区最新精品| 天天摸天天肏| 任你操精品免费| 国产乱妇乱子伦| 五月天开心婷婷久久| AV在线资源| www.97碰碰com| 97久久精品视频| 五月Huangsewang| 99re6在线视频精品免费| 99色色视频| 亚洲最大在线| 99精品一二三四视频| 天天色综合综合| 亚洲无码成人性爰网| 夜夜操天天爽| 99er国产| 五月天婷婷黄色视频| 婷婷的99视频网站| 秋霞成人毛片一级A片| 激情图片99| 日笨久久网| 九九在线91| 一级韩国产精品毛| 亚洲啪啪自拍| 91猫咪国产在线播放| 婷婷五月丁香色色| 久久婷五月影院| 91re色综合视频| 久久婷婷五月天大香蕉| 激情综合色婷婷啪啪五月天| 天天操综合网| 97视频.干com| 九九色热| 性做久久久久久久免费看| VA婷婷亚洲| 狠狠插日日干撸| www.25五月婷婷| 99在线观看视频蜜臀| 26uu| 激情开心五月天婷婷基地丁香社区| 可以免费看AV网站| 99re8在这里只有精品| 五月天综合在线| 淫视馆aV二区一区| 五月丁香综合| 秋霞免费视频| 丁香五月激情啪啪综合| 色婷婷香蕉丁丁网| 97精品欧美91久久久久久久| 五月丁香六月婷婷啪啪综合 | 综合激情站| xxx日本东京热| 这里只有精品视频| 成人av在线网址| 欧美A级成人婬片免费看理论| 级人人91| 99热碰碰| 97人人草| WWW久| 亚洲综合婷婷| 人人操五月天| 婷婷五月天小说| 久久A极片| 人人97碰| 综合激情网激情五月。| 丁香婷婷视频| 99热8| 万月丁香狠狠爱| 久久草中文日韩欧美| 丁香五月宝贝激情网| 天天噜噜| 92久久精品一区二区| 免费看片在线观看| www.国产亚洲69ty.久久久久久久久久久久| 久久色五月天综合网| 91九色首页| 永久天堂日本| 这里只有精品99视频| 99热在线观看免费精品| 欧美大肥婆大肥BBBBB| 成人Av在线大片| 久热这里只有精品66| 97人人操人人干| 免费观看2018www黄色操逼网站| 99啪视频在线观看| 五月婷婷六月丁香综合| 色婷婷五月基地在线| 久操福利| 碰97久久| 五月婷婷,六月婷婷| 国产熟妇的荡欲午夜视频| 丁香九月激情| www夜夜操| 婷婷久久久久| 九九热视频精品999| 天天爽夜爽| site:xmssd.com| 五月天天综合| 激情婷婷五月社区| 国外亚洲成AV人片在线观看| 天天舔夜夜操www com| 亚洲九九99精品视频在线播放| 九九色综合九九色| 日亚二欧美| 噜噜噜精品欧美成人在线观看| 天天综合色| 99这里只有精品|v| 成人五月天丁香| 思思99re这里只有| 乱岳熟女50岁| 天天爽天天爽天天爽天天爽天天爽天天爽天天| 99亚洲视频| 5月丁香综合图区| 丁香五月香蕉| 桃色五月天| 色婷婷丁香AV综合| 免费久久这里只有精品99| 色激情五月天| 第五婷婷伊人丁香| 色婷婷视频在线| av五月天婷婷丁香| 激情5月天天天| 五月丁香六月激情综合| 综合激情啪啪| 另类激情综合| 全网最新网黄大秀直播高清,主播国产录屏在线| 色婷五月天| 九九黄色网| 玖玖资源在线视频| 九九久久99| 色欧洲| 久久婷色| 蜜桃人妻无码AV天堂三区| 精品导航在线x不卡| 大天天伊人| Www99热| 婷婷色中文字幕| 97成人丁香婷婷| 久久女婷| 99九九视屏| 亚洲电影中文字幕| 天天综合色| 丁香九月婷| 99re这里只有| 天天干天天 亚洲| 97超级免费无码| 五月婷视频| 噜噜噜噜噜在线| 国产精产国品一二三在观看| 开心婷婷五月天激情网| 激情文学第四色婷婷丁香五月| 久久综合中文| 深情六月婷婷综合久久| 9视频在线成人网站| 女人天堂AV| 五月激情丁香五月| 怎么样可以看免费的一级av| 亚洲欧美999| 97色久| 性爱久久| 五月亚洲| 色色色在线播放| 美女视频图片久久91| 99超级碰碰| 亚洲 激情 中文| 午夜免费高清AV片| 精品一二三区久久AAA片| 99国产这里只有精品| 玖玖爱资源站| 亚洲精级| 激情六月天| 五月激情六月丁香| 婷婷五六日| 激情文学第四色婷婷丁香五月| AA片在线观看视频在线播放| 久草狼人| 亭亭五月激情亚洲在线| 亚洲操操| 99热线观看9| 九九av| 天天色色天天| 色五月亚洲五月天| 超碰免费人妻| 亚州第一黄网| 97五月天| 91操碰| 五月丁香六月婷婷激情视频在线观看免费 | 色婷婷免费观看| 97碰碰九九视频| 公的粗大挺进了我的密道| 九九黄色网| 亚洲碰碰碰| 婷婷五月色亚洲| 五月综亚洲| 欧美欧盟性爱网| 欧美色偷拍| 九九热在线视频| 色五月亚洲| 天天干,天天日| 九九热在线精品视频| 五月丁香 啪啪啪| 996热re视频精品视频| 婷婷激情五月天桃花网| 久久久国产精品黄毛片| 久久99精品视频| av九九| 九九九九综合| 久超超碰| 综合99综合久久久久久久| 99热8| 五月丁香六月婷婷在线观看| 丁香五月婷婷在线视频| 五月情涩综合婷婷| 日本成人小说婷婷六月| 久久五月天激情视频| 综合xx网| 婷婷五月天六月综合| 五月丁香婷色| 亚洲丁香花色| 久久免费操| 99热www| 99碰碰中文| 第五婷婷伊人丁香| 热99精品视频五月| 婷婷成人综合| 丁香婷婷五月天网站| 午夜色丁香| 五月丁香六月婷婷的女人| 青青日韩| 欧美五月婷婷| 另类视频在线| 久久婷婷五月综合色播| 丁香五月a| 天天干天天干天天操| 激情五月影院| 天天干天干| 国外亚洲成AV人片在线观看| 婷婷丁香91综合| 操逼视频网址| www.99热在线| 伊人影音无码一区二区三区| 69精品人人人人| 五月婷庭丁香在线| 思思精品热在线| 五月婷婷色色色| 激情五月婷婷| 亚洲AV免费国产电影| 夜夜躁狠狠| 操人妻视频91| 色99在线视频| 色九月丁香婷婷蜜桃在线观看| 热婷婷久| 中文字幕丰满乱孑伦无码专区| 伊人青草成人| 草逼大片| 五月综合视频| 成人短视频免费| 丁香六月婷婷| 99热骚货| 黄色片区子| 婷婷丁香18| 五月婷久草| 国产成人高清| 国产精品色一哟哟| 日韩在线视频中文字幕| 久久精品国产精品| 婷婷操久久| 丁香五月婷婷天激情| 女婷久久| 亚洲AV日韩AV永久无码网站| 五月婷婷六月天| 婷婷丁香五月天色区| 成人在线日韩欧美| 国产SUV精品一区二区883| 色九月国产| 我爱婷婷五月天综合88| 综合五月婷婷| 91伦| 国产26uuu视频| 久久综合五月天| 五月婷婷黄网站大全| 丁香婷婷五月天亚洲| 91色噜噜狠狠狠狠色综合| 教师性爱毛片| 91精品熟女| 欧美日朝成人| 久久婷婷激情四射五月天| 五月婷婷黄色视频| 99碰超| 久久大国产香蕉| 色色99色色| 91精品婷婷国产综合久久| 久久区区一二三av| 五月天俺去也| 狠狠CAO日日穞夜夜穞AV| 五月婷婷影| 免费精品99| 97成人视频| 思思干精品| 丁香五月综合图片在线观看| 99∨VTV| 欧美 日韩 成人在线| 色婷婷久久综合中文久久一本| 伊人久久婷婷五月综合97色| 五月天欧美 另类小说| 91欧美| 久久三级视频| 婷婷伊人综合中文字幕| 日本久久99久久| 亚洲综合另类| 女人天堂久久| 色五月欧美| 婷婷丁香五月天熟女丝袜| 亚洲天天综合| ww超碰在线| 五月丁香中文字幕| 国产婷婷久久| 97五月天| 亚洲av无码精品色午夜| 人人操AV| 五月停停999| xx久久| 91人人爽狠狠狠| 婷婷天天日婷婷| 男人天堂亚洲综合| 青草视频在线播放| www.丁香五月| 国产av网| 99热在线播放| 91丨九色丨43老版熟女| 狠狠色成人影片| 激情小说婷婷小说| 欧美人与性动交CCOO| 99碰超| 五月综合激情| 五月丁香六月激情狠狠| 亚洲人成播放网站| 91ncm视频| 欧美又粗又大一区二区在线观看| 久久婷婷五月综合| 丁香九月婷婷色| 亭亭色网| 综合天堂AV久久久久久久| 天天狠狠色噜噜| 久久婷婷人人| 99热网站| 99热手机在线精品| 五月天婷婷色在线视频免费观看| 嫩草极品| 婷婷丁香五月亚洲| 91综合色| 色色五月婷婷网| 亭亭色网| 五月香六月婷| 婷婷丁香成人五月天| 久久久国产精品黄毛片| 日韩aaaaa| 五月天丁香| 色五月婷婷啪啪五月| 91精品婷婷国产综合久久| 色五月婷婷综合| 26uuu欧美亚洲日韩| 26uuu国产色| 超级碰碰一区| 久久艹99| 欧美性爱五月天| 色欲天天综合网| 日韩婷久| 熟女激情网| 天天色综合色| 亚洲视频国产一区| 免费看欧美成人A片无码| 狠狠操狠狠插| 久久精彩免费视频精彩免费视频| 《亚洲操B久久免费在线观看,亚洲操B久久在线播放》在线播放 - 高清资源 - 97 | www.99热最新视频8| www.久久99| 婷婷五月天偷拍| 婷婷五月色| 丁香五月天激情四射网| 激情久久久| 中文字幕婷婷在线| 五月婷婷官网色| 激情综合五月| 91成人看片| 91无码一区人妻A片蜜| 久久婷青青草原| 牛色色碰| 熟女激情五月天| 2025天天爽天天摸| 丁香婷婷老熟女综合网| 另类激情五月| 五月天伊人久久| 夜夜谢天天干| 深爱激情网五月| 色综合色综合网| 99啪99| 婷婷五月欧美综合| 亚洲黄色影视| 五月天婷婷色综合| 一起草性爱不卡视频| 97干欧美| 婷五月天| 五月天久久婷婷| 99久久精品视频女神1| 亚洲成人电影aaaa| 五月天 综合 在线| 九九热只有精品6| www.超碰在线| 欧美三级欧美一级| 99色色| 99操无码视频观看| 九九精品热播| 国产精品成人AV在线| 亚洲av日韩无码| 婷婷色九月| 日本天堂免费99| 久婷首页| 91尤物九色在线| 色色色色色色色色五月先| 国产韩日亚洲美州欧亚综合在线| 97精品综合久久内射| 99热热热国产超碰| 国产五月天婷婷| 欧美日韩成人在线| 那里有AV网址| 亚洲日韩26uuu| 99国产这里只有精品| 日日操日日撸| 日本高清久| 丁香婷婷激情六月五月开心| 亚州操操| 九九亚洲| 久久ri精品| 99热资源在线| 丁香五月激情啪啪| 玖玖在线资源视频| 99小视频网站| 国产精品爽爽久久久久久| 九月婷婷色色| 日韩成人综合网| 色婷婷五月综合激情中文字幕| 九月色婷婷婷| 综合色99| 激情九九综合网| 精a品a| 五月丁香综合在线| 国产XXXX搡XXXXX搡麻豆| 无毒黄色网址| 99超级碰碰| 天天舔天天摸天天射| 天天操天爱综合| 99人人干| 亚洲AV成人一区二区在线观看| 午夜成人av在线| 狼人久草| 日日射天天射| 亚洲成人AV在线播放| 欧美丰满熟妇BBB久久久| 五月丁香久久色| 天堂爱爱| 久久99热在线观看| 婷婷99狠狠躁天天躁| 97干在线看| www.五月婷婷| www,婷婷五月天,com| 色婷婷精品视频| 激情都市另类| 美女黄频aⅴ视频| 亚洲bt丁香五月天婷婷激情小说| 4399成人黄A片| 69久热| 色久五月| 五月婷婷激情综合| 国产一级黄色影片,| 久热 91| 噜噜噜久久亚洲精品国产品91| 91精品综合久久婷婷九色| 雪千夏麻豆| 婷婷在线操| 色久天| 六月丁香婷婷综合狠狠爱夜夜爱| 亚洲VA在线| 国产熟女一区二区三区五月婷| 中文字幕欧美精品久久| 五月天色婷婷图片| 综合网亚洲| 婷婷97碰碰| 婷婷爱五月天| 色偷偷人人| 开心五月婷婷六月丁香| www,色婷婷| 午夜成人在线免费视频| 久久婷婷五月天亚洲欧美| 欧类av怡春院| 五月丁香婷婷综合在线| 五月激情小说网| 色综合狠狠色| 26uuu成人网| 国产操逼网站| 亚洲色99| 肏日网在线看| 五月天亚洲色| 日韩激情网站| 欧美婷婷五月天| 六月丁香开心婷婷欧美| 天天色综网| 婷婷五月在线| 婷婷五月情| 六月婷婷综合久久| 中文字幕AV网址| 丁香五月中文字幕| 伊人激情| 久久婷婷综合网| 可以看的AV| 99热这里只有精品10| 香蕉久久国产AV一区二区| 色五月婷婷激情五月| 激情五月婷婷老师| 精品久久66| 蜜臀久久99精品久久久久久酒店| SS丁香五月婷婷| 久久激丁香| 26uuu欧美宗合| 婷婷激情四射五月天| 五月天色色婷婷| 色色色在线| 99热| 五月天激情综合10p| 婷婷天天五月天| 91热视频色网站| 久久66er久久| 久草五月丁香婷婷综合| 国产 亚洲 在线| 少妇口诉沐足视频播放器网址| 九九色情网五月天| 很很干天天干| 99免费在线| 99在线观看这里都是精品| 奇米网大香蕉| 中国丰满熟女A片免费观| 色婷婷裸体色性在线| 大香蕉AV在线| 欧美三级韩国三级日本三斤| 九九五月天| 六月婷婷狠狠做| 偷偷操99| 成人丁香婷婷| 九九热这里| 亚洲综合色网| 色综合中文色综合网| 麻豆AV一区二区三区| 日本色五月| 五月天玖玖狠狠色色| 三级毛片7979| www,婷婷,com| 色婷婷激情| 五月丁香啪啪综合网| 国产亚洲在线| 最近中文字幕2019视频1| 日韩AAAAA| 婷婷涩涩网| 九九99免费视频| 亚洲国产精品VA在线看黑人| 色五月天丁香婷婷| 五月天中文字幕在线婷婷| PORNY九色9l自拍视频成人| 五月天久久小说| 看片视频在线免费日产在线看| 夜夜夜夜夜操| 六月色婷婷欧美| 91日本在线观看| 91岛国片| www...com黄在线观看| 五月丁香亚洲婷婷| 日本a片网址| 九九99在线视频| 97婷婷五月丁香| 免费视频WWW在线观看网站| 九九99久久| 67194成I人在线观看线路1| 日本乱论99| 久草热在线视频| 丁香六月亚洲综合| 人妻系列久久久久久久久久久| 综合激情九月婷婷,激情综合婷婷中文字 | 婷婷九月在线| 激情亚洲婷婷六月| 在线区区区| 丁香六月啪啪啪| 九月激情综合| 日本99在线| 五月丁香激情四射综合| 国产婷婷五月天| 综久久久| 丁香色婷婷色手机免费在线| 另类视频五月天| 久热免费| 超碰免费人人| 五月婷在线| 伊人五月综合网| 人妻少妇色综合| 亚洲精品99| 疯狂做受XXXX高潮A片| 99热精品在线| 久久综合九九| 色五月综合网| 久久婷婷亚洲| 九九热免费视频| 久久久噜噜噜久久人妻| 大香蕉综合网| 国产乱子轮XXX农村| 99成人| 国产黄色在线观看| 久久久91精品| avh片在线观看| 四季AV综合网| 五月色婷婷在线观看| 超碰狠狠色| 97资源碰碰| 婷婷五月丁香图片人人操| 色偷偷色婷婷| 久久久久久久综合狠狠综合| 直接看的AV网站| 五月婷婷激情五月| 欧美一级a| 情色五月天网站| 久久综合热17c| 日韩精品二三区| 综合网亚洲| 五月天久草| 日韩超碰在线| 综合激情网五月激情| 深夜激情网| 中文字幕在线免费观看视频| 丁香五夜激情四射夜夜夜| 五月开心婷婷极品激情| 26uuu亚洲精品国产| 激情婷婷五月黑人| 亚洲婷婷五月| 欧美三级巜人妻互换| 99碰| 操逼123网| 91久久综合亚洲鲁鲁五月天| 99re久久| av首页在线| 天天骑日日爽| 久8色色| 九九色逼| 拍色综合| 狠狠88综合久久久久噜噜噜| 女力报到正好爱上你| 99热久| 久久日韩婷婷五月| 亚洲熟妇AV综合网五月丁香伊人 | 久久婷婷五月丁香| 人人操五月天| 日本片日本片祼观看网站在线看中文版网页在线看 | 人人爽欧美婷婷久久久五月丁香| 97色五月婷婷在线| 久久新地址| 美女丁香五婷婷| 国产又色又爽又黄又免费| 丁香六月丁香婷婷激情| 五月婷婷日| 国产乱码久久| 五月婷婷欧美激情|