无人驾驶车辆的侧倾机理与侧倾稳定性判据

稳定性分析：横摆稳定性、侧倾稳定性
动力学模型——寻找合理的控制边界，制定有效的操纵稳定性判据——在线评价；
稳定性判据体现在性能评价函数——评价函数最优化——准确反映无人驾驶在横摆和侧滑等非线性动力学约束的操作稳定性。
模型预测——跟踪控制——高速稳定性，面临挑战——模型复杂度和计算实时性
高精度模型——有效性、需要更长的预测时域
动力学模型的离散化方法
零阶保持和一阶保持
横摆角速度、横向速度，轮胎最大侧偏力，推导基于包络线的横摆稳定性判据
轮胎侧偏角的取值范围
道路环境约束、执行机构饱和约束

参数	数值
预测时域长度Np	20
Ns	10
短步长离散化步长	0.05
长步长离散化步长	0.2
轮胎侧偏角限制	0.1
车辆侧偏角	0.4
车辆侧偏角变化率	0.08

参数	数值
航向偏差极值	0.15
横向侧偏极值	2
航向变化率极值	1
横向侧偏权重	500
航向权重	500
松弛因子权重	50
增重权重	5

车辆侧倾的机理分析
车辆侧倾：绊倒侧倾、曲线运动引起的侧倾
侧倾稳定性判据
难点：在于侧倾开始发生的时刻难以预测；
时变道路曲率和侧向坡度角时的车辆侧倾趋势预测。
侧倾机理——零力矩点的侧倾稳定性判据 （ZMP）
跟踪期望：横向偏差、航向偏差

function [sys,x0,str,ts] =chapter8_3_3(t,x,u,flag)
%***************************************************************%
% This is a Simulink/Carsim joint simulation solution for safe driving
% envelope control of high speed autonomous vehicle
% Linearized spatial bicycle vehicle dynamic model is applied.
% No successive linearizarion. No two time scale of prediction horizon
% Constant high speed, curve path tracking 
% state vector =[beta,yawrate,e_phi,s,e_y]
% control input = [steer_SW]
% many other parameters are also outputed for comparision.

% Input:
% t是采样时间, x是状态变量, u是输入(是做成simulink模块的输入,即CarSim的输出),
% flag是仿真过程中的状态标志(以它来判断当前是初始化还是运行等)

% Output:
% sys输出根据flag的不同而不同(下面将结合flag来讲sys的含义), 
% x0是状态变量的初始值, 
% str是保留参数,设置为空
% ts是一个1×2的向量, ts(1)是采样周期, ts(2)是偏移量
%---------------------------------------------------------------%
% Published by: Kai Liu
% Email:leoking1025@gmail.com
% My homepage: https://sites.google.com/site/kailiumiracle/  
%***************************************************************% 
    switch flag,
        case 0 % Initialization %
            [sys,x0,str,ts] = mdlInitializeSizes; % Initialization
        case 2 % Update %
            sys = mdlUpdates(t,x,u); % Update discrete states
        case 3 % Outputs %
            sys = mdlOutputs(t,x,u); % Calculate outputs
        case {1,4,9} % Unused flags
            sys = [];            
        otherwise % Unexpected flags %
            error(['unhandled flag = ',num2str(flag)]); % Error handling
    end %  end of switch    
%  End sfuntmpl

function [sys,x0,str,ts] = mdlInitializeSizes
%==============================================================
% Initialization, flag = 0，mdlInitializeSizes
% Return the sizes, initial conditions, and sample times for the S-function.
%==============================================================
sizes = simsizes;%用于设置模块参数的结构体用simsizes来生成
sizes.NumContStates  = 0;  %模块连续状态变量的个数
sizes.NumDiscStates  = 6;  %模块离散状态变量的个数,实际上没有用到这个数值，只是用这个来表示离散模块
sizes.NumOutputs     = 15;  %S函数的输出，包括控制量和其它监测量
sizes.NumInputs      = 38; %S函数模块输入变量的个数，即CarSim的输出量
sizes.DirFeedthrough = 1;  %模块是否存在直接贯通(direct feedthrough). 1 means there is direct feedthrough.
% 直接馈通表示系统的输出或可变采样时间是否受到输入的控制。
% a.  输出函数（mdlOutputs或flag==3）是输入u的函数。即，如果输入u在mdlOutputs中被访问，则存在直接馈通。
% b.  对于一个变步长S-Function的“下一个采样时间”函数（mdlGetTimeOfNextVarHit或flag==4）中可以访问输入u。
% 正确设置直接馈通标志是十分重要的，因为它影响模型中块的执行顺序，并可用检测代数环。
sizes.NumSampleTimes = 1;  %模块的采样次数，>=1

sys = simsizes(sizes);    %设置完后赋给sys输出

x0 = zeros(sizes.NumDiscStates,1);%initial the  state vector， of no use

str = [];             % 保留参数，Set str to an empty matrix.

ts  = [0.05 0];       % ts=[perio