基于卡尔曼的道路曲率估计
附赠自动驾驶学习资料和量产经验:链接
道路曲率对目标筛选、自车轨迹规划等具有重要作用。当前对道路曲率估计的方式包含:传感器测量、基于动力学根据前轮转角和横摆角速度估计自车轨迹曲率进而估计道路曲率。
单一的曲率估计方式无法抵抗环境的绕动。因而需要对多传感器信息进行融合处理,进而估计出稳定可靠的道路曲率。
1 自车轨迹曲率:
根据动力学信息计算出自车估计曲率,见(基于方向盘转角计算自车轨迹曲率1、2)
:自车纵向车速 m/s
:横摆角速度 rad/s
:前轮转角 rad
:轴距 m
2 传感器检测道路曲率
传感器检测道路表达形式为:
、、3车道线方程系数。
3 构建KF估计道路曲率方程
: 传刚器检测道路曲率
:传感器检测道路曲率变化率
则:
状态方程为:
则:
: 采样周期
观测方程:
使用卡尔曼进行曲率融合:
其中,Q,R分别为系统噪声和测量噪声的协方差,利用多维测量信息进行反馈校正.其中矫正因子为:
根据需求进行权重分配
融合处理后结果如下所示: