详情

原文传递目标轨迹离散化方法、泊车路径跟踪误差优化方法及系统

专利名称：	目标轨迹离散化方法、泊车路径跟踪误差优化方法及系统
摘要：	本发明公开的一种目标轨迹离散化方法、泊车路径跟踪误差优化方法及系统，包括：路径更新，计算圆弧的圆心角deltA，每个点之间的圆心角StepA，总的点数，最后计算每个点的位置坐标；计算直线的总长度和斜率，最后求出每个点的坐标，对圆弧和直线的组合，若上一段还剩LastPhaseRes，下一段需要从预设间距‑LastPhaseRes的地方开始，求出每个离散点的目标航向角。本发明用于平行车位、垂直车位和斜列车位等场景下泊车过程中路径跟踪误差的计算和优化，以提高泊车的精度，改善泊车效果及体验。
专利类型：	发明专利
国家地区组织代码：	重庆;50
申请人：	重庆长安汽车股份有限公司
发明人：	王朝美;党建民;万凯林;贺勇;文滔
专利状态：	有效
申请日期：	2021-12-10T00:00:00+0800
发布日期：	2022-03-29T00:00:00+0800
申请号：	CN202111508323.8
公开号：	CN114248761A
代理机构：	重庆华科专利事务所
代理人：	谭小琴
分类号：	B60W30/06;B;B60;B60W;B60W30;B60W30/06
申请人地址：	400023 重庆市江北区建新东路260号
主权项：	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次以上才会更新，系统的周期为T，位置误差更新的周期就为NT，N为标定量；然后对位置误差限幅，位置误差每次的变化不能超过预设长度； S52：针对航向角误差，首先进行消抖处理，当新的航向角误差与当前航向角误差连续变化N次以上才会更新，航向角误差更新的周期就为NT;然后对航向角误差限幅，航向角误差每次的变化不能超过预设角度； S53：将处理之后的位置误差和航向角误差输出到横向控制模块。 5.根据权利要求3所述的自动泊车路径跟踪误差优化计算方法，其特征在于：所述S22中，当向前行驶时，若方向盘左正右负，则位置误差的符号为左负右正，反之亦然；当向后行驶时，车辆的横向误差的符号与前进时的符号相反。 6.根据权利要求4或5所述的自动泊车路径跟踪误差优化计算方法，其特征在于：所述S51中，预设长度为10cm。 7.根据权利要求6所述的自动泊车路径跟踪误差优化计算方法，其特征在于：所述步骤S52中，所述预设角度为5°。 8.一种目标轨迹离散化系统，其特征在于：包括存储器和控制器，所述存储器内存储有计算机可读程序，所述计算机可读程序被调用时能执行如权利要求1至3任一所述的目标轨迹离散化方法的步骤。 9.一种自动泊车路径跟踪误差优化计算系统，其特征在于：包括存储器和控制器，所述存储器内存储有计算机可读程序，所述计算机可读程序被调用时能执行如权利要求4至7任一所述的自动泊车路径跟踪误差优化计算方法的步骤。
所属类别：	发明专利