目标轨迹离散化方法、泊车路径跟踪误差优化方法及系统.pdf

(19)中华人民共和国国家知识产权局(12)发明专利申请(10)申请公布号CN114248761A(43)申请公布日2022.03.29(21)申请号202111508323.8(22)申请日2021.12.10(71)申请人重庆长安汽车股份有限公司地址400023重庆市江北区建新东路260号(72)发明人王朝美党建民万凯林贺勇文滔(74)专利代理机构重庆华科专利事务所50123代理人谭小琴(51)Int.Cl.B60W30/06(2006.01)权利要求书2页说明书4页附图3页(54)发明名称目标轨迹离散化方法、泊车路径跟踪误差优化方法及系统(57)摘要本发明公开的一种目标轨迹离散化方法、泊车路径跟踪误差优化方法及系统，包括：路径更新，计算圆弧的圆心角deltA，每个点之间的圆心角StepA，总的点数，最后计算每个点的位置坐标；计算直线的总长度和斜率，最后求出每个点的坐标，对圆弧和直线的组合，若上一段还剩LastPhaseRes，下一段需要从预设间距‑LastPhaseRes的地方开始，求出每个离散点的目标航向角。本发明用于平行车位、垂直车位和斜列车位等场景下泊车过程中路径跟踪误差的计算和优化，以提高泊车的精度，改善泊车效果及体验。CN114248761ACN114248761A权利要求书1/2页1.一种目标轨迹离散化方法，其特征在于，包括以下步骤：将路径规划输出的轨迹按照预设距离等间距离散成点；若轨迹是圆弧，根据起始点坐标、终止点坐标和圆心坐标，利用反正切函数计算出需要走过的圆心角deltA；根据间距和半径计算出每个点之间的圆心角StepA，用deltA/StepA计算出该段圆弧总的点数，根据总的点数，结合起始圆心角，计算出每个离散点的圆心角；并根据每个点的圆心角和圆心坐标计算出每个离散点的坐标；若轨迹是直线，根据起始点和终止点坐标计算出该段直线总的长度和斜率；根据该段距离的长度和间隔距离计算出总的点数；并根据起始点坐标、间隔距离和斜率计算出每个离散点的坐标；若轨迹是圆弧和直线的组合，其中，圆弧段轨迹部分按照圆弧轨迹处理的方法计算出弧形段上每个离散点的坐标，直线段轨迹部分按照直线轨迹处理的方法计算出直接段上每个离散点的坐标；且圆弧段轨迹部分和直线段轨迹部分的连接处，若上一段还剩LastPhaseRes，下一段需要从预设间距‑LastPhaseRes的地方开始；根据轨迹的所有离散点位置坐标，用反正切函数计算每个离散点的目标航向角。2.根据权利要求1所述的目标轨迹离散化方法，其特征在于：离散的规则是每更新一步路径就离散化一次，每一步支持最大的轨迹距离为16m。3.根据权利要求1或2所述的目标轨迹离散化方法，其特征在于：所述预设间距为10cm。4.一种自动泊车路径跟踪误差优化计算方法，其特征在于，包括以下步骤：步骤1，目标轨迹离散化，采用如权利要求1至3任一所述的目标轨迹离散化方法对目标轨迹进行离散化处理，得到每个离散点的位置坐标和目标航向角；步骤2，计算自动泊车路径跟踪横向误差：S21：根据轨迹的所有离散点位置坐标，结合车辆当前的位置坐标，搜索出离其最近的轨迹点P1，并根据其航向角计算出下一个点P2的坐标；S22：计算车辆的横向误差，车辆的横向误差的绝对值为车辆当前位置到线段P1P2间的距离，车辆的横向误差的符号与方向盘角度正负有关；步骤3，计算出泊车过程中车辆与目标点的实时距离：S31：根据轨迹的所有离散点位置坐标，结合车辆当前的位置坐标，找出离当前车辆位置最近的轨迹点；S32：根据轨迹总的点数和匹配的当前轨迹点，计算出车辆当前位置到目标位置的实际距离，该距离等于剩余的点数乘以间隔距离加上最后一段离散化剩下的距离LastPhaseRes；步骤4，计算自动泊车路径跟踪航向角误差：S41：根据轨迹的所有离散点位置坐标，结合车辆当前的位置坐标，找出离当前车辆位置最近的轨迹点；S42：根据轨迹的所有离散点航向角，结合当前车辆的航向角，当车辆向前行驶时，所述航向角误差等于最近轨迹点的目标航向角减去当前航向角，当车辆向后行驶时，航向角误差取反；步骤5，自动泊车路径跟踪误差优化处理：S51：针对位置误差，首先进行消抖处理，当新的位置误差与当前位置误差连续变化N次2CN114248761A权利要求书2/2页以上才会更新，系统的周期为T，位置误差更新的周期就为N*T，N为标定量；然后对位置误差限幅，位置误差每次的变化不能超过预设长度；S52：针对航向角误差，首先进行消抖处理，当新的航向角误差与当前航向角误差连续变化N次以上才会更新，航向角误差更新的周期就为N*T;然后对航向角误差限幅，航向角误差每次的变化不能超过预设角度；S53：将处理之后的位置误差和航向角误差输出到横向控制模块。5.根据权利要求3所述的自动泊车路径跟踪误差优化计算方法，其特