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

ARTICLE DETAIL

資訊詳情

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

Matlab實(shí)現(xiàn)LQR車輛軌跡跟蹤:從狀態(tài)空間模型到完整源碼

Matlab實(shí)現(xiàn)LQR車輛軌跡跟蹤:從狀態(tài)空間模型到完整源碼 簡介本資源是一份面向本科畢業(yè)設(shè)計與控制類課程作業(yè)的Matlab實(shí)踐項目聚焦線性二次調(diào)節(jié)器LQR在車輛軌跡跟蹤中的工程實(shí)現(xiàn)幫助學(xué)習(xí)者將控制理論轉(zhuǎn)化為可運(yùn)行的閉環(huán)控制系統(tǒng)。壓縮包共9個文件包含4個.mat數(shù)據(jù)文件存儲預(yù)設(shè)軌跡與誤差數(shù)據(jù)、2個.m主程序腳本含主控邏輯與圓軌跡生成、2個.png仿真結(jié)果圖直觀展示跟蹤效果及1份README.md說明文檔整體僅107KB輕量易讀、結(jié)構(gòu)清晰。已有474人學(xué)習(xí)下載適合作為控制系統(tǒng)課程設(shè)計或畢業(yè)課題的參考范例。讀者可直接運(yùn)行main.m復(fù)現(xiàn)完整LQR控制器設(shè)計流程從車輛狀態(tài)空間建模、Riccati方程求解、反饋增益矩陣K計算到實(shí)時軌跡跟蹤仿真與橫向誤差分析代碼注釋詳盡關(guān)鍵參數(shù)可調(diào)便于理解權(quán)重矩陣Q/R對跟蹤性能的影響機(jī)制。 一直做路徑規(guī)劃相關(guān)的仿真最近被問得最多的就是“怎么讓車沿著期望軌跡走而且別走偏”。用單純的PID調(diào)參調(diào)到懷疑人生換MPC又覺得推導(dǎo)太復(fù)雜、參數(shù)太多。其實(shí)很多場景下LQR是最合適的折中方案尤其是用于簡單的車輛軌跡跟蹤既不需要復(fù)雜的非線性優(yōu)化也不需要海量標(biāo)定數(shù)據(jù)只要把狀態(tài)空間模型和權(quán)重矩陣?yán)砬宄惶譓atlab代碼就能跑出很穩(wěn)的跟蹤效果。這篇內(nèi)容圍繞“Matlab實(shí)現(xiàn)LQR實(shí)現(xiàn)車輛軌跡跟蹤完整源碼”展開我會先解釋為什么選LQR、怎么建立車輛誤差狀態(tài)模型再給出可復(fù)現(xiàn)的完整仿真代碼最后分享調(diào)參經(jīng)驗(yàn)和實(shí)際坑點(diǎn)。不管是正在做課程設(shè)計、畢業(yè)設(shè)計還是剛開始接觸車輛橫向控制的工程師都能在這篇文章里找到可以直接抄作業(yè)的思路。1. 軌跡跟蹤問題拆解先搞清楚我們在解決什么1.1 路徑跟蹤和軌跡跟蹤是兩個完全不同的任務(wù)很多第一次做車輛控制的人會把“路徑跟蹤”和“軌跡跟蹤”混為一談但這兩者的底層邏輯完全不一樣。路徑跟蹤Path Following只要求車輛貼著參考幾何路徑走不關(guān)心何時到達(dá)某個點(diǎn)參考路徑通常由弧長參數(shù)化車輛速度可以獨(dú)立控制。軌跡跟蹤Trajectory Tracking則要求車輛在特定的時間戳到達(dá)特定的空間位置參考軌跡不僅包含位置信息還包含期望速度、期望加速度。本文對應(yīng)的“車輛軌跡跟蹤”屬于后者但在具體實(shí)現(xiàn)中如果只關(guān)注橫向控制我們通常會把縱向速度固定轉(zhuǎn)而把問題分解為“橫向位置偏差和航向偏差的反饋鎮(zhèn)定問題”。一旦速度恒定軌跡跟蹤就退化為路徑跟蹤這讓LQR這種線性控制器有了用武之地。還有一點(diǎn)特別容易踩坑LQR本質(zhì)是線性系統(tǒng)的最優(yōu)控制器但車輛運(yùn)動模型本身是非線性的橫向偏差、航向偏差、轉(zhuǎn)向角之間存在三角函數(shù)關(guān)系。所以直接套用LQR幾乎不可能穩(wěn)定必須先做線性化處理。通常的做法是采用運(yùn)動學(xué)自行車模型并在參考工作點(diǎn)附近進(jìn)行小角度假設(shè)。1.2 為什么是LQR而不是PID或者M(jìn)PCPID是很多人的第一反應(yīng)畢竟實(shí)現(xiàn)簡單、直覺清晰。但用PID做軌跡跟蹤會遇到一個很尷尬的問題橫向偏差和航向偏差存在強(qiáng)耦合關(guān)系單看某一個誤差的PID很難同時兼顧曲線跟蹤性能與穩(wěn)定性。轉(zhuǎn)彎的時候橫向偏差PID會產(chǎn)生較大的轉(zhuǎn)向角輸入但航向PID同時在反向修正兩個環(huán)容易互相干擾最終出現(xiàn)振蕩。MPC雖然效果好能顯式處理約束但需要構(gòu)建預(yù)測時域上的優(yōu)化問題。在Matlab里跑MPC仿真代碼量通常比LQR多一個數(shù)量級而且每次要重新構(gòu)造QP問題對剛?cè)腴T的朋友并不友好。LQR正好處于兩者之間它基于狀態(tài)空間模型能同時考量橫向偏差和航向偏差的多個狀態(tài)變量借助Riccati方程直接求出最優(yōu)反饋增益。這里的最優(yōu)是指在給定Q、R權(quán)重下的線性二次型性能指標(biāo)最小化它是在“性能最優(yōu)”和“控制能量最小”之間做權(quán)衡。如果系統(tǒng)可控LQR增益唯一存在不需要像PID那樣反復(fù)試湊。2. 車輛運(yùn)動學(xué)模型與誤差狀態(tài)方程2.1 自行車模型的基本假設(shè)車輛在實(shí)際行駛中有復(fù)雜的橫向動力學(xué)特性考慮輪胎側(cè)偏、側(cè)傾、懸架等但在低速軌跡跟蹤場景下運(yùn)動學(xué)自行車模型已經(jīng)足夠精準(zhǔn)。它的核心假設(shè)是車輛左右輪的轉(zhuǎn)向角相等把整車等效為前輪轉(zhuǎn)向、后輪驅(qū)動的兩輪自行車模型。模型的核心方程如下x_dot v * cos(psi) y_dot v * sin(psi) psi_dot v / L * tan(delta)其中x和y是后軸中心坐標(biāo)psi是車輛航向角delta是前輪轉(zhuǎn)向角L是軸距v是縱向速度。這個模型的狀態(tài)量是[x, y, psi]控制量是[delta]如果車速可調(diào)則控制量還包括油門/制動。在后軸中心位置建立參考點(diǎn)時該模型具有一定的不完整性約束但用于LQR的設(shè)計并不會造成太大問題因?yàn)槲覀兒竺鏁?gòu)造誤差狀態(tài)模型而不是直接對x、y做控制。2.2 從誤差角度建立LQR需要的狀態(tài)空間方程軌跡跟蹤的本質(zhì)是讓車輛的橫向位置誤差和航向誤差逐漸收斂到零。我習(xí)慣把誤差定義在“參考軌跡上離車輛最近的點(diǎn)”基準(zhǔn)下e_y 橫向位置誤差車輛后軸中心到參考點(diǎn)的垂向距離 e_psi 航向誤差車輛航向角減去參考點(diǎn)處的參考航向角如果車速恒定行駛路徑曲率緩變那么誤差狀態(tài)滿足如下近似的線性微分方程e_y_dot v * e_psi % 橫向位置誤差的變化率由航向誤差和車速決定 e_psi_dot v / L * delta - psi_ref_dot其中psi_ref_dot就是參考點(diǎn)處的航向角變化率在參考軌跡已知的情況下可以直接計算psi_ref_dot v * kappa_refkappa_ref是參考路徑曲率。寫成矩陣形式就非常清晰了d/dt [e_y ] [0 v] [e_y ] [ 0 ] [ 0 ] [e_psi] [0 0] [e_psi] [ v/L ] delta [ -v * kappa_ref ]這里的A矩陣、B矩陣和擾動項非常直觀。我們定義狀態(tài)向量x_c [e_y; e_psi]控制量u delta那么標(biāo)準(zhǔn)狀態(tài)空間表達(dá)式為x_c_dot A * x_c B * u w其中A [0, v; 0, 0] B [0; v/L] w [0; -v * kappa_ref]2.3 可控性與前饋補(bǔ)償?shù)谋匾则?yàn)證一下系統(tǒng)的可控性計算可控性矩陣Qc [B, A*B]。代入A和B可得B [0; v/L] A*B [v * v/L; 0]由于v和L都不為零Qc是滿秩矩陣系統(tǒng)完全可控。這說明只要反饋增益選取得當(dāng)我們一定能讓橫向誤差和航向誤差收斂。但注意擾動項w并非零直接使用反饋控制u -K*x_c無法完全消除參考曲率帶來的穩(wěn)態(tài)誤差。因此實(shí)際工程中要在反饋控制的基礎(chǔ)上加入前饋項。前饋補(bǔ)償?shù)奈锢砗x很直接當(dāng)車輛在曲率半徑為R的彎道中勻速行駛時需要維持一個穩(wěn)態(tài)轉(zhuǎn)向角delta_feedforward L * kappa_ref。這是因?yàn)檗D(zhuǎn)向角的穩(wěn)態(tài)值本來就由幾何關(guān)系決定反饋控制只需要在這個基準(zhǔn)上修正偏差。所以最終控制律為delta -K * x_c delta_feedforward這樣的結(jié)構(gòu)讓LQR控制器在彎道中也不會出現(xiàn)明顯的跟隨滯后。很多只有純反饋的LQR實(shí)現(xiàn)會在圓形軌跡跟蹤時出現(xiàn)持續(xù)的橫向偏差原因就是漏了前饋項。3. Matlab完整源碼實(shí)現(xiàn)與逐步解析3.1 代碼整體架構(gòu)設(shè)計整套代碼按功能分成幾個文件便于閱讀和調(diào)試main.m 主程序負(fù)責(zé)參數(shù)設(shè)置、仿真循環(huán)和可視化 create_reference.m 生成參考軌跡包括直線、圓、雙移線等 bicycle_dynamics.m 自行車模型運(yùn)動學(xué)方程 compute_reference.m 在參考軌跡上尋找最近點(diǎn)并計算參考航向角、曲率 lqr_solve.m 求解LQR增益矩陣K如果你是第一次跑不用一次性看完所有文件直接從main.m入口開始按順序理解“生成參考軌跡 - 初始化車輛狀態(tài) - 求解LQR增益 - 進(jìn)入仿真循環(huán) - 繪制結(jié)果”這個流程。3.2 主程序整體流程與關(guān)鍵參數(shù)設(shè)置直接從main.m開始看%% 主程序LQR車輛軌跡跟蹤仿真 clear; clc; close all; % 車輛參數(shù) L 2.5; % 軸距 m v 5.0; % 恒定車速 m/s % 仿真參數(shù) dt 0.05; % 采樣時間 s T 30; % 仿真總時長 s N T / dt; % 總仿真步數(shù) % LQR權(quán)重矩陣 Q diag([5, 2]); % 狀態(tài)權(quán)重橫向誤差權(quán)重5航向誤差權(quán)重2 R 0.5; % 控制量權(quán)重轉(zhuǎn)向角 % 生成參考軌跡半徑為15m的圓形軌跡 radius 15; cx 0; cy 0; ref_path create_reference(radius, cx, cy, v, dt, T); % 求解LQR增益 [K] lqr_solve(A, B, Q, R); % 注意這里的A、B需要先由車輛參數(shù)和車速構(gòu)造 A [0, v; 0, 0]; B [0; v / L]; [K] lqr_solve(A, B, Q, R); % 初始車輛狀態(tài) x0 radius 2; % 在圓形軌跡外側(cè)2m處起步 y0 0; psi0 pi / 2; % 初始航向角90度 state [x0; y0; psi0]; % 存儲歷史數(shù)據(jù) x_hist zeros(1, N); y_hist zeros(1, N); psi_hist zeros(1, N); delta_hist zeros(1, N); ey_hist zeros(1, N); %% 仿真主循環(huán) for k 1:N % 1. 在參考軌跡上尋找最近點(diǎn) [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, state(1), state(2), k); % 2. 計算橫向誤差和航向誤差 % 橫向誤差車輛位置到參考點(diǎn)的橫向偏差有符號 dx state(1) - ref_x; dy state(2) - ref_y; % 把位置偏差投影到參考航向的法線方向 cross_error -sin(ref_psi) * dx cos(ref_psi) * dy; % 航向誤差標(biāo)準(zhǔn)化到[-pi, pi] e_psi wrapToPi(state(3) - ref_psi); % 3. 使用LQR反饋 前饋計算轉(zhuǎn)向角 delta_fb -K(1) * cross_error - K(2) * e_psi; delta_ff L * ref_kappa; % 前饋項 delta delta_fb delta_ff; % 限制轉(zhuǎn)向角范圍 max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta)); % 4. 使用自行車模型更新車輛狀態(tài) state bicycle_dynamics(state, v, delta, dt, L); % 5. 保存歷史數(shù)據(jù) x_hist(k) state(1); y_hist(k) state(2); psi_hist(k) state(3); delta_hist(k) delta; ey_hist(k) cross_error; end這里有一個細(xì)節(jié)值得說一下LQR反饋計算時我直接寫成了delta_fb -K(1)*cross_error - K(2)*e_psi。由于K是1x2的行向量所以更通用的寫法是delta_fb -K * [cross_error; e_psi]效果是一樣的。重點(diǎn)在于這里的狀態(tài)是誤差狀態(tài)而不是車輛本身的x、y、psi狀態(tài)。有人會誤把車輛的x、y當(dāng)作LQR輸入狀態(tài)那必然無法收斂。3.3 參考軌跡生成圓形軌跡與曲率的計算圓是最經(jīng)典的LQR測試路徑因?yàn)樗那屎愣ǚ奖泸?yàn)證前饋補(bǔ)償?shù)男Ч?。create_reference函數(shù)的實(shí)現(xiàn)如下function ref_path create_reference(radius, cx, cy, v, dt, T) % 生成圓形參考軌跡并以弧長為參數(shù)存儲 % 返回結(jié)構(gòu)體x, y, psi, kappa, s total_len v * T; s 0:dt*v:total_len; % 弧長序列 theta s / radius; % 圓心角序列 ref_path.x cx radius * cos(theta); ref_path.y cy radius * sin(theta); % 參考航向角圓的切向方向 ref_path.psi wrapToPi(theta pi/2); % 參考曲率1/radius ref_path.kappa ones(size(s)) / radius; ref_path.s s; ref_path.v v * ones(size(s)); end生成參考軌跡時要注意一個細(xì)節(jié)參考航向角psi的公式是theta pi/2這個推導(dǎo)很容易錯。圓軌跡參數(shù)方程是x cx Rcos(theta)y cy Rsin(theta)對theta求導(dǎo)得到切向量[-Rsin(theta), Rcos(theta)]所以切線方向角是theta pi/2。如果你用arctan2(y_dot, x_dot)去算結(jié)果是一樣的但要注意歸一化處理。3.4 最近點(diǎn)搜索與參考信息的提取compute_reference是整套代碼里最容易導(dǎo)致性能瓶頸的環(huán)節(jié)但也是保證跟蹤精度的關(guān)鍵。它的任務(wù)是找到參考軌跡上離車輛當(dāng)前位置最近的點(diǎn)并返回該點(diǎn)的航向角和曲率。function [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, x_veh, y_veh, k) % 在參考軌跡上找到離車輛位置最近的點(diǎn) % 為提高效率只搜索當(dāng)前索引附近的局部區(qū)域 n length(ref_path.x); search_range 50; % 最多向前搜索50個點(diǎn) % 在當(dāng)前參考索引附近搜索 idx_start max(1, k - 5); idx_end min(n, k search_range); dist (ref_path.x(idx_start:idx_end) - x_veh).^2 ... (ref_path.y(idx_start:idx_end) - y_veh).^2; [~, loc] min(dist); idx idx_start loc - 1; ref_x ref_path.x(idx); ref_y ref_path.y(idx); ref_psi ref_path.psi(idx); ref_kappa ref_path.kappa(idx); end這種“局部搜索”的做法大幅提升了仿真速度因?yàn)檐壽E跟蹤場景下車輛不會偏離參考軌跡太遠(yuǎn)沒必要從第1個點(diǎn)開始全軌跡搜索。但是要注意在仿真初期如果車輛初始偏差很大或者車輛與軌跡間存在較大的分離局部搜索可能找到的不是全局最近點(diǎn)。所以我把搜索起點(diǎn)設(shè)成k-5并允許向后索引一些點(diǎn)如果仍然擔(dān)心魯棒性可以在仿真前幾十步使用全局搜索作為熱啟動。3.5 自行車模型更新四階Runge-Kutta還是歐拉法在仿真精度要求不高的場景下使用簡單的歐拉法就夠了。但如果你希望看到更平滑的軌跡或者后續(xù)要把代碼擴(kuò)展到硬件在環(huán)仿真我會建議至少使用二階Runge-Kutta中點(diǎn)法。這里給出一個簡單而穩(wěn)健的方法function state_new bicycle_dynamics(state, v, delta, dt, L) % 自行車模型運(yùn)動學(xué)更新歐拉法 x state(1); y state(2); psi state(3); x_dot v * cos(psi); y_dot v * sin(psi); psi_dot v / L * tan(delta); state_new [x x_dot * dt; y y_dot * dt; psi psi_dot * dt]; state_new(3) wrapToPi(state_new(3)); end這里的wrapToPi很重要MATLAB內(nèi)置的wrapToPi把角度限制在[-pi, pi]區(qū)間如果不做這個處理長時間仿真后航向角累積誤差會導(dǎo)致cos和sin計算精度下降。這也是新手最容易忽視的細(xì)節(jié)。3.6 LQR求解不要手寫Riccati方程有相當(dāng)一部分教程會教你手寫Riccati方程的迭代解法但在MATLAB中完全沒必要。直接調(diào)用lqr函數(shù)即可function K lqr_solve(A, B, Q, R) % 求解LQR反饋增益矩陣K % 注意需要Control System Toolbox [K, ~, ~] lqr(A, B, Q, R); end這段代碼簡潔到讓我自己都覺得“是不是太簡單了”。但lqr函數(shù)確實(shí)做了所有核心工作驗(yàn)證矩陣維度、求解連續(xù)時間代數(shù)Riccati方程CARE、返回最優(yōu)反饋增益。如果不想依賴Control System Toolbox也可以用care函數(shù)替代但既然裝了Matlab直接用lqr就行了。3.7 可視化畫出跟蹤效果和誤差變化沒有可視化的控制仿真等于白做。繪制三張圖就夠用了%% 結(jié)果可視化 figure(Position, [100, 100, 1200, 400]); % 子圖1軌跡跟蹤對比 subplot(1, 3, 1); plot(ref_path.x, ref_path.y, k--, LineWidth, 1.5); hold on; plot(x_hist, y_hist, b-, LineWidth, 1.2); xlabel(x (m)); ylabel(y (m)); legend(參考軌跡, 實(shí)際軌跡); title(軌跡跟蹤對比); grid on; axis equal; % 子圖2橫向誤差變化 subplot(1, 3, 2); t_axis (1:N) * dt; plot(t_axis, ey_hist * 100, r-, LineWidth, 1.2); xlabel(時間 (s)); ylabel(橫向誤差 (cm)); title(橫向誤差變化); grid on; % 子圖3轉(zhuǎn)向角變化 subplot(1, 3, 3); plot(t_axis, delta_hist * 180 / pi, g-, LineWidth, 1.2); xlabel(時間 (s)); ylabel(轉(zhuǎn)向角 (deg)); title(轉(zhuǎn)向角變化); grid on;畫完之后你會看到即使從初始橫向偏差2米處起步LQR控制器也能在前幾秒內(nèi)把車輛拉回到參考軌跡附近穩(wěn)態(tài)橫向誤差基本在厘米級甚至毫米級這就是前饋補(bǔ)償?shù)男Ч?. 調(diào)參經(jīng)驗(yàn)與常見問題排查4.1 權(quán)重矩陣Q和R的調(diào)參策略先別急著追求對稱之美Q矩陣和R矩陣的選取直接決定LQR的控制行為這也是很多人卡住的地方。要知道LQR沒有“絕對正確”的Q和R只有“符合需求”的Q和R。一個實(shí)用的調(diào)參策略是這樣的先把R固定為1Q先設(shè)為對角線為1的矩陣跑一下仿真觀察橫向誤差收斂速度和轉(zhuǎn)向角大小。如果你希望橫向誤差收斂更快增大Q的第一行第一列橫向誤差權(quán)重。你會發(fā)現(xiàn)車“糾正偏差”的意愿更強(qiáng)但轉(zhuǎn)向角波動也會變大。如果你希望轉(zhuǎn)向更柔和、執(zhí)行器負(fù)擔(dān)更小增大R。轉(zhuǎn)向角變化會明顯變緩但誤差收斂速度也會下降。關(guān)于航向誤差權(quán)重Q(2,2)我會讓它低于橫向誤差權(quán)重。因?yàn)楹较蛘`差的變化率本身與橫向誤差關(guān)聯(lián)過高的航向權(quán)重會導(dǎo)致車輛頻繁修正航向表現(xiàn)就是轉(zhuǎn)向振蕩。用一組經(jīng)驗(yàn)參數(shù)參考車速5m/s、軸距2.5m的模型中Q diag([5, 1])、R 0.3已經(jīng)是比較均衡的起點(diǎn)。具體怎么調(diào)還是要配合仿真圖來看。調(diào)參的過程比較枯燥但相信我經(jīng)過幾次“Q加大導(dǎo)致抖動、R加大導(dǎo)致跟蹤變慢”的反復(fù)之后你會對LQR的控制邏輯有更直觀的理解。4.2 為什么我的LQR在彎道中總有一圈穩(wěn)態(tài)誤差這是最常見的問題幾乎每隔幾天就會有人問一次。如果你只用了反饋控制u -Kx_c在任何曲率不為零的路徑上都會存在穩(wěn)態(tài)誤差。原因就是前面提到的擾動項w [0; -vkappa_ref]沒有被抵消。通俗點(diǎn)說LQR反饋只負(fù)責(zé)“糾偏”但車輛在彎道上本身就需要的那個基礎(chǔ)轉(zhuǎn)向角反饋控制是“管不著”的。如果沒有前饋補(bǔ)償控制器需要積累一個恒定的航向誤差才能讓車輪產(chǎn)生足夠的轉(zhuǎn)向角于是車輛就會一直在彎道內(nèi)側(cè)“壓著”軌跡走表現(xiàn)為持續(xù)的橫向偏差。解決方法就是加上delta_ff L * ref_kappa的前饋補(bǔ)償。加了之后車輛在進(jìn)入彎道前就能提前打好方向反饋只需要處理模型誤差和外界擾動。我在調(diào)試圓形軌跡時對比過不加前饋時穩(wěn)態(tài)橫向誤差大約20cm加上前饋后直接降到1cm以內(nèi)。4.3 轉(zhuǎn)向角飽和控制器能算出超過物理極限的轉(zhuǎn)角Matlab仿真中一切輸入輸出都是理想化的但任何真實(shí)車輛都有轉(zhuǎn)向角限制。一般家用車的方向盤轉(zhuǎn)角換算到前輪轉(zhuǎn)角大概在±30度到±35度如果你在LQR設(shè)計中完全不管這個限制控制器可能會計算出一個完全無法執(zhí)行的大轉(zhuǎn)角仿真結(jié)果看似正常但實(shí)際應(yīng)用時車輛會失控。解決方式是在控制律之后加一個飽和函數(shù)代碼中我已經(jīng)寫了max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta));加飽和函數(shù)之后要注意一個問題如果車輛初始偏差非常大LQR會輸出飽和的轉(zhuǎn)向角但仍然不夠快地把誤差拉回這導(dǎo)致實(shí)際響應(yīng)比無飽和場景差很多。這種情況可以適當(dāng)增加Q中橫向誤差的權(quán)重讓控制器“更激進(jìn)”地糾偏而飽和限幅本身也能保證轉(zhuǎn)角不超出物理限制。飽和決策應(yīng)該在仿真中盡早驗(yàn)證否則做硬件時容易心態(tài)崩。4.4 為什么車速改變后LQR增益需要重新求解很多人會認(rèn)為LQR增益矩陣K只要解一次就夠了但事實(shí)上系統(tǒng)矩陣A和B中包含車速v車速變化后整個系統(tǒng)模型就變了。從A [0, v; 0, 0]B [0; v/L]可以看出v從5m/s變成10m/s系統(tǒng)的動態(tài)響應(yīng)特性完全不同同一個K在高速時可能穩(wěn)定低速時卻可能出現(xiàn)振蕩。這就是變增益LQRGain Scheduling的出發(fā)點(diǎn)。標(biāo)準(zhǔn)的做法是在不同速度區(qū)間分別計算對應(yīng)的K并做查表插值。但如果你只想在固定速度下做軌跡跟蹤那直接在初始化階段把K算好就可以了。本文代碼設(shè)定的v 5m/s如果改變v記得重新運(yùn)行l(wèi)qr求解。4.5 初始航向偏差特別大時為什么跟蹤出現(xiàn)了“繞圈”現(xiàn)象當(dāng)車輛初始航向與參考航向相差接近180度時LQR狀態(tài)模型中的小角度假設(shè)已經(jīng)失效。我的自行車模型運(yùn)動學(xué)方程本身是非線性的但誤差模型是線性的這要求航向誤差足夠小LQR才有效。如果初始航向誤差達(dá)到90度以上車輛可能先去繞一個圈再慢慢糾正回來。解決思路有兩個在啟動LQR之前增加一個“方向調(diào)整階段”比如先用一個簡單的純追蹤算法把航向大致對齊再切換到LQR。使用非線性控制比如后推法或滑模控制但這會讓代碼復(fù)雜度大幅上升。更簡單的工程妥協(xié)修改初始狀態(tài)讓仿真從一個更合理的初始條件開始比如初始航向誤差小于20度LQR的表現(xiàn)就能很好。對于絕大多數(shù)軌跡跟蹤場景車輛不會在極端姿態(tài)下開始控制所以這個問題在使用時注意一下即可。4.6 采樣時間dt如何選擇dt的選擇直接影響仿真的穩(wěn)定性和真實(shí)性。dt過大歐拉法離散化誤差會累積甚至導(dǎo)致系統(tǒng)不穩(wěn)定dt過小仿真的計算量增大但沒有實(shí)際的精度收益。對于運(yùn)動學(xué)自行車模型車速5m/s軸距2.5mdt選擇0.05s已經(jīng)能滿足大部分仿真需求。如果你要擴(kuò)展到真實(shí)控制器考慮一下控制器的運(yùn)行頻率一般底盤控制的周期在10到50ms之間dt取0.01s到0.05s是比較合理的范圍。需要對比測試的話可以寫一個dt參數(shù)的循環(huán)在相同條件下跑多個仿真觀察橫向誤差的收斂軌跡選擇誤差不再顯著減小的那個dt作為臨界值。4.7 完整源碼中可能遇到的Matlab版本兼容問題我在不同的Matlab版本上跑過這份代碼主要兼容性差異集中在兩個點(diǎn)一是lqr函數(shù)需要Control System Toolbox。如果用的是MATLAB Online基本版或者沒有安裝工具箱lqr會直接報錯。建議在腳本開頭加一句環(huán)境檢查if ~license(test, Control_Toolbox) error(需要安裝 Control System Toolbox 才能運(yùn)行LQR求解); end二是wrapToPi函數(shù)屬于Mapping Toolbox。雖然大部分發(fā)行版都內(nèi)置但少數(shù)基礎(chǔ)版可能沒有。如果遇到這個函數(shù)未定義可以自己實(shí)現(xiàn)function psi_norm wrapToPi(psi) psi_norm atan2(sin(psi), cos(psi)); end這個自實(shí)現(xiàn)代碼不受工具箱限制而且因?yàn)檫@里三角函數(shù)計算比較穩(wěn)定放在常規(guī)版本下也表現(xiàn)得很好。5. 進(jìn)階擴(kuò)展從簡單圓形到復(fù)雜軌跡5.1 雙移線軌跡更接近真實(shí)駕駛場景的測試圓軌跡雖然便于驗(yàn)證基礎(chǔ)原理但說實(shí)話太“理想”了。真實(shí)道路中車輛經(jīng)歷的更多是漸變曲率、直線和曲線交替的復(fù)雜路段。雙移線Double Lane Change是車輛橫向控制測試中的經(jīng)典場景它模擬的是高速行駛中變道避障再回到原車道的動作。如果要把參考軌跡換成雙移線核心改動在create_reference函數(shù)。雙移線的參考Y坐標(biāo)表達(dá)式可以寫成% 雙移線參考軌跡 function ref_path create_double_lane_change(v, dt, T) total_len v * T; s 0:dt*v:total_len; % 雙移線橫向偏移使用斜坡加平滑過渡 lane_offset 3.5; % 車道寬度 X s; Y lane_offset / 2 * (1 tanh(0.2 * (X - 20))) - ... lane_offset / 2 * (1 tanh(0.2 * (X - 60))); ref_path.x X; ref_path.y Y; % 參考航向角由路徑導(dǎo)數(shù)計算 dYdX 0.2 * lane_offset / 2 * (1 - tanh(0.2 * (X - 20)).^2) - ... 0.2 * lane_offset / 2 * (1 - tanh(0.2 * (X - 60)).^2); ref_path.psi atan2(dYdX, 1); % 參考曲率由二階導(dǎo)計算 d2YdX2 -0.04 * lane_offset * tanh(0.2 * (X - 20)) .* (1 - tanh(0.2 * (X - 20)).^2) ... 0.04 * lane_offset * tanh(0.2 * (X - 60)) .* (1 - tanh(0.2 * (X - 60)).^2); ref_path.kappa abs(d2YdX2) ./ (1 dYdX.^2).^(3/2); end這里用tanh函數(shù)生成平滑的過渡曲線最大的好處是曲率連續(xù)不會對轉(zhuǎn)向角產(chǎn)生階躍沖擊。曲率計算直接用二階導(dǎo)公式雖然比數(shù)值微分方法略復(fù)雜但精度更高前饋補(bǔ)償?shù)姆€(wěn)定性也更好。5.2 車速變化時的處理思路本文代碼中車速固定為零5m/s但真實(shí)場景中車輛在彎道中通常要減速。如果你希望加入縱向速度控制LQR設(shè)計就需要修改為縱向控制器與橫向控制器協(xié)同工作。一個簡單的思路是根據(jù)參考軌跡的曲率預(yù)先計算期望速度例如v_ref min(v_max, sqrt(a_lat_max / abs(kappa)))其中a_lat_max是最大橫向加速度??v向控制使用一個簡單的PID控制器跟蹤期望速度。橫向LQR的增益需要根據(jù)實(shí)時速度動態(tài)插值這也是前面提到的變增益LQRGain Scheduling思想。這樣的完整系統(tǒng)雖然代碼量更大但配合本文的動態(tài)LQR增益計算在架構(gòu)上是非常自然的擴(kuò)展。5.3 離散化LQR從連續(xù)到離散的跨越代碼中使用的lqr函數(shù)求解的是連續(xù)時間系統(tǒng)的Riccati方程。但在實(shí)際工程特別是嵌入式控制器中系統(tǒng)的采樣是離散的更嚴(yán)謹(jǐn)?shù)淖龇ㄊ鞘褂秒x散LQR即dlqr函數(shù)。離散LQR的問題定義是最小化J sum(xQx uRu)而不是積分形式。如果你的控制器運(yùn)行頻率很高比如100Hz以上連續(xù)LQR與離散LQR之間的增益差異可以忽略不計。但如果運(yùn)行頻率較低例如10到20Hz連續(xù)LQR算出來的增益可能導(dǎo)致系統(tǒng)不穩(wěn)定此時必須用dlqr。Matlab中使用dlqr也非常簡單% 將連續(xù)系統(tǒng)離散化 sys_c ss(A, B, eye(2), 0); sys_d c2d(sys_c, dt, zoh); Ad sys_d.A; Bd sys_d.B; [Kd, ~, ~] dlqr(Ad, Bd, Q, R);這里的c2d使用零階保持器ZOH它假設(shè)控制量在一個采樣周期內(nèi)保持不變這正好符合數(shù)字控制器的實(shí)際行為。對采樣頻率較高的系統(tǒng)連續(xù)和離散的區(qū)別不大但理解這一點(diǎn)能幫你避免不少硬件調(diào)試時的低級問題。6. 實(shí)操心得與工程落地建議這套LQR軌跡跟蹤代碼我在課程設(shè)計、畢業(yè)設(shè)計輔助和幾個小型原型車上都反復(fù)用過。說幾個感觸最深的地方第一別迷信單一指標(biāo)的最優(yōu)性。LQR的“最優(yōu)”是在Q、R定義下性能指標(biāo)最小化但指標(biāo)本身是你定的如果Q、R的工程含義沒有想清楚得到的結(jié)果很可能“最優(yōu)但不好用”。有一個做法是用仿真結(jié)果逆推Q、R的實(shí)際意義。比如橫向誤差從2米收斂到0.1米花了5秒你覺得太慢就找橫向誤差權(quán)重的貢獻(xiàn)到底體現(xiàn)在哪一步而不是盲目把Q提高100倍。第二我強(qiáng)烈建議在改任何參數(shù)之前先把所有歷史數(shù)據(jù)保存下來。調(diào)參最容易犯的錯誤是“同時改了好幾個參數(shù)”然后發(fā)現(xiàn)某個現(xiàn)象變好了卻不知道是哪個改動起了作用。正確的做法是保持其他不變單次只改動一個參數(shù)記錄仿真曲線再做對比。第三這份代碼里最容易被忽略但收益最高的是前饋補(bǔ)償。如果你只想要一個“能跑”的軌跡跟蹤純反饋LQR也能跑彎道會有一點(diǎn)偏差但不會發(fā)散。不過只要加上一行前饋效果立刻脫胎換骨。這個經(jīng)驗(yàn)不只是LQR在MPC、Stanley等其他控制器里也一樣成立要充分利用參考軌跡提供的曲率信息。第四如果要做實(shí)物驗(yàn)證建議先用Simulink搭建一個完整的整車模型做硬件在環(huán)測試不要一上來就把這份Matlab腳本部署到嵌入式控制器里。因?yàn)槟_本中的最近點(diǎn)搜索、角度歸一化、飽和約束等邏輯在硬件里需要重新設(shè)計成實(shí)時可執(zhí)行的代碼這中間涉及到的工程細(xì)節(jié)比控制器本身還多。最后分享一個小技巧如果想讓車輛啟動時更平穩(wěn)可以在仿真前1秒內(nèi)對LQR的輸出做一個線性斜坡限幅讓轉(zhuǎn)向角從零逐漸過渡到LQR算出的值。這個處理在純仿真里看不到什么區(qū)別但在實(shí)際車輛上能明顯減少啟動時的沖擊感。代碼實(shí)現(xiàn)非常簡單在控制律之后加一個限幅斜率即可很多底盤控制器本身也支持。這份源碼從模型建立、誤差推導(dǎo)、LQR求解到仿真驗(yàn)證已經(jīng)覆蓋了車輛軌跡跟蹤最核心的閉環(huán)邏輯。你拿到手之后先原樣跑通一遍然后動手改Q、R權(quán)重看效果再嘗試換參考軌跡最后考慮添加速度控制。走完這幾步LQR軌跡跟蹤控制就算真正掌握了。本文還有配套的精品資源點(diǎn)擊獲取
返回列表
PREV
查看更多資訊
NEXT
返回資訊列表
99在线观看精品视频| 婷婷丁香九色| 色婷婷精品小视频| 丁香五月天啪啪| 影音先锋女人av鲁色资源网小说免费 | 殴美激情综合网| 狠狠色狠狠爱| 超碰AV在线| 婷婷的99视频网站| 五月婷婷综合网| 色婷婷激情小说网| 欧美三级韩国三级日本三斤| 高清无码网址| 久久九九怡红院| 99久久激情视频| 91热久久| 青草青草视频2免费观看| 亚洲性爱干干| 国产精品24r| 色日本网| 丁香五月婷婷黑人妻黄色电影院| 婷婷 激情 五月| 99人妻碰碰久久久禁片| 99热免费在线| 色噜噜婷婷| 久操热线| 九九在线精点品| 91久久综合亚洲噜噜成人在线| 超碰免费人妻| 91大屁股| 狠狠色狠狠| 久久五月丁香| 丁香五月天啪啪a日本| 色香久久| 中字幕视频在线永久在线观看免费 | 久久久中文| 98毛片| 九九成人电影婷婷| 久久久网站| 欧美日韩大黄| 91视频综合网| www,999日本色| 97 A I色色| 亚洲AV中文在线| 中文字幕黄色片| 人人干Av| 久热这里只精品| 亚洲精品又粗又大又爽A片 | 99精品一二三四视频| 丁香午月AV中文字幕| 大香蕉综合视频在线| www.婷婷.com| 婷婷六月丁香久| 九月停停| 激情综合一| 刘玥精品一区| 亚洲舔观看| 欧美久久久中文字幕| 婷婷内射视频在线| 婷婷久久亚洲| 丁香五月婷婷成人色区| 色伊人婷婷| 午夜不卡久久精品无码免费| 原琪琪色影院| 天天插AV丝袜中| 综合久久97| 国产毛片精品一区二区色欲黄A片| 久久九九视频| 五月色情网| av第一二区| 综合久久婷婷| 久久久久久久97| 成人网站免费sxj| 99九九视频| 搡BBBB搡BBB搡五十| 天天爱综合网| 日韩人妻操逼视频| 天堂五月婷婷| 五月天色婷伊人| 色五月自偷自拍婷婷婷婷| 亚洲精品白浆高清久久久久久| 亚洲第一综合| 色狠狠色综合| 五月永久激情| 天天搽天天射| 999热在线视频| 久久婷婷五月天丁香| 草草操操| 毛片新网地| 欧美成人精品一区二区 | 久久ab| 99re鈥哸鈥唙| 草做免费在线观看| 色吧网综合| 亚洲综合五月天| 婷婷五月天综合在线| 丁香六月婷婷缴情欧美| 激情小说五月天| 婷婷综合另类| 99热在线网站| 成人在线免费网址| 琪琪色网址| 色婷婷色99国产综合精品| 色色五月天婷婷丁香| 九九热视频免费的| 伊人婷婷五月| 五月花婷婷最新| 婷婷色激情网| 激情综合自拍五月婷婷色五月| www.yw色| 99碰| 99riAV国产精品视频| 任我肏视频精品| 久久97久久99久久综合欧美| 成人国产欧美大片一区| 日韩aaaaa| 色女人久久| www激情五月天| 天天色天天噜| 久久人妻高清中文| 涩婷婷五月天| 91午夜激情| 色婷婷视频综合| 九九草热在线观看| 婷婷丁香www视频日本韩国| 精品夜夜澡人妻无码AV| 日逼影音先锋男人AV资源站| 伊人激情综合| 久久五月天激情婷婷| 5月丁香综合图区| 黑人巨粗进入警花疼哭A片| 色婷婷久久| 97久久久| 欧美性猛交99久久久久99按摩| 激情九月婷婷| 午夜不卡久久精品无码免费| WWW.五月com| 日韩一级一片内射视频4K| 六月丁香网| 久操婷婷| 潘金莲AAAAAAAAAA| 综合网网欲色| 91黄址| 五月天丁香网站| 人伦30P| 五月丁香六月色| 成人视频在线免费播放| 欧美在线视频99| 久久这里只有精品久久| 婷婷激情综合色五月久久,色婷婷丁香花,丁香婷婷五月情天,久久婷婷五月综合色 | 五月丁香婷婷色色| 超碰国产在线| 五月天小说激情| 亚洲人人操| 五月婷婷天堂| 丁香五月综合| 96精品国产综合久久久久久| 婷婷黄色| 天天色伊人| 激情综合网,婷婷五月天| 欧美黑人巨大猛烈cuckold| 婷婷深爱五月丁香网| A网在线欧洲| 99视频在线观看欧| 激情综合一| 99riAV国产精品视频| 激情综合区| AAA久久| 少妇人妻丰满做爰XXX| 69凹凸成人综合网| 超碰在线视屏| 久久久区区一久久久久久| 五月丁香婷婷成人综合网| 99久久玖玖| 丁香激情网| 激情六月综合| 草久私拍| 人妻性爱av网站| 五月激激网w'w'w| 色婷婷综合网站| 天天做天天爱| 熟妇人妻中文字幕无码老熟妇 | 可以看的AV网站| 激情AV在线| 91久久婷婷| 久综合4| av操一操| 天天色色婷婷| 亚洲色碰| 五月六月伦理| 大香蕉伊人久久| 久久99免费视频网站| 夜夜 操无码| 激情五月综合免费| 超碰大香蕉网| 中文字幕高清av| 91色五月| 青青草原福利在线| 久久丁香五月婷婷| 91精品又长又大又粗又爽又猛| 丁香五月天欧美成人| 综合色综合| 色五月天激情| 丁香六月婷婷社区| 九九热在线视频,| 亚洲人成色A777777在线观看 | 日本女人久久| 99久久国产综合精品五月天喷水\| 色九月婷婷丁香| www.99热最新视频8| 亚洲丁香五月天视频| 九九色逼| 九色91视频| 六月婷婷七月丁香| 久久这里只有精品热在99| 久久婷婷六月综合国际| 小视频在线亚洲| 丁香六月激情综合| 97操资源婷婷| 久久只这里有精品| 99综合免费视频| 久久视频婷婷| 婷婷开心久久| 4438亚洲欧美| 国产激情综合五月| 99精品久久| 亚洲天堂有码| 91人妻视频| 色综合播放| 天天做天天爱天天玩| 98国产精品综合一区二区三区| 久久婷五月综合| 开心五月深爱五月婷| 97热视频| 91在线日本| 激情婷婷五月天在线观看| 深爱综合网| 日本99色| 激情五月激情综合网| 亚洲欧美日韩VIP| 五月丁香久人妻中文| 在线观看免费狠狠色丁香香综合| www综合久久| 99热这里只有精品1998| 日日躁夜夜躁狠狠久久AV| 亚洲中文字幕av| 天天操天天干天天日| .青娱乐天天操B| 欧美天天五月丁香免费观看| 99热欧美| 婷婷色无码| 五月婷婷激情综合网| 99热综合色图| 久久9视频欧美| 97久久五月丁香婷婷| 五月天婷婷丁香社区| 国产做爰视频免费播放| 色五月婷婷丁香婷婷| 国产精品色| 激情五月天在线观看色婷婷| 五月丁香拍拍激情综合| 五月丁香六月激情| 色色啊| 亚洲天天| 婷婷五月亚洲激情| 中文字幕按摩做爰| 白天AV月月| 内射爽无广熟女亚洲| 俺来也综合网精品一区| www.狠狠操| peg 2区三区四区的| 99九九久久| 99热99网| 欧美性猛交XXXX乱大交极品| 97色色色色色色色色色色色色色| 99视频综合| 97好吊操| 男人天堂 久久| 久久国产色| 欧美一线视频| www.五月婷婷久久.com| ww亚洲ww在线观看| 无码AV免费精品一区二区三区 | 91 原创 在线 九色| 丁香五月天资源网| 武则天精品久久| 色婷婷网| 综合99在线| 天天日夜夜爽| 色婷婷视频综合| 肏日网在线看 | 狠狠色噜噜狠狠狠狠综合| 九九亚洲视频| 精品一二三区久久AAA片| 婷婷六月丁香在线| 色综合综合综合| 97色伦另类图片小说视频 | 色操综合| 九九久久99精品免费观看www| 欧美综合五月丁香六月婷| 九月婷婷久久久| AV电影在线播放| 91操操| 五月丁香久久丝袜啪啪| 五月婷婷婷| 成人婷99最新| 五月天色婷婷图片| 另类视屏| 99热只有| 久久怕怕视频| 狠狠色丁香综合| av操逼网| 色五月天丁香婷婷| 日日色五月天| 大香蕉人人人| 色综合色色| 色婷五月| 99热e| 99在线观看| 五月天成人在线播放丁香| 国产操逼网站| 五月婷丁香| 26uuu欧美激情另类| 在线资源av-超碰中文在线-成人AV| 五月婷婷偷| 亚洲乱码日产精品BD| 婷婷五月天激情诱惑| 婷婷五月丁香综合激情| 琪琪色网在线| 粉嫩av蜜桃av蜜臀av| 新99思思视频| 综合色影院| 天天色天天干天天插| 亚洲精品永久久久久久| 婷婷五月天伊人在线| 色九九中文字幕| 97超碰在线免费观看| 99re思思在线视频| 97干在线观看| 久久婷婷五月天激情新地址| 99久久婷婷| 天天拍久久| 手机旧版看人妻1025| 午夜不卡久久精品无码免费| 色五月婷婷在线| 很很干在线视频| 婷婷精品综合| 性爱技巧五月| 极品少妇XXXX精品少妇偷拍| 五月天综合激情网| 亚洲欧美另类在线23p| 九热视频| 91精品久久久久久久久| 超碰在线免费观看日韩| 激情丁香五月天| 色色网站免费| 婷婷色日本| 美女黄频aⅴ视频| 99婷五月| 天堂草在线观| www色婷婷久久综合久色 | 丁香五月天色综合| 色婷婷操逼| 中文字幕丰满乱孑伦无码专区| 久热这里只有精品99re| 色五月婷婷操逼| 狠狠操狠狠干综合| 天天天天天操| 婷婷激情五月吧| 激情四射婷婷色色色| 伊人久久婷婷五月综合97色| 91色久| 欧洲色色| 久久精品只有这| 五月婷婷就去色| 狠狠色色| 日韩人妻在线观看| ′久久99一| 黄色aa观看aaguochan| 少妇被躁爽到高潮无码文| 亚洲综合色色| 久久天堂网| 五月丁香六月激情欧美综合| www夜夜操com| 综合色激情| 婷婷久久六月费| 欧美婷婷六月丁香综合色连续高潮抽搐| 亚洲碰碰碰| 野外99热| 婷婷五月天你懂的| AVV黄| 色播激情| 色色热日| 激情淫乱男女| 青青草原伊人网| 丁香六月欧美| 99九九热视频免费| 九九综舍久久| 日本系列_4页_777FP| 五月丁香毛片| 9月色婷婷| 国产色99| 婷婷内射视频在线| 五月天丁香综合| 黑人熟妇一区二区三区| 婷婷综合视频| 久热大香蕉| 色婷婷在线播放| 91操在线视频| 淫荡家庭AV| 丁香婷婷激情综合五月激情| 丁香五月激情天AV无码| 安息电影在线观看完整版| 色五月之第四色| 久久五月天婷婷视频| 99re这里只有精品9| 99热最新地址在线| 综合五月天亚洲婷婷| 亭亭丁香aV| 午夜婷婷丁香| www.99视频| 激情久久五月天| 久热 91| aa久久| 丁香婷婷网| 久久精品性爱视频,| 九九人妻福利| 美女激情婷婷| 99热一区| 九九人人操| 51国精产品自偷自偷综合| 五月丁香婷婷啪啪| www99精品日韩| 色婷婷AV在线观看| 91久久久久久久91| 色婷婷丁香网| 天天射天天射一道本日本社区 | 97在线天堂| 亚洲AV电影美洲AV电影| 99精品一二三四视频| 丁香啪啪| 亚洲综合婷婷| 精品久久99码| 国产精品美女| 中文字幕在线视频播放| 色婷婷天堂| 性色欲情 网站| 啪啪五月天啪啪| 综合久久六月| 99综合久久| 99热91| 香蕉曰比| 99日本黄站| 色必久悠悠影院| 亚洲丁香五月| 婷色五月天| 99热18| 99久| 亚洲激情四射| 99日韩网站| www.91婷婷| 久操操| 五月天婷婷丁香人人操91| 欧洲亚洲免费视频9 | 国产97色在线| www久久久久久久久久久| 久久色吧| 狠色狠色狠狠色综合网| 六月激情久久| 蜜桃五月天| 婷婷伊人五月丁香天堂网| 亚洲国产成人在线| 99日韩网站| AAA久久| seav天堂| 激情五月天丁香| 一级A片天天操夜夜操| 手机旧版看人妻1025| 狠狠色丁香| 九色PORNY在线精品酒店| 开心五月激情| 色狠狠综合网| 91怕怕网| 开心深爱五月天| 五月天狠狠| 丁香六月激情综合| 久久伦乱| 丁香六月无码播放| av在线资源| 婷婷丁香综合| www激情网| 91久久婷婷| 国产黄色大片| 激情婷婷丁香色情五月天| 狠狠色97| 婷婷激情97| 77799热| 婷婷射丁香| 色色国产| 日本婷久久| 深爱激情六月| 全部老头和老太XXXXX| 国产97色在线| 国产熟女日日骚五月丁香爱| 国产无套精品一区二区| 婷婷97C| 丁香五月婷婷久久久| 久久丁香五月天| 女人被躁到高潮嗷嗷叫小| 久久婷婷亚洲| 影音先锋天天日| 好吊兆人妻| 婷婷激情五月天网站| 久久99综合网| 五月天婷婷激情在线色图| 热久久66| 亚洲精品又粗又大又爽A片| 亚洲、欧美、国产另类笫二区| 婷婷六月久久综合导航| 亚洲无码影音| 色婷婷香蕉丁丁网| 亚洲国产精品二二三三区| 九九综合久久| 午夜免费试看| 久久性操| 亚洲视频二区| 五月天久久www| 99亚州综合精品成人网| 爱iii做iiii日日| 久久成人精品视频| 国产AV熟妇人震精品一品二区| 亚洲小视频免费看| www.五月天婷婷| 婷婷伊人综合中文字幕| 玖玖99免费视频| 99久久精品亚洲综合| 丁香六月无码播放| 五月天婷婷激情四射综合| 久久婷婷色五月| 精品爱欲五| 五五月五月| 亚洲精品久久久无码| 99亚洲综合| 久久精品99| site:xmssd.com| 激情综合五月| 777精品久无码人妻蜜桃| 五月婷亚洲精品AV天堂| 婷婷色五月丁香六月欧美啪| 五月天亚洲图片婷婷| 97在线视频观看| 97AV在线视频| 天天色天天日| 79精品在线视频| 婷婷五月色| 五月婷婷亚洲综合网| 一起草AV| 六月色色婷婷| 91女人18毛片水多国产| 日本成人噜噜噜| 玖玖婷婷婷丁香五月| 26uuu欧美| 91人人澡人人爽人人看| 日本久久天堂| 少妇丁香婷婷 | 这里只有精品视频视频在线观看| 九九色婷婷| 久久狠狠色| 日本激情综合| 99精在线| 丁香色六月婷婷| 99在线小视频| 3p日韩网站视频| 综合久久十三| 日本色婷婷| 亚洲啪啪啪啪| 夜夜爽77777妓女免费下载| 日韩精品色| 婷婷久久婷婷色五月| 人人摸人人澡人人| 婷婷和五月天| 久久综合五月天激情小说网站| 婷婷十月激情综合网| 99热线观看9| 九九aV| 思思视频精品| 91丁香婷婷综合久久欧美| 色五月婷婷在线| 久久婷婷亚洲| 天天狠狠综合精区| 人人摸人人澡人人| 涩综合在线| 久久婷综| 久久综合首页| 激情五月伊人婷婷| 超级碰碰碰久久网站视频| www久久久久久久97| 天天操夜夜爽天天操| 97亚洲视频在线| 婷婷爱五月天| 丁香五月自拍| 密视AV综合在线| 丁香五月久久| 99热这里只有的精品视| 色色色色综合网| 99人妻碰碰久久久禁片| 激情综合五月| 五月天激情网址| 九伊人网| 亚洲亚洲人成综合网络| 日日日日日| 人妻性爱| 91九色欧美| 九色视频91| 亚洲bt丁香五月天婷婷激情小说| 亚洲人妻Av| 亚洲亚洲人成综合网络| 丁香久月| hd五月婷婷在线| 五月天婷婷社区久久综合| 99这里只有精品| 久狠狠狠| 五月天婷婷色小说| 色综合久久综合中文综合网| 丁香五月婷婷五月基地| 久久538| 夜夜干夜夜操| 丁香婷停五月激情综合深爱| 婷婷黄色| www99xxxx五月丁| 超碰国产在线| 久久99久久99精品免观看粉嫩| 丁香五月婷婷色综合基地| 激情图片99| 午夜天天精品视频| 色墦五月丁香| 99这里有精品视频| 五月开心激情网| 婷婷色色五月| 亚洲色色五月天| 色五月婷婷在线观看第一页舔| 婷婷激情性爱| 色欲一区二区三区精品A片| 激情综合色五月六月婷婷| www.精品99| 亚洲99一级无嗎特制在线| 五月丁香啪啪啪| 色天堂A| 日本AAAAAAAAAAAAAA片| 天天草天天爱| 99九九久久| 色婷婷影| 九九激情| 日韩啪啪视品| 欧亚成人A片一区二区| 97色婷| 天天摸天天舔| 久 久9 9 热 视 频| 五月天另类综合网| 激情宗合哪里能看| 久久99视频| 538任你爽视频不一样的| 婷婷六久久| 欧美三级巜人妻互换| 91色久| 秋霞A V毛片| 精品久久久人妻| 狠狠另类视频| 日逼影音先锋男人AV资源站| 天天插天天射| 婷婷伊人綜合中文字幕| 激情五月天色网站| 涩综合网| 精品99视频| 亚洲男女激情| 99久久玖玖| 婷婷五月天首页| 六月婷婷狠狠做| 千人斩操逼| 最新午夜理论片| 9.1综合网| 天天日天天舔天天摸| www久| 色色色欧美| 久九色| 五月婷婷丁香| 99久久精彩视频。| 第四色色六月色综合| 婷色人人狠| 精品在线| www.狠狠操.con| 色插综合网| 九九一综合精品| 午夜无码精品色综合久久| 婷婷五月丁香五月天| 99久久成人| 五月香蕉婷婷| 九九热这里只有精品12| 五月婷婷激情中心| 国产午夜精品久久久观看| 99re思思久久| 五月丁香六月婷| 色五月丁香五月| 日本色爽| 色在线99| 天天插综合| www狠狠| 激情婷婷五月色| 色丁香五月婷婷在线| 99热.com| 色玖玖爱| 亚洲另类在线观看| www.婷婷com| 4438全国最大视频成人网站在线观看| WWW.久久久久久久久久久久久| www.99精品视频| 婷婷综合五月天| 九九RE视频在线精品| 婷婷激情九月| 99热99ai| 热99久久这里只有精品| 五月天六月婷婷电影| 激情六月日韩| AV在线免费播放| 色色婷| 国产毛片操B| 99精品女人天堂| 91Chinese在线| 九九成人高清视频| 九九亚洲视频| 91狠狠色| 欧美色一级色| 日本九九网| 婷婷五月天综合激情| 亚洲综合视频一下| 狠狠操狠狠插| 婷婷五月天av| 国产精品 的国产| 五月丁香91| 插插干干干色| 天天色天天日| 五月天伊人| 91操女| 97碰免费视频在线| 久久这里99| 神马欧美精| 婷婷精品视频| 91日综合欧美| 五月综合视频在线| 99re这里| 99人人干| 欧美在线视频9| 亚洲激情校园| 九九無妻| 99国产精品久久久久久久久久久| 人妻六月天| 噜噜噜噜噜久| 可以看的AV| 五月婷婷激情| se.久久视频在线观看| 婷婷丁香五月亚洲欧美| 欧美美美女性色视频| 久久伊人五月天| 真实熟女-91九色| 丁香六月激情综合| 日笨久久网| 久久久久久天天日天天爱| 精品乱码久久久久| 少妇AB又爽又紧无码网站| 99久久精品国产色欲| WWW、日本色丁香、co m| 久久婷婷色综合| 丁香六月丁香婷婷激情| 99久热在线精品| 婷婷六月丁| 国产特级毛片AAAAAAA高清| 久久免费丁香| 天天草天天日| 丁香五月影院| 激情丁香九九五月综合网| 亚洲婷婷五月天激情综合| 另类天堂| 五月丁香综合影院| 色五月天在线| 激情五月综合| 日韩av在线电影| www五月天com| 超碰免费人| 99在线精品视频观看免费下载| 99视频| 激情6月| 久久丁香五月婷婷激情综合网| 91青娱乐青青草| 日本97在线看片| 五月的丁香六月的婷婷| 色播五月婷婷| w婷婷五月婷婷w| 人人摸人人搞| 国产精品成人网址| 久久五月激情网| 色色色色色色色色网站| 日本女人久久| 九九这里都是精品| 九九99精品视品| 色婷婷丁香五月天在线视频| 四色99久久| www..999热久| 亚洲激情综合| 国产高清视频91九九九久久久| 五月婷婷色丁香| 色五月激情五月开心五月| 久99久热| 七七九色| 日本操片| 色五月婷婷老师| bukadeavzaixian| 五月天婷婷丁香六月| 操逼五月婷婷| 99视频这里只有久久精品| 99在线精品免费视频| 天天五月香欧美| 日本色五月| 欧亚洲在线高清视频| 日韩久久视频| 丁香五月婷婷AV| 综合久久久| 超碰成人av| 色婷婷综合影院| 熟女啪啪视频| 激情久久丁香| 五月天婷婷色色网| 26uuu丁香婷婷五月| 人人操人人爽成人AV| 99热这里只有精品在线观看| 超碰啪啪网| 欧美人妻一区二区| 青青草Avb在线| 无码色色色| 五月婷久久久| 热五月婷婷| 欧美 日韩 成人 在线| 中文AV在线播放| 99自拍网| 日日干天天| 丁香六月 婷婷六月| 丁香五月综合| 99久久成人| 色欲AVV| 99热这里只有精品 搜| 丁香八月综合激情| 美女五月天| 九9九9无码| 另类精品视频在线观看| 丁香婷婷五月综合色情| 五月天色婷婷综合| 国产日批视频| 超碰成人公开| 九九激情视频| 色婷婷综合影院| AV在线大香蕉| 色五月xxx| 亚洲精品国产熟女久久久| 五月天艹天天| 91啪啪啪啪| 综合大香蕉| 色激情综合狠狠婷婷| 青青草视频免费观看| 五月丁香花激情综合网| 大香蕉人人人| 色综合色色| 丁香五月天在线直播观看| 99热热热天天人人人超超碰| 四月丁香五月婷婷久久| www.婷婷亚洲基地| 色婷婷丁香五月| 26uuu亚洲欧美| 久热伊人9| 人妻无码精品一区| 五月婷婷亚洲综合网| 琪琪色网址| 99碰网站| 色欲婷婷夜夜| www.久久99精品| 97人人搞| 黄色99网| 十区av| 天天干天天操天天爽| 97色色色色色色色色色色色色色| 国产亚洲成人综合| 久99| 天天干天天色天天干| 日本色色网站| 91久久久久久久| 五月婷婷之美女图片| 五月久熟女| 中文字幕乱码亚洲精品一区| 六月激情久久| 五月天天天操天天爽夜夜操| 亚洲a片免费观看| 啪啪啪综合网| 亭亭社区五月天| 色噜噜狠狠色综合网| 粉嫩AV久久一区二区三区| 日日夜夜狠狠操| 日日操夜夜爽白洁| 久久大香蕉伊人| 狠狠色噜噜狠狠狠888| 五月丁香龟婷婷| 国产激情综合| 久久久久视剧HD| 五月激情啪啪| 亚洲另类噜噜| 九九热这里有精品视频| 五月亭久久无码视频| 久/久精品99看9| 99精品热| 色婷綜合网| 手机在线日韩视频中文字幕| 国产性av| 婷婷五月丁香香蕉| 六月丁香激情网| 亚洲激情 久久| 国产精品国产| 亚洲日韩久久婷婷伊人| 久草天堂| 婷婷五月激情天| 99re久热只有精品6在线直播| 欧美伊人9| 婷婷色影音天| 丁香五月天激情视频| 五月久久婷婷成人网| 天天艹夜夜爽| 亚洲超碰在线| 七月激情六月婷婷综合在线播放| 国产欧美精品AAAAAA片| 日本色色图| 人人摸人人摸| 亚洲九九99精品视频在线播放| 五月婷婷免费在线| 777久久久| 丁香8月手机综合| 久久综合激情| 婷婷五月天久久| 九九成人高清视频| 丁香婷婷综合激情五月色| 最近中文字幕2019视频1| AV在线免费播放| 激情五月综合网最新 | 在线成人av播放| 欧美大片| 伊人激情影院| 99色热视频| 久久99热这里只有精品| 中文字幕 久久9999| www.25五月婷婷| 激情综合无码| 精品99在线看| 欧美性交一区二区三区| 真实的国产乱XXXX在线91| www.99热| 亭亭五月丁香五月天激情| 久久婷婷91| 99热这里是精品| 婷婷丁香五月综合激情小说| 激情丁香五月激情婷婷| 五月婷婷五月丁香综合| 色色色999| 成人Av在线大片| 另类丁香综合| 9久热在线精品| 婷婷色资源| 懂色av粉嫩av蜜臀av| 裸睡玩奶头(高H)| 国产激情综合五月久久| 成人丁香五月| 久久久91| 婷婷综合五月天| 99riAV国产精品视频| 亚洲精品大片| 久久久精品人妻录| 婷婷五月天人妻| 五月丁香婷婷综合激情基地| 天天爽—爽| 激情五月伊人婷婷| 亚洲综合视频天天精品| 91综合在线| 91热在线| 亚洲综合色色| 97人人操人人爽| 天堂A∨在线| www.超碰在线| av大香蕉| 天天天久久久| 99热20| 婷婷色五月亚洲| 天天婷婷操| 综合热无码| 亚洲成人乱码av网站| 综合图区激情| 亚艹艹| 色五月婷婷亚洲| 五月丁香婷婷欧美| 色婷婷五月综合在线| 久久色五月天| 9 1超碰九色| 色色性爱视频| 欧美槡BBBB槡BBB少妇| 日韩成人无码| 午夜不卡久久精品无码免费| 五月婷婷黄色网址| 久久天堂女人| 色综合香蕉| 97碰超级人人看| 激情网站五月| 五月婷婷在线视频免费观看| 99久久66| 天天草天天舔| 99热这里只有精品一| 欧美英丁香开心快乐六月天网| 色婷婷五月天在线| 天天插天天插天天插| 婷婷久久草| 思思热在线精品视频| 久久九九国产精品怡红院| 婷婷淫淫狠狠六月| 日本九九网| 久久三级视频| 婷婷激情四射| 九九热AV| 9国产在线视频| 色丁香五月婷婷综合久久| 欧美性久| 丁香久久| 婷婷激情五月天亚洲综合| 天天激情| 超碰人人操人人干| 欧美人与性动交CCOO| 亚洲在线网站| 五月婷婷久久大香蕉| 秋霞三级色戒| www.minyis.com【JT】国内CDN落地页保证转化QQ2101460746 | 精品网站:999WWW| 色爱亚洲| 成人无码精品1区2区3区免费看| 三日本无码| 婷婷五月天精品| 色五月激情综合网| 天天操天天日天天爱| 亚洲五月综合色播| 亚洲第一综合| 五月丁香影院| 九九热内射| 婷婷五月丁香综合| 精品九九视频| 六月婷婷色色色| 日日日日日| 综合网色综合| 狠狠五月激情在线| 99操逼| 开心激情网五月天| 婷婷五月天亚洲综合网| 欧美色色色色色| 日韩按摩二区| 97碰碰在线看视频免费| 97在线观视频免费观看| 久久hd| 天天做天天要天天爽| 天天爽天天操| www.五月天| 99久久99视频| 天天射影视综合网| 九月丁香亭亭| 91碰免费视频| 99色婷婷| 日韩AV大全| 久久奄也去色色网站| 99精品视频免费观看| 99精品视频免费观看近期发布| 婷婷五月天网址| 中文不卡一二三区| 大香蕉99| 成人AV播放| 久久婷婷五月综合97色一本| 色婷婷色五月天| 丁香五月天激情| 婷婷五月丁香色播| 婷婷丁香五月激情中文字幕版| 玖玖婷婷五月| 久久久精品免费啪啪国| 五月婷婷色| 国产成人亚洲综合A∨婷婷| 五月婷婷六月天| 99狠狠色| 99在线精品免费视频| 色99色| 婷婷五月丁香综合激情| dingxiangtingtingliuyue| 久草热在线视频| 我去色色网五雨天| 狠狠狠五月婷婷六月丁香| 26uuu精品一区二区| 丁香五月婷婷大香蕉| 欧美Va在线| 丁香婷婷五月综合色情| 97人人操com| 色综合色综合色综合高潮| 欧美在线操| 五月婷婷激情在线| 激情另类综合| 激情六月丁香| 丁香六月色| 开心五月综合| 9色在线| 大香蕉啪啪| 亚洲激情网| 好叼操在线观看| 色欲操| 91久热| 五月天久久网站| 久久五月综合| 久爱综合| 六月丁香射婷婷欧美色图片| 91久久久久久| 在线观看亚洲视频影院| 天天狠狠夜夜狠狠2023| 成人国产网| 婷婷丁香视频| 人妻少妇色综合| av九九| 激情综合婷婷| 国产精品操| 婷婷五月激情六月| 超级碰碰碰久久网站视频| 日日做天天操夜夜爽| 天天天日天天天干| 五月丁香在线| 五月天婷婷久久日| 深爱激情五月网| 99久久99九九99九九九| WWW.17C亚洲精品| 《丁香激情综合久久伊人久久》影视在线观看 -高清预告手机免费播放 -三妹影院 | 免费AV播放| 天天性视频| 天天干天天干天天干天天干天| 99九九在线精品热动漫| 狠狠色大香蕉| 玖玖婷婷色五月| 桃色五月天| 99碰视频| 99免费在线视频| 影音先锋 一区| 噜噜噜久久| 亚洲婷婷婷| 97性视频| 国内一级精品| 国产精品成人AV在线| 久久免费操| 五月丁香久久网| 大香蕉久艹| 五月激情视频| 婷婷五月天成人在线视频| 欧美va在线| 五月丁香六月婷婷精品| 九九色精品| 五月色情婷婷开心五月色情| 麻豆WWWCOM内射软件| 婷婷基地五月色| 久久丁香综合香蕉| 婷婷五月天在线观看第二页| 久久女人九九| 婷婷久久亚洲| 精品导航在线x不卡| 天天爽天天摸人妻综合网| 九九九九九999999| 狠狠狠五月婷婷六月丁香| 婷婷成人av| 热99在线| 免费97碰碰| AV九九| 99视频久久免费视频| www.9操| 日本久久爱| 97日在线视频| 久久婷婷伊人| 99久热这里有精品| 大香蕉视频婷| 伊人网啪啪| 婷婷五月成人有| 色域五月丁香| 亚洲综合视频网| www.粉嫩av.com| 婷婷伊人五月天| 91狠狠色丁香| 久月丁香爱婷婷综合| 激情九月婷婷九月| www.婷婷六月天| caopeng超碰| 亚洲成人AV一区在线观看| 玖玖午夜视频| 婷婷第六色| 五月停性愛| 五月婷婷成人| 六月丁香花婷婷| 五月天婷婷综合色| 日日爽夜夜爽| 日日骑夜夜撸| 丁香五月天操B| 色五月五月天色婷婷色五月| 97高清国语自产拍| 五月天婷婷色五月天| 七月丁香五月婷婷在线| 天天干天天干天天干天天干天天干| 九九亚洲综合| 亚洲乱码日产精品BD在线观看| 99re热免费观看视频精品| 操人久久| 另类少妇人与禽zOZZ0性伦| 第四色首页| 五月婷婷色情| 色狠狠色综合久久久绯色aⅴ影视| 色婷婷五月天激情在线观看| 色色操| WWW.桔色成人.COM| 天天干天天爽天天爽| 久久99久久久| 26uuu最新地址| 亚洲99视频| 激情婷婷网| 天天噜噜| 岛国av电影网站| 超碰猛烈的性猛交| 国产精品国产成人国产三级| 久久久五月四色| 五月天久久久| 亚洲在线网站| 97啪啪| 丁香五月天啪啪激情综和网| 97久久草草超级碰碰碰| 99精品视频免费观看近期发布| 五月丁香色综合| 亚洲综合新99视频| 国产色99| 99精品偷自拍| www,色婷婷| 天天干天天干天天干天天干天天| 99精品视频偷拍| 爱操天堂| 天天综合网站| 五月丁香五月婷婷| 国熟女视频| 97精品欧美91久久久久久久| 久久91久久精品久久| 亭亭五月色男人| 丁香婷婷视频在线| 亚洲色婷婷网站| 97视频.干com| 婷婷综合久久综合| 婷婷啪啪| 婷婷丁香18| 婷婷玉月丁香五月在线视频| 无码四色色色| 五月婷婷啪啪网| 99日视频在线| 国外亚洲成AV人片在线观看| 婷婷五月激情丁香| 国产成人av在线播放|