详情

原文传递一种智能车避障轨迹规划方法及装置

专利名称：	一种智能车避障轨迹规划方法及装置
摘要：	本发明实施例提供了一种智能车避障轨迹规划方法，根据每个障碍物的位置得到所有的避障格的位置；根据自车的位置、自车的速度、障碍物的速度和所有避障格的位置，得到至少一个避障格路径；将避障格代价最小的避障格路径作为目标避障格路径；根据自车的位置、自车的速度和第一个目标避障格的位置，得到第一个目标避障格的避障轨迹；根据任一个目标避障格的位置、在任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度，得到任一个目标避障格的避障轨迹；根据目标避障格路径中所有目标避障格对应的避障轨迹，得到目标避障格路径对应的目标避障轨迹。本发明实施例根据障碍物的速度规划路径，提高了避障的准确性。
专利类型：	发明专利
国家地区组织代码：	安徽;34
申请人：	芜湖汽车前瞻技术研究院有限公司
发明人：	赛影辉;李垚;唐得志
专利状态：	有效
申请日期：	2019-05-14T00:00:00+0800
发布日期：	2019-08-02T00:00:00+0800
申请号：	CN201910399631.8
公开号：	CN110077397A
代理机构：	北京三高永信知识产权代理有限责任公司
代理人：	唐述灿
分类号：	B60W30/09(2012.01);B;B60;B60W;B60W30
申请人地址：	241006 安徽省芜湖市鸠江区凤鸣湖北路21号
主权项：	1.一种智能车避障轨迹规划方法，其特征在于，方法包括：获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度；根据每个所述障碍物的位置得到所有的避障格的位置，每个所述避障格为与相对应的所述障碍物的一侧相距预设距离且无障碍物的预设大小的格子；根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置，得到至少一个避障格路径，每个所述避障格路径包括：从所述自车的当前位置起避过所有相关的所述障碍物所依次经过的每个所述避障格的顺序以及对应的所述避障格；计算每个所述避障格路径的避障格代价，将避障格代价最小的避障格路径作为目标避障格路径；对于所述目标避障格路径中所述自车经过的第一个目标避障格，根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置，得到所述第一个目标避障格的避障轨迹；对于所述目标避障格路径中所述自车经过的第一个目标避障格之外的任一个目标避障格，根据该任一个目标避障格的位置、在该任一个目标避障格之前所述自车经过的上一个目标避障格的位置和所述自车的速度，得到该任一个目标避障格的避障轨迹；根据所述目标避障格路径中所有目标避障格对应的避障轨迹，得到所述目标避障格路径对应的目标避障轨迹。 2.根据权利要求1所述的方法，其特征在于，在所述获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度之前，所述方法还包括：判断是自车周围是否存在障碍物；当存在所述障碍物时，判断所述自车与同一车道上的相邻的障碍物是否有发生碰撞的可能；若是，则执行获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度的步骤。 3.根据权利要求2所述的方法，其特征在于，所述判断所述自车与同一车道上的相邻的障碍物是否有发生碰撞的可能，包括：获取相邻的障碍物的速度；将所述自车的速度与相邻的障碍物的速度进行比较，若相邻的障碍物的速度小于所述自车的速度，则判定有发生碰撞的可能；若相邻的障碍物的速度大于所述自车的速度，则判定没有发生碰撞的可能。 4.根据权利要求1所述的方法，其特征在于，所述根据每个所述障碍物的位置得到所有的避障格的位置，包括：获取所述自车的尺寸；基于所述自车的尺寸设置避障格的目标尺寸；在每一障碍物的两侧的预设距离处设置目标尺寸的避障格，得到所有避障格的位置，其中所述避障格的位置上无障碍物。 5.根据权利要求1所述的方法，其特征在于，所述根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置，得到至少一个避障格路径，包括：在所有所述避障格之中确定所有最终避障格，其中每个所述最终避障格为从其开始直行而无碰撞障碍物的可能性的避障格；对于每个所述最终避障格的位置，确定其和所述自车的位置之间在行驶方向上存在的中间避障格的位置，进而得到所有的可能路径，其中每个可能路径不包括中间避障格或者包括至少一个中间避障格，其中每个可能路径中的任两个中间避障格都不属于同一个障碍物两侧的两个避障格；对于每个可能路径，当所述自车的位置和下一个经过的避障格的位置之间的可能行车区域中以及每一个中间避障格的位置与下一个经过的避障格的位置之间的可能行车区域中均不存在障碍物，且当所述自车的当前位置和下一个经过的避障格的位置之间的纵向距离以及每一个中间避障格的位置与下一个经过的避障格的位置之间的纵向距离均不小于避障所需的最小纵向距离时，将其作为避障格路径，所述最小纵向距离是根据所述自车的速度和所述障碍物的速度得到。 6.根据权利要求1所述的方法，其特征在于，所述计算每个所述避障格路径的避障格代价，将避障格代价最小的避障格路径作为目标避障格路径，包括：当所述避障格为第一个避障格时，根据所述自车的位置和所述第一避障格的位置，得到所述自车与所述第一个避障格之间的横向距离；当所述避障格为第一个避障格之外的任一避障格时，根据所述任一避障格的位置以及与所述任一避障格相邻的避障格的位置，得到两相邻避障格之间的横向距离；将自车与所述第一个避障格之间的横向距离或两相邻避障格之间横向距离、所述障碍物的速度输入到第一公式中，得到所述避障格对应的避障格代价，所述第一公式为： χ＝χw×σw+χv×σv σw＝w/wm σv＝vb/vm 其中，w表示自车与第一个避障格之间的横向距离或两相邻避障格之间横向距离，vb为避障格的速度，wm为预先设定的距离值，vm为预先设定的速度值，σw为相邻避障格与自车之间横向距离对应的代价函数，σv表示避障格的速度对应的代价函数，χw为避障格与自车之间的横向距离对应的权重系数，χv为避障格的速度对应的权重系数，χ为避障格代价；将每个避障格路径中的避障格所对应的避障格代价相加，得到每个避障格路径的避障格代价；将所有避障格路径的避障格代价进行比较，得到避障格代价最小的避障格路径，将避障格代价最小的避障格路径作为目标避障格路径。 7.根据权利要求1所述的方法，其特征在于，所述对于所述目标避障格路径中所述自车经过的第一个目标避障格，根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置，得到所述第一个目标避障格的避障轨迹，包括：基于预设的标准时刻设置第一时长，基于标准的横摆角加速度设置横摆角加速度的预设值；将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中，得到角加速度，所述横摆角加速度公式为：其中，tb-ta＝td-tc＝te-td＝tg-tf＝ti-th＝tj-ti＝tl-tk＝T1，tc-tb＝tf-te＝th-tg＝tk-tj＝T2，T2＝5T1，ta为避障轨迹的开始时刻，tl为避障轨迹的结束时刻，tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻，T1为第一时长，T2为第二时长，为横摆角加速度的预设值，为t时刻的横摆角加速度；将横摆角加速度输入到横摆角速度公式中，得到横摆角速度，所述横摆角速度的公式为：其中，θd(ta)＝0，θd(ta)为ta时刻的横摆角速度，为ta的横摆角加速度，θd(t)为t时刻的横摆角速度；将所述自车的速度和所述横摆角速度输入到横向距离公式中，得到自车距离所述第一目标避障格预测的横向距离，所述横向距离公式为：其中，vz表示自车的车速，θd(t)为t时刻的横摆角速度，Sby为避障轨迹中自车距离所述第一目标避障格预测的横向距离，ta为避障轨迹的开始时刻，tl为避障轨迹的结束时刻；根据所述自车的位置和所述第一目标避障格的位置，得到所述自车与所述第一目标避障格之间实际的横向距离；判断所述自车距离所述第一目标避障格预测的横向距离是否等于所述自车与所述第一目标避障格之间实际的横向距离，当所述自车距离所述第一目标避障格预测的横向距离不等于所述自车与所述第一目标避障格之间实际的横向距离时，按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值，将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的设定值，再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中得到角加速度，直到所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离为止，所述初始值设定公式为：其中，为横摆角加速度的设定值，T1为第一时长，i依次取值1，2......n，n为常数，T1S为预设的标准时刻，为标准的横摆角加速度；当所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离时，获取所述自车距离所述第一目标避障格预测的横向距离等于所述自车与所述第一目标避障格之间实际的横向距离时的横摆角速度；将所述横摆角速度输入到避障轨迹公式中，得到自车的第一目标避障格的避障轨迹，所述避障轨迹公式为：其中，ta≤t≤tl，ta为避障轨迹的开始时刻，tl为避障轨迹的结束时刻，(x(t),y(t))表示t时刻的车辆质心的位置，vz表示自车的车速，θd(t)为t时刻的横摆角速度。 8.根据权利要求1所述的方法，其特征在于，所述对于目标避障格路径中自车经过的第一个目标避障格之外的任一个目标避障格，根据该任一个目标避障格的位置、在该任一个目标避障格之前自车经过的上一个目标避障格的位置和自车的速度，得到该任一个目标避障格的避障轨迹，包括：基于预设的标准时长设置第一时长，基于标准的横摆角加速度设置横摆角加速度的预设值；将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中，得到角加速度，所述横摆角加速度公式为：其中，tb-ta＝td-tc＝te-td＝tg-tf＝ti-th＝tj-ti＝tl-tk＝T1，tc-tb＝tf-te＝th-tg＝tk-tj＝T2，T2＝5T1，ta为避障轨迹的开始时刻，tl为避障轨迹的结束时刻，tb、tc、td、te、tf、tj、th、ti、tj和tk分别为位于避障轨迹的开始时刻和结束时刻之间的时刻、T1为第一时长、T2为第二时长，为横摆角加速度的预设值，为t时刻的横摆角加速度；将横摆角加速度输入到横摆角速度公式中，得到横摆角速度，所述横摆角速度的公式为：其中，θd(ta)＝0，θd(ta)为ta时刻的横摆角速度，为ta的横摆角加速度，θd(t)为t时刻的横摆角速度；将所述自车的速度和所述横摆角速度输入到横向距离公式中，得到两相邻目标避障格之间的预测的横向距离，所述横向距离公式为：其中，vz表示自车的车速，θd(t)为t时刻的横摆角速度，Sby为两相邻目标避障格之间的预测的横向距离，ta为避障轨迹的开始时刻，tl为避障轨迹的结束时刻；根据所述两相邻目标避障格的位置，得到所述两相邻目标避障格之间的实际的横向距离；判断所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时，当所述两相邻目标避障格的预测的横向距离不等于所述两相邻目标避障格之间实际的横向距离，按照初始值设定公式更新所述第一时长和所述横摆角加速度的预设值，将更新后的所述第一时长和更新后的所述横摆角加速度的预设值作为所述第一时长和所述横摆角加速度的预设值，再次将第一时长和所述横摆角加速度的预设值输入到横摆角加速度公式中，得到角加速度，直到所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离为止，所述初始值设定公式为：其中，为横摆角加速度的预设值，T1为第一时长，i依次取值1，2......n，n为常数，T1S为预设的标准时长，为标准的横摆角加速度；当所述两相邻目标避障格的预测的横向距离是否等于所述两相邻目标避障格之间实际的横向距离时，获取所述两相邻目标避障格的预测的横向距离等于所述两相邻目标避障格之间实际的横向距离时的横摆角速度；将所述横摆角速度输入到避障轨迹公式中，得到所述第一个目标避障格之外的任一个目标避障格的避障轨迹，所述避障轨迹公式为：其中ta≤t≤tl，ta为避障轨迹的开始时刻，tl为避障轨迹的结束时刻，(x(t),y(t))表示t时刻的车辆质心的位置，vz表示自车的车速，θd(t)为t时刻的横摆角速度。 9.根据权利要求1所述的方法，其特征在于，根据所述目标避障格路径中所有目标避障格对应的避障轨迹，得到所述目标避障格路径对应的目标避障轨迹，包括：将所述目标避障格对应的避障轨迹中相邻的避障轨迹之间通过直线相连，得到目标避障格路径对应的目标避障轨迹。 10.一种智能车避障轨迹规划装置，其特征在于，所述装置包括：获取模块，用于获取自车的位置、所述自车的速度、所述自车周围相关的障碍物的位置和每个所述障碍物的速度；第一确定模块，用于根据每个所述障碍物的位置得到所有的避障格的位置，每个所述避障格为与相对应的所述障碍物的一侧相距预设距离且无障碍物的预设大小的格子；第二确定模块，用于根据所述自车的位置、所述自车的速度、所述障碍物的速度和所述所有避障格的位置，得到至少一个避障格路径，每个所述避障格路径包括：从所述自车的当前位置起避过所有相关的所述障碍物所依次经过的每个所述避障格的顺序以及对应的所述避障格；计算模块，用于计算每个所述避障格路径的避障格代价，将避障格代价最小的避障格路径作为目标避障格路径；第三确定模块，用于对于所述目标避障格路径中所述自车经过的第一个目标避障格，根据所述自车的位置、所述自车的速度和所述第一个目标避障格的位置，得到所述第一个目标避障格的避障轨迹；第四确定模块，用于对于所述目标避障格路径中所述自车经过的第一个目标避障格之外的任一个目标避障格，根据该任一个目标避障格的位置、在该任一个目标避障格之前所述自车经过的上一个目标避障格的位置和所述自车的速度，得到该任一个目标避障格的避障轨迹；第五确定模块，用于根据所述目标避障格路径中所有目标避障格对应的避障轨迹，得到所述目标避障格路径对应的目标避障轨迹。
所属类别：	发明专利