CN116605231A - 纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质 - Google Patents

纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质 Download PDF

Info

Publication number
CN116605231A
CN116605231A CN202310553766.1A CN202310553766A CN116605231A CN 116605231 A CN116605231 A CN 116605231A CN 202310553766 A CN202310553766 A CN 202310553766A CN 116605231 A CN116605231 A CN 116605231A
Authority
CN
China
Prior art keywords
point
path
reference point
determining
planned path
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310553766.1A
Other languages
English (en)
Inventor
王路遥
于华洋
吕卫卫
陈坤煌
任令辉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shanghai Ofilm Intelligent Vehicle Co ltd
Original Assignee
Shanghai Ofilm Intelligent Vehicle Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shanghai Ofilm Intelligent Vehicle Co ltd filed Critical Shanghai Ofilm Intelligent Vehicle Co ltd
Priority to CN202310553766.1A priority Critical patent/CN116605231A/zh
Publication of CN116605231A publication Critical patent/CN116605231A/zh
Pending legal-status Critical Current

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W40/00—Estimation or calculation of non-directly measurable driving parameters for road vehicle drive control systems not related to the control of a particular sub unit, e.g. by using mathematical models
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B60—VEHICLES IN GENERAL
    • B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
    • B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units
    • B60W30/06—Automatic manoeuvring for parking
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)

Abstract

本申请提供一种纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质，所述预瞄点确定方法包括：获取所述车辆的当前位姿和规划路径；根据所述车辆的当前位姿与所述规划路径中的多个路径点的相对位置关系，从所述规划路径中的多个路径点中确定当前跟随点；根据规划路径的形状，从规划路径中排列在当前跟随点之后的路径点中确定第一参考点；以及根据第一参考点的位置确定预瞄点。本申请提供的预瞄点确定方法可以根据规划路径的形状选择合适的预瞄距离，可以避免出现预瞄距离过短造成车辆控制震荡的问题以及预瞄距离过长造成转向合理性偏低的问题，能够提升车辆跟随路径的鲁棒性。

Description

纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质
技术领域
本申请涉及自动泊车技术领域，尤其涉及一种纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质。
背景技术
APA(Auto Parking Assist System，自动泊车系统)功能是一种根据目标泊车位生成规划路径，然后根据规划路径、车辆状态数据以及车辆定位数据，对车辆进行路径跟踪控制(包括纵向的速度控制和横向的转向控制)，从而控制车辆跟踪规划路径运动至目标泊车位的功能。车辆在APA模式中，路径跟踪控制方法的鲁棒性直接影响运动过程中驾乘人员的安全性和舒适性。
目前，横向控制技术主要采用纯跟踪算法(Pure Pursuit)，现有的纯跟踪算法一般根据固定的预瞄距离确定预瞄点，经研究发现，现有的纯跟踪算法的鲁棒性较差：如果设置的预瞄距离过短，会造成车辆控制的不稳定甚至震荡，导致纯跟踪算法的控制稳定性不足；如果设置的预瞄距离过长，车辆在跟随转弯的规划路径时会出现内切的现象，即车辆在大转角处转向不足，导致纯跟踪算法的转向合理性偏低。
发明内容
有鉴于此，本申请的主要目的在于提出纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质，旨在解决现有的纯跟踪算法的鲁棒性较差的问题。
为实现上述目的，本申请的第一方面提供一种纯跟踪算法的预瞄点确定方法，应用于车辆，所述预瞄点确定方法获取所述车辆的当前位姿和规划路径；其中，所述规划路径包括多个路径点；
根据所述车辆的当前位姿与所述规划路径中的多个路径点的相对位置关系，从所述规划路径中的多个路径点中确定当前跟随点；
根据所述规划路径的形状，从所述规划路径中排列在所述当前跟随点之后的路径点中确定第一参考点；以及
根据所述第一参考点的位置确定所述预瞄点；
其中，所述路径点是指所述规划路径中的位置点；所述当前跟随点为所述规划路径中的多个路径点中位于所述车辆当前的运动方向的前方且距离所述车辆最近的一个路径点；所述第一参考点为基于所述当前跟随点，沿着所述规划路径往所述车辆当前的运动方向延伸一段距离的一个路径点，延伸距离与规划路径的形状相关；所述预瞄点为基于所述第一参考点确定出的当前目标点，用于计算出所述车辆的目标方向盘转角。
本申请提供的预瞄点确定方法，根据所述车辆的当前位姿与规划路径中的多个路径点的相对位置关系确定当前跟随点，并根据规划路径的形状，从所述当前跟随点往前延伸相应的距离得到第一参考点，再根据第一参考点的位置确定预瞄点，如此，可以实现根据规划路径的形状选择合适的延伸距离得到预瞄点，可以避免出现预瞄距离过短造成车辆控制震荡的问题以及预瞄距离过长造成转向合理性偏低的问题，能够提升车辆跟随路径的鲁棒性。
在一些实施例中，所述根据所述车辆的当前位姿与所述规划路径中的多个路径点的相对位置关系，从所述规划路径中的多个路径点中确定当前跟随点，包括：
以所述车辆的预设部位为坐标原点建立坐标系，确定所述规划路径中的各个路径点在所述坐标系中的位置坐标；
判断所述车辆当前的运动方向是前进还是后退；
若所述车辆当前的运动方向是前进，则将所述规划路径中的多个路径点中纵坐标大于零且纵坐标的绝对值最小的路径点确定为所述当前跟随点；其中，所述纵坐标的延伸方向为所述车辆的预设部位指向所述车辆的车头的方向；
若所述车辆当前的运动方向是后退，则将所述规划路径中的多个路径点中纵坐标小于零且纵坐标的绝对值最小的路径点确定为所述当前跟随点；
根据所述当前跟随点在所述规划路径中的位置确定所述第一跟随点；
判断所述当前跟随点是否为所述规划路径中的第一个路径点；
若所述当前跟随点为所述规划路径中的第一个路径点，则将所述规划路径中排列在所述当前跟随点之后的第一个路径点确定为第一跟随点；以及
若所述当前跟随点不为所述规划路径中的第一个路径点，则将所述当前跟随点确定为所述第一跟随点。
如此，根据所述当前跟随点是否为所述规划路径中的第一个跟随点，分为两种情况来选取第一跟随点，能够提升预瞄点的精准性。
在一些实施例中，所述规划路径的形状包括直线形、单侧转弯弧形以及S形，其中，所述单侧转弯弧形包括左转弯弧形和右转弯弧形；
所述根据所述规划路径的形状，从所述规划路径中排列在所述当前跟随点之后的路径点中确定第一参考点，包括：
判断所述规划路径的形状是直线形、单侧转弯弧形还是S形；
若所述规划路径的形状是直线形，则根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与L之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第L个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点；
若所述规划路径的形状是单侧转弯弧形，则根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与M之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点；以及
若所述规划路径的形状是S形，则根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与N之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第N个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点；其中，L为第一预设前移路径点个数，M为第二预设前移路径点个数，N为第三预设前移路径点个数，且L>M>0，L>N>0。
如此，设置L>M以及L>N，让所述车辆在跟随直线形的规划路径运动时，将预瞄点设置得更远，可以避免出现预瞄距离过短造成车辆控制震荡的问题；让所述车辆在跟随单侧转弯弧形或S形的规划路径运动时，将预瞄点设置得更近，可以避免出现预瞄距离过长造成转向合理性偏低的问题。
在一些实施例中，所述根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与L之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第L个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点，包括：
判断所述规划路径中排列在所述第一跟随点之后的路径点的个数是否大于L；
若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于L，则将所述规划路径中排列在所述第一跟随点之后的第L个路径点确定为所述第一参考点；以及
若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于L，则将所述规划路径中的最后一个路径点确定为所述第一参考点；
所述根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与M之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点，包括：
判断所述规划路径中排列在所述第一跟随点之后的路径点的个数是否大于M；
若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于M，则将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点；以及
若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于M，则将所述规划路径中的最后一个路径点确定为所述第一参考点；
所述根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与N之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第N个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点，包括：
判断所述规划路径中排列在所述第一跟随点之后的路径点的个数是否大于N；
若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于N，则将所述规划路径中排列在所述第一跟随点之后的第N个路径点确定为所述第一参考点；以及
若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于N，则将所述规划路径中的最后一个路径点确定为所述第一参考点。
如此，在所述车辆运动至接近所述规划路径的终点时，将最后一个路径点确定为所述第一参考点，可以避免出现无法确定预瞄点的情况。
在一些实施例中，所述根据所述当前跟随点在所述规划路径中的位置确定所述第一跟随点，还包括：
在确定第一跟随点之后，将所述规划路径中排列在所述当前跟随点之前的第一个路径点确定为第二跟随点；以及
计算所述第二跟随点指向所述车辆的当前位姿的向量在所述第二跟随点指向所述第一跟随点的向量上的第一投影距离；
所述根据所述第一参考点的位置确定所述预瞄点，包括：
判断所述第一参考点是否为所述规划路径中的最后一个路径点；
若所述第一参考点为所述规划路径中的最后一个路径点，则判断所述规划路径的形状是否为直线；
若所述规划路径的形状为直线，则将沿着所述第一参考点的朝向往前延伸第一预设距离的位置点确定为所述预瞄点；
若所述规划路径的形状不为直线，则根据所述规划路径中的路径点的总个数，将沿着所述第一参考点的朝向往前延伸第二预设距离的位置点确定为所述预瞄点，或者，将沿着所述第一参考点所在的转弯圆弧往前延伸第三预设距离的位置点确定为所述预瞄点，其中，第一预设距离>第二预设距离≥第三预设距离；以及
若所述第一参考点不为所述规划路径中的最后一个路径点，则根据所述第一投影距离确定所述预瞄点。
如此，在所述车辆运动至接近所述规划路径的终点时，从所述规划路径的最后一个路径点往前延伸得到所述预瞄点，从而解决车辆运动至最后一个路径点时丢失预瞄点的问题。
在一些实施例中，所述根据所述规划路径的形状，从所述规划路径中排列在所述当前跟随点之后的路径点中确定第一参考点，还包括：
在确定所述第一参考点之后，将所述规划路径中排列在所述第一参考点之后的第一个路径点确定为第二参考点；
所述根据所述第一投影距离确定所述预瞄点，包括：
判断所述第一投影距离是否大于0；
若所述第一投影距离小于或者等于0，则将所述第一参考点确定为所述预瞄点；以及
若所述第一投影距离大于0，则根据所述第一参考点的曲率、所述第二参考点的曲率以及所述第一投影距离，沿着所述第一参考点往所述第二参考点延伸得到所述预瞄点。
从而，在第一参考点不为所述规划路径中的最后一个路径点时，根据所述车辆的位姿与所述第一跟随点之间的位置关系以及所述第一参考点的位置，确定所述预瞄点，使得所述预瞄点随着所述车辆的运动而移动，从而解决车辆在跟随路径过程中方向盘的控制不线性、不平顺、出现卡顿的问题。
在一些实施例中，所述根据所述第一参考点的曲率、所述第二参考点的曲率以及所述第一投影距离，沿着所述第一参考点往所述第二参考点延伸得到所述预瞄点，包括：
判断所述第一参考点的曲率和所述第二参考点的曲率是否满足三个预设条件中的任意一个；
若所述第一参考点的曲率和所述第二参考点的曲率满足三个预设条件中的任意一个，则将从所述第一参考点开始，沿着所述第一参考点指向所述第二参考点的方向往前延伸第一投影距离的位置点确定为所述预瞄点；
若所述第一参考点的曲率和所述第二参考点的曲率不满足三个预设条件中的任何一个，则以所述第一参考点为最初的父节点沿着所述第一参考点到所述第二参考点的圆弧段，重复执行预瞄点搜索步骤得到所述预瞄点，其中，所述父节点是指执行所述执行预瞄点搜索步骤的起始点；
所述预瞄点搜索步骤，包括：
从所述父节点开始，按照预设步长在所述第一参考点到所述第二参考点的圆弧段上延伸，得到候选位置点；
判断所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离是否大于或者等于所述第一投影距离；
若所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离小于所述第一投影距离，则以所述候选位置点为新的父节点继续执行所述预瞄点搜索步骤；以及
若所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离大于或者等于所述第一投影距离，则将所述候选位置点确定为所述预瞄点，并结束所述预瞄点搜索步骤；
所述三个预设条件包括：
所述第一参考点的曲率的绝对值小于预设曲率阈值，且所述第一参考点的曲率与所述第二参考点的曲率之间的差值的绝对值小于第一预设曲率差值；
所述第一参考点的曲率与所述第二参考点的曲率之间的差值的绝对值大于第二预设曲率差值；以及
所述第一参考点的曲率与所述第二参考点的曲率的符号相反。
如此，可以减少运算量以及可以避免程序错误。
在一些实施例中，M的取值包括M1、M2以及M3；其中，M1>M2>M3；
所述将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点，包括：
判断所述规划路径中的路径点的总个数与第一预设个数阈值和第二预设个数阈值之间的大小关系；其中，所述第一预设个数阈值大于所述第二预设个数阈值；
若所述规划路径中的路径点的总个数大于所述第一预设个数阈值，则将所述规划路径中排列在所述第一跟随点之后的第M1个路径点确定为所述第一参考点；
若所述规划路径中的路径点的总个数小于或者等于所述第一预设个数阈值且大于所述第二预设个数阈值，则将所述规划路径中排列在所述第一跟随点之后的第M2个路径点确定为所述第一参考点；
若所述规划路径中的路径点的总个数小于或者等于所述第二预设个数阈值，则将所述规划路径中排列在所述第一跟随点之后的第M3个路径点确定为所述第一参考点。
如此，设置M1>M2>M3，让所述车辆在跟随单侧转弯弧形的规划路径运动时，根据路径点的总个数X，即根据所述规划路径的总长度来选取预瞄距离，在所述规划路径的总长度较短时，相应地选择较短的预瞄距离，可以让车辆更加紧密地跟随所述规划路径运动，从而可以进一步避免出现预瞄距离过长造成转向合理性偏低的问题。
本申请的第二方面还提供一种纯跟踪算法的预瞄点确定装置，所述纯跟踪算法的预瞄点确定装置包括存储器和处理器，所述存储器与所述处理器连接，所述存储器中存储有计算机程序，所述计算机程序被所述处理器执行时实现如上述第一方面所述的纯跟踪算法的预瞄点确定方法。
本申请的第三方面还提供一种车辆，所述车辆包括：
车体；以及
如上述第二方面所述的预瞄点确定装置，所述纯跟踪算法的预瞄点确定装置设于车体上，所述纯跟踪算法的预瞄点确定装置用于为所述车辆确定预瞄点，从而基于所述预瞄点控制所述车辆运动。
本申请的第四方面还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现如上述第一方面所述的纯跟踪算法的预瞄点确定方法。
本申请提供的纯跟踪算法的预瞄点确定装置、车辆以及存储介质，同样可以实施上述第一方面所述的纯跟踪算法的预瞄点确定方法，根据所述车辆的当前位姿与规划路径中的多个路径点的相对位置关系确定当前跟随点，并根据规划路径的形状，从所述当前跟随点往前延伸相应的距离得到第一参考点，再根据第一参考点的位置确定预瞄点，如此，可以实现根据规划路径的形状选择合适的延伸距离得到预瞄点，可以避免出现预瞄距离过短造成车辆控制震荡的问题以及预瞄距离过长造成转向合理性偏低的问题，能够提升车辆跟随路径的鲁棒性。
本申请的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。
附图说明
图1为本申请实施例提供的车辆的目标方向盘转角的计算示意图；
图2为本申请实施例提供的纯跟踪算法的预瞄点确定方法的步骤流程图；
图3为图2中步骤20的细化流程图；
图4为图3中步骤25的第一种细化流程图；
图5a为本申请实施例提供的第一种场景下的预瞄点的确定规则示意图；
图5b为本申请实施例提供的第二种场景下的预瞄点的确定规则示意图；
图5c为本申请实施例提供的第三种场景下的预瞄点的确定规则示意图；
图5d为本申请实施例提供的第四种场景下的预瞄点的确定规则示意图；
图5e为本申请实施例提供的第五种场景下的预瞄点的确定规则示意图；
图6为图2中步骤31的第一种细化流程图；
图7为图2中步骤31的第二种细化流程图；
图8为图7中步骤3132的细化流程图；
图9为图7中步骤3142的细化流程图；
图10为图2中步骤32的第一种细化流程图；
图11为图3中步骤25的第二种细化流程图；
图12为图2中步骤32的第二种细化流程图；
图13为图2中步骤31的第三种细化流程图；
图14为图12中步骤326的第二种细化流程图；
图15为图14中步骤3263的第二种细化流程图；
图16为本申请实施例提供的预瞄点搜索步骤的流程图；
图17是本申请实施例提供的预瞄点搜索步骤的场景示意图；
图18为本申请实施例提供的纯跟踪算法的预瞄点确定装置的示意图；
图19为本申请实施例提供的车辆的示意图。
附图标记说明如下：
车辆 1
预瞄点确定装置 60
存储器 61
处理器 62
计算机程序 610
车体 80
候选位置点D2 D1～D3
子圆弧段Y1～Y4
如下具体实施方式将结合上述附图进一步说明本申请。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。根据本申请中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本申请的描述中，需要说明的是，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性。
在APA(Auto Parking Assist System，自动泊车系统)功能的运行过程中，需要控制车辆跟随已经生成的规划路径运动，具体如下：路径跟随(Path Follow,PF)模块根据路径规划(Path Plan,PP)模块给出的路径信息、车辆状态数据以及车辆定位数据，对车辆纵向的速度和横向的转向分别进行控制，确保车辆跟随PP模块生成的规划路径运动。其中，PF模块通过向EPS(Electric Power Steering，电动助力转向系统)输出一个目标方向盘转角，再由EPS控制方向盘跟随目标方向盘转角，从而实现对车辆横向的转向的控制。
现有的PF模块的横向控制主要采用纯跟踪算法，在纯跟踪算法中，需要计算车辆的后轴中心连接车辆的预瞄点的圆弧曲率。其中，预瞄点的位置由预瞄距离ld确定。如图1所示，仅使用车辆航向方向与预瞄点方向之间的角度α，即可确定车辆的转向角δ。将正弦定律应用于图1，可得到如下公式：
将上述公式写成曲率形式，则为：
其中，κ为后轴轨迹曲率。另一方面，根据车辆轴距H以及PID(Proportion-Integral-Derivative，比例-积分-微分控制器)控制前馈角度的计算公式，可得：
δ＝arctan(kH) (3)
将κ的表达式代入，就可以得到纯跟踪算法的控制方程为：
根据横向偏差的定义可得：
则曲率公式可改写为：
那么，纯跟踪算法的控制方程可改写为：
根据纯跟踪算法的控制方程(7)可以看出，由纯跟踪算法得到的车辆的转向角δ与横向偏差和预瞄距离ld相关。现有的纯跟踪算法的预瞄距离ld是定值，这就导致车辆跟随路径的鲁棒性较差：如果设置的预瞄距离ld过短，会造成车辆控制的不稳定甚至震荡，导致纯跟踪算法的控制稳定性不足；如果设置的预瞄距离ld过长，车辆在跟随转弯的规划路径时会出现内切的现象，即车辆在大转角处转向不足，导致纯跟踪算法的转向合理性偏低。
有鉴于此，请参阅图2，本申请实施例提供一种纯跟踪算法的预瞄点确定方法，所述纯跟踪算法的预瞄点确定方法包括以下步骤：
步骤10，获取车辆的当前位姿和规划路径。其中，所述规划路径包括多个路径点。
步骤20，根据所述车辆的当前位姿与所述规划路径中的多个路径点的相对位置关系，从所述规划路径中的多个路径点中确定当前跟随点。
步骤31，根据所述规划路径的形状，从所述规划路径中排列在所述当前跟随点之后的路径点中确定第一参考点。
步骤32，根据所述第一参考点的位置确定所述预瞄点。
示例性地，所述规划路径可以是车辆中的路径规划模块生成的，所述规划路径中任意两个相邻的路径点之间的距离均相等，例如可以为0.3m。
需要说明的是，在本申请中，所述路径点是指车辆进行路径规划时虚拟地设定的位置点(通过点)，所述当前跟随点为所述规划路径中的多个路径点中位于所述车辆当前的运动方向的前方且距离所述车辆距离最近的路径点，用于指示所述车辆的当前位姿在所述规划路径中的大致位置。所述第一参考点为基于所述当前跟随点，沿着所述规划路径往所述车辆当前的运动方向延伸一段距离的一个路径点，为初步的预瞄点，用于为精确地确定出所述预瞄点提供参考，其中，延伸距离与规划路径的形状相关。所述预瞄点为基于所述第一参考点确定出的当前目标点，用于计算出所述车辆的目标方向盘转角，进而可以控制车辆跟随规划路径运动。
本申请提供的预瞄点确定方法，根据所述车辆的当前位姿与规划路径中的多个路径点的相对位置关系确定当前跟随点，并根据规划路径的形状，从所述当前跟随点往前延伸相应的距离得到第一参考点，再根据第一参考点的位置确定预瞄点，如此，可以实现根据规划路径的形状选择合适的延伸距离得到预瞄点，可以避免出现预瞄距离过短造成车辆控制震荡的问题以及预瞄距离过长造成转向合理性偏低的问题，能够提升车辆跟随路径的鲁棒性。
需要说明的是，在本实施例中，术语“位姿”包括所述车辆的位置以及车头朝向角度。
进一步地，请参阅图3，所述步骤20包括步骤21～步骤25，具体如下：
步骤21，以所述车辆的预设部位为坐标原点建立坐标系，确定所述规划路径中的各个路径点在所述坐标系中的位置坐标。
步骤22，判断所述车辆当前的运动方向是前进还是后退。若所述车辆当前的运动方向是前进，则执行步骤23。若所述车辆当前的运动方向是后退，则执行步骤24。
步骤23，将所述规划路径中的多个路径点中纵坐标大于零且纵坐标的绝对值最小的路径点确定为所述当前跟随点。
步骤24，将所述规划路径中的多个路径点中纵坐标小于零且纵坐标的绝对值最小的路径点确定为所述当前跟随点。
步骤25，根据所述当前跟随点在所述规划路径中的位置确定所述第一跟随点。
在本申请实施例中，所述车辆的预设部位为所述车辆的后轴中心，所述坐标系为直角坐标系，其中，直角坐标系的纵坐标的延伸方向为所述车辆的后轴中心指向所述车辆的车头的方向，直角坐标系的横坐标的延伸方向与纵坐标的延伸方向垂直。各个路径点相对于所述车辆的位置关系是指相对于所述车辆的后轴中心的位置关系，例如，某一路径点位于所述车辆当前的运动方向的前方是指所述车辆的后轴中心指向该路径点的方向与所述车辆当前的运动方向大致相同，即两个方向之间的夹角小于90度。当然，在其他实施例中，也可以以其他点(例如所述规划路径中的第一个路径点)建立坐标系确定所述车辆的当前跟随点，此处不作限定。
可以理解的是，当所述车辆的运动方向为前进时，纵坐标大于零且纵坐标的绝对值最小的路径点即为位于所述车辆的前方且与所述车辆距离最近的路径点；当所述车辆的运动方向为后退时，纵坐标小于零且纵坐标的绝对值最小的路径点即为位于所述车辆的后方且与所述车辆距离最近的路径点。如此，根据所述车辆的运动方向来确定当前跟随点，可以提升预瞄点的精准性。
在一种实施例中，请参阅图4，所述步骤25包括步骤251～步骤253，具体如下：
步骤251，判断所述当前跟随点是否为所述规划路径中的第一个路径点。若所述当前跟随点为所述规划路径中的第一个路径点，则执行步骤252。若所述当前跟随点不为所述规划路径中的第一个路径点，则执行步骤253。
步骤252，将所述规划路径中排列在所述当前跟随点之后的第一个路径点确定为所述第一跟随点。
步骤253，将所述当前跟随点确定为所述第一跟随点。
示例性地，在图5a所示的场景下，所述规划路径包括依次编号的12个路径点，所述车辆的运动方向为向前，第1个路径点即为所述车辆的当前跟随点，第二个路径点即为所述第一跟随点。在图5b所示的场景下，所述规划路径包括依次编号的16个路径点，所述车辆的运动方向为向前，第1个路径点的纵坐标刚好为0，第2个路径点的纵坐标大于零且纵坐标的绝对值最小，因此，第2个路径点即为所述车辆的当前跟随点，也即所述第一跟随点。在本申请中，在当前跟随点为所述规划路径中的第一个路径点时，将所述规划路径中排列在所述当前跟随点之后的第一个路径点，即第2个路径点确定为所述第一跟随点；在当前跟随点为所述规划路径中的第一个路径点时，将当前跟随点确定为所述第一跟随点，并再从所述第一跟随点沿着所述车辆当前的运动方向往前延伸一段距离，得到第一参考点。需要说明的是，车辆在进行路径规划时得到的路径点是基于车辆的初始位姿、车速等车辆信息，往车辆的运动方向往前延伸拓展得到的一系列虚拟位置点，因此，所述规划路径中的第1个路径点位于车辆的初始位姿的前方。
在本申请实施例中，所述规划路径的形状包括直线形、单侧转弯弧形以及S形，其中，所述单侧转弯弧形包括左转弯弧形和右转弯弧形。示例性地，可以根据所述规划路径中各个路径点对应的曲率来判断所述规划路径的形状，例如，当各个路径点的曲率均接近零时(即曲率的绝对值均小于或者预设曲率阈值κ0时)即可确定所述规划路径的形状为直线形(如图5a所示)，当各个路径点中的若干个路径点的曲率大于κ0时即可确定所述规划路径的形状为右转弯弧形(如图5b所示)，当各个路径点中的若干个路径点的曲率大于κ0，另外若干个路径点的曲率小于-κ0时即可确定所述规划路径的形状为S形(如图5c所示)，其中，κ0>0，例如，κ0＝0.02。一个路径点的曲率即为车辆运动至该路径点时的后轴轨迹曲率。
进一步地，本申请实施例中，在执行步骤31时，在所述规划路径的形状为直线形时，从所述第一跟随点开始沿着所述车辆当前的运动方向往前延伸第一延伸距离(即从所述第一跟随点往前延伸L个路径点对应的距离)，得到第一参考点；在所述规划路径的形状为单侧转弯弧形时，从所述第一跟随点开始沿着所述车辆当前的运动方向往前延伸第二延伸距离(即从所述第一跟随点往前延伸M个路径点对应的距离)，得到第一参考点；在所述规划路径的形状为S形时，从所述第一跟随点开始沿着所述车辆当前的运动方向往前延伸第三延伸距离(即从所述第一跟随点往前延伸N个路径点对应的距离)，得到第一参考点。其中，L为第一预设前移路径点个数，M为第二预设前移路径点个数，N为第三预设前移路径点个数。其中，第一延伸距离大于第二延伸距离，且第一延伸距离大于第三延伸距离。示例性地，L取值为8，相邻两个路径点之间的距离为0.3m，那么，所述第一延伸距离即为2.4m。
具体地，请参阅图6，所述步骤31包括步骤311～步骤314：
步骤311，判断规划路径的形状是直线形、单侧转弯弧形还是S形。若所述规划路径的形状是直线形，则执行步骤312。若所述规划路径的形状是单侧转弯弧形，则执行步骤313。若所述规划路径的形状是S形，则执行步骤314。
步骤312，根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与L之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第L个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点。
步骤313，根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与M之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点。
步骤314，根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与N之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第N个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点。
其中，L>M>0，L>N>0。
如此，设置L>M以及L>N，让所述车辆在跟随直线形的规划路径运动时，从所述第一跟随点前移更多路径点得到第一参考点，从而将预瞄点设置得更远，可以避免出现预瞄距离过短造成车辆控制震荡的问题；让所述车辆在跟随单侧转弯弧形或S形的规划路径运动时，从所述第一跟随点前移更多路径点得到第一参考点，从而将预瞄点设置得更近，可以避免出现预瞄距离过长造成转向合理性偏低的问题。
进一步地，请参阅图7，所述步骤312包括步骤3121～步骤3123，所述步骤313包括步骤3131～步骤3133，所述步骤314包括步骤3141～步骤3143，具体如下：
步骤3121，判断规划路径中排列在第一跟随点之后的路径点的个数是否大于L。若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于L，则执行步骤3122。若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于L，则执行步骤3123。
步骤3122，将所述规划路径中排列在所述第一跟随点之后的第L个路径点确定为所述第一参考点。
步骤3123，将所述规划路径中的最后一个路径点确定为所述第一参考点。
步骤3131，判断规划路径中排列在第一跟随点之后的路径点的个数是否大于M。若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于M，则执行步骤3132。若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于M，则执行所述步骤3123。
步骤3132，将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点。
步骤3141，判断规划路径中排列在第一跟随点之后的路径点的个数是否大于N。若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于N，则执行步骤3142。若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于N，则执行所述步骤3123。
步骤3142，将所述规划路径中排列在所述第一跟随点之后的第N个路径点确定为所述第一参考点。
示例性地，在图5c-图5d所示的场景下，假设N＝2，所述规划路径的形状为S形，且包括依次编号的12个路径点，所述车辆的运动方向为向前。如图5c所示，此时，以所述车辆的后轴中心为坐标原点建立坐标系，规划路径中的多个路径点中第6个路径点的纵坐标大于零且纵坐标的绝对值最小，因此，第6个路径点即为所述车辆的当前跟随点，也即所述第一跟随点。此时，第一跟随点之后的路径点的个数为6，大于N，因此，第一跟随点之后的第2个路径点，即第8个路径点为第一参考点。
如图5d所示，此时，以所述车辆的后轴中心为坐标原点建立坐标系，规划路径中的多个路径点中第11个路径点的纵坐标大于零且纵坐标的绝对值最小，因此，第11个路径点即为所述车辆的当前跟随点，也即所述第一跟随点。此时，第一跟随点之后的路径点的个数为1，小于N，因此，所述规划路径中的最后一个路径点，即第12个路径点为第一参考点。
如此，在所述车辆运动至接近所述规划路径的终点时，将最后一个路径点确定为所述第一参考点，可以避免出现无法确定预瞄点的情况。
需要说明的是，此处以规划路径的形状为S形为例，对确定第一参考点的步骤进行举例说明，而不应理解为对本申请的限定。同理，在本申请实施例的其他各步骤中，当规划路径为其他形状时，确定第一参考点的步骤的示例也类似，此处不一一赘述。
进一步地，本申请实施例中，在执行步骤3132时，具体根据规划路径中的路径点的总个数X与第一预设个数阈值X1和第二预设个数阈值X2之间的大小关系确定M的取值，其中，M的取值包括M1、M2以及M3，M1>M2>M3。如此，让所述车辆在跟随单侧转弯弧形的规划路径运动时，根据路径点的总个数X，即根据所述规划路径的总长度来选取预瞄距离，在所述规划路径的总长度较短时，相应地选择较短的预瞄距离，可以让车辆更加紧密地跟随所述规划路径运动，从而可以进一步避免出现预瞄距离过长造成转向合理性偏低的问题。
具体地，请参阅图8，所述步骤3132包括步骤31321～步骤31324：
步骤31321，判断所述规划路径中的路径点的总个数X与第一预设个数阈值X1和第二预设个数阈值X2之间的大小关系。若所述规划路径中的路径点的总个数X大于所述第一预设个数阈值X1，则执行步骤31322。若所述规划路径中的路径点的总个数X小于或者等于所述第一预设个数阈值X1且大于所述第二预设个数阈值X2，则执行步骤31323。若所述规划路径中的路径点的总个数X小于或者等于所述第二预设个数阈值X2，则执行步骤31324。
其中，所述第一预设个数阈值X1大于所述第二预设个数阈值X2。
步骤31322，将所述规划路径中排列在所述第一跟随点之后的第M1个路径点确定为所述第一参考点。
步骤31323，将所述规划路径中排列在所述第一跟随点之后的第M2个路径点确定为所述第一参考点。
步骤31324，将所述规划路径中排列在所述第一跟随点之后的第M3个路径点确定为所述第一参考点。
示例性地，X1＝15，X2＝6，M1＝6，M2＝3，M3＝1。
需用说明的是，X1、X2、M1、M2、M3的取值可以根据车辆的性能、规划路径中相邻两个路径点之间的距离灵活选择，可以通过试验获得，本申请实施例中所例举的具体数值仅作示例，以便于理解本申请所提供的实施例，并不代表仅能选用本申请所例举的数值，不应当视为对于本申请的限制。
进一步地，本申请实施例中，在执行步骤3142时，具体根据车辆的运行模式确定N的取值。其中，N的取值包括N1以及N2。N1<N2。如此，当所述车辆的运行模式为自动泊车模式时，N取较小值N1，可以使得所述车辆更紧密地跟踪所述规划路线运行，从而能够提升自动泊车的安全性。当所述车辆的运行模式不为自动泊车模式时，N取较大值N2，可以减小运算量。
具体地，请参阅图9，所述步骤3142包括步骤31421～步骤31423：
步骤31421，判断所述车辆当前的运行模式是否为自动泊车模式。若所述车辆当前的运行模式为自动泊车模式，则执行步骤31422。若所述车辆当前的运行模式不为自动泊车模式，则执行步骤31423。
步骤31422，将所述规划路径中排列在所述第一跟随点之后的第N1个路径点确定为所述预瞄点。
步骤31423，将所述规划路径中排列在所述第一跟随点之后的第N2个路径点确定为所述预瞄点。
示例性地，N1＝4，N2＝2。
需用说明的是，N1、N2的取值可以根据车辆的性能、规划路径中相邻两个路径点之间的距离灵活选择，可以通过试验获得，本申请实施例中所例举的具体数值仅作示例，以便于理解本申请所提供的实施例，并不代表仅能选用本申请所例举的数值，不应当视为对于本申请的限制。
进一步地，在一种实施例中，所述步骤32包括：将所述第一参考点确定为所述预瞄点。本实施例直接将第一参考点确定为预瞄点，控制简单。
经研究发现，当所述车辆运动至接近所述规划路径的最后一个路径点时，如果将预瞄点一直选定为所述规划路径的最后一个路径点，那么，当车辆运动至最后一个路径点时，所述车辆将丢失预瞄点，从而出现方向盘转角不受控的问题。
有鉴于此，请参阅图10，在另一种实施例中，在所述车辆运动至接近所述规划路径的终点时，从所述规划路径的最后一个路径点往前延伸得到所述预瞄点，从而解决车辆运动至最后一个路径点时丢失预瞄点的问题。
在本实施例中，所述步骤32包括步骤321～步骤325，具体如下：
步骤321，判断所述第一参考点是否为所述规划路径中的最后一个路径点。若所述第一参考点为所述规划路径中的最后一个路径点，则执行步骤322。若所述第一参考点不为所述规划路径中的最后一个路径点，则执行步骤325。
步骤322，判断所述规划路径的形状是否为直线。若所述规划路径的形状为直线，则执行步骤323。若所述规划路径的形状不为直线，则执行步骤324。
步骤323，将沿着所述第一参考点的朝向往前延伸第一预设距离L10的位置点确定为所述预瞄点。
步骤324，根据所述规划路径中的路径点的总个数X，将沿着所述第一参考点的朝向往前延伸第二预设距离L20的位置点确定为所述预瞄点；或者，将沿着所述第一参考点所在的转弯圆弧往前延伸第三预设距离L30的位置点确定为所述预瞄点。其中，L10>L20≥L30。其中，所述第一参考点的朝向是指所述车辆达到所述第一参考点时车头的朝向。
步骤325，将所述第一参考点确定为所述预瞄点。
进一步地，所述步骤324具体包括：当X≥X3时，将沿着所述第一参考点的朝向往前延伸第二预设距离L20的位置点确定为所述预瞄点。当X<X3时，将沿着所述第一参考点所在的转弯圆弧往前延伸第三预设距离L30的位置点确定为所述预瞄点。
示例性地，X3＝5，L10＝2m，L20＝L30＝0.8m。
需用说明的是，X3、L10、L20以及L30的取值可以根据车辆的性能、规划路径中相邻两个路径点之间的距离灵活选择，可以通过试验获得，本申请实施例中所例举的具体数值仅作示例，以便于理解本申请所提供的实施例，并不代表仅能选用本申请所例举的数值，不应当视为对于本申请的限制。
如此，设置L10>L20≥L30，让所述车辆在跟随直线形的规划路径运动并接近规划路径的最后一个路径点时，将最后一个路径点往前延伸较远距离L10的位置点确定为预瞄点，可以避免出现预瞄距离过短造成车辆控制震荡的问题；让所述车辆在跟随单侧转弯弧形或S形的规划路径运动并接近规划路径的最后一个路径点时，将最后一个路径点往前延伸较近距离L20或L30的位置点确定为预瞄点，可以避免出现预瞄距离过长造成转向合理性偏低的问题。此外，根据经验总结，当所述规划路径中的路径点的总个数X小于X3时，通常为将方向盘转角设置到最大(即将方向盘打到底)的情况，因此，当X<X3，将沿着所述第一参考点所在的转弯圆弧往前延伸第三预设距离L30的位置点确定为所述预瞄点，可以确保所述车辆在到达所述规划路径的最后一个路径点时方向盘转角仍然保持为当前状态，即将方向盘打到底的状态。
经研究发现，如果直接将所述规划路径中的路径点确定为所述预瞄点，会出现车辆在跟随路径过程中方向盘的控制不线性、不平顺、出现卡顿的问题，具体原因如下：以图5b为例，当车辆从第1个路径点朝向第2个路径点运动的过程中，第一跟随点即为第2个路径点，相应地，预瞄点一直都为第8个路径点而不会随着车辆的运动而移动。而当车辆到达第2个路径点时，第一跟随点则变为第3个路径点，相应地，预瞄点则会从第8个路径点跳变至第9个路径点，那么，车辆的方向盘转角会发生突变，从而导致方向盘的控制不线性、不平顺、出现卡顿的问题，影响用户体验。
有鉴于此，请参阅图11～图12，在又一种实施例中，在第一参考点不为所述规划路径中的最后一个路径点时，根据所述车辆的位姿与所述第一跟随点之间的位置关系以及所述第一参考点的位置，确定所述预瞄点，使得所述预瞄点随着所述车辆的运动而移动，从而解决车辆在跟随路径过程中方向盘的控制不线性、不平顺、出现卡顿的问题。
在本实施例中，所述步骤25包括步骤251～步骤255，所述步骤32包括步骤321～步骤324、步骤326，具体如下：
步骤251，判断所述当前跟随点是否为所述规划路径中的第一个路径点。若所述当前跟随点为所述规划路径中的第一个路径点，则执行步骤252。若所述当前跟随点不为所述规划路径中的第一个路径点，则执行步骤253。
步骤252，将所述规划路径中排列在所述当前跟随点之后的第一个路径点确定为所述第一跟随点。
步骤253，将所述当前跟随点确定为所述第一跟随点。
在执行完步骤252或步骤253之后，继续执行步骤254。
步骤254，在确定第一跟随点之后，将所述规划路径中排列在所述当前跟随点之前的第一个路径点确定为第二跟随点。
步骤255，计算所述第二跟随点指向所述车辆的当前位姿的向量在所述第二跟随点指向所述第一跟随点的向量上的第一投影距离L1。
步骤321，判断所述第一参考点是否为所述规划路径中的最后一个路径点。若所述第一参考点为所述规划路径中的最后一个路径点，则执行步骤322。若所述第一参考点不为所述规划路径中的最后一个路径点，则执行步骤326。
步骤322，判断所述规划路径的形状是否为直线。若所述规划路径的形状为直线，则执行步骤323。若所述规划路径的形状不为直线，则执行步骤324。
步骤323，将沿着所述第一参考点的朝向往前延伸第一预设距离L10的位置点确定为所述预瞄点。
步骤324，根据所述规划路径中的路径点的总个数X，将沿着所述第一参考点的朝向往前延伸第二预设距离L20的位置点确定为所述预瞄点；或者，将沿着所述第一参考点所在的转弯圆弧往前延伸第三预设距离L30的位置点确定为所述预瞄点。
步骤326，根据所述第一投影距离L1确定所述预瞄点。
进一步地，请参阅图13，所述步骤31包括步骤311～步骤315，其中步骤311～步骤314已经在前文中进行介绍，此处不再赘述。在执行完步骤312、步骤313或者步骤314其中任一个之后，继续执行步骤315，所述步骤315包括：
在确定所述第一参考点之后，将所述规划路径中排列在所述第一参考点之后的第一个路径点确定为第二参考点。
如前文所述，如果直接将所述规划路径中的路径点确定为所述预瞄点，会出现车辆在跟随路径过程中方向盘的控制不线性、不平顺、出现卡顿的问题。因此，本申请实施例中，在确定所述第一参考点之后，将所述规划路径中排列在所述第一参考点之后的第一个路径点确定为第二参考点，并且在执行步骤326时，具体根据所述第一投影距离L1，将所述第一参考点确定为所述预瞄点；或者，沿着所述第一参考点往所述第二参考点延伸得到所述预瞄点，如此，使得预瞄点会随着车辆的运动而实时更新，从而可以避免出现预瞄点从一个路径跳变至另一个路径点的情况，从而使得所述车辆在跟随路径过程中方向盘的控制更加线性、更加平顺。
具体地，请参阅图14，所述步骤326包括步骤3261～步骤3263：
步骤3261，判断所述第一投影距离L1是否大于0。若所述第一投影距离L1小于或者等于0，则执行步骤3262；若所述第一投影距离L1大于0，则执行步骤3263。
步骤3262，将所述第一参考点确定为所述预瞄点。
步骤3263，根据所述第一参考点的曲率、所述第二参考点的曲率以及所述第一投影距离L1，沿着所述第一参考点往所述第二参考点延伸得到所述预瞄点。
需要说明的是，车辆在进行路径规划时，会模拟出车辆运动至规划路径中各个路径点时的转向角，从而可以根据转向角确定各个路径点的曲率。
可以理解的是，第一投影距离L1小于0是指所述第二跟随点指向所述车辆的当前位姿的向量与所述第二跟随点指向所述第一跟随点的向量之间的夹角大于90度，例如图5a所示，此时，第1个路径点位于所述车辆当前运动方向的前方，所述第二跟随点(即第1个路径点)指向所述车辆的当前位姿的向量与所述第二跟随点指向所述第一跟随点(即第2个路径点)的向量之间的夹角刚好等于180度，因此，L1小于0。
为便于理解，下面分别以图5a和图5e所示的场景为例介绍本实施例，如图5a所示，所述规划路径的形状为直线形，假设所述车辆的运动方向为前进，所述规划路径中的路径点的总个数X＝12，L＝8。此时，以所述车辆的后轴中心为坐标原点建立坐标系，规划路径中的多个路径点中第1个路径点的纵坐标大于零且纵坐标的绝对值最小，因此，第1个路径点即为所述车辆的当前跟随点，从而，第1个路径点之后的第一个路径点即为第2个路径点为第一跟随点，那么，第一跟随点之前的第一个路径点，即第1个路径点为所述第二跟随点。由于规划路径的形状为直线形，因此，排列在所述第一跟随点(即第2个路径点)之后的第8个路径点，即第10个路径点为所述第一参考点，那么，排列在所述第一参考点之后的第一个路径点，即第11个路径点为所述第二参考点。由于L1<0，因此，所述第一参考点(即第10个路径点)为预瞄点。并且，在所述车辆运动至第1个路径点的过程中，预瞄点一直保持为第10个路径点。当所述车辆运动至第1个路径点至第2个路径点之间时，此时，以所述车辆的后轴中心为坐标原点建立坐标系，规划路径中的多个路径点中第2个路径点的纵坐标大于零且纵坐标的绝对值最小，因此，第2个路径点即为所述车辆的当前跟随点，也即为第一跟随点，那么，所述第一跟随点之前的第一个路径点，即第1个路径点为所述第二跟随点。也就是说第一跟随点和第二跟随点保持不变，相应地，第一参考点和第二参考点也保持不变，即第10个路径点为所述第一参考点、第11个路径点为所述第二参考点。那么，由于此时L1>0，因此，根据所述第一参考点的曲率、所述第二参考点的曲率以及所述第一投影距离L1，沿着第10个路径点往第11个路径点延伸的到所述预瞄点，如此，当所述车辆运动至第1个路径点之前时，所述预瞄点一直为第10个路径点，当所述车辆在从第1个路径点运动朝向第2个路径点运动的过程中，所述预瞄点的位置会随着所述车辆的运动而移动，即随着所述第一投影距离L1的增大而往第11个路径点靠近，那么，在所述车辆的预瞄点会实时更新而不会出现从一个路径点跳变至另一个路径点的情况，从而使得所述车辆在跟随路径过程中方向盘的控制更加线性、更加平顺。
相比之下，如果直接以当前跟随点为基准往所述车辆当前的运动方向延伸一段距离得到预瞄点，那么，当所述车辆运动至第1个路径点之前时，当前跟随点为第1个路径点，排列在所述当前跟随点(即第1个路径点)之后的第8个路径点，即第9个路径点为所述第一参考点，由于此时L1<0，因此，所述第一参考点(即第9个路径点)为预瞄点。之后，当所述车辆刚好到达第1个路径点时，此时，以所述车辆的后轴中心为坐标原点建立坐标系，第1个路径点的纵坐标为0，因此，第2个路径点的纵坐标大于零且纵坐标的绝对值最小，为当前跟随点，也即为第一跟随点，相应的，此时的第一参考点则跳变为第10个路径点，由于此时L1＝0，因此，所述第一参考点(即第10个路径点)为预瞄点。也就是说，所述车辆在从第1个路径点之前运动至第1个路径点的过程中，会出现预瞄点突变的情况，而本申请实施例中，在当前跟随点为第1个路径点时，将第1个路径点之后的第一个路径点确定为所述第一跟随点，再以第一跟随点为基准往所述车辆当前的运动方向延伸一段距离得到预瞄点，能够避免出现预瞄点突变的问题。
如图5e所示，所述规划路径的形状为右转弯弧形，假设所述车辆的运动方向为前进，所述规划路径中的路径点的总个数X＝17>X1＝15，M1＝6。此时，以所述车辆的后轴中心为坐标原点建立坐标系，规划路径中的多个路径点中第2个路径点的纵坐标大于零且纵坐标的绝对值最小，因此，第2个路径点即为所述车辆的当前跟随点，也即所述第一跟随点，那么，所述当前跟随点之前的第一个路径点，即第1个路径点为所述第二跟随点。由于规划路径的形状为右转弯弧形，且所述规划路径中的路径点的总个数X>X1，因此，排列在所述第一跟随点(即第2个路径点)之后的第6个路径点，即第8个路径点为所述第一参考点，那么，排列在所述第一参考点之后的第一个路径点，即第9个路径点为所述第二参考点。由于L1>0，因此，根据所述第一参考点的曲率、所述第二参考点的曲率以及所述第一投影距离L1，沿着第8个路径点往第9个路径点延伸的到所述预瞄点，如此，当所述车辆在从第一个路径点运动朝向第二个路径点运动的过程中，所述预瞄点的位置会随着所述车辆的运动而移动，即随着所述第一投影距离L1的增大而往第9个路径点靠近，那么，在所述车辆的预瞄点会实时更新而不会出现从一个路径点跳变至另一个路径点的情况，从而使得所述车辆在跟随路径过程中方向盘的控制更加线性、更加平顺。
需要说明的是，此处以规划路径的形状为右转弯弧形为例，对确定预瞄点的步骤进行举例说明，而不应理解为对本申请的限定。同理，在本申请实施例的其他各步骤中，当规划路径为其他形状时，确定预瞄点的步骤的示例也类似，此处不一一赘述。
进一步地，请参阅图15，所述步骤3263包括步骤32631～步骤32633，具体如下：
步骤32631，判断所述第一参考点的曲率和所述第二参考点的曲率是否满足三个预设条件中的任意一个。若所述第一参考点的曲率和所述第二参考点的曲率满足三个预设条件中的任意一个，则执行步骤32632。若所述第一参考点的曲率和所述第二参考点的曲率不满足三个预设条件中的任何一个，则执行步骤32633。
步骤32632，将从所述第一参考点开始，沿着所述第一参考点指向所述第二参考点的方向往前延伸第一投影距离L1的位置点确定为所述预瞄点。
步骤32633，以所述第一参考点为最初的父节点沿着所述第一参考点到所述第二参考点的圆弧段，重复执行预瞄点搜索步骤得到所述预瞄点。其中，所述父节点是指执行所述执行预瞄点搜索步骤的起始点，即从所述父节点开始进行预瞄点搜索。
在本申请实施例中，所述三个预设条件包括条件A～C，具体如下：
条件A：所述第一参考点的曲率的绝对值小于预设曲率阈值，且所述第一参考点的曲率与所述第二参考点的曲率之间的差值的绝对值小于第一预设曲率差值。
条件B：所述第一参考点的曲率与所述第二参考点的曲率之间的差值的绝对值大于第二预设曲率差值。
条件C：所述第一参考点的曲率与所述第二参考点的曲率的符号相反。
示例性地，所述预设曲率阈值为0.02，所述第一预设曲率差值为0.005，所述第二预设曲率差值为0.03。
需要说明的是，所述预设曲率阈值、所述第一预设曲率差值以及所述第二预设曲率差值的取值可以通过试验获得，本申请实施例中所例举的具体数值仅作示例，以便于理解本申请所提供的实施例，并不代表仅能选用本申请所例举的数值，不应当视为对于本申请的限制。
如图5e所示，假设所述第一参考点(即第8个路径点)的曲率和所述第二参考点(即第9个路径点)的曲率满足条件B，那么，从第8个路径点开始，沿着第8个路径点指向第9个路径点的方向往前延伸L1即可得到所述预瞄点。
需要说明的是，在满足条件A时，即表明所述从所述第一参考点至所述第二参考点的路径为近似直线；在满足条件B时，即表明所述从所述第一参考点至所述第二参考点，曲率变化较大，从所述第一参考点开始沿着指向所述第二参考点的直线方向往前延伸第一投影距离L1，得到所述预瞄点，如此，可以减少运算量。在满足条件C时，即表明路径点的曲率是从正变为负，或者从负变为正，即这段路径中间存在曲率为零的点，而曲率为零时对应的转弯半径为无穷大，如此，可能导致程序错误。本实施例中在满足条件C时，从所述第一参考点开始沿着指向所述第二参考点的直线方向往前延伸第一投影距离L1，得到所述预瞄点，可以避免程序错误。
进一步地，请参阅图16，所述预瞄点搜索步骤包括以下步骤：
步骤S1，从所述父节点开始，按照预设步长在所述第一参考点到所述第二参考点的圆弧段上延伸，得到候选位置点。
步骤S2，判断所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离L2是否大于或者等于所述第一投影距离L1。若所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离L2小于所述第一投影距离L1，则执行步骤S3，执行完步骤S3之后继续执行步骤S1。若所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离L2大于或者等于所述第一投影距离L1，则执行步骤S4。
步骤S3，以所述候选位置点为新的父节点。
步骤S4，将所述候选位置点确定为所述预瞄点，并结束所述预瞄点搜索步骤。
在本实施例中，从第一参考点到所述第二参考点的圆弧段由T个子圆弧段依次连接而成，所述T个子圆弧段的长度均等于所述预设步长，所述T个子圆弧段的曲率按照预设曲率步长Δκ依次递增或递减，其中，所述第一参考点的曲率κ1、所述第二参考点的曲率κ2以及预设曲率步长Δκ之间存在以下关系：
κ1+T*Δκ＝κ2
所述预设步长可以通过试验获得，示例性地，所述预设步长可以为0.05m。为便于理解，以图17为例进行说明，从第一参考点到所述第二参考点的圆弧段由子圆弧段Y1～Y4依次连接而成，子圆弧段Y1上的点的曲率均为κ1，子圆弧段Y2上的点的曲率均为κ1+Δκ，子圆弧段Y3上的点的曲率均为κ1+2*Δκ，子圆弧段Y4上的点的曲率均为κ1+3*Δκ，所述第二参考点κ2＝κ1+4*Δκ。执行所述预瞄点搜索步骤的示例如下：以所述第一参考点为父节点执行所述预瞄点搜索步骤，得到候选位置点D1，由于所述第一参考点指向D1的向量在所述第一参考点指向所述第二参考点的向量上的投影距离小于L1，再次以D1为父节点执行所述预瞄点搜索步骤，得到候选位置点D2，由于所述第一参考点指向D1的向量在所述第一参考点指向所述第二参考点的向量上的投影距离小于L1，再次以D2为父节点执行所述预瞄点搜索步骤，得到候选位置点D3，由于所述第一参考点指向D1的向量在所述第一参考点指向所述第二参考点的向量上的投影距离大于L1，因此，D3即为所述预瞄点。
可选地，所述纯跟踪算法的预瞄点确定方法还可以包括：在确定所述预瞄点之后，由纯跟踪算法的控制方程计算出所述车辆运动至所述预瞄点所需的目标方向盘转角，使得EPS控制方向盘跟随目标方向盘转角，从而实现对车辆横向的转向的控制。
请参阅图18，本申请实施例还提供一种纯跟踪算法的预瞄点确定装置60，所述预瞄点确定装置60包括存储器61和处理器62，所述存储器61与所述处理器62连接，所述存储器61中存储有计算机程序610。所述处理器62执行所述计算机程序610时实现上述方法实施例中所述的纯跟踪算法的预瞄点确定方法的步骤。
可以理解的是，所述示意图18仅仅是预瞄点确定装置60的示例，并不构成对所述预瞄点确定装置60的限定，所述预瞄点确定装置60可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如，所述预瞄点确定装置60还可以包括输入输出设备等。
所述处理器62可以是中央处理单元(Central Processing Unit，CPU)，还可以包括其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application SpecificIntegrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable GateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者所述处理器也可以是任何常规的处理器等，所述处理器62是所述预瞄点确定装置60的控制中心，利用各种接口和线路连接整个纯跟踪算法的预瞄点确定装置60的各个部分。
所述存储器61可用于存储所述计算机程序610，所述处理器62通过运行或执行存储在所述存储器61内的计算机程序610，以及调用存储在存储器61内的数据，实现所述预瞄点确定装置60的各种功能。存储器61可以包括外部存储介质，也可以包括内存。此外，存储器61可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(FlashCard)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
本申请提供的纯跟踪算法的预瞄点确定装置通过采用上述纯跟踪算法的预瞄点确定方法，根据所述车辆的当前位姿与规划路径中的多个路径点的相对位置关系确定当前跟随点，并根据规划路径的形状，从所述当前跟随点往前延伸相应的距离得到第一参考点，再根据第一参考点的位置确定预瞄点，如此，可以实现根据规划路径的形状选择合适的延伸距离得到预瞄点，可以避免出现预瞄距离过短造成车辆控制震荡的问题以及预瞄距离过长造成转向合理性偏低的问题，能够提升车辆跟随路径的鲁棒性。
请参阅图19，本申请还提供一种车辆1，包括如上述实施例所述的预瞄点确定装置60和车体80，所述预瞄点确定装置60设于所述车体80上，所述预瞄点确定装置60用于为所述车辆1确定预瞄点。
本申请提供的车辆中的纯跟踪算法的预瞄点确定装置通过采用上述纯跟踪算法的预瞄点确定方法，根据所述车辆的当前位姿与规划路径中的多个路径点的相对位置关系确定当前跟随点，并根据规划路径的形状，从所述当前跟随点往前延伸相应的距离得到第一参考点，再根据第一参考点的位置确定预瞄点，如此，可以实现根据规划路径的形状选择合适的延伸距离得到预瞄点，可以避免出现预瞄距离过短造成车辆控制震荡的问题以及预瞄距离过长造成转向合理性偏低的问题，能够提升车辆跟随路径的鲁棒性。
本申请还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法实施例中所述纯跟踪算法的预瞄点确定方法的步骤。
本申请的所述预瞄点确定装置60如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
对于本领域技术人员而言，显然本申请不限于上述示范性实施例的细节，而且在不背离本申请的精神或基本特征的情况下，能够以其他的具体形式实现本申请。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本申请的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本申请内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由同一个单元或装置通过软件或者硬件来实现。
尽管已经示出和描述了本申请的实施例，本领域的普通技术人员可以理解：在不脱离本申请的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本申请的范围由权利要求及其等同物限定。

