%%?此程序功能：根據(jù)簡(jiǎn)化動(dòng)力學(xué)模型（考慮小角度假設(shè)下）計(jì)算雅克比矩陣
%?版本：V1.0，編寫(xiě)時(shí)間2013.12.11
%?計(jì)算出的雅克比矩陣是與車輛固有參數(shù)緊密相關(guān)的，參數(shù)變化了，雅克比矩陣也就相應(yīng)變化了
%?滑移率只是一個(gè)估計(jì)值，后續(xù)如有必要可以做出調(diào)整
clc
clear?all;
%%?以下為程序
%車輛參數(shù)定義?
syms?x_dot?y_dot?phi?phi_dot?Y?X;%車輛狀態(tài)量
syms?delta_f??%前輪偏角控制量
%syms?sf?sr;%分別為前后車輪的滑移率需要提供
Sf=0.2;?Sr=0.2;
%syms?a?b;%前后車輪距離車輛質(zhì)心的距離，車輛固有參數(shù)
a=1.232;b=1.468;
%syms?C_cf?C_cr?C_lf?C_lr;%分別為前后車輪的縱橫向側(cè)偏剛度，車輛固有參數(shù)
Ccf=66900;Ccr=62700;Clf=66900;Clr=62700;
%syms?m?g?I;%m為車輛質(zhì)量，g為重力加速度，I為車輛繞Z軸的轉(zhuǎn)動(dòng)慣量，車輛固有參數(shù)
m=1723;g=9.8;I=4175;

%?車輛動(dòng)力學(xué)模型
dy_dot=-x_dot*phi_dot+2*（Ccf*（delta_f-（y_dot+a*phi_dot）/x_dot）+Ccr*（b*phi_dot-y_dot）/x_dot）/m;
dx_dot=y_dot*phi_dot+2*（Clf*Sf+Clr*Sr+Ccf*delta_f*（delta_f-（y_dot+phi_dot*a）/x_dot））/m;
%dphi_dot=dphi_dot;
dphi_dot=（2*a*Ccf*（delta_f-（y_dot+a*phi_dot）/x_dot）-2*b*Ccr*（b*phi_dot-y_dot）/x_dot）/I;
Y_dot=x_dot*sin（phi）+y_dot*cos（phi）;
X_dot=x_dot*cos（phi）-y_dot*sin（phi）;

%?雅克比矩陣求解
f=[dy_dot;dx_dot;phi_dot;dphi_dot;Y_dot;X_dot];%動(dòng)力學(xué)模型
kesi=[y_dotx_dotphiphi_dotYX];%系統(tǒng)狀態(tài)量
v=delta_f;
R=jacobian（fkesi）;%矩陣A（t）-連續(xù)
R2=jacobian（fv）;%矩陣B（t）-連續(xù)

%?控制矩陣求解（將連續(xù)矩陣轉(zhuǎn)換為離散矩陣，采用近似算法??A=I+T*A（t）B=T*B（t））
I=eye（6）;
syms?T;
A=I+T*R;
B=T*R2;
A1=vpa（A3）;
B1=vpa（B3）;

?屬性????????????大小?????日期????時(shí)間???名稱
-----------?---------??----------?-----??----
?????目錄???????????0??2016-12-30?09:19??實(shí)例\
?????目錄???????????0??2014-05-30?13:51??實(shí)例\chapter2-2-2（魔術(shù)輪胎模型）\
?????文件????????3916??2014-05-30?10:20??實(shí)例\chapter2-2-2（魔術(shù)輪胎模型）\Chapter2_3_Pacejka89_Tyremodel.m
?????目錄???????????0??2014-05-30?13:51??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\
?????目錄???????????0??2014-05-30?13:51??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\Chapter_3_5_3\
?????文件?????????631??2014-05-30?10:20??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\Chapter_3_5_3\Func_Alpha_Pos.m
?????文件?????????873??2014-05-30?10:20??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\Chapter_3_5_3\Func_CircularReferenceTrajGenerate.m
?????文件?????????128??2014-05-30?10:20??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\Chapter_3_5_3\Func_Theta_Pos.m
?????文件?????????838??2014-05-30?10:20??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\Chapter_3_5_3\Func_VehicleKineticModule_Euler.m
?????文件????????3502??2014-05-30?10:20??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\Chapter_3_5_3\LQR_1.m
?????文件????????4528??2014-05-30?10:20??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\chapter3_3_3.m
?????文件????????1767??2014-05-30?10:20??實(shí)例\chapter3-3-3（無(wú)人駕駛車輛的直線跟蹤仿真）\chapter3_4_3.m
?????目錄???????????0??2014-05-30?13:51??實(shí)例\chapter4-4-4（基于運(yùn)動(dòng)學(xué)的軌跡跟蹤）\
?????文件????????6639??2014-05-30?10:20??實(shí)例\chapter4-4-4（基于運(yùn)動(dòng)學(xué)的軌跡跟蹤）\chapter4_4_3.m
?????目錄???????????0??2014-05-30?13:51??實(shí)例\5章\
?????文件????????1462??2014-05-30?10:20??實(shí)例\5章\chapter5_1_1.m
?????文件???????12503??2014-05-30?10:20??實(shí)例\5章\chapter5_2_2.m
?????目錄???????????0??2014-05-30?13:51??實(shí)例\第6章\
?????文件???????32121??2014-05-30?10:20??實(shí)例\第6章\MPCContrandTrajPlan.mdl
?????文件???????14496??2014-05-30?10:20??實(shí)例\第6章\MPC_Controller.m
?????文件????????9536??2014-05-30?10:20??實(shí)例\第6章\MPC_TrajPlanner.m
?????文件??????????68??2014-05-30?10:20??實(shí)例\第6章\說(shuō)明.txt