详情

原文传递基于传感器融合的前车横向与纵向运动状态联合估计方法

专利名称：	基于传感器融合的前车横向与纵向运动状态联合估计方法
摘要：	本发明公开了一种基于传感器融合的前车横向与纵向运动状态联合估计方法，涉及无人驾驶与智能控制技术领域，解决了现有前车横向与纵向运动状态估计精度不高的技术问题，其技术方案要点是综合考虑了前车的纵向运动与侧向运动，利用V2V信息和车载传感器信息对前车的运动状态进行估计，既弥补了V2V通信速率低带来的估计精度下降的问题，也提升了估计方法对环境的适应性，同时还解决了传感器采样速率不一致情况下的异步估计问题。
专利类型：	发明专利
国家地区组织代码：	江苏;32
申请人：	东南大学
发明人：	徐利伟;董锋威;殷国栋
专利状态：	有效
申请日期：	2022-09-23T00:00:00+0800
发布日期：	2023-01-06T00:00:00+0800
申请号：	CN202211164025.6
公开号：	CN115571156A
代理机构：	北京德崇智捷知识产权代理有限公司
代理人：	曹婷
分类号：	B60W60/00;B60W50/00;B;B60;B60W;B60W60;B60W50;B60W60/00;B60W50/00
申请人地址：	210096 江苏省南京市江宁区东南大学路2号
主权项：	1.一种基于传感器融合的前车横向与纵向运动状态联合估计方法，其特征在于，包括： S1：通过V2V和自车传感器获取前车状态量及参数信息； S2：基于V2V获取的前车状态量及参数信息对前车横向与纵向状态联合估计器进行设计； S3：基于自车传感器获取的前车状态量及参数信息对前车侧向状态估计器进行设计； S4：基于分布式融合卡尔曼滤波将前车横向与纵向状态联合估计器与前车侧向状态估计器进行融合，融合后对前车横向与纵向运动状态进行联合估计。 2.如权利要求1所述的方法，其特征在于，基于V2V获取的前车状态量包括前车的横向加速度、纵向加速度和前轮转角，参数信息包括前车的质量和轴距；利用自车传感器获取前车在大地坐标系下的位置、相对于道路的位姿和道路曲率。 3.如权利要求2所述的方法，其特征在于，所述步骤S2包括：基于V2V通信获取的前车状态量及参数信息构建第一前车三自由度动力学模型，通过所述第一前车三自由度动力学模型对所述前车横向与纵向状态联合估计器进行设计；所述第一前车三自由度动力学模型表示为：其中，r表示前车横摆角速度；vx表示前车纵向速度；ax表示前车加速度；vy表示前车侧向速度；ay表示前车侧向加速度；m表示前车质量；Iz表示前车绕Z轴的转动惯量；a表示前车质心到前轴的距离；b表示前车质心到后轴的距离；kf表示与前车前轴等效的侧偏刚度；kr表示与前车后轴等效的侧偏刚度；δ表示前车前轮转角；令x1，k＝[vx，vy,r]T，z1,k＝[ay]T，uk＝[ax，δ]T，则所述前车三自由度动力学模型的离散化方程表示为：其中，wk和vk分别表示模型的过程噪声和测量噪声，且符合高斯分布；Q表示过程噪声的协方差矩阵；R表示测量噪声的协方差矩阵；f1(.)表示状态转移函数；h1(.)表示输出函数；对式(2)进行线性化，则线性化后的方程表示为：其中，A1,k-1表示f1相对于x1的雅可比矩阵；B1,k-1表示f1相对于u的雅可比矩阵；H1,k表示h1相对于x1的雅可比矩阵；Γ1,k表示h1相对于u的雅可比矩阵。 4.如权利要求3所述的方法，其特征在于，根据A1和H1并采用信息滤波形式的拓展卡尔曼滤波对前车横向与纵向状态联合估计器进行设计，前车横向与纵向状态联合估计器的估计步骤包括时间更新和测量更新；所述时间更新包括：所述测量更新包括：其中，表示k时刻的先验估计值，P1,k/k-1表示k时刻的先验误差协方差矩阵，A1,k-1表示f1对系统状态向量x的雅可比矩阵表示k时刻的状态估计值，P1,k-1表示k-1时刻的误差协方差矩阵，H1,k表示h1对系统状态向量x的雅可比矩阵 5.如权利要求4所述的方法，其特征在于，所述步骤S3包括：基于自车传感器获取的前车状态量及参数信息构建前车纵向运动模型，根据所述前车纵向运动模型对前车纵向速度估计器进行设计；将所述第一前车三自由度动力学模型与Serret-Frenet方程结合得到第二前车三自由度动力学模型；在所述前车纵向速度估计器的基础上，根据所述第二前车三自由度动力学模型对所述前车侧向状态估计器进行设计。 6.如权利要求5所述的方法，其特征在于，所述前车纵向运动模型为离散化后的CTRA模型，离散化后的CTRA模型表示为：其中，Td表示采样时间；X、Y、分别表示前车在大地坐标中的纵向坐标值、横向坐标值、方向角；ak表示k时刻的前车纵向加速度；vk表示k时刻的前车纵向车速；rk表示k时刻的前车横摆角速度；xk表示k时刻的前车状态向量；fCTRA表示CTRA模型的状态转移函数；hCTRA表示CTRA模型的输出函数。 7.如权利要求6所述的方法，其特征在于，根据所述前车纵向运动模型对前车纵向速度估计器进行设计，前车纵向速度估计器通过容积卡尔曼滤波对前车的纵向速度进行估计，估计步骤包括： 1)初始化：零时刻的估计值为误差协方差矩阵容积点按式(7)进行计算，表示为：其中，n为状态的维度，c为容积点的数量； 2)时间更新：采用奇异值分解法将前一时刻的误差协方差矩阵进行分解，随后依次计算并传递容积点，表示为：其中，Uk-1、Sk-1和都是在k-1时刻误差协方差矩阵经奇异值分解后获得的矩阵；χi,k-1表示第i个容积点；表示k-1时刻的状态估计值；表示经过状态方程迭代后的容积点；对经过时间更新的状态预测值和误差协方差矩阵Pk/k-1进行估计，表示为： 3)测量更新：采用奇异值分解法将当前时刻的先验误差协方差矩阵进行分解，随后依次计算并传递容积点，表示为：对测量值的预测值进行估计，并计算新息方差Pzz,k/k-1和互协方差Pxz,k/k-1，表示为：结合式(7)至式(11)以及滤波器增益Kk完成对状态量和误差协方差矩阵Pk的测量更新，表示为： 8.如权利要求7所述的方法，其特征在于，所述第二前车三自由度动力学模型的构建包括： Serret-Frenet方程表示为：根据Serret-Frenet方程和所述第一前车三自由度动力学模型，得到离散化的所述第二前车三自由度动力学模型，表示为：该第二前车三自由度动力学模型线性化后的方程表示为：其中，x2，k＝[vx，vy，r，e，ψ，C]T，z2，k＝[vx，e，ψ，C]T，uk＝[ax，δ]T；e表示前车相对于道路的横向位置偏差，ψ表示前车航向相对于道路方向的夹角，C表示道路曲率；测量值中的vx来自基于自车传感器的前车纵向速度估计器，其余测量值来自自车的视觉传感器及雷达； f2(.)表示状态转移函数；h2(.)表示输出函数； A2,k-1表示f2相对于x2的雅可比矩阵；B2,k-1表示f2相对于u的雅可比矩阵；H2,k表示h2相对于x2的雅可比矩阵；Γ2,k表示h2相对于u的雅可比矩阵。 9.如权利要求8所述的方法，其特征在于，步骤S4中，通过状态变换矩阵Φj将前车横向与纵向状态联合估计器和前车侧向状态估计器进行融合，表示为：其中，Φ1和Φ2均为状态变换矩阵；I3×3表示3维的对角线全为1的矩阵；融合中心的状态x与x2相同，融合后的联合估计步骤包括时间更新和测量更新，时间更新表示为：测量更新表示为：其中，Ak-1＝A2,k-1，Bk-1＝B2,k-1；表示k时刻融合中心计算出的先验估计状态；表示k-1时刻融合中心估计的状态；Pk/k-1表示k时刻融合中心计算出的先验误差协方差矩阵；Pk表示k-1时刻融合中心计算出的误差协方差矩阵；j＝1时，各矩阵为状态转移函数f1(.)对应的各个矩阵；j＝2时，各矩阵为状态转移函数f2(.)对应的各个矩阵。 10.如权利要求1所述的方法，其特征在于，在V2V采样间隔期间，各估计器的输入ax和δ采用恒定变化速率的方法进行估计获得。