Claims (11)

1.一种纯跟踪算法的预瞄点确定方法，应用于车辆，其特征在于，包括：
获取所述车辆的当前位姿和规划路径；其中，所述规划路径包括多个路径点；
根据所述车辆的当前位姿与所述规划路径中的多个路径点的相对位置关系，从所述规划路径中的多个路径点中确定当前跟随点；
根据所述规划路径的形状，从所述规划路径中排列在所述当前跟随点之后的路径点中确定第一参考点；以及
根据所述第一参考点的位置确定所述预瞄点；
其中，所述路径点是指所述规划路径中的位置点；所述当前跟随点为所述规划路径中的多个路径点中位于所述车辆当前的运动方向的前方且距离所述车辆最近的一个路径点；所述第一参考点为基于所述当前跟随点，沿着所述规划路径往所述车辆当前的运动方向延伸一段距离的一个路径点，延伸距离与规划路径的形状相关；所述预瞄点为基于所述第一参考点确定出的当前目标点，用于计算出所述车辆的目标方向盘转角。
2.如权利要求1所述的纯跟踪算法的预瞄点确定方法，其特征在于，所述根据所述车辆的当前位姿与所述规划路径中的多个路径点的相对位置关系，从所述规划路径中的多个路径点中确定当前跟随点，包括：
以所述车辆的预设部位为坐标原点建立坐标系，确定所述规划路径中的各个路径点在所述坐标系中的位置坐标；
判断所述车辆当前的运动方向是前进还是后退；
若所述车辆当前的运动方向是前进，则将所述规划路径中的多个路径点中纵坐标大于零且纵坐标的绝对值最小的路径点确定为所述当前跟随点；其中，所述纵坐标的延伸方向为所述车辆的预设部位指向所述车辆的车头的方向；
若所述车辆当前的运动方向是后退，则将所述规划路径中的多个路径点中纵坐标小于零且纵坐标的绝对值最小的路径点确定为所述当前跟随点；
判断所述当前跟随点是否为所述规划路径中的第一个路径点；
若所述当前跟随点为所述规划路径中的第一个路径点，则将所述规划路径中排列在所述当前跟随点之后的第一个路径点确定为第一跟随点；以及
若所述当前跟随点不为所述规划路径中的第一个路径点，则将所述当前跟随点确定为所述第一跟随点。
3.如权利要求2所述的纯跟踪算法的预瞄点确定方法，其特征在于，所述规划路径的形状包括直线形、单侧转弯弧形以及S形，其中，所述单侧转弯弧形包括左转弯弧形和右转弯弧形；
所述根据所述规划路径的形状，从所述规划路径中排列在所述当前跟随点之后的路径点中确定第一参考点，包括：
判断所述规划路径的形状是直线形、单侧转弯弧形还是S形；
若所述规划路径的形状是直线形，则根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与L之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第L个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点；
若所述规划路径的形状是单侧转弯弧形，则根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与M之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点；以及
若所述规划路径的形状是S形，则根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与N之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第N个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点；
其中，L为第一预设前移路径点个数，M为第二预设前移路径点个数，N为第三预设前移路径点个数，且L>M>0，L>N>0。
4.如权利要求3所述的纯跟踪算法的预瞄点确定方法，其特征在于，所述根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与L之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第L个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点，包括：
判断所述规划路径中排列在所述第一跟随点之后的路径点的个数是否大于L；
若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于L，则将所述规划路径中排列在所述第一跟随点之后的第L个路径点确定为所述第一参考点；以及
若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于L，则将所述规划路径中的最后一个路径点确定为所述第一参考点；
所述根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与M之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点，包括：
判断所述规划路径中排列在所述第一跟随点之后的路径点的个数是否大于M；
若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于M，则将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点；以及
若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于M，则将所述规划路径中的最后一个路径点确定为所述第一参考点；
所述根据所述规划路径中排列在所述第一跟随点之后的路径点的个数与N之间的大小关系，将所述规划路径中排列在所述第一跟随点之后的第N个路径点确定为所述第一参考点，或者，将所述规划路径中的最后一个路径点确定为所述第一参考点，包括：
判断所述规划路径中排列在所述第一跟随点之后的路径点的个数是否大于N；
若所述规划路径中排列在所述第一跟随点之后的路径点的个数大于N，则将所述规划路径中排列在所述第一跟随点之后的第N个路径点确定为所述第一参考点；以及
若所述规划路径中排列在所述第一跟随点之后的路径点的个数小于或者等于N，则将所述规划路径中的最后一个路径点确定为所述第一参考点。
5.如权利要求3所述的纯跟踪算法的预瞄点确定方法，其特征在于，所述根据所述当前跟随点在所述规划路径中的位置确定所述第一跟随点，还包括：
在确定第一跟随点之后，将所述规划路径中排列在所述当前跟随点之前的第一个路径点确定为第二跟随点；以及
计算所述第二跟随点指向所述车辆的当前位姿的向量在所述第二跟随点指向所述第一跟随点的向量上的第一投影距离；
所述根据所述第一参考点的位置确定所述预瞄点，包括：
判断所述第一参考点是否为所述规划路径中的最后一个路径点；
若所述第一参考点为所述规划路径中的最后一个路径点，则判断所述规划路径的形状是否为直线；
若所述规划路径的形状为直线，则将沿着所述第一参考点的朝向往前延伸第一预设距离的位置点确定为所述预瞄点；
若所述规划路径的形状不为直线，则根据所述规划路径中的路径点的总个数，将沿着所述第一参考点的朝向往前延伸第二预设距离的位置点确定为所述预瞄点，或者，将沿着所述第一参考点所在的转弯圆弧往前延伸第三预设距离的位置点确定为所述预瞄点，其中，第一预设距离>第二预设距离≥第三预设距离；以及
若所述第一参考点不为所述规划路径中的最后一个路径点，则根据所述第一投影距离确定所述预瞄点。
6.如权利要求5所述的纯跟踪算法的预瞄点确定方法，其特征在于，所述根据所述规划路径的形状，从所述规划路径中排列在所述当前跟随点之后的路径点中确定第一参考点，还包括：
在确定所述第一参考点之后，将所述规划路径中排列在所述第一参考点之后的第一个路径点确定为第二参考点；
所述根据所述第一投影距离确定所述预瞄点，包括：
判断所述第一投影距离是否大于0；
若所述第一投影距离小于或者等于0，则将所述第一参考点确定为所述预瞄点；以及
若所述第一投影距离大于0，则根据所述第一参考点的曲率、所述第二参考点的曲率以及所述第一投影距离，沿着所述第一参考点往所述第二参考点延伸得到所述预瞄点。
7.如权利要求6所述的纯跟踪算法的预瞄点确定方法，其特征在于，所述根据所述第一参考点的曲率、所述第二参考点的曲率以及所述第一投影距离，沿着所述第一参考点往所述第二参考点延伸得到所述预瞄点，包括：
判断所述第一参考点的曲率和所述第二参考点的曲率是否满足三个预设条件中的任意一个；
若所述第一参考点的曲率和所述第二参考点的曲率满足三个预设条件中的任意一个，则将从所述第一参考点开始，沿着所述第一参考点指向所述第二参考点的方向往前延伸第一投影距离的位置点确定为所述预瞄点；
若所述第一参考点的曲率和所述第二参考点的曲率不满足三个预设条件中的任何一个，则以所述第一参考点为最初的父节点沿着所述第一参考点到所述第二参考点的圆弧段，重复执行预瞄点搜索步骤得到所述预瞄点，其中，所述父节点是指执行所述执行预瞄点搜索步骤的起始点；
所述预瞄点搜索步骤，包括：
从所述父节点开始，按照预设步长在所述第一参考点到所述第二参考点的圆弧段上延伸，得到候选位置点；
判断所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离是否大于或者等于所述第一投影距离；
若所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离小于所述第一投影距离，则以所述候选位置点为新的父节点继续执行所述预瞄点搜索步骤；以及
若所述第一参考点指向所述候选位置点的向量在所述第一参考点指向所述第二参考点的向量上的第二投影距离大于或者等于所述第一投影距离，则将所述候选位置点确定为所述预瞄点，并结束所述预瞄点搜索步骤；
所述三个预设条件包括：
所述第一参考点的曲率的绝对值小于预设曲率阈值，且所述第一参考点的曲率与所述第二参考点的曲率之间的差值的绝对值小于第一预设曲率差值；
所述第一参考点的曲率与所述第二参考点的曲率之间的差值的绝对值大于第二预设曲率差值；以及
所述第一参考点的曲率与所述第二参考点的曲率的符号相反。
8.如权利要求3所述的纯跟踪算法的预瞄点确定方法，其特征在于，M的取值包括M1、M2以及M3；其中，M1>M2>M3；
所述将所述规划路径中排列在所述第一跟随点之后的第M个路径点确定为所述第一参考点，包括：
判断所述规划路径中的路径点的总个数与第一预设个数阈值和第二预设个数阈值之间的大小关系；其中，所述第一预设个数阈值大于所述第二预设个数阈值；
若所述规划路径中的路径点的总个数大于所述第一预设个数阈值，则将所述规划路径中排列在所述第一跟随点之后的第M1个路径点确定为所述第一参考点；
若所述规划路径中的路径点的总个数小于或者等于所述第一预设个数阈值且大于所述第二预设个数阈值，则将所述规划路径中排列在所述第一跟随点之后的第M2个路径点确定为所述第一参考点；
若所述规划路径中的路径点的总个数小于或者等于所述第二预设个数阈值，则将所述规划路径中排列在所述第一跟随点之后的第M3个路径点确定为所述第一参考点。
9.一种纯跟踪算法的预瞄点确定装置，其特征在于，所述纯跟踪算法的预瞄点确定装置包括存储器和处理器，所述存储器与所述处理器连接，所述存储器中存储有计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1至8中任意一项所述的纯跟踪算法的预瞄点确定方法。
10.一种车辆，其特征在于，所述车辆包括：
车体；以及
如权利要求9所述的纯跟踪算法的预瞄点确定装置，所述纯跟踪算法的预瞄点确定装置设于车体上，所述纯跟踪算法的预瞄点确定装置用于为所述车辆确定预瞄点，从而基于所述预瞄点控制所述车辆运动。
11.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至8中任意一项所述的纯跟踪算法的预瞄点确定方法。
CN202310553766.1A 2023-05-16 2023-05-16 纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质 Pending CN116605231A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310553766.1A CN116605231A (zh) 2023-05-16 2023-05-16 纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310553766.1A CN116605231A (zh) 2023-05-16 2023-05-16 纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质

