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

%?車輛動力學模型
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];%動力學模型
kesi=[y_dotx_dotphiphi_dotYX];%系統狀態量
v=delta_f;
R=jacobian（fkesi）;%矩陣A（t）-連續
R2=jacobian（fv）;%矩陣B（t）-連續

%?控制矩陣求解（將連續矩陣轉換為離散矩陣，采用近似算法??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）;

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