CN110239526B - 一种基于准均匀b样条曲线的泊车路径规划方法 - Google Patents

一种基于准均匀b样条曲线的泊车路径规划方法 Download PDF

Info

Publication number
CN110239526B
CN110239526B CN201910484871.8A CN201910484871A CN110239526B CN 110239526 B CN110239526 B CN 110239526B CN 201910484871 A CN201910484871 A CN 201910484871A CN 110239526 B CN110239526 B CN 110239526B
Authority
CN
China
Prior art keywords
parking
vehicle
coordinates
point
control points
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.)
Active
Application number
CN201910484871.8A
Other languages
English (en)
Other versions
CN110239526A (zh
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.)
Beijing Institute of Technology BIT
Original Assignee
Beijing Institute of Technology BIT
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 Beijing Institute of Technology BIT filed Critical Beijing Institute of Technology BIT
Priority to CN201910484871.8A priority Critical patent/CN110239526B/zh
Publication of CN110239526A publication Critical patent/CN110239526A/zh
Application granted granted Critical
Publication of CN110239526B publication Critical patent/CN110239526B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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/80—Technologies aiming to reduce greenhouse gasses emissions common to all road transportation technologies
    • Y02T10/84—Data processing systems or methods, management, administration

Landscapes

  • Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Transportation (AREA)
  • Mechanical Engineering (AREA)
  • Steering Control In Accordance With Driving Conditions (AREA)
  • Control Of Driving Devices And Active Controlling Of Vehicle (AREA)

Abstract

一种基于准均匀B样条曲线的泊车路径规划方法，将部分控制点的坐标进行固定，从而减小控制量的数量，对控制点坐标进行优化，得到泊车路径轨迹，其中以控制点生成的泊车路径轨迹曲线的曲率最大值最小作为目标函数的优化目标；以车辆几何碰撞约束条件来建立约束函数。由于采用准均匀B样条曲线，保证生成的曲线会经过两端的控制点，即保证生成的轨迹曲线对应的车辆姿态在起始点和终止点附近均能够满足泊车要求，本发明通过控制点对应控制量的简化，有效减少泊车轨迹规划过程中所建立的优化求解问题的约束函数的数量，提高优化求解过程的计算速度。

Description