Publications (1)

Publication Number Publication Date
CN116605231A true CN116605231A (zh) 2023-08-18

Family

ID=87675896

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310553766.1A Pending CN116605231A (zh) 2023-05-16 2023-05-16 纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质

Country Status (1)

Country Link
CN (1) CN116605231A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117519158A (zh) * 2023-11-14 2024-02-06 霞智科技有限公司 一种基于贝塞尔曲线的机器人充电站对桩算法

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117519158A (zh) * 2023-11-14 2024-02-06 霞智科技有限公司 一种基于贝塞尔曲线的机器人充电站对桩算法

Similar Documents

Publication Publication Date Title
US20200269873A1 (en) Method and apparatus for planning speed of autonomous vehicle, and storage medium
EP3699048B1 (en) Travelling track prediction method and device for vehicle
CN111830979B (zh) 一种轨迹优化方法和装置
CN107585163B (zh) 移动路线生成装置以及移动路线生成方法
JP6722616B2 (ja) 駐車支援装置
JP6854095B2 (ja) 駐車支援装置
CN110836671B (zh) 轨迹规划方法、轨迹规划装置、存储介质与电子设备
CN111071247A (zh) 自动驾驶控制装置以及自动驾驶路径运算方法
EP3922949A1 (en) Path planning method and path planning system
CN110398963A (zh) 无轨导航纠偏控制方法、装置、存储介质及控制器
KR20190021930A (ko) 주행 차로 변경 시 경로를 생성하는 시스템 및 방법
CN116605231A (zh) 纯跟踪算法的预瞄点确定方法、装置、车辆以及存储介质
CN113548041B (zh) 应用于垂直车位的泊车控制方法、电子设备及车辆
CN112327830A (zh) 车辆自动驾驶变道轨迹的规划方法及电子设备
CN112677959A (zh) 一种泊车的方法和装置
CN115309170A (zh) 一种考虑舒适性约束的轨迹规划方法、装置和系统
CN113335270A (zh) 一种泊车路径规划方法和装置
CN112882476A (zh) 用于控制agv车体转向的控制方法及控制装置
CN116476840B (zh) 变道行驶方法、装置、设备及存储介质
CN115014380A (zh) 泊车路径规划方法以及装置、电子设备、存储介质
CN114506314A (zh) 车辆平行泊出方法、装置、设备及存储介质
Qin et al. A feasible parking algorithm in form of path planning and following
US20230347930A1 (en) Apparatus for Generating U-Turn Path of Vehicle and Method Thereof
CN117002490B (zh) 辅助泊车方法、装置、计算机设备及存储介质
CN114296452B (zh) 自动泊车路径规划方法、设备及计算机可读存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination