日韩精品一区二区三区在线视频放-无码中文字幕V?一区二区-成年片免费观看视频-国内少妇人妻丰满av-国产精品中文字幕免费观看-亚洲成人久久一区二区三区-国内少妇偷人精品视频无缓冲-一区二区国产精品日本一区二区三区在线网

ARTICLE DETAIL

資訊詳情

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

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

Matlab實現(xiàn)LQR車輛軌跡跟蹤:從狀態(tài)空間模型到完整源碼 簡介本資源是一份面向本科畢業(yè)設(shè)計與控制類課程作業(yè)的Matlab實踐項目聚焦線性二次調(diào)節(jié)器LQR在車輛軌跡跟蹤中的工程實現(xiàn)幫助學(xué)習(xí)者將控制理論轉(zhuǎ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è)課題的參考范例。讀者可直接運行main.m復(fù)現(xiàn)完整LQR控制器設(shè)計流程從車輛狀態(tài)空間建模、Riccati方程求解、反饋增益矩陣K計算到實時軌跡跟蹤仿真與橫向誤差分析代碼注釋詳盡關(guān)鍵參數(shù)可調(diào)便于理解權(quán)重矩陣Q/R對跟蹤性能的影響機(jī)制。 一直做路徑規(guī)劃相關(guān)的仿真最近被問得最多的就是“怎么讓車沿著期望軌跡走而且別走偏”。用單純的PID調(diào)參調(diào)到懷疑人生換MPC又覺得推導(dǎo)太復(fù)雜、參數(shù)太多。其實很多場景下LQR是最合適的折中方案尤其是用于簡單的車輛軌跡跟蹤既不需要復(fù)雜的非線性優(yōu)化也不需要海量標(biāo)定數(shù)據(jù)只要把狀態(tài)空間模型和權(quán)重矩陣?yán)砬宄惶譓atlab代碼就能跑出很穩(wěn)的跟蹤效果。這篇內(nèi)容圍繞“Matlab實現(xiàn)LQR實現(xiàn)車輛軌跡跟蹤完整源碼”展開我會先解釋為什么選LQR、怎么建立車輛誤差狀態(tài)模型再給出可復(fù)現(xiàn)的完整仿真代碼最后分享調(diào)參經(jīng)驗和實際坑點。不管是正在做課程設(shè)計、畢業(yè)設(shè)計還是剛開始接觸車輛橫向控制的工程師都能在這篇文章里找到可以直接抄作業(yè)的思路。1. 軌跡跟蹤問題拆解先搞清楚我們在解決什么1.1 路徑跟蹤和軌跡跟蹤是兩個完全不同的任務(wù)很多第一次做車輛控制的人會把“路徑跟蹤”和“軌跡跟蹤”混為一談但這兩者的底層邏輯完全不一樣。路徑跟蹤Path Following只要求車輛貼著參考幾何路徑走不關(guān)心何時到達(dá)某個點參考路徑通常由弧長參數(shù)化車輛速度可以獨立控制。軌跡跟蹤Trajectory Tracking則要求車輛在特定的時間戳到達(dá)特定的空間位置參考軌跡不僅包含位置信息還包含期望速度、期望加速度。本文對應(yīng)的“車輛軌跡跟蹤”屬于后者但在具體實現(xiàn)中如果只關(guān)注橫向控制我們通常會把縱向速度固定轉(zhuǎn)而把問題分解為“橫向位置偏差和航向偏差的反饋鎮(zhèn)定問題”。一旦速度恒定軌跡跟蹤就退化為路徑跟蹤這讓LQR這種線性控制器有了用武之地。還有一點特別容易踩坑LQR本質(zhì)是線性系統(tǒng)的最優(yōu)控制器但車輛運動模型本身是非線性的橫向偏差、航向偏差、轉(zhuǎn)向角之間存在三角函數(shù)關(guān)系。所以直接套用LQR幾乎不可能穩(wěn)定必須先做線性化處理。通常的做法是采用運動學(xué)自行車模型并在參考工作點附近進(jìn)行小角度假設(shè)。1.2 為什么是LQR而不是PID或者M(jìn)PCPID是很多人的第一反應(yīng)畢竟實現(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. 車輛運動學(xué)模型與誤差狀態(tài)方程2.1 自行車模型的基本假設(shè)車輛在實際行駛中有復(fù)雜的橫向動力學(xué)特性考慮輪胎側(cè)偏、側(cè)傾、懸架等但在低速軌跡跟蹤場景下運動學(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)則控制量還包括油門/制動。在后軸中心位置建立參考點時該模型具有一定的不完整性約束但用于LQR的設(shè)計并不會造成太大問題因為我們后面會構(gòu)造誤差狀態(tài)模型而不是直接對x、y做控制。2.2 從誤差角度建立LQR需要的狀態(tài)空間方程軌跡跟蹤的本質(zhì)是讓車輛的橫向位置誤差和航向誤差逐漸收斂到零。我習(xí)慣把誤差定義在“參考軌跡上離車輛最近的點”基準(zhǔn)下e_y 橫向位置誤差車輛后軸中心到參考點的垂向距離 e_psi 航向誤差車輛航向角減去參考點處的參考航向角如果車速恒定行駛路徑曲率緩變那么誤差狀態(tài)滿足如下近似的線性微分方程e_y_dot v * e_psi % 橫向位置誤差的變化率由航向誤差和車速決定 e_psi_dot v / L * delta - psi_ref_dot其中psi_ref_dot就是參考點處的航向角變化率在參考軌跡已知的情況下可以直接計算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 可控性與前饋補償?shù)谋匾则炞C一下系統(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)誤差。因此實際工程中要在反饋控制的基礎(chǔ)上加入前饋項。前饋補償?shù)奈锢砗x很直接當(dāng)車輛在曲率半徑為R的彎道中勻速行駛時需要維持一個穩(wěn)態(tài)轉(zhuǎn)向角delta_feedforward L * kappa_ref。這是因為轉(zhuǎn)向角的穩(wěn)態(tài)值本來就由幾何關(guān)系決定反饋控制只需要在這個基準(zhǔn)上修正偏差。所以最終控制律為delta -K * x_c delta_feedforward這樣的結(jié)構(gòu)讓LQR控制器在彎道中也不會出現(xiàn)明顯的跟隨滯后。很多只有純反饋的LQR實現(xiàn)會在圓形軌跡跟蹤時出現(xiàn)持續(xù)的橫向偏差原因就是漏了前饋項。3. Matlab完整源碼實現(xiàn)與逐步解析3.1 代碼整體架構(gòu)設(shè)計整套代碼按功能分成幾個文件便于閱讀和調(diào)試main.m 主程序負(fù)責(zé)參數(shù)設(shè)置、仿真循環(huán)和可視化 create_reference.m 生成參考軌跡包括直線、圓、雙移線等 bicycle_dynamics.m 自行車模型運動學(xué)方程 compute_reference.m 在參考軌跡上尋找最近點并計算參考航向角、曲率 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. 在參考軌跡上尋找最近點 [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, state(1), state(2), k); % 2. 計算橫向誤差和航向誤差 % 橫向誤差車輛位置到參考點的橫向偏差有符號 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]效果是一樣的。重點在于這里的狀態(tài)是誤差狀態(tài)而不是車輛本身的x、y、psi狀態(tài)。有人會誤把車輛的x、y當(dāng)作LQR輸入狀態(tài)那必然無法收斂。3.3 參考軌跡生成圓形軌跡與曲率的計算圓是最經(jīng)典的LQR測試路徑因為它的曲率恒定方便驗證前饋補償?shù)男Ч?。create_reference函數(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 最近點搜索與參考信息的提取compute_reference是整套代碼里最容易導(dǎo)致性能瓶頸的環(huán)節(jié)但也是保證跟蹤精度的關(guān)鍵。它的任務(wù)是找到參考軌跡上離車輛當(dāng)前位置最近的點并返回該點的航向角和曲率。function [ref_x, ref_y, ref_psi, ref_kappa, idx] ... compute_reference(ref_path, x_veh, y_veh, k) % 在參考軌跡上找到離車輛位置最近的點 % 為提高效率只搜索當(dāng)前索引附近的局部區(qū)域 n length(ref_path.x); search_range 50; % 最多向前搜索50個點 % 在當(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這種“局部搜索”的做法大幅提升了仿真速度因為軌跡跟蹤場景下車輛不會偏離參考軌跡太遠(yuǎn)沒必要從第1個點開始全軌跡搜索。但是要注意在仿真初期如果車輛初始偏差很大或者車輛與軌跡間存在較大的分離局部搜索可能找到的不是全局最近點。所以我把搜索起點設(shè)成k-5并允許向后索引一些點如果仍然擔(dān)心魯棒性可以在仿真前幾十步使用全局搜索作為熱啟動。3.5 自行車模型更新四階Runge-Kutta還是歐拉法在仿真精度要求不高的場景下使用簡單的歐拉法就夠了。但如果你希望看到更平滑的軌跡或者后續(xù)要把代碼擴(kuò)展到硬件在環(huán)仿真我會建議至少使用二階Runge-Kutta中點法。這里給出一個簡單而穩(wěn)健的方法function state_new bicycle_dynamics(state, v, delta, dt, L) % 自行車模型運動學(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ù)確實做了所有核心工作驗證矩陣維度、求解連續(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(參考軌跡, 實際軌跡); 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)橫向誤差基本在厘米級甚至毫米級這就是前饋補償?shù)男Ч?. 調(diào)參經(jīng)驗與常見問題排查4.1 權(quán)重矩陣Q和R的調(diào)參策略先別急著追求對稱之美Q矩陣和R矩陣的選取直接決定LQR的控制行為這也是很多人卡住的地方。要知道LQR沒有“絕對正確”的Q和R只有“符合需求”的Q和R。一個實用的調(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)重。因為航向誤差的變化率本身與橫向誤差關(guān)聯(lián)過高的航向權(quán)重會導(dǎo)致車輛頻繁修正航向表現(xiàn)就是轉(zhuǎn)向振蕩。用一組經(jīng)驗參數(shù)參考車速5m/s、軸距2.5m的模型中Q diag([5, 1])、R 0.3已經(jīng)是比較均衡的起點。具體怎么調(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]沒有被抵消。通俗點說LQR反饋只負(fù)責(zé)“糾偏”但車輛在彎道上本身就需要的那個基礎(chǔ)轉(zhuǎn)向角反饋控制是“管不著”的。如果沒有前饋補償控制器需要積累一個恒定的航向誤差才能讓車輪產(chǎn)生足夠的轉(zhuǎn)向角于是車輛就會一直在彎道內(nèi)側(cè)“壓著”軌跡走表現(xiàn)為持續(xù)的橫向偏差。解決方法就是加上delta_ff L * ref_kappa的前饋補償。加了之后車輛在進(jìn)入彎道前就能提前打好方向反饋只需要處理模型誤差和外界擾動。我在調(diào)試圓形軌跡時對比過不加前饋時穩(wěn)態(tài)橫向誤差大約20cm加上前饋后直接降到1cm以內(nèi)。4.3 轉(zhuǎn)向角飽和控制器能算出超過物理極限的轉(zhuǎn)角Matlab仿真中一切輸入輸出都是理想化的但任何真實車輛都有轉(zhuǎn)向角限制。一般家用車的方向盤轉(zhuǎn)角換算到前輪轉(zhuǎn)角大概在±30度到±35度如果你在LQR設(shè)計中完全不管這個限制控制器可能會計算出一個完全無法執(zhí)行的大轉(zhuǎn)角仿真結(jié)果看似正常但實際應(yīng)用時車輛會失控。解決方式是在控制律之后加一個飽和函數(shù)代碼中我已經(jīng)寫了max_delta 30 * pi / 180; delta max(-max_delta, min(delta, max_delta));加飽和函數(shù)之后要注意一個問題如果車輛初始偏差非常大LQR會輸出飽和的轉(zhuǎn)向角但仍然不夠快地把誤差拉回這導(dǎo)致實際響應(yīng)比無飽和場景差很多。這種情況可以適當(dāng)增加Q中橫向誤差的權(quán)重讓控制器“更激進(jìn)”地糾偏而飽和限幅本身也能保證轉(zhuǎn)角不超出物理限制。飽和決策應(yīng)該在仿真中盡早驗證否則做硬件時容易心態(tài)崩。4.4 為什么車速改變后LQR增益需要重新求解很多人會認(rèn)為LQR增益矩陣K只要解一次就夠了但事實上系統(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ā)點。標(biāo)準(zhǔn)的做法是在不同速度區(qū)間分別計算對應(yīng)的K并做查表插值。但如果你只想在固定速度下做軌跡跟蹤那直接在初始化階段把K算好就可以了。本文代碼設(shè)定的v 5m/s如果改變v記得重新運行l(wèi)qr求解。4.5 初始航向偏差特別大時為什么跟蹤出現(xiàn)了“繞圈”現(xiàn)象當(dāng)車輛初始航向與參考航向相差接近180度時LQR狀態(tài)模型中的小角度假設(shè)已經(jīng)失效。我的自行車模型運動學(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)定性和真實性。dt過大歐拉法離散化誤差會累積甚至導(dǎo)致系統(tǒng)不穩(wěn)定dt過小仿真的計算量增大但沒有實際的精度收益。對于運動學(xué)自行車模型車速5m/s軸距2.5mdt選擇0.05s已經(jīng)能滿足大部分仿真需求。如果你要擴(kuò)展到真實控制器考慮一下控制器的運行頻率一般底盤控制的周期在10到50ms之間dt取0.01s到0.05s是比較合理的范圍。需要對比測試的話可以寫一個dt參數(shù)的循環(huán)在相同條件下跑多個仿真觀察橫向誤差的收斂軌跡選擇誤差不再顯著減小的那個dt作為臨界值。4.7 完整源碼中可能遇到的Matlab版本兼容問題我在不同的Matlab版本上跑過這份代碼主要兼容性差異集中在兩個點一是lqr函數(shù)需要Control System Toolbox。如果用的是MATLAB Online基本版或者沒有安裝工具箱lqr會直接報錯。建議在腳本開頭加一句環(huán)境檢查if ~license(test, Control_Toolbox) error(需要安裝 Control System Toolbox 才能運行LQR求解); end二是wrapToPi函數(shù)屬于Mapping Toolbox。雖然大部分發(fā)行版都內(nèi)置但少數(shù)基礎(chǔ)版可能沒有。如果遇到這個函數(shù)未定義可以自己實現(xiàn)function psi_norm wrapToPi(psi) psi_norm atan2(sin(psi), cos(psi)); end這個自實現(xiàn)代碼不受工具箱限制而且因為這里三角函數(shù)計算比較穩(wěn)定放在常規(guī)版本下也表現(xiàn)得很好。5. 進(jìn)階擴(kuò)展從簡單圓形到復(fù)雜軌跡5.1 雙移線軌跡更接近真實駕駛場景的測試圓軌跡雖然便于驗證基礎(chǔ)原理但說實話太“理想”了。真實道路中車輛經(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ù)雜但精度更高前饋補償?shù)姆€(wěn)定性也更好。5.2 車速變化時的處理思路本文代碼中車速固定為零5m/s但真實場景中車輛在彎道中通常要減速。如果你希望加入縱向速度控制LQR設(shè)計就需要修改為縱向控制器與橫向控制器協(xié)同工作。一個簡單的思路是根據(jù)參考軌跡的曲率預(yù)先計算期望速度例如v_ref min(v_max, sqrt(a_lat_max / abs(kappa)))其中a_lat_max是最大橫向加速度。縱向控制使用一個簡單的PID控制器跟蹤期望速度。橫向LQR的增益需要根據(jù)實時速度動態(tài)插值這也是前面提到的變增益LQRGain Scheduling思想。這樣的完整系統(tǒng)雖然代碼量更大但配合本文的動態(tài)LQR增益計算在架構(gòu)上是非常自然的擴(kuò)展。5.3 離散化LQR從連續(xù)到離散的跨越代碼中使用的lqr函數(shù)求解的是連續(xù)時間系統(tǒng)的Riccati方程。但在實際工程特別是嵌入式控制器中系統(tǒng)的采樣是離散的更嚴(yán)謹(jǐn)?shù)淖龇ㄊ鞘褂秒x散LQR即dlqr函數(shù)。離散LQR的問題定義是最小化J sum(xQx uRu)而不是積分形式。如果你的控制器運行頻率很高比如100Hz以上連續(xù)LQR與離散LQR之間的增益差異可以忽略不計。但如果運行頻率較低例如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ù)字控制器的實際行為。對采樣頻率較高的系統(tǒng)連續(xù)和離散的區(qū)別不大但理解這一點能幫你避免不少硬件調(diào)試時的低級問題。6. 實操心得與工程落地建議這套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的實際意義。比如橫向誤差從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ù)記錄仿真曲線再做對比。第三這份代碼里最容易被忽略但收益最高的是前饋補償。如果你只想要一個“能跑”的軌跡跟蹤純反饋LQR也能跑彎道會有一點偏差但不會發(fā)散。不過只要加上一行前饋效果立刻脫胎換骨。這個經(jīng)驗不只是LQR在MPC、Stanley等其他控制器里也一樣成立要充分利用參考軌跡提供的曲率信息。第四如果要做實物驗證建議先用Simulink搭建一個完整的整車模型做硬件在環(huán)測試不要一上來就把這份Matlab腳本部署到嵌入式控制器里。因為腳本中的最近點搜索、角度歸一化、飽和約束等邏輯在硬件里需要重新設(shè)計成實時可執(zhí)行的代碼這中間涉及到的工程細(xì)節(jié)比控制器本身還多。最后分享一個小技巧如果想讓車輛啟動時更平穩(wěn)可以在仿真前1秒內(nèi)對LQR的輸出做一個線性斜坡限幅讓轉(zhuǎn)向角從零逐漸過渡到LQR算出的值。這個處理在純仿真里看不到什么區(qū)別但在實際車輛上能明顯減少啟動時的沖擊感。代碼實現(xiàn)非常簡單在控制律之后加一個限幅斜率即可很多底盤控制器本身也支持。這份源碼從模型建立、誤差推導(dǎo)、LQR求解到仿真驗證已經(jīng)覆蓋了車輛軌跡跟蹤最核心的閉環(huán)邏輯。你拿到手之后先原樣跑通一遍然后動手改Q、R權(quán)重看效果再嘗試換參考軌跡最后考慮添加速度控制。走完這幾步LQR軌跡跟蹤控制就算真正掌握了。本文還有配套的精品資源點擊獲取
返回列表
PREV
查看更多資訊
NEXT
返回資訊列表
99精品九九九九九九| 啊啊啊啊免费视频| 欧美强奸乱能| 日韩av电影网站| 免费啪啪一级视频| 国产AV天美传媒一区二区三区 | 99久久九九| 丝袜天堂网| 蜜乳AV网址| 欧美日日人人天天| 日韩人妻操B| 日韩 欧美 校园一区| 免费av在线播放二区| 99xav| 国产精品美女在线一区| 91在线丝袜| www.狠狠操| 91 丝袜在线播放| 免费9 1久久| 激情国产乱伦Av| 亚洲文学偷乱拍啪啪啪啪| av天堂手机版追回| 99久国产精品午夜性色福利| 亚洲人妻色图| 色九月综合| 91人妻素女| 亚洲激情在线| 伊人 俄罗斯 a v| 99热免费| 日韩精品午夜操呦呦不卡影院| 91久久精品蜜臀| 中文字幕乱码人妻二区三区| 九九热av| 九九九九免费高| 99综合免费视频| 人人看黄色视频| 国产麻豆一区二三区| 久久伊人五月天| 欧美gv在线观看| 日本不卡免费二区| 久久久∴| 国产欧美伊人| 色噜噜人妻丝袜a∨先锋影 | 久久XX| 无遮挡又黄又刺激的视频| 一区二区三区色综合| 天天操综合网| 夜夜操夜夜高潮夜夜爽国产精品区| 精品国产一区二区三区av在线资源| 日韩av免费一级电影| 婷婷五月天色网| 国产精品午夜成人福利| 国产免费永久精品无码| 日韩字幕一区| 热99re69精品8在线播放| 天堂涩涩| 91在线限制级| 中文字幕在线观看丝袜| 婷婷中文字幕| 欧亚乱色熟女一区二区| 欧美综合国产精品久久丁香| 欧美综合色图片| 97人亚洲综合字幕| 久久人妻精品| 99久久精品国产高潮| 精品一二三区四视频| 六月丁丁香| 一级特级aaaa毛片免费观看| 中文字幕日韩精品久久| 中文字幕三四区| 国产又粗又长的视频| 裸体1区| 亚洲国产一区二区三区在线| www.色婷婷| 欧美强奸乱能| 国产亚洲精品精AV.| 变态综合色| 色色色综合| 亚洲一区二区三区欧美日韩| 免费一级a毛片久久久久久鸭绿欲 国产精品亚洲天堂网址 | 无码高清操逼| 大香蕉中文网| 超碰欧美97资源| 色超碰综合| 精品久久在线区一区| 亚洲一区中文字幕一区| 久久久久久久亚洲Av无码| 天天射天天操天天干天天吃2018| 色臀av| 欧美色91| 99少妇内射| 国产熟女自拍| 色香网| 久久婷婷欧美| 熟妇高潮精品一区二区三区下载| 911粉嫩人妻| 日韩欧美成人性爱在线| 欧美一级久久久久久久大片动画| 做爱A级亚欧| 久久精品人人做人人看| 日韩成人无码| 91久久青青草原精品| 久久超碰com| 国产美女高潮视频| 风间由美日韩欧美久久| 青青草一本道福利视频| 色哟哟av| 蜜臀久久99精品久久久久久成人小说| 人妻丰满熟妇av无码区蜜桃| 亚洲91综合| 成年女人18级毛片毛片免费观看| 十八禁av无码免费网站APP| 台湾一区国产高清在线| 欧美一区二区三区蜜桃| 国产操偷| 在线v中文字幕一区二区三区| 欧美日本中字另类在线| 免费看黄视频亚洲网站| 日韩精品中文字幕一| 欧亚性爱在线视频| 岛国毛片手机在线观看| 午夜偷拍久久熟女| 欧美色综合影院| 丰满人妻一区二区三区免费 | 嗯阿好爽好紧| 久久久久久久久久久久久9999| 午夜精品一区二区三区三上悠亚| 夜夜中出国产| 老师充足的奶水小说| 欧美伦乱爱| 色婷婷淫色网| 偷窥自拍亚洲| 国产深喉视频一区二区| 蜜臀无码视频在线观看| 黄色大片免费在线| 伊人97| 少妇色欲综合网2| 夜夜爽夜夜操| 91在线超高颜值国产| 波多野结衣先锋影音| 果冻国产精品麻豆成人av| 日本岛国黄色网址| 天天日老熟妇| 久久鲁夜| 久久精品一区二区三区蜜桃臀| 女人香蕉久久毛毛片精品| 国产精品农村妇女| 国产精品分类在线观看| 婷婷丁香五月天亚洲天堂网| 色五月亚洲| 国产精品久久久久久夜夜夜| 在线中文字幕| 欧美人妻久久精品二区三区| 久久综合婷婷| 91嫩草欧美| 91n免费处女| 久久线上视频免费看| 又大又大又大又粗爽高潮观看 | 26uuu性物| www.91逼逼.com| 精品久久久久久无码| 国产区91柔拿会所技师| av网页一区二区三区| 探花精品视频| 欧美狠狠弄| 色在线69堂| 在线洲亚线| 熟女久久久| 超碰色图| 色网在线视频观看免费| 国产精品国产拍高清AV| 日韩欧美aⅴ综合网站发布| 国产久久久久久久久一区二区 | 天天搞在线综合网| 色综合色综合网| 秋霞男人网| 日韩有码回春沙龙第一页| 夫妻天天操岛国视频| 精品中文字幕一区二区| 日韩久久三区| 美熟女逼导航AV操逼| 久操国产在线| 午夜男人av| 99色色| 啊啊啊啊二区好大| 超碰97玖玖爱| 91四海无码日韩欧美| 亚洲国产精品成人综合| 色五月婷婷网| 在线观看午夜婷婷久久久久清性观看| 午夜在线播放| 免费观看的黄色的网站| 禁止观看美女黄| 91在线丝袜| av一区二区三区不卡| 制服乱伦| 日韩天天本| 97综合久久| 国产成人无码网站在线视频| 亚洲色图国产另类| 日夜伊人网| 免费中文在线| 国产强奸超碰AV| 91老司机在线视频免费观看| 天天伊人| 国产超碰| 久久精品无码熟妇一区二区三区视频导航 | 揉揉日日日日| 乱伦系列一区二区| 国产亚洲日本| 亚洲男人久久综合天堂| 禁止观看美女黄| 精品免费成人久久| 秋霞久久亚洲精品成人| 天天综合网91入口| 成年人黄色| 日本午夜久久电影| 91综合天天看| 欧美懂色综合网| 欧美强奸乱| 日韩精品人妻中文字有码在线| 久久久久亚洲| 婷婷五月丁香五月| 日本道人妻久久久在线不卡色视频| 国产av强奸美女| 中文字幕少妇色| 欧美在线啊啊| 97香焦色区| 亚洲熟妇A V黑人| 99操逼| 夜夜无码| 日本岛国黄色网址| 久草成人影片| 超碰九区| 五月婷色| 高颜值美女口爆高潮浪叫| 少妇高潮对白在线观看| 日韩欧无码一区二区三区免费不卡 | 亚洲最新a在线观看| 中文字幕制服欧美久久一区| 日本精品一区二区三区四区的功能| 无遮挡猛进视频免费无限观看| 日韩淫色网| 欧美操逼熟女| 中文字幕二区| 亚洲综合成人网| 欧美少妇高潮视频| 亚洲激情av| 欧亚第一综合网| 成人熟女区| 国产精品嫩草影院午夜两性 | 精品久久久久久久| 天天添天天干电影| 久久精品国产精品一区| 欧美日韩性爱视屏免费看了| 超碰吊日色| 色综合久| 日韩欧美资源| 国产精品久久久久久 百度| 亚洲综合色在线| 国产女生在线| 人妻熟女一区二区三区视频| 中文字幕啊啊啊在线观看视频| 爱逼综合| 一区二区三区四区五区高清无码永久视频 | 一本大道青青| 91美女中出| 亚洲欧美高清| 久草综合视频| 日本精品高清一二区一本到| 亚洲综合色图欧美| 天美传媒av 在线| 小说区 图片区色 综合区| 欧亚不卡| 综合欧美日韩在线观看| 啊啊啊啊好疼| 神马午夜久久久| 成人aⅴ一区二区三区| 日本精品九九九| 99操逼| 五月天久久婷婷亚洲 | 日韩不卡在线一区二区| 日1区2区3区2020| 欧美另类色图片| 成年人网站在线免费观看| 天天综合网入口~91| 欧美日韩操操操| 九九五月天| 99精品无码| 欧美性爱精品七区| 啊啊啊好疼| 午夜丁香| 熟妇xxxxx性春色| 日本黄页视频在线观看| 无码色| 日韩欧美操逼xxx| 91国精产品| 91色情黑丝搞鸡在线观看一区二区三区三州| 少妇久久久久久| 日韩美女,国产传媒,视频一区| 欧美亚洲日本激情在线| 91精品国产91综合久久蜜臀| 亚洲一区二区 麻豆传媒| 91亚洲综合在线| 亚洲综合性感在线| 婷婷伊人| 欧美人妻精品一区二区| 91操操操操| 国产精品丝袜久久亚洲不卡| 久99| 欧美极品性爱天天射| 97视频播放| 99RE在线视频精品,这里只有精品| 亚洲中文字母在线播放| 91久久青青草原精品| 美女黄码视频午夜| 色爽爽文学| 亚洲色图日韩精品| 一区二区乱码福利| 久久九九热| 艹少妇网站| 亚洲一区二区AV| 欧美大片天天看| 加勒比大香蕉视频在线| 日韩操逼HD| 天啪| 东京热伊久| 欧美色图片91| 六月天婷婷| 国产黄片精品在线| 亚洲97| 99最新日韩偷拍视频| 中日无幕一二三四区| 不卡一区二区日本视频| 亚洲综合第一页| 99综合免费视频| 少妇无码av专区线| 91搡老女人老妇女老熟女歌词翻译| 亚欧美综合网。| 日韩草久视频| 国产成人精品亚洲日本| 欧美1区二区三区公司| 天天干夜夜鈤| 亚洲色图第一页| 裸体女人草逼视频播放一区,二区,三区,四区,五区 | 亚洲97超碰| 欧洲自拍色图gif在线| 欧美亚洲激情小说| 日本在线一二| 欧美在线干| 91成人久久| 影音先锋每日最新资源在线观看 | 亚洲欧洲另类| 国产伦乱91| 夜夜天天噜狠狠爱2021| 天天谢天天干| 日韩精品色呦呦| 97干97色| 夜夜嗨视频| 91n处女在线观看| 国产热av| 亚洲 欧美 综合 91| 伊人991| 久久久久久久久久久久久久久乱码| 五月天激情网图片| 操国产高清| 亚洲丝袜少妇在线| 加勒比99999| 夜夜国自区| 日本九九久久99| 国产污视频麻豆传媒一区二区 | 亚欧无码在线| 欧洲成人性爱视频| 欧美成人性爱视频免费观看 | 国产强奸超碰AV| 一类无码操逼视频| 欧美日韩国产三级黄色| 熟女五十路一区二区三| 中日韩熟女| 91社操逼| 国产69精品久久久久99尤物| 国产三级中文有码在线视频| 日韩精品亚洲专区在线影视| 成人性爱美曰韩| 综合操逼| 熟女突然公开看18禁影片| 日韩不卡毛片Av免费高清| 中文字幕狠狠玩| 在线强奷到舒服的无码视频| 婷婷亚洲综合| 久久麻豆一区二区| 情色日播放AV| 天堂国产AV| 91电影色诱| 国产成自自拍在线观看| 午夜福利区| 大香蕉欧美日韩| 神马久久69| 91精品久久久久久77777| 亚洲综合69| 曰韩少妇无码| 少妇久久久免费| 91中文字幕在线观看| 97久久久网站| 亚洲中文字幕精品一区| 9丨亚洲一区二区在线| 国产AV色黄看到爽| 日本一区二区成人在线| 欧美色五月| 天综合网| 日韩国产十八禁| 走光一区92下载| 福利天天都操| 日本女人久久久| 欧美激情激情xxxx欧美专区| 中国少妇XXXX做受| 国产18精品亚洲精品| 九九热精品| 欧美淫穴| 亚洲 欧美 另类 日韩 人妻一区| 曰韩少妇无码| 麻豆精品.欧美精品.日韩精品.| 日韩欧无码一区二区三区免费不卡| 操逼无码操逼| 超碰成人最新最好看| 色啪网| 爆乳免费黄网站| 秋霞午夜视频一区二区| 熟女熟妇一区二区三区视频| 97视频在线看| 天天日天天搞天天干| 无码聚合| renqi久久久久久久久久久久| 久久精品亚洲东京热色播| 色天堂综合| 后入综合久久| 亚洲欧美日韩制服另类| 成人福利视频网| 国产人妻精品久久久一区二区三区 | 干B网| 欧美精品精品一区二区| 精品久久久久av影院| 噜噜噜狠狠色综合| 热久久99999| 中文字幕av片| 亚洲精品国语在线播放| 5278欧美一区二区三区| 18精品一区| 天天操天天看| 天天干人人看综合| 青娱乐久久艹| 日韩一级二级在线| 久久大香蕉97| 一级毛片久久久久久久女人18| 5月婷婷6月六月丁香| 日韩美女久久一区二区三区| 亚洲系列欧美| 国产强奸AV在线| 国产精品无码AV网站| 欧美大香蕉在线观看| 91美女视频电影| 亚洲色图综合| 欧美视频在线视频免费va| wwe 天天干.com| 伊人国产AV| 欧美黄色大片在线观看| 综合五月天| 天海翼久久| 欧美综合网| 久久欧美性爱视频| 成人女人国产| 亚洲在线a| 亚洲精品中文字幕一区在线视频| 欧美精品丝袜久久久中文字幕| av日韩中文字幕| 豆花视频操逼网址| 防屏蔽在线视频| 欧美体内射精| 欧美不卡在线一区二区| 精品人妻伦一二三区久久| 91性| 国产精品嫩草影院午夜两性| 日韩熟妇二区| 91综合天天| 中文字幕一区电影在线观看| 精品久久久久黄少妇| 91精品国产91熟女| 国产18精品亚洲精品| 日韩精品9999| 网站A V在线| 性爱久久| 校园春色 亚洲| 黄色AAAAAAAAAAA大片| 国产专区路线| 91久久青青草原精品| 久久婷婷色| 在线中文AV| 天天躁日日躁成人字幕aⅴ| 久久精品一区二区三区不卡| 91天天综合在线观看| www.99中文字幕| 亚洲棕合电彰| 俺去啦俺来也久久综合| 91啪9色| 久久久久久999| · —级AA伦aa坐爱午夜极速ⅴA一区天天噪天天噪天天噪 | 26uuu国产成人综合| 久久有码| 色在线综合| 久久久久久久九九九九九九| 综合 亚洲 欧美| 99操| 久久精品28| 久热9| 欧美综合综合| 91久久99久久91熟女精品| 亚洲涩图欧美| 免费操逼视频下载| 久久久久久裸体| 国产久久av| 亚洲日韩东京热一区| 亚洲国产婷婷在线播放| 色娱乐色呦呦夜夜夜夜av| 国产精品久久天天干| 69AV女优男人的天堂| 91操熟女| 欧美久久毛片基地| 无码 黑人一区二区三区| 91偷拍欧美亚洲| 午夜亚洲WWW湿好大| 黄色高清久久无码依人| 国产精品久久久久无码Av网曝门| 蜜桃在线观看一区二区三区| 日本色色视频网站| se,,,亚洲欧美| 丰满岳乱妇一区二区三区| 欧洲精品人妻| 中文字幕亚韩| 欧美色蜜桃97| 国产农村妇女一区二区| 欧美色网| 91精品成人| 日本东京热加勒比久久| 97天天插| 色色五月丁香| 91五十路| 国产高清不卡视频| 亚洲导航深夜福利| 激情视频图片| www.夜夜操| 99久久99久久免费精品蜜臀| 国产性感在线观看| 操操逼操操逼操操逼逼| 国产青青综合伊人| 999久久久精品国产| 伊人影院日本| 老女人老91妇女老热女| 亚洲成人久久美女| 亚洲成人贴图| 麻豆伊人网| 日本精品高清一二区一本到| 精品人妻一区二区三区不卡断 | 日韩丝袜二区| 免费在线视频97| 日韩三级久久久| 国产精品久久天天干| 98超碰日本| 99色综合| 亚洲国产精品无码AV久久久| 91 国产丝袜在线播放-百度| 囯产精品一区二区三区线|亚洲人成无码网WWW动漫|国产精品免费一级... | 亚洲综合五月天婷婷丁香| 国产深夜福利| 午夜综合在线| www.超碰| 国产91av在线播放| 97在线观看视频| 天天操女人| 欧美强奸乱能| 9丨久久九九九 | 欧美高清18A片| 亚洲精品97中文字幕| 99这里有精品| 日韩国产欧美伦理在线| 亚洲欧美碰碰| 人妻 欧美 中文| 国产精品久久久久av| 日本一道在线播放高清| 在线中文AV| 爱爱动态120秒| 中文字幕一区二区在线日韩精品| 欧美的精品的视频| 久久久性爱视频| aaa一级黄片| AV乱伦专区| 91男人综合| 综合网欧| 国产激情久久| 久久久久久电影| 久久久久久久久久久六六| 国产精品又黄又猛又粗| 四虎精品亚洲| 婷婷香网站| 又黑又大又粗| 手机看av网站在线看| 中文AV制服乱伦| 日韩欧美麻豆| 婷婷大香蕉| 秋霞成人做爱| 91久久久久久久久18| 天天日美女的B| 啊啊啊啊啊操我视频| 日韩欧美经典在线观看| 亚洲国产一区二区日韩专区| 人妻丝袜一区二区三区在线| 欧美日韩人妻少妇 一区二区三区| 久九九九九九九热| 99色在线视频| 天天视频综合在线观看视频| 1769一区| 后入国产| 亚洲日本大香蕉1| 欧美少妇性乱| 久久久不能久久久久| 色丁香久久| 亚洲va有码在线天堂| 亚洲本色精品一区二区久久| 久久久极品| 啊啊啊啊好疼视频| 日日黄色三级网站| 日韩伦理久 久久 清纯 | 很狠操| 99综合自拍| 不卡在线一区,精品一区二区三区中| 成人免费福利在线观看| 亚洲欧美日韩免费电影| 青娱乐淫乱1314| 国产熟女无套内射| 超碰97人妻自拍| daxiangjiao你懂的| 熟妇艹鸡八| 97免费在线视频在线观看| 久久丁香五月婷婷| 综合久久欧美| 午夜福利免费精品视频| 亚洲蜜乳av| 午夜乱轮操逼视频免费看| 亚洲无992tv| 天久久久噜噜噜久久国产精品爽爽| 大香蕉手机在线| 爱丝福利| 哑洲在线| 女人天堂网| 超碰综合97在线| 人妻激情在线视频| 成人天天爽| 激情五月天校园春色网| 欧美91丝袜| 99精品丰满人妻| 婷婷精品国产一区二区三区日韩| 91国产大片| 蜜乳AV一区| 在线色导航| 艹精品| 涩涩这里只有精品视频| 国产成人午夜视频网址| 中文字幕国产| 最好看的中文字幕在线2018| 老熟妇一区二区三区…| 国产操偷| 免费a在线播放v| 欧美十八禁在线看| 凹凸视频特色日本特黄| 国产精品电影| 一区二区三区在线资源| 麻豆 欧美 日韩| 岛国AV一区二区电影| 欧美久久毛片基地| 欧美日韩国产电影| 97频视在线| 激情四射婷婷六月天| 乱操9999| 久久香蕉国产线看观看亚洲女人 | 成人免费性爱视视| 亚洲精品自拍| 日韩人妻有码免费视频| 久久九九视频九九视频| 中文字幕123| 国产成人+综合亚洲+天堂| 久热久| 日韩在线视频1234| 亚洲棕合电彰| 亚洲偷拍欧美激情| 人妻激情视频| 国产精品天堂| 9国产超碰| 亚洲成人av电影在线| 干婷婷综合网| 猛交交| 欧美大香蕉97| 99久在线精品99re8| 六月激情网| 色呦呦、国产精品| 久久超碰大香蕉| 亚洲少妇激情一区二区三区| 91白虎| 中文字幕av亚洲精品| 在线无码视频| 日韩丝袜二区| 日韩国语字幕| 久久一二三四不卡| 丝袜视频网国产90| 亚洲欧洲激情卡通另类文学四射小说网站| 久久国产在线一区二区| 高清无码 国产精品| 人妻中文在线| 蜜臀久久99精品久久久久免费观| 碰超人人在线一区二区三区| 青草精品视频一日本久久久久网站| 日韩欧美日韩| 一级aaaaa欧美中文字幕录像片| 免费A V在线| 成人av影院在线观看| 国产精品久久久久久 百度| 狠狠久久亚洲欧美专区| 精品然女一区二区| 性生活性生大爱77AV国产| 国产精品美女视频诱惑| 东北丰满熟女国产一区| 五月婷婷激情综合| 欧美A√综合网| 亚洲色婷婷综合久久一区二区三区| 91久久青青草原精品| 一区操逼日比视频| 亚洲激情网一二三四区| 大香蕉综合在线| 亚洲性天堂| 在线国产福利网址导航| 一区二区三区精品黑丝白丝酒店对鸡 | 中文字幕久久精视频久久大全| 国产毛片精品一区二区色欲黄A片| 色噜噜人妻av 中文字幕| 久久6热精品99视频| 99久久99久久免费精品蜜臀| 亚洲AV操| 麻豆国产原创AV色哟哟| 色97国产69香蕉| 国产丝袜欧美在线视频| 久久婷婷视频| 91丝袜在线视频| 爱爱动态60秒| 久久一区二区三区入口| yazhousetuoumei| 大茄子熟女AV导航| 日韩亚洲中文有码视频| 人妻天天夜夜爽一区二区| 丰满人妻一区二区中文| 性色AV蜜色av色欲av| 婷婷丁香在线| 色偷综合| 2018天天日天天日| 丁香六月啪啪| 天欧美在线| 操操操五月天婷婷丁香影院| 色色五月天婷婷| 亚洲综合一| A 天堂| www.色五月| 精品久久艹| 日韩一级性爱无码| 亚洲天堂,男人| 天操天操夜操夜月操月年年操操| 十八禁视频一区二区| 久久久久久亚洲精品不卡人乳| 久操| 成年人黄色小视频网站| 欧洲乱码一区二区| 国产精品96| 思思热国产高清| 国产一级片| 超碰视97中文| 神马久久久久久伦理片| 青青草吊丝| 人妻久久一区二区三区 | 韩日色费| 欧美超碰9798| 亚洲视频精选| 99久久网站| 日本三级韩国三级美三级91| 370p日韩欧美亚洲精品| 91 国产丝袜在线放观看| 国产精品夜夜| 免费强奸av| 大香蕉青青9| 国产美女裸体秘 永久无遮挡| 天美传媒国产原创中文字幕亚洲欧美另类 | 97超碰总站| 中文字幕日韩电影人妻| 人人澡人人爽人人精品| 97精品免费视频网站| 99精品在线| 婷婷激情丁香| 91超碰人人操| 亚洲操操操无码| 蜜乳AV色欲AVAV无码| 美熟女逼导航AV操逼| 国产成人一级av88| 小明看看网址| 密乳无码| 欧美亚州色的图| 亚州操逼图| 五月婷婷色色| 东北老熟女| 婷色五月| 综合亚洲欧美| 狠狠综合网| 国产视频第二页| 超踫中文字幕| 人妻夜夜爽天天爽三区麻豆AV网站| 午夜精品久久久久久久男人的天堂| 91岛国动作片| 欧美亚州手机在线| 97天堂| 刺激性视频黄页| 三级日本一区二区三区| 久久久久久久久九九久孕交| 亚洲熟女乱色一区二区三区| 性色国产东北露脸精品视频| 高潮毛片无遮挡高清免费| www.高清无码诱惑一区.com | 免費黃色視頻觀看一| 日本熟妇自慰性高潮一区二区三区| 女人午夜视频777| 亚洲日韩资源| 99亚洲天堂| 亚洲天天操| 二区熟妇韩日| 国产一级内射无挡观看| 人妻激情在线视频| 5月婷婷6月六月丁香| 99少妇| 麻豆 欧美 日韩| 综合伊人激情| 日本高清熟女久久一区| 成人日本视频人妻在线| 91青青在线| 国产精品亚洲色婷婷久久久| 99蜜桃臀久久久欧美精品网站| 日韩一区二区高清在线观看的| 日韩欧美天堂| 五月婷婷综合网| 成人免费性爱视视| 夜精品久无码| 亚洲第一在线视频| 国产精品网址| 神马午夜久久久| 性爱乱伦网址| 大香蕉中文在线| 欧美中文字幕一区| 丁香六月东京热| 啊啊啊啊啊啊在线| 禁十八久久| 中文字幕亚韩| 999综合网| 嗯嗯啊啊视频一区二区三区| 日本一本道A级黄色毛片试看60分钟| 97se亚洲综合自| 黄色二级片网站| 久久91精品国产9丨久久分亭 | 亚洲美女自拍偷拍视频| 超碰久超碰久| 亚洲天堂自拍| 操婷婷逼| 美女天天干| 欧美第一页| 亚洲精品视频二区| 午夜激情成人在线观看| 青草伊人网| 久久e6只有精品| 亚洲超碰AV| 久久久久亚洲| 91无码人妻| 97人肏| 日本东京热加勒比久久| 中文字幕一区二区三区蜜臀| 午夜男人av| 大香蕉线| 欧美亚州综合图片| 五月天激情国产综合婷婷婷| 国产美女销魂在线观看不卡| 风间由美日韩欧美久久| 亚洲欧美色综合| 久久艹逼视频| 免费观看性欧美一级| 色97欧美| 欧美日韩国产男人| 人妻久久久| 色官网色综合| 热99这里有精品综合久久 | 国产精品午夜福利视频| 日韩乱码Av| 91成人在线免费视频| 2024黄色视频| 中文字幕无码不卡啪啪| 99re9这里只有精品| 五月丁香婷婷综合网| 久久久久亚洲三级电影| 婷婷8月天青娱乐| 97AV在线免费观看| 久久色人体| 日韩精品永久在线观看| 国产亚洲精品一区二区三区| 后入人妻无码| 国产精品熟妇一区二区三| 殴美,日韩国产伦精品| 婷婷五月天激情网| www.久久制服糖| 亚洲人天堂| 亚洲第一页综合在线| 日本99一区二区| 78综合网| www.99视频| 国产精品自在线发布| 内射老妇BBWX0C0CK| 97av在线观看| 国产乱码久久久| 伊人色综合欧美| 久久精品一区| 高凊专区人人操| 久久98| 最近2019中文字幕国语免费版| 无码直播久久久| 久久91| 97超碰9| 亚春色色| 拍拍拍拍大尺度黄色三级片拍拍拍拍拍照| 伊人操操| 91色图片| 亚州操操穴网| 99国产人成精品| 久久九色| 亚洲四虎熟女精品| 久久久久久国产手机AV| 欧美黄色片在线播放| 国产亚洲色停停久久99精品91| 欧美综合第一页| 黑人性欧美| 91成人亚洲色图| 热热色国产一二区AV| 欧洲精品久久| 乱欲性色| 欧美韩国你懂得在线 | 91激情国产| 亚洲精品一卡二卡三卡福利视频网站 | 久久午夜神马| 久久久性少妇| 精品二区三四区五电影| 日韩中文字幕宗合在线| 色欲色香天天天综合网www-亚洲综合国| 亚洲情色1区| 区一在线观看| 欧美色图片91| 亚洲欧美天堂在线| 亚熟在线| 亚洲av热热色| 天天干天天日天天射黄色片| 激情亚洲天堂| 磁力99AV| 先锋音影AV| 欧美亚洲厕所精品偷拍91| 国产精品第二页| 久噜噜| 亚洲熟女偷拍在线观看| 亚洲人码13| 亚洲少妇免费视频\| 久久成人网站| 亚洲资源吧| 国产伊人精品在线| 一二三区精品视频| 亚洲熟女av中文字幕| 极品白嫩美少妇在地板上位骑射淫水泛滥| 久久久久久久久久9| 九九九九精品| A级国产欧美激情在线| 一区二区免费电影久久| 日日噜噜夜夜狠狠视频无| 日韩无码a片| 日韩一性一交一A片俄罗斯| 日本性爱少妇| av国产无码| 色色色综合| 少妇免费视频| 色婷婷综合久久中文字幕雪峰| 亚洲天堂第一页| 亚洲性少妇| 夜夜嗨一区二区三区三州加勒比| 97久久超碰亚洲| 国产精品乱码久久久久| 香蕉欧美| 国产在线激情视频| 中文字幕成人理论在线| 五月天色色网站| 亚 欧 美 综合| 欧美狠狠操| 久久久久久久六六| 色天天野狼综合社区| 无码人妻一区二区三区四区老鸭窝| 91网站18| 国产又黄又粗的视频| 亚洲五码一区二区三区| 婷婷色中文字幕| AV天天综合| 77国产精品| 大香蕉手机在线| 欧美激情专区| Av手机版天堂网| 色综合久| 99丝袜福利在线播放| 人人妻人人色| chaopen97久久| 七月婷婷综合| 91精品国产日韩欧美综合| 中文字幕一区二区三区视频播放| 久久极品一区二区| 精品久久久久久久久久久久| 日本 情色 1区2区3区| 亚洲国产精品无石码久久| 亚洲电影中字一区二区| 色yeye成人免费视频| 五月开心网| 人人超碰在线观看黄| 少妇精品久久久八区九区| 98久久| 激情无码日韩| 五月天激情小说| 情色五月天久久久| 久超碰这里只有精品| 国产h小视频在线观看免费| 亚洲中文一区二区三区| 五月天激情小说| www.高清无码诱惑一区.com | 97干在线视频| av午夜玫瑰| 国产青视频| 日本不卡一区二区| 午夜一区二区三区国产| 激情五月综合网| 91欧美美女日韩国产婷婷| 人人澡人人澡人人| 免费AV中文网在线观看| 久久国内| 中文字幕日产av人| 台湾佬中文娱乐网久久久久久久久久com| 91一区二匹| 日韩一区二区高清在线观看的| 玖玖视频在线资源一区二区三区| 四季AV一区二区凹凸精品小说| 欧美日韩欧美| 天天看天天在线精品| 国产欧美日韩在线观看麻豆传媒公司| 欧美亚洲se91| 资源新线在线天堂| 婷婷丁香五月天亚洲天堂网| 国产suv精品一区二区四| 97天天插| 国产一区二区在线播放量| 久久xx| jiujiujiujingpin| 97在线免费视频| 性爱av在线免费观看| 欧美精品双插| 啊啊啊久久| 成年人性爱日韩| 久久亚洲不卡一区二区三区| 91丰满| 日本国产高清色www视频在线| 这里是精品| 午夜啊啊| 91快色色色色色| 东北女人被操| 精品78| 亚洲另类电影| 在线观看午夜婷婷久久久久清性观看| 男人的天堂 在线一区| 国产精品久久久久久久久AV大片 | 91精品伊人久久久大香线蕉91| 麻豆九九九| 啊啊啊爽爽| 激情专区综合| 1024午夜激情男人的天堂| 草草电影院| 人妻嗯啊啊在线播放| 久久9精品网站| 精品国产一区二区三区香蕉欧美| 日韩精品.久久精品.AV女优.天美传媒 | 亚洲综合 欧美| 岛国激情视频在线观看| 国产原创精品| 91黄站| 精品国产片亚洲一区| 色呦色呦色精品| 91香蕉视频在线观看免费| 伊人青青一区成人视频在线观看区| 国产区在线| 日韩不卡av一二三| 人妻激情在线视频| 国产成人午夜视频网址| 欧美欲色| 日韩性爱啪啪视频| 情色AV电影| 中国少妇啪啪视频| 丁香九月激情啪| 超碰综合97在线| 国产操逼逼网| 色娱乐色呦呦夜夜夜夜av| 亚洲黄色视频在线观看视频| AV天天综合| 99re9| 淫淫总合网| 色婷婷一区二区三区久久| a网站免费观看| 久夜操| 五月丁香综合激情| 精品少妇一区二区| 欧美刺激色黄片免费看| 日本精品一区二区三| 口爆吞精在线观看| 欧美亚州色的图| 青青伊人这里只有精品| 久热久| 精品一区二区综合熟妇| 天天日美女的B| 91小视频| 另类老少妇| 亚洲AV成人无码一区二区三区在线观看| 嗯嗯啊啊的视频| 性爱乱伦网址| 超碰社区97| 91一起操| 友优传媒精品在线一区二区| 激情五月天色播| 亚洲图片第一页| 91原创在线观看| 永久免费观看的毛片的网站| 青青青操| 狠狠操,使劲操| 久久99手机免费视频| 9丨亚洲一区二区在线| 白丝少妇一区二区| 2019久久久久久久久福利| 操91| 国产热RE99久久6国产精品首| 最新国产亚洲精品精品国产亚洲综合| 91九色丨国产丨爆乳| 天天影视色香欲综合网小说| 国产久久日韩网站导航| 26uuu欧美日韩| 亚洲综合一| 爱做久久久久久| 色香天天|