一种基于准均匀B样条曲线的泊车路径规划方法
技术领域
本发明涉及一种泊车路径规划方法，特别是涉及一种基于准均匀B样条曲线的泊车路径规划方法。
背景技术
自动泊车是智能辅助驾驶系统的重要组成部分，主要是以提升车辆舒适性和安全性为主要目的。在自动泊车过程中，泊车路径规划是其中的核心工作之一，其目标在于从车辆起始位置到目标停车位之间规划出一条无碰撞，满足车辆运动学约束条件的车辆行驶轨迹。传统的圆弧直线式泊车轨迹虽然能够满足车辆运动学约束条件，但由其产生的泊车轨迹存在曲率不连续等问题，导致车辆在泊车过程中存在原地转向等情况，造成轮胎磨损等问题的出现，因此需要寻求更加合理的泊车轨迹生成方法。
B样条曲线由贝塞尔样条曲线拓展而成，是通过逼近由控制点决定的特征多边形而获得的曲线，在继承了后者的优点基础上，还具有精度更高，控制点局部修改特性，曲线阶数可控等优点。控制点坐标与所生成的样条曲线之间的具体数学关系包括三种：均匀B样条曲线，准均匀B样条曲线，和非均匀B样条曲线。然而现有的B样条曲线泊车路径规划方法中，如论文《基于滑模控制的自动泊车系统路径跟踪研究》和《基于滑模控制的自动泊车系统路径跟踪研究》都使用均匀B样条曲线来构建轨迹，均匀B样条曲线在使用过程中存在一个问题，就是生成的曲线的两端与控制点不接触。
此外另一个亟待解决的问题是当前优化问题的约束条件及求解参数数量仍然较多，优化求解计算速度慢。
发明内容
本发明的目的正是针对上述泊车路径规划问题，提出了一种能够同时满足车辆几何碰撞约束、方向角平滑、曲率连续的使用准均匀B样条曲线泊车路径规划方法，实现平行泊车任务的单次路径规划。
首先根据泊车场景的几何尺寸适当的选择控制点的数量；然后基于泊车场景的相关几何参数给出控制点的初始坐标，并将其中要优化的控制点坐标作为优化问题的控制量；根据车辆停泊过程中的几何关系建立约束条件，以车辆曲率最大值尽量小作为目标进行优化求解。为了显性的建立车辆起始位置和目标泊车位置与样条曲线起始位置和终止位置之间的联系，本发明采用准均匀B样条曲线，准均匀B样条曲线的实际控制点在两个端点有n个重复度，n为样条曲线的阶数，则可以保证生成的曲线会经过两端的控制点，即保证生成的轨迹曲线的起始位置和终止位置与对应的控制点序列的起始点和终止点坐标是重合的，从而生成的轨迹曲线对应的车辆姿态在起始点和终止点附近均能够满足泊车要求，这样能够通过改变控制点的位置来改变准B样条曲线的端点位置，从而直接决定轨迹曲线起始和终止位置及状态。而均匀B样条曲线不满足这样的条件。
本发明充分利用准均匀B样条曲线的几何特性来构建泊车轨迹，通过合理针对所建立的优化问题进行控制点对应控制量的简化，从而减小控制量的数量，有效减少泊车轨迹规划过程中所建立的优化求解问题的约束函数的数量，提高优化求解过程的计算速度。
附图说明
图1是泊车场景及坐标系示意图
图2是左前端碰撞示意图
图3是右侧碰撞示意图
图4是右前方与泊车区域碰撞示意图
图5是右后方与泊车区域碰撞示意图
具体实施方式
1.根据泊车参数建立泊车场景坐标系，泊车参数包括停车位纵向距离、停车位横向距离、道路宽度、车辆长度、车辆宽度和轴距。需要用的参数及示例取值如下表所示。
表1泊车参数
Figure BDA0002085061120000021
Figure BDA0002085061120000031
根据上述泊车参数，可以建立如图1所示的泊车场景。本示例中，以车辆右后方所在的停车位端点作为原点，以车辆前进方向作为x轴正方向，以车辆左侧作为y轴正方向。所建立的泊车场景如图1所示。
2.基于上述参考系，以车辆后轴中心处作为参考点，可以定义出泊车起始位置和泊车终止位置。在示例中，相关取值如表2所示。
表2泊车起终点坐标示例
Figure BDA0002085061120000032
3.由于准均匀B样条曲线的构建是通过一些离散的控制点生成的，当控制点的坐标确定后，B样条曲线的样子就唯一确定；通过改变控制点的坐标位置，就可以改变B样条曲线在平面上的样子。这样，我们就通过优化B样条曲线控制点的坐标位置，来确保B样条曲线所对应的泊车路径能够符合我们的各种要求。
根据泊车起始位置和泊车终止位置的坐标，按照公式(1)计算出待优化控制点的初值。公式(1)实现优化控制点的分布符合特点：两端的控制点比较密集，中间的控制点比较稀疏。
控制点的数量则是根据经验来选取，本示例中，控制点数量选择为9，其初值如下表所示。
表3泊车控制点坐标初值
Figure BDA0002085061120000033
Figure BDA0002085061120000041
Figure BDA0002085061120000042
其中，xstart和ystart表示泊车起始点的坐标，xend和yend表示泊车终止点的坐标；n表示控制点的数量,Pix表示第i个控制点的x坐标值，Piy表示第i个控制点的y坐标；
4.根据泊车任务的要求，可以建立如下的优化问题：
其中，f0为目标函数，具体定义为控制点生成的泊车路径轨迹曲线的最大曲率，该轨迹曲线通过n个控制点生成；第n个控制点坐标Pn＝(Pnx,Pny)。上述目标函数的自变量为控制点的坐标。
目标函数的优化目标是：整个泊车过程中车辆参考点移动行程的轨迹曲线的曲率最大值最小，因为曲率是变化的，不同位置不一样，我们关心的是曲率最大值有没有超过车辆物理结构的限制，也就是以整条曲线的曲率最大值作为目标函数，希望泊车轨迹曲线的最大曲率尽量小。
目标函数确保了车辆能够符合转向的几何约束条件，即：车辆能够以不小于最小转弯半径的转向方式实现泊车过程。由于车辆最小转弯半径(即曲率半径)和曲率之间是倒数关系，因而该条件就相当于车辆泊车过程中要通过的路径对应的最大曲率不大于车辆最小转弯半径对应的曲率。在这一条件下，车辆能够实现转弯，否则，车辆不能实现转弯。在满足这个条件的基础上，我们还希望泊车路径对应的最大曲率不要太大，这样对应的曲率半径也就不会太小，这样使得车辆能够容易实现转弯，也为车辆轨迹跟踪控制等环节减轻了控制精度要求较高的负担。
f1至fm为约束函数，具体定义为车辆在泊车过程中存在的各类几何碰撞约束条件，共m个；上述约束函数的自变量为控制点的坐标。
约束函数的定义确保了车辆在泊车过程中能够满足几何碰撞约束条件。为保证生成的轨迹曲线的曲率连续变化，使得车辆在跟踪轨迹曲线过程中方向盘转角能够连续变化，B样条曲线的种类定义为4次准均匀样条曲线，优选地也可以使用更高次数以满足更高的设计要求。
5.控制量简化
由于存在n个控制点，因此该优化问题对应的控制量为k＝2n个，为了简化优化问题的求解难度，我们将部分控制点的坐标进行固定，以减少n+2个优化控制量的数量。
表4控制点坐标固定方式
Figure BDA0002085061120000051
每个控制点有x和y坐标，因此n个控制点具有2n个控制量，被固定的控制点的坐标包括：第1,2,3个控制点的y坐标，第n-2,n-1,和n控制点的y坐标，第4,5,6至n-3控制点的x坐标，以及第1和n这两个控制的x坐标。共计n+2个，所以通过固定部分控制点，使得原优化问题的控制量被减小至k＝n-2个。
6.约束条件定义
基于前述控制量简化方式，只需定义如下4个约束条件即可保证车辆在泊车过程中不产生运动干涉碰撞。
(1)为避免车辆左前端超过道路宽度对应的范围，如图2所示，应建立如下约束条件：
y1≤lw+lk,x∈[0,xstart] (3)
其中，y1表示车身左前端纵坐标，lw表示道路宽度，lk表示车位宽度，x为车辆参考点的坐标。
(2)为避免车辆右侧与泊车区域前方端点发生碰撞，如图3所示，应建立如下约束条件：
tan(θ)(lC-x3)+y3≥lk,x2≥lc∩x3≤lc (4)
其中，lc表示车位长度，x3,y3表示车身右后点坐标，θ表示车辆在泊车轨迹上的方向角，x2右前端横坐标；
(3)为避免车辆右前端与泊车区域前方发生碰撞，如图4所示，应建立如下约束条件：
x2≤lc,y2≤lk (5)
x2，y2表示车身右前端坐标；
(4)为避免车辆右后端与泊车区域侧方发生碰撞，如图5所示，应建立如下约束条件
0≤miny3,x∈[0,x0] (6)
x0表示车辆参考点横坐标的最大值，该值不小于泊车起始位置车辆参考点的横坐标值。
因此，泊车任务对应的优化问题就可以被具体定义为如下形式的目标函数，表示生成的轨迹曲线的曲率的最大值，我们希望目标函数最小，也就是最大曲率能够尽量小：
Figure BDA0002085061120000071
k个控制量(k＝n-2)与坐标点之间的关系为：
Figure BDA0002085061120000072
控制量的优化取值范围取值范围为：
Figure BDA0002085061120000073
四个约束条件分别为：
Figure BDA0002085061120000074
车辆的方向角由控制点确定的轨迹曲线给出。轨迹曲线是一条准均匀B样条曲线，其次数确定为4次及以上，通过控制点所生成的准均匀B样条曲线的导数来计算所生成的轨迹各处的方向角及曲率。车身四周坐标Mp按照如下公式给出。
Figure BDA0002085061120000081
其中，(x1,y1)(x2,y2)(x3,y3)(x4,y4)分别表示车辆左前方、右前方、右后方和左后方端点的平面坐标，M1,M2和M3分别为如下格式
Figure BDA0002085061120000082
其中，Lk为车宽，Lf为前悬，Lr为后悬，L为轴距。

Claims (9)

1.一种基于准均匀B样条曲线的泊车路径规划方法，包括如下步骤：
(1)根据泊车参数建立泊车场景坐标系；
(2)定义泊车起始位置和泊车终止位置；
(3)确定准均匀B样条曲线的控制点的数量，根据泊车起始位置和泊车终止位置的坐标，计算准均匀B样条曲线待优化控制点的初值；
(4)将部分控制点的坐标进行固定，从而减小控制量的数量；其中，被固定的控制点的坐标包括：第1,2,3个控制点的y坐标，第n-2,n-1,和n控制点的y坐标，第4,5,6至n-3控制点的x坐标，以及第1和n这两个控制点的x坐标；
(5)对控制点坐标进行优化，得到泊车路径轨迹，其中以控制点生成的泊车路径轨迹曲线的曲率最大值最小作为目标函数的优化目标；以车辆几何碰撞约束条件来建立约束函数，所述基于准均匀B样条曲线的泊车路径规划方法生成的泊车路径轨迹曲线的起始位置和终止位置与所述控制点的起始点和终止点坐标是重合的。
2.根据权利要求1所述的泊车路径规划方法，其特征在于：所述泊车参数包括停车位纵向距离、停车位横向距离、道路宽度、车辆长度、车辆宽度和轴距。
3.根据权利要求1所述的泊车路径规划方法，其特征在于：以车辆右后方所在的停车位端点作为原点，以车辆前进方向作为x轴正方向，以车辆左侧作为y轴正方向建立泊车场景坐标系。
4.根据权利要求1所述的泊车路径规划方法，其特征在于：以车辆后轴中心处作为参考点定义泊车起始位置和泊车终止位置。
5.根据权利要求1所述的泊车路径规划方法，其特征在于：所述的待优化控制点的初值选择方式按照如下公式进行：
Figure FDA0002488104300000021
其中，xstart和ystart表示泊车起始点的坐标，xend和yend表示泊车终止点的坐标；n表示控制点的数量,Pix表示第i个控制点的x坐标值，Piy表示第i个控制点的y坐标。
6.根据权利要求1所述的泊车路径规划方法，其特征在于：其中所述的优化问题的构建过程中，准均匀B样条曲线的次数确定为4次以上，并通过准均匀B样条曲线的导数来计算所生成的轨迹各处的方向角及曲率。
7.根据权利要求1所述的泊车路径规划方法，其特征在于：所述约束条件包括：
避免车辆左前端超过道路宽度对应的范围；
避免车辆右侧与泊车区域前方端点发生碰撞；
避免车辆右前端与泊车区域前方发生碰撞；
避免车辆右后端与泊车区域侧方发生碰撞。
8.根据权利要求1所述的泊车路径规划方法，其特征在于：所述约束条件为
Figure FDA0002488104300000022
其中，lw表示道路宽度，lk表示车位宽度，x为车辆参考点的坐标，xstart泊车起始点的横坐标，lc表示车位长度，x3,y3表示车身右后点坐标，y1表示车辆左前点坐标；x2,y2表示车辆右前点的坐标，θ表示车辆在泊车轨迹上的方向角，x0表示车辆参考点x坐标变化范围的最大值，该值应不小于泊车起始位置车辆参考点x坐标值。
9.根据权利要求1所述的泊车路径规划方法，其特征在于：控制量与坐标点之间的关系为：
Figure FDA0002488104300000031
控制量的优化取值范围如下：
Figure FDA0002488104300000032
其中，xstart和ystart表示泊车起始点的坐标，xend和yend表示泊车终止点的坐标；n表示控制点的数量。
CN201910484871.8A 2019-06-05 2019-06-05 一种基于准均匀b样条曲线的泊车路径规划方法 Active CN110239526B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910484871.8A CN110239526B (zh) 2019-06-05 2019-06-05 一种基于准均匀b样条曲线的泊车路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910484871.8A CN110239526B (zh) 2019-06-05 2019-06-05 一种基于准均匀b样条曲线的泊车路径规划方法

