CN114701488A - 基于横纵向解耦的自动泊车路径跟踪方法 - Google Patents

基于横纵向解耦的自动泊车路径跟踪方法 Download PDF

Info

Publication number
CN114701488A
CN114701488A CN202210345718.9A CN202210345718A CN114701488A CN 114701488 A CN114701488 A CN 114701488A CN 202210345718 A CN202210345718 A CN 202210345718A CN 114701488 A CN114701488 A CN 114701488A
Authority
CN
China
Prior art keywords
point
target
pntid
calculating
vehicle
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
CN202210345718.9A
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.)
Chongqing Changan Automobile Co Ltd
Original Assignee
Chongqing Changan Automobile 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 Chongqing Changan Automobile Co Ltd filed Critical Chongqing Changan Automobile Co Ltd
Priority to CN202210345718.9A priority Critical patent/CN114701488A/zh
Publication of CN114701488A publication Critical patent/CN114701488A/zh
Pending legal-status Critical Current

Links

Images

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
    • 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, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
    • B60W30/06—Automatic manoeuvring for parking

Abstract

本发明公开了一种基于横纵向解耦的自动泊车路径跟踪方法，包括根据车辆实时位置坐标和目标点位置坐标计算车辆实时位置到目标点的距离DTH，并将所计算出的距离输出到车辆纵向控制模块；利用目标轨迹和实时车辆航向角计算航向角误差EYaw、利用目标轨迹和实时车辆位置坐标计算横向位置误差EPos、利用目标轨迹计算目标曲率Curvature，并将所计算出的航向角误差、横向位置误差、目标曲率结果输出到车辆横向控制模块。本方法能够根据规划好的路径精确地进行跟踪控制，实时地对车辆姿态调整，从而使得车辆精准地泊到目标位置。

Description

基于横纵向解耦的自动泊车路径跟踪方法
技术领域
本发明属于自动驾驶、自动泊车领域，具体涉及一种基于横纵向解耦的自动泊车路径跟踪方法。
背景技术
随着智能技术的发展，很多领域都开始尝试自动化技术，汽车行业也逐渐趋于智能化，最为重要和尖端的两项技术就是自动驾驶和自动泊车技术了。其中自动泊车因为可以显著提高泊车的安全性和是舒适度，所以早就在很多车型中得到量产。目前，众多主机厂和供应商都在研究自动泊车技术，但各自的成熟度和泊车效果及体验还是参差不齐。路径跟踪作为自动泊车重要环节之一，不仅影响着泊车的精度，而且也影响泊车过程中方向盘的转动，从而对泊车体验也很重要。
目前，公开号为114148320的中国专利公开了一种路径跟踪控制方法及设备；所述方法包括：基于模糊控制算法，通过根据获取当前车速和当前路径曲率来计算基准预瞄距离，通过根据获取到的车辆行驶偏差和车辆行驶偏差变化率来计算得到调整预瞄距离，进而根据调整预瞄距离对基准预瞄距离进行补偿计算，以得到目标预瞄距离。上述路径跟踪控制仅仅从车辆行驶偏差和车辆行驶偏差变化率来计算调整预瞄距离后，计算获得目标预瞄距离。仅仅从角度上考量，存在控制结果不够精确的问题。
发明内容
针对上述现有技术的不足，本发明所要解决的技术问题是：如何提供一种能够为车辆泊车过程中纵向控制系统和横向控制系统实时输出目标位置、偏差信息，精确控制车辆路径，使车辆泊车位置更精准的自动泊车路径跟踪方法。
为了解决上述技术问题，本发明采用了如下的技术方案：
一种基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，包括根据车辆实时位置坐标和目标点位置坐标计算车辆实时位置到目标点的距离DTH，并将所计算出的距离输出到车辆纵向控制模块；利用目标轨迹和实时车辆航向角计算航向角误差EYaw、利用目标轨迹和实时车辆位置坐标计算横向位置误差EPos、利用目标轨迹计算目标曲率Curvature，并将所计算出的航向角误差、横向位置误差、目标曲率结果输出到车辆横向控制模块。
进一步的，在计算航向角误差、横向位置误差、目标曲率前，先判定所规划的路径是否更新，若路径更新，则判定路径是否有效；当路径无效时，则结束路径跟踪，当路径有效时，执行如下步骤：先根据目标轨迹的中更新的路径点利用三点计算曲率的方法计算出每一个点的目标曲率，并根据路径点利用反正切函数计算每一个点的目标航向角，然后，再遍历所有的路径点的x、y坐标，匹配出离当前车辆位置最近的路径点，最后，计算出当前位置匹配的目标轨迹点(xPntId,yPntId)。
进一步的，所匹配出的离当前车辆位置最近的路径点通过如下方式计算而出：先计算出所有轨迹点的坐标和当前位置坐标，然后，分别计算出各个轨迹点x坐标与当前位置x坐标之差的绝对值distX、各个轨迹点y坐标与当前位置y坐标之差的绝对值distY，最后，并求出每个点对应的distX、distY之和dist，dist值最小对应的轨迹点就是当前位置匹配到的目标轨迹点(xPntId,yPntId)；具体的，计算公式如下：
distX(i)＝|xi-x|
distY(i)＝|yi-y|
dist(i)＝distX(i)+distY(i)。
进一步的，计算航向角误差EYaw的步骤如下：1)、根据目标轨迹点的x、y坐标计算出每个点的目标航向角heading，公式如下：
dx＝xi+1-xi
dy＝yi+1-yi，
当dx大于等于0时，则heading＝tan-1(dy/dx)，当dx小于0时，如果tan-1(dy/dx)＞0，则heading＝-π+tan-1(dy/dx)，如果tan-1(dy/dx)＜＝0，则heading＝π+tan-1(dy/dx)；
2)、结合目标轨迹点(xPntId,yPntId)和目标航向角heading，根据对应点的目标航向角headingPntId和车辆的实时航向角VehAngle，计算出航向角误差EYaw，具体计算公式如下：
EYaw＝headingPntId-VehAngle。
进一步的，横向位置误差EPos的计算步骤如下：根据当前的车辆位置坐标P0(VehX,VehY)，结合目标轨迹点P1(xPntId,yPntId)和目标航向角headingPntId，计算出位于目标轨迹线中与目标轨迹相切点P2的坐标：
(xPntId+cos(headingPntId),yPntId+sin(headingPntId))；
横向位置误差EPos即为P0点到P1、P2线段的距离，具体计算公式如下：
deltX＝VehX-xPntId
deltY＝VehY-yPntId
EPos＝sin(headingPntId)*deltX-cos(headingPntId)*deltY
式中，deltX为当前车辆位置x坐标和路径上最近点x坐标之差，deltY为当前车辆位置y坐标和路径上最近点y坐标之差。
进一步的，计算目标轨迹中其中一个坐标点(x2,y2)的目标曲率Curvature时，先选择在目标轨迹上位于该点之前一个点(x1，y1)，位于该点之后的一个点(x3，y3)，最后，再根据所选定的三点坐标，计算坐标点(x2,y2)的目标曲率Curvature，计算公式如下：
num＝2*((x2-x1)*(y3-y2)-(y2-y1)*(x3-x2)) (1)
Figure BDA0003576458870000041
Curvature＝num/den；式中，num为公式(1)的结果，den为公式(2)的结果。
进一步的，计算车辆实时位置到目标点的距离DTH的步骤如下：基于向量内积，输入车辆当前的位置坐标(VehX,VehY)、目标轨迹的结束点坐标(EndPoseX,EndPoseY)和结束点的航向角StepEndPose，采用如下公式计算：
e＝[cos(StepEndPose),sin(StepEndPose)]
Figure BDA0003576458870000042
Figure BDA0003576458870000043
式中，StepEndPose为结束点的航行角，e为沿着结束点航向角方向的方向向量，
Figure BDA0003576458870000044
为当前位置到结束点的向量。
进一步的，当目标路径为圆弧时，所计算的DTH比实际的DTH小。
进一步的，所述航向角误差EYaw、位置误差EPos、目标曲率Curvature和DTH均是在档位为前进挡时所计算的结果，若档位为后退档，上述所有计算结果都要乘以-1。
与现有技术相比，本发明得到的基于横纵向解耦的自动泊车路径跟踪方法具有如下优点：
1、能够根据规划好的路径精确地进行跟踪控制，实时地对车辆姿态调整，从而使得车辆精准地泊到目标位置。
2、不仅能够进一步减小车辆与规划的路径横纵向误差，横向偏差10cm，角度2°以内，提高了泊车精度，而且方向盘转动更加平滑，从而改善自动泊车效果和体验。
附图说明
图1为实施例中自动泊车路径跟踪方法的流程图；
图2为路径跟踪模块被激活之后，且路径更新情况下的路径跟踪流程；
图3为实施例中车辆实时位置到目标点的距离计算示意图。
具体实施方式
下面结合附图和实施例对本发明进一步说明。
实施例：
如图1、图2所示，本实施例提供的基于横纵向解耦的自动泊车路径跟踪方法，包括根据车辆实时位置坐标和目标点位置坐标计算车辆实时位置到目标点的距离DTH，并将所计算出的距离输出到车辆纵向控制模块；利用目标轨迹和实时车辆航向角计算航向角误差EYaw、利用目标轨迹和实时车辆位置坐标计算横向位置误差EPos、利用目标轨迹计算目标曲率Curvature，并将所计算出的航向角误差、横向位置误差、目标曲率结果输出到车辆横向控制模块。
进一步的，在计算航向角误差、横向位置误差、目标曲率前，先判定所规划的路径是否更新，若路径更新，则判定路径是否有效；当路径无效时，则结束路径跟踪，当路径有效时，执行如下步骤：先根据目标轨迹的中更新的路径点利用三点计算曲率的方法计算出每一个点的目标曲率，并根据路径点利用反正切函数计算每一个点的目标航向角，然后，再遍历所有的路径点的x、y坐标，匹配出离当前车辆位置最近的路径点，最后，计算出当前位置匹配的目标轨迹点(xPntId,yPntId)。
具体的，当路径无更新时，先直接遍历所有的路径点的x、y坐标，匹配出离当前车辆位置最近的目标轨迹点(xPntId,yPntId)后，再计算航向角误差、横向位置误差和目标曲率。
进一步的，所匹配出的离当前车辆位置最近的路径点通过如下方式计算而出：先计算出所有轨迹点的坐标和当前位置坐标，然后，分别计算出各个轨迹点x坐标与当前位置x坐标之差的绝对值distX、各个轨迹点y坐标与当前位置y坐标之差的绝对值distY，最后，并求出每个点对应的distX、distY之和dist，dist值最小对应的轨迹点就是当前位置匹配到的目标轨迹点(xPntId,yPntId)；具体的，计算公式如下：
distX(i)＝|xi-x|
distY(i)＝|yi-y|
dist(i)＝distX(i)+distY(i)。
进一步的，计算航向角误差EYaw的步骤如下：1)、根据目标轨迹点的x、y坐标计算出每个点的目标航向角heading，公式如下：
dx＝xi+1-xi
dy＝yi+1-yi，
当dx大于等于0时，则heading＝tan-1(dy/dx)，当dx小于0时，如果tan-1(dy/dx)＞0，则heading＝-π+tan-1(dy/dx)，如果tan-1(dy/dx)＜＝0，则heading＝π+tan-1(dy/dx)；
2)、结合匹配到的目标轨迹点(xPntId,yPntId)和目标航向角heading，根据对应点的目标航向角headingPntId和车辆的实时航向角VehAngle，计算出航向角误差EYaw，具体计算公式如下：
EYaw＝headingPntId-VehAngle。
具体的，上述横向位置误差EPos的计算步骤如下：根据当前的车辆位置坐标P0(VehX,VehY)，结合目标轨迹点P1(xPntId,yPntId)和目标航向角headingPntId，计算出位于目标轨迹线中与目标轨迹相切点P2的坐标：
(xPntId+cos(headingPntId),yPntId+sin(headingPntId))；
横向位置误差EPos即为P0点到P1、P2线段的距离，具体计算公式如下：
deltX＝VehX-xPntId
deltY＝VehY-yPntId
EPos＝sin(headingPntId)*deltX-cos(headingPntId)*deltY
式中，deltX为当前车辆位置x坐标和路径上最近点x坐标之差，deltY为当前车辆位置y坐标和路径上最近点y坐标之差。
进一步的，计算目标轨迹中其中一个坐标点(x2,y2)的目标曲率Curvature时，先选择在目标轨迹上位于该点之前一个点(x1，y1)，位于该点之后的一个点(x3，y3)，最后，再根据所选定的三点坐标，计算坐标点(x2,y2)的目标曲率Curvature，计算公式如下：
num＝2*((x2-x1)*(y3-y2)-(y2-y1)*(x3-x2)) (1)
Figure BDA0003576458870000071
Curvature＝num/den；式中，num为公式(1)的结果，den为公式(2)的结果。。
如图2所示，A是起点，C是终点，灰色弧形线条即为路径规划输出的目标轨迹，其中AB是一段圆弧，BC是与圆弧相切的一段直线，CE是沿着结束点航向角StepEndPose方向的方向向量。的值为，用此方法计算出来的跟实际的关系为：当路径是直线时，是准确的，当路径是圆弧时，DTH＝DTHreal*sinα/α，其中α是圆心角，由此可知，当圆心角越小，此方法计算的结果越接近真实值。
具体来说，计算车辆实时位置到目标点的距离DTH的步骤如下：基于向量内积，输入车辆当前的位置坐标(VehX,VehY)、目标轨迹的结束点坐标(EndPoseX,EndPoseY)和结束点的航向角StepEndPose，采用如下公式计算：
e＝[cos(StepEndPose),sin(StepEndPose)]
Figure BDA0003576458870000081
Figure BDA0003576458870000082
式中，StepEndPose为结束点的航行角，e为沿着结束点航向角方向的方向向量，
Figure BDA0003576458870000083
为当前位置到结束点的向量。
进一步的，当目标路径(即规划的目标轨迹)为圆弧时，所计算的DTH比实际的DTH小。本实施例中所述航向角误差EYaw、位置误差EPos、目标曲率Curvature和DTH均是在档位为前进挡时所计算的结果，若档位为后退档，上述所有计算结果都要乘以-1。
最后需要说明的是，以上实施例仅用以说明本发明的技术方案而非限制技术方案，尽管申请人参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，那些对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，均应涵盖在本发明的权利要求范围当中。

