本发明属于集装箱测量技术领域，具体涉及一种基于尺寸测量装置的货车状态惯性参数联合估计方法。
背景技术：
近年来，主动安全系统得到了迅速发展，这些系统包括电子稳定系统和车道保持系统、侧倾稳定控制系统等系统极大地改善了货车的操纵舒适性和紧急工况下的平顺性使驾驶员更舒适、安全地行驶。然而货车行驶过程中的惯性参数是主动安全汽车系统闭环中很重要的一环，然而不同的驾驶条件下，比如集装箱货车载货的多少将会导致惯性参数变化。因此要实现主动安全系统有效可靠的控制，就需要实时准确获取货车行驶过程中的货车状态参数信息和惯性参数信息。
目前在对于车辆惯性参数的研究中,大部分研究都将货物或者行人简化为质点，不考虑本身的转动惯量，而对于集装箱货车装载的货物往往是体积大，质量大基于实际考虑是不能简化为质点的，因此对于集装箱货物转动惯量量化也是工程应用中的难题。
目前的集装箱货车状态估计研究中，大部分研究主要集中在对货车空载在运行过程中的车辆状态进行估计，而对于货车装载货物在运行过程中的货车状态和货物加载致使集装箱货车的惯性参数改变的研究还很少。由于货物的加载会使得集装箱货车的质量分布、质心位置、质心高度和因质量、质心位置改变致使转动惯量也产生改变，将直接影响集装箱货车底盘系统的操作特性、稳定性和控制性能。因此实时获取对集装箱载货行驶时货车状态和惯性参数的信息就显得极其重要。
本发明的目的提供了一种集装箱货车载货运行时货车状态与货车惯性参数估计的方法和设计了获取货物整体尺寸的装置。首先建立了集装箱货车载货行驶时的货车非线性动力学模型，由于集装箱货物的尺寸和质量较大其货物本身的转动惯量不可忽略，因此设计一种近似获取货物的整体尺寸的装置来获取货物的转动惯量，在此基础上通过采用双自适应容积卡尔曼滤波算法同时对集装箱货车的纵向速度、质心侧偏角等状态和集装箱货车整车质量、质心高度、质心距前轴的距离和横摆转动惯量和侧倾转动惯量进行估计。基于实际情况考虑对货车行驶过程中惯性参数不可能同时估计出，因此设计一种开关根据实际情况分步估计出最优惯性参数，具有实时性强、精度高等优点。
技术实现要素：
本发明的技术方案为一种基于尺寸测量装置的货车状态惯性参数联合估计方法。
本发明所述尺寸测量装置包括：
can总线采集装置、微控制器、加速度计传感器、陀螺仪传感器、第一激光测距传感器至第八激光测距传感器；
所述微控制器于所述can总线采集装置通过有线方式连接；
can总线采集装置分别与所述的加速度计传感器、陀螺仪传感器、第一激光测距传感器至第八激光测距传感器通过有线方式依次连接；
所述加速度计传感器、陀螺仪传感器加装在集装箱货车上；所述的第一激光测距传感器至第四激光传感器分别在集装箱上侧内部以固定距离依次排列布置；所述的第五激光测距传感器和第七激光测距传感器加装在集装箱左货车门处，所述的第六激光测距传感器、第八激光测距传感器加装在集装箱右货车门处；所述的第五激光测距传感器至第八激光测距传感器分别以固定距离向上排列布置；
所述加速度计传感器、陀螺仪传感器用于采集货车纵向加速度、横向加速度、横摆角速度和侧倾角速度并通过所述can总线采集装置传输至所述微处理器；
所述的can总线采集装置通过can总线获取纵向加速度、横向加速度、横摆角速度和侧倾角速度；
所述的第一激光测距传感器至第四激光传感器分别用来采集集装箱内部上侧至货物之间的距离信息，所述的第五激光测距传感器至第八激光测距传感器分别用于采集集装内部后侧至货物之间的距离信息。
所述货车状态惯性参数联合估计方法，其特征在于，包括以下步骤：
步骤1：微处理器通过所述的第一激光测距传感器至第四激光测距传感器采集集装箱内上侧至货物之间的距离、通过所述的第五激光测距传感器至第八激光测距传感器采集集装内后侧至货物之间的距离，进一步来计算集装箱货车中货物的总体尺寸，通过自适应双容积卡尔曼滤波算法估计货车装载货物时的总质量减去货车空载时的质量进而计算货物的质量，计算货车中货物的转动惯量，结合货车空载时的货车的质心位置和转动惯量，通过平行轴定理来获取货车装载货物之后的新的质心位置和货车装载货物之后绕新质心位置的转动惯量。
步骤1通过所述进一步来计算集装箱货车中货物的总体尺寸，具体为：
而激光测距传感器的主要原理则是通过激光脉冲测距，激光脉冲测距就是将对距离的测量转变为对时间差的测量，其计算公式为：
hp,i＝c*ti/(2n)
i∈[1,4]
lp,j＝c*tj/(2n)
j∈[5,8]
上式中，hp,i为第i激光测距传感器采集的集装箱内上侧到货物之间的距离，lp,j为第j激光测距传感器采集的集装箱后侧到货物之间的距离，ti为第i激光测距传感器的测量点与货物之间所用的时间，c为激光在空气中传播的速度，n为测量时大气折射率，tj为第j激光测距传感器的测量点与货物之间所用的时间。
上式中hp,i为第i激光测距传感器采集的集装箱内上侧到货物之间的距离,其中i∈[1,4]；lp,j为第j激光测距传感器采集的集装箱后侧到货物之间的距离，其中j∈[5,8]；为了能够准确的获取集装箱内部上侧到货物之间的距离，引入权重a1,a2,a3，其中a1为集装箱内上侧第二个激光测距传感器的权重，a2为集装箱内上侧第三个激光测距传感器的权重，a3为集装箱内上侧第四个激光测距传感器的权重，其公式入下式所示：
其中权重的最小取值范围为amin，最大取值范围为amax，如果amin≤αi≤amax，其中i∈[1,3]，则ai则取值为1，如果ai，i∈[1,3]的取值范围小于最小取值范围amin或大于最大取值范围amax，则将ai,i∈[1,3]的值设置为零；此时只信任第一个激光测距传感器所测量出来的数值。
为了能够准确的获取集装箱内部后侧到货物之间的距离，引入权重ηi,i∈[1,3]，其中η1为集装箱内后侧第二个激光测距传感器的权重，η2为集装箱内后侧第三个激光测距传感器的权重，η3为集装箱内后侧第四个激光测距传感器的权重，其公式入下式所示：
其中权重的最小取值范围为ηmin，最大取值范围为ηmax，如果ηmin≤ηi≤ηmax，其中i∈[1,3]，则ηi则取值为1，如果ηi,i∈[1,3]的取值范围小于最小取值范围ηmin或大于最大取值范围ηmax，则将ηi,i∈[1,3]的值设置为零；此时只信任第一个激光测距传感器所测量出来的数值。
步骤1所述集装箱货车中货物的总体尺寸为：
上式中，lpn为集装箱内部的长。wpn为集装箱内部的宽，hpn为集装箱内部的高；lpl为集装箱内部后侧距货物的距离长度；hph为集装箱内部上侧距货物的距离高度；wp为集装箱内货物的宽度；其中lp为集装箱内货物的长；wp为集装箱内货物的宽；hp为集装箱内货物的高；
步骤1所述计算货车中货物的转动惯量为：
上式中，mp货物的质量；
步骤1所述通过平行轴定理来获取货车装载货物之后的新的质心位置和货车装载货物之后绕新质心位置的转动惯量量为：
集装箱货车空载时集装箱货车质心的原始坐标为(x,y,z)和转动惯量为(ix,emp,iy,emp,iz,emp)，以及质心到前轴和后轴的距离分别为和lf,emp和lr,emp,质心到左前轮和右前轮的距离为bf,emp和br,emp。当货车装载货物时，由于集装箱货车装载货物的摆放的规则，即质心沿y方向不变，质心坐标只在x方向和z方向变化。货物质量mp被加载，加载后的集装箱货车质量为mn＝me mp，由于集装箱货车货物加载后只在x和z方向变化，因此选择xz平面为工作平面，并计算位于原坐标系下的新的质心位置坐标(xn,yn,zn)：
mnrc＝me0 mpra
ra＝(mn/mp)/rc
上式中，mn集装箱货车加货物的质量；me集装箱货车的质量；mp货物的质量；rc集装箱货车装载货物的新坐标系距货车空载质心的原坐标系之间的距离；ra货物据原坐标系之间的距离；
由于货物装载货物时由于货物本身质量大和体积大，其货物本身的转动惯量不可忽略，根据转动惯量的平行轴定理,可以得出货物相对于原质心处的新的转动惯量为：
在此通过平行轴定理可以推导出加载后质心处的转动惯量于新坐标系下转动惯量之间的关系：
进一步推导得到：
将ra＝(mn/mp)/rc代入上式中，则可将新坐标下的转动惯量可以表示为：
上式中，
则货车满载货物的新质心位置的坐标距离原质心位置坐标的距离
上式中，rc集装箱货车装载货物的新坐标系距货车空载质心的原坐标系之间的距离；lf,emp为集装箱货车原质心到前轴的距离；lf为集装箱货车装载货物之后质心距前轴的距离；lh,emp为集装箱货车原质心高度；lh为集装箱货车装载货物之后的质心高度位置；
则新坐标系下的绕x轴的转动惯量
同理可以通过上述的方法来获得在新坐标系下的绕z轴的转动惯量
其中，新质心坐标系距前轴、后轴、左右轮和地面的距离(lf，lr，bh，lh)入下式所示：
lf＝lf,emp-xn；
lr＝lr,emp xn；
lh＝lh,emp yn；
bh＝bf,emp＝br,emp
上面式子中：ix,emp集装箱货车绕货车原质心坐标x轴的转动惯量；me集装箱货车的质量；lh,emp为集装箱货车原质心高度；lf,emp为集装箱货车原质心到前轴的距离；mp货物的质量；
步骤2：构建集装箱货车装载货物时四自由度非线性动力学模型，以货车空载时的四自由度非线性动力学模型为基础，将步骤1中得到的新质心位置和装载货物之后绕新质心位置的转动惯量代入货车空载时的四自由度非线性动力学模型中，进而构建货车装载货物之后的惯性参数变化的四自由度非线性动力学模型。
在所示的步骤2中，将步骤1中得到的新质心位置(lr，lf，bh，lh)和货车装载货物后绕新质心的转动惯量
纵向运动：
横向运动：
横摆运动：
侧倾运动：
式中：mn为集装箱货车的总质量，vx为集装箱货车质心的纵向速度，vy为集装箱货车质心的横向速度，rz为集装箱货车质心的横摆角速度，β为集装箱货车质心的侧偏角，
步骤3：定义系统状态变量、系统观测变量、系统输入变量、系统惯性参数变量，将考虑集装箱货车装载货物时惯性参数变化的货车四自由度非线性动力学模型通过前向欧拉法进行离散化转化为货车四自由度非线性状态空间模型，通过can总线获得的纵向加速度、横向加速度、横摆角速度、侧倾角速度和输入的前轮转角，进行判断是直行或转弯进而来选择惯性参数模型开始执行设计双自适应容积卡尔曼滤波的状态估计和惯性估计系统。
步骤3所述系统状态变量为：
步骤3所述系统观测变量为：
步骤3所述系统输入变量为：
u＝[δf,ωi,j]
步骤3所述系统惯性参数变量为：
其中，vx为为集装箱货车质心的纵向速度，vy为集装箱货车质心的横向速度，β为集装箱货车质心的侧偏角，rz为集装箱货车质心的横摆角速度，
将步骤二所建的动力学方程写成状态方程和观测方程的形式，再通过前向欧拉法对系统的状态方程和观测方程进行离散化可以写成如下形式：
fn＝(fyfl(t)sinδfl-fxfl(t)cosδfl)bl (fxfr(t)cosδfr-fyfr(t)sinδfr)br (fxrr(t)br-fxrl(t))bl-(fyrr(t) fyrl(t))lr
只有当满足纵向加速度ax、横向加速度ay、横摆角速度rz和前轮转角δf满足相关要求时状态估计器和惯性参数估计器才会起作用对特性情况下的具体惯性参数和货车状态进行联合估计。
而惯性参数mn的状态方程和观测方程为：
而惯性参数lf的状态方程和观测方程为：
而惯性参数lh的状态方程和观测方程为：
fn(t)＝(fyfl(t)sinδfl-fxfl(t)cosδfl)bl (fxfr(t)cosδfr-fyfr(t)sinδfr)br (fxrr(t)br-fxrl(t))bl-(fyrr(t) fyrl(t))lr
步骤4：通过双自适应容积卡尔曼滤波算法分别对货车纵向速度、横向速度、横摆角速度、质心侧偏角、侧倾角速度、货车整体质量、侧倾转动惯量、横摆转动惯量、质心高度和质心距前轴的距离进行联合估计。
在所述的步骤4中所述的双自适应容积卡尔曼滤波器的具体包括以下步骤：
步骤4.1：初始化；
输入系统中的固定参数信息包括集装箱货车空载质量me、质心的原始坐标为(x,y,z)和转动惯量为(ix,emp,iy,emp,iz,emp)，以及质心到前轴和后轴的距离分别为和lf,emp和lr,emp,质心到左前轮和右前轮的距离为bf,emp和br,emp。并设置估计状态初值
步骤4.2：参数变量时间更新；
参数变量进行更新时首先进行对纵向加速度ax、横向加速度ay、横摆角速度rz和前轮转角δf＝0.52deg进行判断：其中纵向加速度界限判定值为axa＝0.7m/s^2，横向加速度界限判定值为aya＝0.25m/s^2，横摆角速度的界限判定值为rza＝1deg/s，前轮转角的界限判定值为δfa＝0.52deg。如果ax>axa、ay<aya、rz<rza和δf<δfa，如果满足上面要求，则对纵向动力学模块进行货车总质量mn:进行自适应容积卡尔曼滤波参数变量时间更新；
参数质量mn状态误差协方差矩阵
计算容积点
计算参数变量的预测值
计算参数变量的误差协方差矩阵的预测值
步骤4.3：状态变量的时间更新
状态变量误差协方差矩阵
计算容积点
计算状态变量的预测值
计算状态变量的误差协方差矩阵的预测值
步骤4.4：参数变量量测更新；
对更新后的参数误差协方差矩阵
计算容积点
更新参数变量量测方程的预测值
更新参数变量量测误差协方差矩阵的预测值
计算互协方差矩阵
计算自适应容积卡尔曼滤波增益矩阵km
计算参数变量的估计值
计算参数变量误差协方差矩阵估计值
定义新息
新息理论协方差矩阵为pvm：
对新息协方差矩阵进行近似估计
则量测误差协方差wm(t)
步骤4.5：状态变量的量测更新；
对更新后的状态误差协方差矩阵
计算容积点
更新状态变量量测方程的预测值
更新状态变量量测误差协方差矩阵的预测值
计算互协方差矩阵
计算自适应容积卡尔曼滤波增益矩阵ks
计算状态变量的估计值
计算状态变量误差协方差矩阵估计值
定义新息
新息理论协方差矩阵为pvc：
对新息协方差矩阵进行近似估计pvc；
则量测误差协方差ws(t)；
步骤4.6：通过自适应容积卡尔曼滤波参数进行时间更新和测量跟新之后对转动惯量估计值计算并计算完之后对车辆状态方程进行更新和通过自适应容积卡尔曼滤波对车辆状态进行时间更新和测量更新之后，得出状态变量估计值，并对参数状态方程里面的初值进行更新；
步骤4.7：对横向加速度即ay、横摆角速度即rz和前轮转角即δf进行判断：横向加速度界限判定值为aya＝0.25m/s^2，横摆角速度的界限判定值为rza＝1deg/s，前轮转角的界限判定值为δfa＝0.52deg，如果ay>aya、rz>rza和δf>δfa，则对新质心距前轴的位置和新质心的高度进行自适应容积滤波参数变量时间更新,其步骤如步骤4.1和步骤4.6。
与现有技术相比，本技术有如下显而易见的突出的实质性特点和显著技术进步
本发明在对集装箱货车进行非线性动力学建模过程中，考虑了集装箱货车载货行驶中，由于货物的加载致使车辆的质量分布、质心位置、质心高度、横摆转动惯量和侧倾转动惯量的变化，在此基础上构建了集装箱货车动力学模型；
本发明根据集装箱货车中的货物质量大且体积大，因此不可再将其作为质点简化，因考虑货物本身产生的转动惯量，设计了集装箱内货物整体尺寸识别装置，进而货物的整体尺寸，进而来计算货物的转动惯量。
本发明在对集装箱货车状态和惯性参数进行联合估计过程中，采用双自适应容积卡尔曼滤波算法同时对货车状态和惯性参数进行估计，在同时估计的同时根据测量所获的信息来判断集装箱货车是直行还是转弯。进而来选择特定工况下的具体惯性参数与车辆状态进行同时估计，最终获取最优惯性参数估计值和状态估计值。
附图说明
图1：是本发明集装箱货车载货时货车惯性参数和货车状态估计方法的整体设计框图
图2：是本发明集装箱内货物总体尺寸的测量装置示意图
图3：是本发明双自适应容积卡尔曼滤波算法框架流程图
具体实施方式
为了便于本领域普通技术人员理解和实施本发明，下面结合附图及实施例对本发明作进一步的详细描述，应当理解，此处所描述的实施示例仅用于说明和解释本发明，并不用于限定本发明。
本发明通过trucksim与matlab/simulink联合仿真将trucksim中货车的仿真模型数据传给simulink，再通过matlab/simulink对设计的双自适应容积卡尔曼滤波算法实现编写，最终实现集装箱货车行驶过程中状态和惯性参数估计。
下面结合图1至图3介绍本发明的具体实施方式为一种基于尺寸测量装置的货车状态惯性参数联合估计方法。
本发明所述尺寸测量装置包括：
can总线采集装置、微控制器、加速度计传感器、陀螺仪传感器、第一激光测距传感器至第八激光测距传感器；
所述微控制器与所述can总线采集装置通过有线方式连接；
所述can总线采集装置分别与所述的加速度计传感器、陀螺仪传感器、第一激光测距传感器至第八激光测距传感器通过有线方式依次连接。
所述can总线采集装置选型为usbcan-ii；
所述微控制器选型为fs32k144hamll；
所述加速度计传感器选型为mxp7205vf；
所述陀螺仪传感器选型为xv9300lp；
所述的第一激光测距传感器至第八激光测距传感器选型为pls-tp11；
所述加速度计传感器、陀螺仪传感器加装在集装箱货车上；所述的第一激光测距传感器至第四激光传感器分别在集装箱上侧内部以固定距离依次排列布置；所述的第五激光测距传感器和第七激光测距传感器加装在集装箱左货车门处，所述的第六激光测距传感器、第八激光测距传感器加装在集装箱右货车门处；所述的第五激光测距传感器至第八激光测距传感器分别以固定距离向上排列布置；
所述加速度计传感器、陀螺仪传感器用于采集货车纵向加速度、横向加速度、横摆角速度和侧倾角速度并通过所述can总线采集装置传输至所述微处理器；
所述的can总线采集装置通过can总线获取纵向加速度、横向加速度、横摆角速度和侧倾角速度；
所述的第一激光测距传感器至第四激光传感器分别用来采集集装箱内部上侧至货物之间的距离信息，所述的第五激光测距传感器至第八激光测距传感器分别用于采集集装内部后侧至货物之间的距离信息。
所述货车状态惯性参数联合估计方法，其特征在于，包括以下步骤：
步骤1：微处理器通过所述的第一激光测距传感器至第四激光测距传感器采集集装箱内上侧至货物之间的距离、通过所述的第五激光测距传感器至第八激光测距传感器采集集装内后侧至货物之间的距离，进一步来计算集装箱货车中货物的总体尺寸，通过自适应双容积卡尔曼滤波算法估计货车装载货物时的总质量减去货车空载时的质量进而计算货物的质量，计算货车中货物的转动惯量，结合货车空载时的货车的质心位置和转动惯量，通过平行轴定理来获取货车装载货物之后的新的质心位置和货车装载货物之后绕新质心位置的转动惯量。
步骤1通过所述进一步来计算集装箱货车中货物的总体尺寸，具体为：
而激光测距传感器的主要原理则是通过激光脉冲测距，激光脉冲测距就是将对距离的测量转变为对时间差的测量，其计算公式为：
hp,i＝c*ti/(2n)
i∈[1,4]
lp,j＝c*tj/(2n)
j∈[5,8]
上式中，hp,i为第i激光测距传感器采集的集装箱内上侧到货物之间的距离，lp,j为第j激光测距传感器采集的集装箱后侧到货物之间的距离，ti为第i激光测距传感器的测量点与货物之间所用的时间，c为激光在空气中传播的速度，n为测量时大气折射率，tj为第j激光测距传感器的测量点与货物之间所用的时间。
上式中hp,i为第i激光测距传感器采集的集装箱内上侧到货物之间的距离,其中i∈[1,4]；lp,j为第j激光测距传感器采集的集装箱后侧到货物之间的距离，其中j∈[5,8]；为了能够准确的获取集装箱内部上侧到货物之间的距离，引入权重a1,a2,a3，其中a1为集装箱内上侧第二个激光测距传感器的权重，a2为集装箱内上侧第三个激光测距传感器的权重，a3为集装箱内上侧第四个激光测距传感器的权重，其公式入下式所示：
其中权重的最小取值范围为amin，最大取值范围为amax，如果amin≤αi≤amax，其中i∈[1,3]，则ai则取值为1，如果ai，i∈[1,3]的取值范围小于最小取值范围amin或大于最大取值范围amax，则将ai,i∈[1,3]的值设置为零；此时只信任第一个激光测距传感器所测量出来的数值。
为了能够准确的获取集装箱内部后侧到货物之间的距离，引入权重ηi,i∈[1,3]，其中η1为集装箱内后侧第二个激光测距传感器的权重，η2为集装箱内后侧第三个激光测距传感器的权重，η3为集装箱内后侧第四个激光测距传感器的权重，其公式入下式所示：
其中权重的最小取值范围为ηmin，最大取值范围为ηmax，如果ηmin≤ηi≤ηmax，其中i∈[1,3]，则ηi则取值为1，如果ηi,i∈[1,3]的取值范围小于最小取值范围ηmin或大于最大取值范围ηmax，则将ηi,i∈[1,3]的值设置为零；此时只信任第一个激光测距传感器所测量出来的数值。
步骤1所述集装箱货车中货物的总体尺寸为：
上式中，lpn为集装箱内部的长。wpn为集装箱内部的宽，hpn为集装箱内部的高；lpl为集装箱内部后侧距货物的距离长度；hph为集装箱内部上侧距货物的距离高度；wp为集装箱内货物的宽度；其中lp为集装箱内货物的长；wp为集装箱内货物的宽；hp为集装箱内货物的高；
步骤1所述计算货车中货物的转动惯量为：
上式中，mp货物的质量；
步骤1所述通过平行轴定理来获取货车装载货物之后的新的质心位置和货车装载货物之后绕新质心位置的转动惯量量为：
集装箱货车空载时集装箱货车质心的原始坐标为(x,y,z)和转动惯量为(ix,emp,iy,emp,iz,emp)，以及质心到前轴和后轴的距离分别为和lf,emp和lr,emp,质心到左前轮和右前轮的距离为bf,emp和br,emp。当货车装载货物时，由于集装箱货车装载货物的摆放的规则，即质心沿y方向不变，质心坐标只在x方向和z方向变化。货物质量mp被加载，加载后的集装箱货车质量为mn＝me mp，由于集装箱货车货物加载后只在x和z方向变化，因此选择xz平面为工作平面，并计算位于原坐标系下的新的质心位置坐标(xn,yn,zn)：
mnrc＝me0 mpra
ra＝(mn/mp)/rc
上式中，mn集装箱货车加货物的质量；me集装箱货车的质量；mp货物的质量；rc集装箱货车装载货物的新坐标系距货车空载质心的原坐标系之间的距离；ra货物据原坐标系之间的距离；
由于货物装载货物时由于货物本身质量大和体积大，其货物本身的转动惯量不可忽略，根据转动惯量的平行轴定理,可以得出货物相对于原质心处的新的转动惯量为：
在此通过平行轴定理可以推导出加载后质心处的转动惯量于新坐标系下转动惯量之间的关系：
进一步推导得到：
将ra＝(mn/mp)/rc代入上式中，则可将新坐标下的转动惯量可以表示为：
上式中，
则货车满载货物的新质心位置的坐标距离原质心位置坐标的距离
上式中，rc集装箱货车装载货物的新坐标系距货车空载质心的原坐标系之间的距离；lf,emp为集装箱货车原质心到前轴的距离；lf为集装箱货车装载货物之后质心距前轴的距离；lh,emp为集装箱货车原质心高度；lh为集装箱货车装载货物之后的质心高度位置；
则新坐标系下的绕x轴的转动惯量
同理可以通过上述的方法来获得在新坐标系下的绕z轴的转动惯量
其中，新质心坐标系距前轴、后轴、左右轮和地面的距离(lf，lr，bh，lh)入下式所示：
lf＝lf,emp-xn；
lr＝lr,emp xn；
lh＝lh,emp yn；
bh＝bf,emp＝br,emp
上面式子中：ix,emp集装箱货车绕货车原质心坐标x轴的转动惯量；me集装箱货车的质量；lh,emp为集装箱货车原质心高度；lf,emp为集装箱货车原质心到前轴的距离；mp货物的质量；
步骤2：构建集装箱货车装载货物时四自由度非线性动力学模型，以货车空载时的四自由度非线性动力学模型为基础，将步骤1中得到的新质心位置和装载货物之后绕新质心位置的转动惯量代入货车空载时的四自由度非线性动力学模型中，进而构建货车装载货物之后的惯性参数变化的四自由度非线性动力学模型。
在所示的步骤2中，将步骤1中得到的新质心位置(lr，lf，bh，lh)和货车装载货物后绕新质心的转动惯量
纵向运动：
横向运动：
横摆运动：
侧倾运动：
式中：mn为集装箱货车的总质量，vx为集装箱货车质心的纵向速度，vy为集装箱货车质心的横向速度，rz为集装箱货车质心的横摆角速度，β为集装箱货车质心的侧偏角，
步骤3：定义系统状态变量、系统观测变量、系统输入变量、系统惯性参数变量，将考虑集装箱货车装载货物时惯性参数变化的货车四自由度非线性动力学模型通过前向欧拉法进行离散化转化为货车四自由度非线性状态空间模型，通过can总线获得的纵向加速度、横向加速度、横摆角速度、侧倾角速度和输入的前轮转角，进行判断是直行或转弯进而来选择惯性参数模型开始执行设计双自适应容积卡尔曼滤波的状态估计和惯性估计系统。
步骤3所述系统状态变量为：
步骤3所述系统观测变量为：
步骤3所述系统输入变量为：
u＝[δf,ωi,j]
步骤3所述系统惯性参数变量为：
其中，vx为为集装箱货车质心的纵向速度，vy为集装箱货车质心的横向速度，β为集装箱货车质心的侧偏角，rz为集装箱货车质心的横摆角速度，
将步骤二所建的动力学方程写成状态方程和观测方程的形式，再通过前向欧拉法对系统的状态方程和观测方程进行离散化可以写成如下形式：
fn＝(fyfl(t)sinδfl-fxfl(t)cosδfl)bl (fxfr(t)cosδfr-fyfr(t)sinδfr)br (fxrr(t)br-fxrl(t))bl-(fyrr(t) fyrl(t))lr
只有当满足ax、ay、rz、δf满足相关要求时状态估计器和惯性参数估计器才会起作用对特性情况下的具体惯性参数和货车状态进行联合估计。
而惯性参数mn的状态方程和观测方程为：
而惯性参数lf的状态方程和观测方程为：
而惯性参数lh的状态方程和观测方程为：
fn(t)＝(fyfl(t)sinδfl-fxfl(t)cosδfl)bl (fxfr(t)cosδfr-fyfr(t)sinδfr)br (fxrr(t)br-fxrl(t))bl-(fyrr(t) fyrl(t))lr
步骤4：通过双自适应容积卡尔曼滤波算法分别对货车纵向速度、横向速度、横摆角速度、质心侧偏角、侧倾角速度、货车整体质量、侧倾转动惯量、横摆转动惯量、质心高度和质心距前轴的距离进行联合估计。
在所述的步骤4中所述的双自适应容积卡尔曼滤波器的具体包括以下步骤：
步骤4.1：初始化；
输入系统中的固定参数信息包括集装箱货车空载质量me、质心的原始坐标为(x,y,z)和转动惯量为(ix,emp,iy,emp,iz,emp)，以及质心到前轴和后轴的距离分别为和lf,emp和lr,emp,质心到左前轮和右前轮的距离为bf,emp和br,emp。并设置估计状态初值
步骤4.2：参数变量时间更新；
参数变量进行更新时首先进行对纵向加速度ax、横向加速度ay、横摆角速度rz和前轮转角δf＝0.52deg进行判断：其中纵向加速度界限判定值为axa＝0.7m/s^2，横向加速度界限判定值为aya＝0.25m/s^2，横摆角速度的界限判定值为rza＝1deg/s，前轮转角的界限判定值为δfa＝0.52deg。如果ax>axa、ay<aya、rz<rza和δf<δfa如果满足上面要求，则对纵向动力学模块进行货车总质量mn:进行自适应容积卡尔曼滤波参数变量时间更新；
参数质量mn状态误差协方差矩阵
计算容积点
计算参数变量的预测值
计算参数变量的误差协方差矩阵的预测值
步骤4.3：状态变量的时间更新
状态变量误差协方差矩阵
计算容积点
计算状态变量的预测值
计算状态变量的误差协方差矩阵的预测值
步骤4.4：参数变量量测更新；
对更新后的参数误差协方差矩阵
计算容积点
更新参数变量量测方程的预测值
更新参数变量量测误差协方差矩阵的预测值
计算互协方差矩阵
计算自适应容积卡尔曼滤波增益矩阵km
计算参数变量的估计值
计算参数变量误差协方差矩阵估计值
定义新息
新息理论协方差矩阵为pvm：
对新息协方差矩阵进行近似估计
则量测误差协方差wm(t)
步骤4.5：状态变量的量测更新；
对更新后的状态误差协方差矩阵
计算容积点
更新状态变量量测方程的预测值
更新状态变量量测误差协方差矩阵的预测值
计算互协方差矩阵
计算自适应容积卡尔曼滤波增益矩阵ks
计算状态变量的估计值
计算状态变量误差协方差矩阵估计值
定义新息
新息理论协方差矩阵为pvc：
对新息协方差矩阵进行近似估计pvc；
则量测误差协方差ws(t)；
步骤4.6：通过自适应容积卡尔曼滤波参数进行时间更新和测量跟新之后对转动惯量估计值计算并计算完之后对车辆状态方程进行更新和通过自适应容积卡尔曼滤波对车辆状态进行时间更新和测量更新之后，得出状态变量估计值，并对参数状态方程里面的初值进行更新；
步骤4.7：对横向加速度即ay、横摆角速度即rz和前轮转角即δf进行判断：其中横向加速度界限判定值为aya＝0.25m/s^2，横摆角速度的界限判定值为rza＝1deg/s，前轮转角的界限判定值为δfa＝0.52deg。如果ay>aya、rz>rza和δf>δfa，则对新质心距前轴的位置和新质心的高度进行自适应容积滤波参数变量时间更新,其步骤如步骤4.1和步骤4.6。
尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例做出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。
显然，本领域的技术人员可以对本发明实施例进行各种改动和变型而不脱离本发明实施例的精神和范围。这样，倘若本发明实施例的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。
1.一种基于尺寸测量装置的货车状态惯性参数联合估计方法，其特征在于，
所述尺寸测量装置包括：
can总线采集装置、微控制器、加速度计传感器、陀螺仪传感器、第一激光测距传感器至第八激光测距传感器；
所述微控制器于所述can总线采集装置通过有线方式连接；
can总线采集装置分别与所述的加速度计传感器、陀螺仪传感器、第一激光测距传感器至第八激光测距传感器通过有线方式依次连接；
所述加速度计传感器、陀螺仪传感器加装在集装箱货车上；所述的第一激光测距传感器至第四激光传感器分别在集装箱上侧内部以固定距离依次排列布置；所述的第五激光测距传感器和第七激光测距传感器加装在集装箱左货车门处，所述的第六激光测距传感器、第八激光测距传感器加装在集装箱右货车门处；所述的第五激光测距传感器至第八激光测距传感器分别以固定距离向上排列布置；
所述加速度计传感器、陀螺仪传感器用于采集货车纵向加速度、横向加速度、横摆角速度和侧倾角速度并通过所述can总线采集装置传输至所述微处理器；
所述的can总线采集装置通过can总线获取纵向加速度、横向加速度、横摆角速度和侧倾角速度；
所述的第一激光测距传感器至第四激光传感器分别用来采集集装箱内部上侧至货物之间的距离信息，所述的第五激光测距传感器至第八激光测距传感器分别用于采集集装内部后侧至货物之间的距离信息；
所述货车状态惯性参数联合估计方法，包括以下步骤：
步骤1：微处理器通过所述的第一激光测距传感器至第四激光测距传感器采集集装箱内上侧至货物之间的距离、通过所述的第五激光测距传感器至第八激光测距传感器采集集装内后侧至货物之间的距离，进一步来计算集装箱货车中货物的总体尺寸，通过自适应双容积卡尔曼滤波算法估计货车装载货物时的总质量减去货车空载时的质量进而计算货物的质量，计算货车中货物的转动惯量，结合货车空载时的货车的质心位置和转动惯量，通过平行轴定理来获取货车装载货物之后的新的质心位置和货车装载货物之后绕新质心位置的转动惯量；
步骤2：构建集装箱货车装载货物时四自由度非线性动力学模型，以货车空载时的四自由度非线性动力学模型为基础，将步骤1中得到的新质心位置和装载货物之后绕新质心位置的转动惯量代入货车空载时的四自由度非线性动力学模型中，进而构建货车装载货物之后的惯性参数变化的四自由度非线性动力学模型；
步骤3：定义系统状态变量、系统观测变量、系统输入变量、系统惯性参数变量，将考虑集装箱货车装载货物时惯性参数变化的货车四自由度非线性动力学模型通过前向欧拉法进行离散化转化为货车四自由度非线性状态空间模型，通过can总线获得的纵向加速度、横向加速度、横摆角速度、侧倾角速度和输入的前轮转角，进行判断是直行或转弯进而来选择惯性参数模型开始执行设计双自适应容积卡尔曼滤波的状态估计和惯性估计系统；
步骤4：通过双自适应容积卡尔曼滤波算法分别对货车纵向速度、横向速度、横摆角速度、质心侧偏角、侧倾角速度、货车整体质量、侧倾转动惯量、横摆转动惯量、质心高度和质心距前轴的距离进行联合估计。
2.根据权利要求1所述的基于尺寸测量装置的货车状态惯性参数联合估计方法，其特征在于，
步骤1通过所述进一步来计算集装箱货车中货物的总体尺寸，具体为：
而激光测距传感器的主要原理则是通过激光脉冲测距，激光脉冲测距就是将对距离的测量转变为对时间差的测量，其计算公式为：
hp,i＝c*ti/(2n)
i∈[1,4]
lp,j＝c*tj/(2n)
j∈[5,8]
上式中，hp,i为第i激光测距传感器采集的集装箱内上侧到货物之间的距离，lp,j为第j激光测距传感器采集的集装箱后侧到货物之间的距离，ti为第i激光测距传感器的测量点与货物之间所用的时间，c为激光在空气中传播的速度，n为测量时大气折射率，tj为第j激光测距传感器的测量点与货物之间所用的时间。
上式中hp,i为第i激光测距传感器采集的集装箱内上侧到货物之间的距离,其中i∈[1,4]；lp,j为第j激光测距传感器采集的集装箱后侧到货物之间的距离，其中j∈[5,8]；为了能够准确的获取集装箱内部上侧到货物之间的距离，引入权重a1,a2,a3，其中a1为集装箱内上侧第二个激光测距传感器的权重，a2为集装箱内上侧第三个激光测距传感器的权重，a3为集装箱内上侧第四个激光测距传感器的权重，其公式入下式所示：
其中权重的最小取值范围为amin，最大取值范围为amax，如果amin≤αi≤amax，其中i∈[1,3]，则ai则取值为1，如果ai，i∈[1,3]的取值范围小于最小取值范围amin或大于最大取值范围amax，则将ai,i∈[1,3]的值设置为零；此时只信任第一个激光测距传感器所测量出来的数值；
为了能够准确的获取集装箱内部后侧到货物之间的距离，引入权重ηi,i∈[1,3]，其中η1为集装箱内后侧第二个激光测距传感器的权重，η2为集装箱内后侧第三个激光测距传感器的权重，η3为集装箱内后侧第四个激光测距传感器的权重，其公式入下式所示：
其中权重的最小取值范围为ηmin，最大取值范围为ηmax，如果ηmin≤ηi≤ηmax，其中i∈[1,3]，则ηi则取值为1，如果ηi,i∈[1,3]的取值范围小于最小取值范围ηmin或大于最大取值范围ηmax，则将ηi,i∈[1,3]的值设置为零；此时只信任第一个激光测距传感器所测量出来的数值；
步骤1所述集装箱货车中货物的总体尺寸为：
上式中，lpn为集装箱内部的长；wpn为集装箱内部的宽，hpn为集装箱内部的高；lpl为集装箱内部后侧距货物的距离长度；hph为集装箱内部上侧距货物的距离高度；wp为集装箱内货物的宽度；其中lp为集装箱内货物的长；wp为集装箱内货物的宽；hp为集装箱内货物的高；
步骤1所述计算货车中货物的转动惯量为：
上式中，mp货物的质量；
步骤1所述通过平行轴定理来获取货车装载货物之后的新的质心位置和货车装载货物之后绕新质心位置的转动惯量量为：
集装箱货车空载时集装箱货车质心的原始坐标为(x,y,z)和转动惯量为(ix,emp,iy,emp,iz,emp)，以及质心到前轴和后轴的距离分别为和lf,emp和lr,emp,质心到左前轮和右前轮的距离为bf,emp和br,emp；当货车装载货物时，由于集装箱货车装载货物的摆放的规则，即质心沿y方向不变，质心坐标只在x方向和z方向变化；货物质量mp被加载，加载后的集装箱货车质量为mn＝me mp，由于集装箱货车货物加载后只在x和z方向变化，因此选择xz平面为工作平面，并计算位于原坐标系下的新的质心位置坐标(xn,yn,zn)：
mnrc＝me0 mpra
ra＝(mn/mp)/rc
上式中，mn集装箱货车加货物的质量；me集装箱货车的质量；mp货物的质量；rc集装箱货车装载货物的新坐标系距货车空载质心的原坐标系之间的距离；ra货物据原坐标系之间的距离；
由于货物装载货物时由于货物本身质量大和体积大，其货物本身的转动惯量不可忽略，根据转动惯量的平行轴定理,可以得出货物相对于原质心处的新的转动惯量为：
在此通过平行轴定理可以推导出加载后质心处的转动惯量于新坐标系下转动惯量之间的关系：
进一步推导得到：
将ra＝(mn/mp)/rc代入上式中，则可将新坐标下的转动惯量可以表示为：
上式中，
则货车满载货物的新质心位置的坐标距离原质心位置坐标的距离
上式中，rc集装箱货车装载货物的新坐标系距货车空载质心的原坐标系之间的距离；lf,emp为集装箱货车原质心到前轴的距离；lf为集装箱货车装载货物之后质心距前轴的距离；lh,emp为集装箱货车原质心高度；lh为集装箱货车装载货物之后的质心高度位置；
则新坐标系下的绕x轴的转动惯量
同理可以通过上述的方法来获得在新坐标系下的绕z轴的转动惯量
其中，新质心坐标系距前轴、后轴、左右轮和地面的距离(lf，lr，bh，lh)入下式所示：
lf＝lf,emp-xn；
lr＝lr,emp xn；
lh＝lh,emp yn；
bh＝bf,emp＝br,emp
上面式子中：ix,emp集装箱货车绕货车原质心坐标x轴的转动惯量；me集装箱货车的质量；lh,emp为集装箱货车原质心高度；lf,emp为集装箱货车原质心到前轴的距离；mp货物的质量；
3.根据权利要求1所述的基于尺寸测量装置的货车状态惯性参数联合估计方法，其特征在于，
在所示的步骤2中，将步骤1中得到的新质心位置(lr，lf，bh，lh)和货车装载货物后绕新质心的转动惯量
纵向运动：
横向运动：
横摆运动：
侧倾运动：
式中：mn为集装箱货车的总质量，vx为集装箱货车质心的纵向速度，vy为集装箱货车质心的横向速度，rz为集装箱货车质心的横摆角速度，β为集装箱货车质心的侧偏角，
4.根据权利要求1所述的基于尺寸测量装置的货车状态惯性参数联合估计方法，其特征在于，
步骤3所述系统状态变量为：
步骤3所述系统观测变量为：
步骤3所述系统输入变量为：
u＝[δf,ωi,j]
步骤3所述系统惯性参数变量为：
其中，vx为为集装箱货车质心的纵向速度，vy为集装箱货车质心的横向速度，β为集装箱货车质心的侧偏角，rz为集装箱货车质心的横摆角速度，
将步骤2所建的动力学方程写成状态方程和观测方程的形式，再通过前向欧拉法对系统的状态方程和观测方程进行离散化可以写成如下形式：
fn＝(fyfl(t)sinδfl-fxfl(t)cosδfl)bl (fxfr(t)cosδfr-fyfr(t)sinδfr)br (fxrr(t)br-fxrl(t))bl-(fyrr(t) fyrl(t))lr
只有当满足ax、ay、rz、δf满足相关要求时状态估计器和惯性参数估计器才会起作用对特性情况下的具体惯性参数和货车状态进行联合估计；
而惯性参数mn的状态方程和观测方程为：
而惯性参数lf的状态方程和观测方程为：
而惯性参数lh的状态方程和观测方程为：
5.根据权利要求1所述的基于尺寸测量装置的货车状态惯性参数联合估计方法，其特征在于，
在所述的步骤4中所述的双自适应容积卡尔曼滤波器的具体包括以下步骤：
步骤4.1：初始化；
输入系统中的固定参数信息包括集装箱货车空载质量me、质心的原始坐标为(x,y,z)和转动惯量为(ix,emp,iy,emp,iz,emp)，以及质心到前轴和后轴的距离分别为和lf,emp和lr,emp,质心到左前轮和右前轮的距离为bf,emp和br,emp；并设置估计状态初值
步骤4.2：参数变量时间更新；
参数变量进行更新时首先进行对纵向加速度ax、横向加速度ay、横摆角速度rz和前轮转角δf＝0.52deg进行判断：其中纵向加速度界限判定值为axa＝0.7m/s^2，横向加速度界限判定值为aya＝0.25m/s^2，横摆角速度的界限判定值为rza＝1deg/s，前轮转角的界限判定值为δfa＝0.52deg。如果ax>axa、ay<aya、rz<rza和δf<δfa，如果满足上面要求，则对纵向动力学模块进行货车总质量mn:进行自适应容积卡尔曼滤波参数变量时间更新；
参数质量mn状态误差协方差矩阵
计算容积点
计算参数变量的预测值
计算参数变量的误差协方差矩阵的预测值
步骤4.3：状态变量的时间更新
状态变量误差协方差矩阵
计算容积点
计算状态变量的预测值
计算状态变量的误差协方差矩阵的预测值
步骤4.4：参数变量量测更新；
对更新后的参数误差协方差矩阵
计算容积点
更新参数变量量测方程的预测值
更新参数变量量测误差协方差矩阵的预测值
计算互协方差矩阵
计算自适应容积卡尔曼滤波增益矩阵km
计算参数变量的估计值
计算参数变量误差协方差矩阵估计值
定义新息
新息理论协方差矩阵为pvm：
对新息协方差矩阵进行近似估计
则量测误差协方差wm(t)
步骤4.5：状态变量的量测更新；
对更新后的状态误差协方差矩阵
计算容积点
更新状态变量量测方程的预测值
更新状态变量量测误差协方差矩阵的预测值
计算互协方差矩阵
计算自适应容积卡尔曼滤波增益矩阵ks
计算状态变量的估计值
计算状态变量误差协方差矩阵估计值
定义新息
新息理论协方差矩阵为pvc：
对新息协方差矩阵进行近似估计pvc；
则量测误差协方差ws(t)；
步骤4.6：通过自适应容积卡尔曼滤波参数进行时间更新和测量跟新之后对转动惯量估计值计算并计算完之后对车辆状态方程进行更新和通过自适应容积卡尔曼滤波对车辆状态进行时间更新和测量更新之后，得出状态变量估计值，并对参数状态方程里面的初值进行更新；
步骤4.7：对横向加速度即ay、横摆角速度即rz和前轮转角即δf进行判断：其中横向加速度界限判定值为aya＝0.25m/s^2，横摆角速度的界限判定值为rza＝1deg/s，前轮转角的界限判定值为δfa＝0.52deg。
如果ay>aya、rz>rza和δf>δfa则对新质心距前轴的位置和新质心的高度进行自适应容积滤波参数变量时间更新,其步骤如步骤4.1至步骤4.6。
技术总结