Publications (2)

Publication Number Publication Date
CN110239526A CN110239526A (zh) 2019-09-17
CN110239526B true CN110239526B (zh) 2020-08-04

Family

ID=67886067

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910484871.8A Active CN110239526B (zh) 2019-06-05 2019-06-05 一种基于准均匀b样条曲线的泊车路径规划方法

Country Status (1)

Country Link
CN (1) CN110239526B (zh)

Families Citing this family (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110843767A (zh) * 2019-10-25 2020-02-28 池州学院 一种基于样条理论的自动入库泊车方法及系统
CN111016886B (zh) * 2019-12-19 2021-07-30 合达信科技集团有限公司 一种基于b样条理论的自动泊车路径规划方法
CN113361152A (zh) * 2020-03-05 2021-09-07 北京京东乾石科技有限公司 轨迹规划方法、轨迹规划装置、存储介质与电子设备
CN111547046B (zh) * 2020-04-30 2022-05-06 惠州华阳通用电子有限公司 一种平行车位预占式自动泊车方法及装置
CN112277932A (zh) * 2020-10-21 2021-01-29 深圳市德航智能技术有限公司 基于北斗定位的农机自动驾驶系统关键技术研发
CN112937555A (zh) * 2021-03-08 2021-06-11 华南理工大学 一种基于车辆运动学模型的平行泊车轨迹规划方法
CN113568403A (zh) * 2021-07-02 2021-10-29 广州小鹏自动驾驶科技有限公司 处理方法、处理装置、车辆及存储介质
CN113978452B (zh) * 2021-11-10 2024-03-22 吉林大学 一种自动平行泊车路径规划方法
CN114137972B (zh) * 2021-11-24 2023-10-31 广州小鹏自动驾驶科技有限公司 路径生成方法、装置、交通工具及存储介质
CN114296452A (zh) * 2021-12-16 2022-04-08 华人运通(上海)自动驾驶科技有限公司 自动泊车路径规划方法、设备及计算机可读存储介质
CN115357032B (zh) * 2022-10-20 2023-01-31 上海仙工智能科技有限公司 一种基于nurbs的移动机器人路径生成方法、系统及存储介质

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102006062390A1 (de) * 2006-12-19 2008-06-26 Valeo Schalter Und Sensoren Gmbh Verfahren zum rückwärtigen Einparken eines Fahrzeugs und Einparkhilfesystem hierfür
CN103158703B (zh) * 2013-03-26 2016-04-06 北京经纬恒润科技有限公司 一种平行泊车的控制方法
CN105197010B (zh) * 2014-06-04 2018-03-27 长春孔辉汽车科技股份有限公司 辅助泊车系统以及辅助泊车控制方法
DE102015201038A1 (de) * 2015-01-22 2016-07-28 Robert Bosch Gmbh Verfahren zur Bahnplanung des Einparkens eines Fahrzeugs in eine Parklücke
CN106874551B (zh) * 2017-01-11 2020-12-01 成都信息工程大学 一种基于三阶反正切函数模型的平行泊车方法
CN108891412B (zh) * 2018-07-19 2019-08-23 吉林大学 一种平行泊车的路径规划方法

Also Published As

Publication number Publication date
CN110239526A (zh) 2019-09-17

Similar Documents

Publication Publication Date Title
CN110239526B (zh) 一种基于准均匀b样条曲线的泊车路径规划方法
EP2243687B1 (en) Parking assisting device
JP5527382B2 (ja) 走行支援システム及び制御装置
CN111806467A (zh) 一种基于车辆行驶规律的变速动态换道轨迹规划方法
CN109017793B (zh) 基于前后轴融合参考的自主停车导航及控制方法
US8511872B2 (en) Predictive adaptive front lighting for a motor vehicle
CN106915349B (zh) 一种车辆侧向控制的方法及装置
JP2007022373A (ja) ステアリング制御システム
JP2014069645A (ja) 駐車支援装置
CN105631217A (zh) 基于本车自适应虚拟车道的前方有效目标选择系统及方法
CN114791288B (zh) 矿区车辆的行驶路径规划方法、装置及计算机设备
CN112109732A (zh) 一种智能驾驶自适应曲线预瞄方法
KR20200017571A (ko) 자율주행 차량을 위한 횡방향 제어 파라미터 보정 장치 및 방법
US7503677B2 (en) Headlight control apparatus and method for vehicle
JP7188236B2 (ja) 車両制御装置
US20220266823A1 (en) Vehicle Motion Control Device, Vehicle Motion Control Method, and Vehicle Motion Control System
CN110362088B (zh) 一种适用于无人驾驶跨运车的循迹控制系统和方法
JP6617582B2 (ja) 車両操舵制御装置
CN112061115B (zh) 车辆行驶路径获取方法、装置和计算机可读存储介质
CN114291092B (zh) 车辆换道控制方法、装置、电子控制单元及存储介质
CN112236352A (zh) 用于确定车辆的经更新的轨迹的方法
CN113311698B (zh) 车道保持控制方法、控制装置和车辆
JPWO2019244490A1 (ja) 車両制御装置
CN114783197A (zh) 建筑群间无信号灯十字路口多车协同换道控制方法
CN109635516B (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
GR01 Patent grant
GR01 Patent grant