Claims (9)

1.一种基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，包括根据车辆实时位置坐标和目标点位置坐标计算车辆实时位置到目标点的距离DTH，并将所计算出的距离输出到车辆纵向控制模块；利用目标轨迹和实时车辆航向角计算航向角误差EYaw、利用目标轨迹和实时车辆位置坐标计算横向位置误差EPos、利用目标轨迹计算目标曲率Curvature，并将所计算出的航向角误差、横向位置误差、目标曲率结果输出到车辆横向控制模块。
2.根据权利要求1所述的基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，在计算航向角误差、横向位置误差、目标曲率前，先判定所规划的路径是否更新，若路径更新，则判定路径是否有效；当路径无效时，则结束路径跟踪，当路径有效时，执行如下步骤：先根据目标轨迹的中更新的路径点利用三点计算曲率的方法计算出每一个点的目标曲率，并根据路径点利用反正切函数计算每一个点的目标航向角，然后，再遍历所有的路径点的x、y坐标，匹配出离当前车辆位置最近的目标轨迹点(xPntId,yPntId)。
3.根据权利要求2所述的基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，所匹配出的离当前车辆位置最近的路径点通过如下方式计算而出：先计算出所有轨迹点的坐标和当前位置坐标，然后，分别计算出各个轨迹点x坐标与当前位置x坐标之差的绝对值distX、各个轨迹点y坐标与当前位置y坐标之差的绝对值distY，最后，并求出每个点对应的distX、distY之和dist，dist值最小对应的轨迹点就是当前位置匹配到的目标轨迹点(xPntId,yPntId)；具体的，计算公式如下：
Figure FDA0003576458860000021
4.根据权利要求2或3所述的基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，计算航向角误差EYaw的步骤如下：1)、根据目标轨迹点的x、y坐标计算出每个点的目标航向角heading，公式如下：
Figure FDA0003576458860000022
当dx大于等于0时，则heading＝tan-1(dy/dx)，当dx小于0时，如果tan-1(dy/dx)＞0，则heading＝-π+tan-1(dy/dx)，如果tan-1(dy/dx)＜＝0，则heading＝π+tan-1(dy/dx)；
2)、结合目标轨迹点(xPntId,yPntId)和目标航向角heading，根据对应点的目标航向角headingPntId和车辆的实时航向角VehAngle，计算出航向角误差EYaw，具体计算公式如下：
EYaw＝headingPntId-VehAngle。
5.根据权利要求4所述的基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，横向位置误差EPos的计算步骤如下：根据当前的车辆位置坐标P0(VehX,VehY)，结合目标轨迹点P1(xPntId,yPntId)和目标航向角headingPntId，计算出位于目标轨迹线中与目标轨迹相切点P2的坐标：
(xPntId+cos(headingPntId),yPntId+sin(headingPntId))；
横向位置误差EPos即为P0点到P1、P2线段的距离，具体计算公式如下：
deltX＝VehX-xPntId
deltY＝VehY-yPntId
EPos＝sin(headingPntId)*deltX-cos(headingPntId)*deltY
式中，deltX为当前车辆位置x坐标和路径上最近点x坐标之差，deltY为当前车辆位置y坐标和路径上最近点y坐标之差。
6.根据权利要求2或3或5所述的基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，计算目标轨迹中其中一个坐标点(x2,y2)的目标曲率Curvature时，先选择在目标轨迹上位于该点之前一个点(x1，y1)，位于该点之后的一个点(x3，y3)，最后，再根据所选定的三点坐标，计算坐标点(x2,y2)的目标曲率Curvature，计算公式如下：
num＝2*((x2-x1)*(y3-y2)-(y2-y1)*(x3-x2)) (1)
Figure FDA0003576458860000031
Curvature＝num/den；式中，num为公式(1)的结果，den为公式(2)的结果。
7.根据权利要求6所述的基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，计算车辆实时位置到目标点的距离DTH的步骤如下：基于向量内积，输入车辆当前的位置坐标(VehX,VehY)、目标轨迹的结束点坐标(EndPoseX,EndPoseY)和结束点的航向角StepEndPose，采用如下公式计算：
e＝[cos(StepEndPose),sin(StepEndPose)]
Figure FDA0003576458860000032
Figure FDA0003576458860000033
式中，StepEndPose为结束点的航行角，e为沿着结束点航向角方向的方向向量，
Figure FDA0003576458860000034
为当前位置到结束点的向量。
8.根据权利要求7所述的基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，当目标路径为圆弧时，所计算的DTH比实际的DTH小。
9.根据权利要求7所述的基于横纵向解耦的自动泊车路径跟踪方法，其特征在于，所述航向角误差EYaw、位置误差EPos、目标曲率Curvature和DTH均是在档位为前进挡时所计算的结果，若档位为后退档，上述所有计算结果都要乘以-1。
CN202210345718.9A 2022-03-31 2022-03-31 基于横纵向解耦的自动泊车路径跟踪方法 Pending CN114701488A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210345718.9A CN114701488A (zh) 2022-03-31 2022-03-31 基于横纵向解耦的自动泊车路径跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210345718.9A CN114701488A (zh) 2022-03-31 2022-03-31 基于横纵向解耦的自动泊车路径跟踪方法

Publications (1)

Publication Number Publication Date
CN114701488A true CN114701488A (zh) 2022-07-05

Family

ID=82171911

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210345718.9A Pending CN114701488A (zh) 2022-03-31 2022-03-31 基于横纵向解耦的自动泊车路径跟踪方法

Country Status (1)

Country Link
CN (1) CN114701488A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115817535A (zh) * 2022-12-23 2023-03-21 远峰科技股份有限公司 基于模糊决策的自动泊车路径规划方法和装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115817535A (zh) * 2022-12-23 2023-03-21 远峰科技股份有限公司 基于模糊决策的自动泊车路径规划方法和装置
CN115817535B (zh) * 2022-12-23 2023-09-29 远峰科技股份有限公司 基于模糊决策的自动泊车路径规划方法和装置

Similar Documents

Publication Publication Date Title
CN107618503B (zh) 一种自动泊车控制方法及系统
CN110949374B (zh) 基于两段二阶贝塞尔曲线的自动平行泊车路径规划方法
US9457807B2 (en) Unified motion planning algorithm for autonomous driving vehicle in obstacle avoidance maneuver
CN107085938B (zh) 基于车道线与gps跟随的智能驾驶局部轨迹容错规划方法
CN113306549A (zh) 一种自动泊车轨迹规划算法
JP2018103704A (ja) 車両用注意喚起装置
WO2018061612A1 (ja) 車両制御装置
JP2008087726A (ja) 車両の走行制御システム
JPH0933651A (ja) 自動車の進行路推定装置
CN113581289B (zh) 零位偏差动态标定方法、装置、电子设备及存储介质
CN112277931B (zh) 垂直泊车轨迹生成方法、装置、车辆及存储介质
CN112327830B (zh) 车辆自动驾驶变道轨迹的规划方法及电子设备
CN109017793A (zh) 基于前后轴融合参考的自主招车导航及控制方法
CN111123947B (zh) 机器人行进的控制方法、装置、电子设备、介质和机器人
WO2019065180A1 (ja) 駐車支援装置
CN105752154A (zh) 车辆转向控制系统及方法
JP2618254B2 (ja) 走行経路表示装置
CN112987734B (zh) 机器人行驶方法、装置及电子设备、存储介质及程序产品
CN114701488A (zh) 基于横纵向解耦的自动泊车路径跟踪方法
CN111610523A (zh) 一种轮式移动机器人的参数校正方法
CN113311845B (zh) 基于路径曲率的纯跟踪控制误差补偿方法及装置
CN113844535B (zh) 基于方向盘转矩的主动转向控制方法
CN112060077B (zh) 机器人控制方法、装置、计算机可读存储介质及机器人
CN115542925B (zh) 一种无人车横向控制用的偏差精准估计方法
JP3024964B1 (ja) 車両の走行制御装置およびそれを用いた車両

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