CN115465263A - 基于库位跟踪的泊车方法及系统 - Google Patents
基于库位跟踪的泊车方法及系统 Download PDFInfo
- Publication number
- CN115465263A CN115465263A CN202210983628.2A CN202210983628A CN115465263A CN 115465263 A CN115465263 A CN 115465263A CN 202210983628 A CN202210983628 A CN 202210983628A CN 115465263 A CN115465263 A CN 115465263A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- parking
- matrix
- estimation value
- arc straight
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000012545 processing Methods 0.000 claims abstract description 52
- 238000001914 filtration Methods 0.000 claims abstract description 39
- 239000011159 matrix material Substances 0.000 claims description 185
- 238000005259 measurement Methods 0.000 claims description 39
- 238000012546 transfer Methods 0.000 claims description 31
- 238000009499 grossing Methods 0.000 claims description 16
- 101000802640 Homo sapiens Lactosylceramide 4-alpha-galactosyltransferase Proteins 0.000 claims description 15
- 102100035838 Lactosylceramide 4-alpha-galactosyltransferase Human genes 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 14
- 238000004891 communication Methods 0.000 claims description 7
- 230000008859 change Effects 0.000 claims description 5
- 230000007704 transition Effects 0.000 claims description 5
- 238000009825 accumulation Methods 0.000 abstract description 6
- 230000008447 perception Effects 0.000 abstract description 5
- 230000006870 function Effects 0.000 description 58
- 238000004590 computer program Methods 0.000 description 19
- 238000003860 storage Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 9
- 238000011065 in-situ storage Methods 0.000 description 4
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000007792 addition Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
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
- B60W30/06—Automatic manoeuvring for parking
-
- 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
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Data Mining & Analysis (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Optimization (AREA)
- Mathematical Analysis (AREA)
- Computational Mathematics (AREA)
- Algebra (AREA)
- Databases & Information Systems (AREA)
- Automation & Control Theory (AREA)
- General Engineering & Computer Science (AREA)
- Software Systems (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Probability & Statistics with Applications (AREA)
- Operations Research (AREA)
- Human Computer Interaction (AREA)
- Evolutionary Biology (AREA)
- Life Sciences & Earth Sciences (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Bioinformatics & Computational Biology (AREA)
- Computing Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种基于库位跟踪的泊车方法及系统，其方法包括以下步骤：当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；控制车辆按照所述圆弧直线泊车轨迹进行泊车操作；因此可在不占用较大算力的基础上解决控制和感知带来的误差积累问题。
Description
技术领域
本发明涉及车辆泊车领域，特别涉及一种基于库位跟踪的泊车方法及系统。
背景技术
在车辆驾驶倒车入库中，一般有平行泊车和垂直泊车，参见附图1所示，对于垂直泊车过程，A点为初始泊入区域，车辆泊车时可按照由A-B、B-C、C-D三段圆弧组成的泊车轨迹行驶，当自车到C点附近时需要进行库位检测，即检测库位角点P0、P1位置；由于车辆在A-B、B-C的行驶过程中由于车辆自身的执行器和底层控制器存在的偏差，导致以车辆自身相对于车库的位置存在偏差，此时自车记录的车位位置为图中虚线所示，而实际位置为图中实线位置，而在进行库位角点检测时因为上述原因会存在传感器精度不够，同时会由于环视系统的标定、拼接、光照、阴影、遮挡，在检测库位时车辆非静止而影像系统帧数有限，而导致偏差问题，从而存在泊车轨迹的偏差问题。同样的，对于水平泊车过程与垂直泊车过程类似，在进行库位角点P0、P1位置检测时也会存在上述问题。
针对上述问题，在相关技术中，一般在车辆行进过程中不断检测是否经过库位，此方法可以时刻消除由于传感器误差引起的路径精度低的问题，但是会导致运算量过大，出现芯片负载较高的问题。
发明内容
本发明的提供一种基于库位跟踪的泊车方法及系统，可在不占用较大算力的基础上解决控制和感知带来的误差积累问题。
第一方面，提供一种基于库位跟踪的泊车方法，包括以下步骤：
当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；
根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；
基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；
控制车辆按照所述圆弧直线泊车轨迹进行泊车操作。
根据第一方面，在第一方面的第一种可能的实现方式中，所述“根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理”步骤，具体包括以下步骤：
根据所述车辆运行信息及所述库位角点信息，获取车辆的车辆状态矩阵、系统观测矩阵、状态转移函数雅可比矩阵、测量函数雅可比矩阵；
根据所述车辆状态矩阵及状态转移函数雅可比矩阵，预测车辆当前时刻的系统状态变量初步估计值；
根据状态转移函数雅可比矩阵，预测车辆当前时刻的协方差矩阵初步估计值；
根据所述测量函数雅可比矩阵及所述协方差矩阵初步估计值，获取车辆当前时刻的卡尔曼滤波增益矩阵；
根据所述系统观测矩阵、所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及系统状态变量初步估计值，更新车辆当前时刻的系统状态变量估计值；
根据所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及所述协方差矩阵初步估计值，更新车辆当前时刻的协方差矩阵估计值。
根据第一方面的第一种可能的实现方式，在第一方面的第二种可能的实现方式中，所述“根据所述车辆运行信息及所述库位角点信息，获取车辆的车辆状态矩阵、系统观测矩阵、状态转移函数雅可比矩阵、测量函数雅可比矩阵”步骤，具体包括以下步骤：
根据所述车辆运行信息及所述库位角点信息；
获取车辆状态矩阵如下：
获取系统观测矩阵如下：
获取状态转移函数雅可比矩阵如下：
获取测量函数雅可比矩阵如下：
式中，k为车辆当前时刻，x(k)为车辆横坐标，x(k-1)为k上一时刻的车辆横坐标，y(k)为车辆纵坐标，y(k-1)为k上一时刻的车辆纵坐标，φ(k)为车辆航向角，φ(k-1)为k上一时刻的车辆航向角，x1(k)为库位角点横坐标，x(k-1)为k上一时刻的库位角点横坐标，y1(k)为库位角点纵坐标，y1(k-1)为k上一时刻的库位角纵坐标，V为车辆速度，Δt为预测时域，L为车辆轴距，γ为车辆前轮转角，ri(k)为预测k时刻车辆前进直线距离，θi(k)为预测k时刻车辆航向变化量，xi(k)为预测k时刻车辆横坐标，yi(k)为预测k时刻车辆纵坐标；
w(k)＝(wx(k),wy(k),wφ(k),0,0)T为过程噪声，v(k)＝(vr(k),vθ(k))T为测量噪声，
为车辆与库位角点的距离。
根据第一方面的第二种可能的实现方式，在第一方面的第三种可能的实现方式中，所述“根据所述车辆状态矩阵及状态转移函数雅可比矩阵，预测车辆当前时刻的系统状态变量初步估计值；根据状态转移函数雅可比矩阵，预测车辆当前时刻的协方差矩阵初步估计值”步骤，具体包括以下步骤：
根据所述车辆状态矩阵X(K)，及状态转移函数雅可比矩阵F(k)，预测车辆当前时刻的系统状态变量初步估计值如下：
根据状态转移函数雅可比矩阵F(k)，预测车辆当前时刻的协方差矩阵初步估计值如下：
P(k)-＝F(k)P(k-1)F(k)T+Q式(6)；
式中，F(k)T为状态转移函数雅可比矩阵F(k)的转置矩阵，
为k上一时刻的系统状态变量初步估计值，P(k-1)为k上一时刻的协方差矩阵初步估计值，R为测量噪声协方差矩阵，Q为状态转移协方差矩阵，X(K)为
或
包含的状态量。
根据第一方面的第三种可能的实现方式，在第一方面的第四种可能的实现方式中，所述“根据所述测量函数雅可比矩阵及所述协方差矩阵的初步估计值，获取车辆当前时刻的卡尔曼滤波增益矩阵”步骤，具体包括以下步骤：
根据所述测量函数雅可比矩阵H(k)，及所述协方差矩阵初步估计值P(k)-，获取车辆当前时刻的卡尔曼滤波增益矩阵如下：
K(k)＝P(k)-H(k)T(H(k)P(k)-H(k)T+R)-1式(7)；
式中，H(k)T为测量函数雅可比矩阵H(k)的转置矩阵。
根据第一方面的第四种可能的实现方式，在第一方面的第五种可能的实现方式中，所述“根据所述系统观测矩阵、所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及系统状态变量初步估计值，更新车辆当前时刻的系统状态变量估计值；根据所述状态转移函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及所述协方差矩阵初步估计值，更新车辆当前时刻的协方差矩阵估计值”步骤，具体包括以下步骤：
根据所述系统观测矩阵Z(k)、所述测量函数雅可比矩阵H(k)、所述卡尔曼滤波增益矩阵K(k)、及系统状态变量初步估计值
更新车辆当前时刻的系统状态变量估计值如下：
根据所述测量函数雅可比矩阵H(k)、所述卡尔曼滤波增益矩阵K(k)、及所述协方差矩阵初步估计值P(k)-，更新车辆当前时刻的协方差矩阵估计值如下：
P(k)＝(I-K(k)H(k))P(k)-式(9)。
根据第一方面的第五种可能的实现方式，在第一方面的第六种可能的实现方式中，所述“基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；控制车辆按照所述圆弧直线泊车轨迹进行泊车操作”步骤，具体包括以下步骤：
基于误差处理后的库位角点信息生成圆弧直线泊车轨迹，并对所述圆弧直线泊车轨迹进行B样条曲线平滑处理；
控制车辆按照B样条曲线平滑处理后的圆弧直线泊车轨迹进行泊车操作。
根据第一方面的第六种可能的实现方式，在第一方面的第七种可能的实现方式中，所述“对所述圆弧直线泊车轨迹进行B样条曲线平滑处理”步骤，具体包括以下步骤：
在所述圆弧直线泊车轨迹上选取多个控制点，根据多个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹。
根据第一方面的第七种可能的实现方式，在第一方面的第八种可能的实现方式中，所述“在所述圆弧直线泊车轨迹上选取多个控制点，根据多个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹”步骤，具体包括以下步骤：
在所述圆弧直线泊车轨迹上选取多个控制点，所述控制点为7个；
第一控制点为圆弧直线泊车轨迹的初始点，第二控制点为初始点的邻近点，第三控制点为圆弧直线泊车轨迹的终点，第四控制点为终点的领近点，第五控制点和第六控制点为圆弧直线泊车轨迹上圆弧与直线的两个交点，第七控制点为圆弧直线泊车轨迹上圆弧的中点；
根据7个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹如下：
其中，Bj,3(s)为三次B样条基函数矩阵，
式中，Ci+j为圆弧直线泊车轨迹第i段中第j个控制点，s为参数，s∈[0,1]。
第二方面，提供一种基于库位跟踪的泊车系统，包括：
库位角点获取模块，用于当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；
误差处理模块，与所述库位角点获取模块通信连接，用于根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；
泊车轨迹模块，与所述误差处理模块通信连接，用于基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；以及，
泊车操作模块，与所述泊车轨迹模块通信连接，用于控制车辆按照所述圆弧直线泊车轨迹进行泊车操作。
与现有技术相比，本发明的优点如下：通过采用卡尔曼滤波减小由传感器带来的偏差问题，即当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；控制车辆按照所述圆弧直线泊车轨迹进行泊车操作；因此可在不占用较大算力的基础上解决控制和感知带来的误差积累问题。
附图说明
图1是车辆泊车入库的泊车轨迹示意图；
图2是本发明一种基于库位跟踪的泊车方法的一实施例的流程示意图；
图3是本发明一种基于库位跟踪的泊车方法的又一实施例的流程示意图；
图4是本发明的B样条曲线平滑处理的示意图；
图5是本发明一种基于库位跟踪的泊车系统的结构示意图。
附图说明：
100、基于库位跟踪的泊车系统；110、库位角点获取模块；120、误差处理模块；130、泊车轨迹模块；140、泊车操作模块。
具体实施方式
现在将详细参照本发明的具体实施例，在附图中例示了本发明的例子。尽管将结合具体实施例描述本发明，但将理解，不是想要将本发明限于所述的实施例。相反，想要覆盖由所附权利要求限定的在本发明的精神和范围内包括的变更、修改和等价物。应注意，这里描述的方法步骤都可以由任何功能块或功能布置来实现，且任何功能块或功能布置可被实现为物理实体或逻辑实体、或者两者的组合。
为了使本领域技术人员更好地理解本发明，下面结合附图和具体实施方式对本发明作进一步详细说明。
注意：接下来要介绍的示例仅是一个具体的例子，而不作为限制本发明的实施例必须为如下具体的步骤、数值、条件、数据、顺序等等。本领域技术人员可以通过阅读本说明书来运用本发明的构思来构造本说明书中未提到的更多实施例。
参见图2所示，本发明实施例提供一种基于库位跟踪的泊车方法，包括以下步骤：
S100，当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；
S200，根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；
S300，基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；
S400，控制车辆按照所述圆弧直线泊车轨迹进行泊车操作。
具体地，本实施例中，当在进行库位角点检测时会因为会存在传感器精度不够，同时会由于环视系统的标定、拼接、光照、阴影、遮挡，在检测库位时车辆非静止而影像系统帧数有限，而导致偏差问题，从而存在泊车轨迹的偏差问题，因此自车记录的车位位置为图1中虚线(无库位跟踪)所示，而实际位置为图1中实线(有库位跟踪)位置。因此本发明通过采用卡尔曼滤波减小由传感器带来的偏差问题，即当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；控制车辆按照所述圆弧直线泊车轨迹进行泊车操作；因此可在不占用较大算力的基础上解决控制和感知带来的误差积累问题。
一般在车辆行进过程中不断检测是否经过库位，此方法可以时刻消除由于传感器误差引起的路径精度低的问题，但是会导致运算量过大，出现芯片负载较高的问题；由于在自动泊车过程中并不会导致在入库前的碰撞问题，所以投入过多的算力在此不可取，本发明在入库前进行一次库位检测，并进行路径重规划不仅可以极大地减少误差积累，避免由于此误差导致的入库碰撞问题。
优选地，在本申请另外的实施例中，参见图3所示，所述“S200，根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理”步骤，具体包括以下步骤：
S210，根据所述车辆运行信息及所述库位角点信息，获取车辆的车辆状态矩阵、系统观测矩阵、状态转移函数雅可比矩阵、测量函数雅可比矩阵；
S220，根据所述车辆状态矩阵及状态转移函数雅可比矩阵，预测车辆当前时刻的系统状态变量初步估计值；
S230，根据状态转移函数雅可比矩阵，预测车辆当前时刻的协方差矩阵初步估计值；
S240，根据所述测量函数雅可比矩阵及所述协方差矩阵初步估计值，获取车辆当前时刻的卡尔曼滤波增益矩阵；
S250，根据所述系统观测矩阵、所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及系统状态变量初步估计值，更新车辆当前时刻的系统状态变量估计值；
S260，根据所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及所述协方差矩阵初步估计值，更新车辆当前时刻的协方差矩阵估计值。
优选地，在本申请另外的实施例中，所述“S210，根据所述车辆运行信息及所述库位角点信息，获取车辆的车辆状态矩阵、系统观测矩阵、状态转移函数雅可比矩阵、测量函数雅可比矩阵”步骤，具体包括以下步骤：
根据所述车辆运行信息及所述库位角点信息；
获取车辆状态矩阵如下：
获取系统观测矩阵如下：
获取状态转移函数雅可比矩阵如下：
获取测量函数雅可比矩阵如下：
式中，k为车辆当前时刻，x(k)为车辆横坐标，x(k-1)为k上一时刻的车辆横坐标，y(k)为车辆纵坐标，y(k-1)为k上一时刻的车辆纵坐标，φ(k)为车辆航向角，φ(k-1)为k上一时刻的车辆航向角，x1(k)为库位角点横坐标，x(k-1)为k上一时刻的库位角点横坐标，y1(k)为库位角点纵坐标，y1(k-1)为k上一时刻的库位角纵坐标，V为车辆速度，Δt为预测时域，L为车辆轴距，γ为车辆前轮转角，ri(k)为预测k时刻车辆前进直线距离，θi(k)为预测k时刻车辆航向变化量，xi(k)为预测k时刻车辆横坐标，yi(k)为预测k时刻车辆纵坐标；
w(k)＝(wx(k),wy(k),wφ(k),0,0)T为过程噪声，v(k)＝(vr(k),vθ(k))T为测量噪声，
为车辆与库位角点的距离。
优选地，在本申请另外的实施例中，所述“S220，根据所述车辆状态矩阵及状态转移函数雅可比矩阵，预测车辆当前时刻的系统状态变量初步估计值；S230，根据状态转移函数雅可比矩阵，预测车辆当前时刻的协方差矩阵初步估计值”步骤，具体包括以下步骤：
根据所述车辆状态矩阵X(K)，及状态转移函数雅可比矩阵F(k)，预测车辆当前时刻的系统状态变量初步估计值如下：
根据状态转移函数雅可比矩阵F(k)，预测车辆当前时刻的协方差矩阵初步估计值如下：
P(k)-＝F(k)P(k-1)F(k)T+Q式(6)；
式中，F(k)T为状态转移函数雅可比矩阵F(k)的转置矩阵，
为k上一时刻的系统状态变量初步估计值，P(k-1)为k上一时刻的协方差矩阵初步估计值，R为测量噪声协方差矩阵，Q为状态转移协方差矩阵，X(K)为
或
包含的状态量。
优选地，在本申请另外的实施例中，所述“S240，根据所述测量函数雅可比矩阵及所述协方差矩阵的初步估计值，获取车辆当前时刻的卡尔曼滤波增益矩阵”步骤，具体包括以下步骤：
根据所述测量函数雅可比矩阵H(k)，及所述协方差矩阵初步估计值P(k)-，获取车辆当前时刻的卡尔曼滤波增益矩阵如下：
K(k)＝P(k)-H(k)T(H(k)P(k)-H(k)T+R)-1式(7)；
式中，H(k)T为测量函数雅可比矩阵H(k)的转置矩阵。
优选地，在本申请另外的实施例中，所述“S250，根据所述系统观测矩阵、所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及系统状态变量初步估计值，更新车辆当前时刻的系统状态变量估计值；S260，根据所述状态转移函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及所述协方差矩阵初步估计值，更新车辆当前时刻的协方差矩阵估计值”步骤，具体包括以下步骤：
根据所述系统观测矩阵Z(k)、所述测量函数雅可比矩阵H(k)、所述卡尔曼滤波增益矩阵K(k)、及系统状态变量初步估计值
更新车辆当前时刻的系统状态变量估计值如下：
根据所述测量函数雅可比矩阵H(k)、所述卡尔曼滤波增益矩阵K(k)、及所述协方差矩阵初步估计值P(k)-，更新车辆当前时刻的协方差矩阵估计值如下：
P(k)＝(I-K(k)H(k))P(k)-式(9)。
优选地，在本申请另外的实施例中，所述“S300，基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；S400，控制车辆按照所述圆弧直线泊车轨迹进行泊车操作”步骤，具体包括以下步骤：
基于误差处理后的库位角点信息生成圆弧直线泊车轨迹，并对所述圆弧直线泊车轨迹进行B样条曲线平滑处理；
控制车辆按照B样条曲线平滑处理后的圆弧直线泊车轨迹进行泊车操作。
具体地，本实施例中，在现有技术中，在泊车轨迹中由于圆弧直线连接处曲率变化不连续，导致在轨迹跟踪时需要出现原地打方向盘来调整航向角的情况，不仅会加大轮胎磨损，还会降低舒适性，影响驾驶体验，因此为了解决上述问题，采用B样条曲线((B-splineCurve))是B-样条基函数的线性组合，是贝塞尔曲线的一般化，在此曲率不连续处进行基于B样条曲线的曲线平滑处理，B样条曲线具有曲率连续，且可以在改变部分曲线而不影响其他部分的优点，无需出现原地打方向盘，舒适入库。
优选地，在本申请另外的实施例中，所述“对所述圆弧直线泊车轨迹进行B样条曲线平滑处理”步骤，具体包括以下步骤：
在所述圆弧直线泊车轨迹上选取多个控制点，根据多个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹。
优选地，在本申请另外的实施例中，参见图4所示，所述“在所述圆弧直线泊车轨迹上选取多个控制点，根据多个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹”步骤，具体包括以下步骤：
在所述圆弧直线泊车轨迹上选取多个控制点，所述控制点为7个；
第一控制点为圆弧直线泊车轨迹的初始点，第二控制点为初始点的邻近点，第三控制点为圆弧直线泊车轨迹的终点，第四控制点为终点的领近点，第五控制点和第六控制点为圆弧直线泊车轨迹上圆弧与直线的两个交点，第七控制点为圆弧直线泊车轨迹上圆弧的中点；
根据7个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹如下：
其中，Bj,3(s)为三次B样条基函数矩阵，
式中，Ci+j为圆弧直线泊车轨迹第i段中第j个控制点，s为参数，s∈[0,1]。
具体地，本实施例中，经过B样条平滑处理后获得曲率连续的泊车轨迹，符合车辆动力学模型，无需原地打方向盘；根据控制点C求得三次B样条曲线表达式，画出平滑处理后的曲线如图4所示，实线为处理前的圆弧直线泊车轨迹-基于误差处理后的库位角点信息生成圆弧直线泊车轨迹，虚线为B样条曲线平滑处理后的圆弧直线泊车轨迹。
同时参见图5所示，本发明实施例还提供了一种基于库位跟踪的泊车系统100，包括：库位角点获取模块110、误差处理模块120、泊车轨迹模块130及泊车操作模块140；
库位角点获取模块110，用于当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；
误差处理模块120，与所述库位角点获取模块110通信连接，用于根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；
泊车轨迹模块130，与所述误差处理模块120通信连接，用于基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；以及，
泊车操作模块140，与所述泊车轨迹模块130通信连接，用于控制车辆按照所述圆弧直线泊车轨迹进行泊车操作。
本发明通过采用卡尔曼滤波减小由传感器带来的偏差问题，即当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；控制车辆按照所述圆弧直线泊车轨迹进行泊车操作；因此可在不占用较大算力的基础上解决控制和感知带来的误差积累问题。
同时，采用B样条曲线，在此曲率不连续处进行基于B样条曲线的曲线平滑处理，经过B样条平滑处理后获得曲率连续的泊车轨迹，符合车辆动力学模型，无需原地打方向盘。
具体的，本实施例与上述方法实施例一一对应，各个模块的功能在相应的方法实施例中已经进行详细说明，因此不再一一赘述。
基于同一发明构思，本申请实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现上述方法的所有方法步骤或部分方法步骤。
本发明实现上述方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，计算机程序包括计算机程序代码，计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读介质可以包括：能够携带计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，RandomAccess Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
基于同一发明构思，本申请实施例还提供一种电子设备，包括存储器和处理器，存储器上储存有在处理器上运行的计算机程序，处理器执行计算机程序时实现上述方法中的所有方法步骤或部分方法步骤。
所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，处理器是计算机装置的控制中心，利用各种接口和线路连接整个计算机装置的各个部分。
存储器可用于存储计算机程序和/或模块，处理器通过运行或执行存储在存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现计算机装置的各种功能。存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(例如声音播放功能、图像播放功能等)；存储数据区可存储根据手机的使用所创建的数据(例如音频数据、视频数据等)。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(SmartMedia Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、服务器或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。
本发明是参照根据本发明实施例的方法、设备(系统)、服务器和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。
Claims (10)
1.一种基于库位跟踪的泊车方法，其特征在于，包括以下步骤：
当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；
根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；
基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；
控制车辆按照所述圆弧直线泊车轨迹进行泊车操作。
2.如权利要求1所述的基于库位跟踪的泊车方法，其特征在于，所述“根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理”步骤，具体包括以下步骤：
根据所述车辆运行信息及所述库位角点信息，获取车辆的车辆状态矩阵、系统观测矩阵、状态转移函数雅可比矩阵、测量函数雅可比矩阵；
根据所述车辆状态矩阵及状态转移函数雅可比矩阵，预测车辆当前时刻的系统状态变量初步估计值；
根据状态转移函数雅可比矩阵，预测车辆当前时刻的协方差矩阵初步估计值；
根据所述测量函数雅可比矩阵及所述协方差矩阵初步估计值，获取车辆当前时刻的卡尔曼滤波增益矩阵；
根据所述系统观测矩阵、所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及系统状态变量初步估计值，更新车辆当前时刻的系统状态变量估计值；
根据所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及所述协方差矩阵初步估计值，更新车辆当前时刻的协方差矩阵估计值。
3.如权利要求2所述的基于库位跟踪的泊车方法，其特征在于，所述“根据所述车辆运行信息及所述库位角点信息，获取车辆的车辆状态矩阵、系统观测矩阵、状态转移函数雅可比矩阵、测量函数雅可比矩阵”步骤，具体包括以下步骤：
根据所述车辆运行信息及所述库位角点信息；
获取车辆状态矩阵如下：
获取系统观测矩阵如下：
获取状态转移函数雅可比矩阵如下：
获取测量函数雅可比矩阵如下：
式中，k为车辆当前时刻，x(k)为车辆横坐标，x(k-1)为k上一时刻的车辆横坐标，y(k)为车辆纵坐标，y(k-1)为k上一时刻的车辆纵坐标，φ(k)为车辆航向角，φ(k-1)为k上一时刻的车辆航向角，x1(k)为库位角点横坐标，x(k-1)为k上一时刻的库位角点横坐标，y1(k)为库位角点纵坐标，y1(k-1)为k上一时刻的库位角纵坐标，V为车辆速度，Δt为预测时域，L为车辆轴距，γ为车辆前轮转角，ri(k)为预测k时刻车辆前进直线距离，θi(k)为预测k时刻车辆航向变化量，xi(k)为预测k时刻车辆横坐标，yi(k)为预测k时刻车辆纵坐标；
w(k)＝(wx(k),wy(k),wφ(k),0,0)T为过程噪声，v(k)＝(vr(k),vθ(k))T为测量噪声，
为车辆与库位角点的距离。
4.如权利要求3所述的基于库位跟踪的泊车方法，其特征在于，所述“根据所述车辆状态矩阵及状态转移函数雅可比矩阵，预测车辆当前时刻的系统状态变量初步估计值；根据状态转移函数雅可比矩阵，预测车辆当前时刻的协方差矩阵初步估计值”步骤，具体包括以下步骤：
根据所述车辆状态矩阵X(K)，及状态转移函数雅可比矩阵F(k)，预测车辆当前时刻的系统状态变量初步估计值如下：
根据状态转移函数雅可比矩阵F(k)，预测车辆当前时刻的协方差矩阵初步估计值如下：
P(k)-＝F(k)P(k-1)F(k)T+Q式(6)；
式中，F(k)T为状态转移函数雅可比矩阵F(k)的转置矩阵，
为k上一时刻的系统状态变量初步估计值，P(k-1)为k上一时刻的协方差矩阵初步估计值，R为测量噪声协方差矩阵，Q为状态转移协方差矩阵，X(K)为
或
包含的状态量。
5.如权利要求4所述的基于库位跟踪的泊车方法，其特征在于，所述“根据所述测量函数雅可比矩阵及所述协方差矩阵的初步估计值，获取车辆当前时刻的卡尔曼滤波增益矩阵”步骤，具体包括以下步骤：
根据所述测量函数雅可比矩阵H(k)，及所述协方差矩阵初步估计值P(k)-，获取车辆当前时刻的卡尔曼滤波增益矩阵如下：
K(k)＝P(k)-H(k)T(H(k)P(k)-H(k)T+R)-1式(7)；
式中，H(k)T为测量函数雅可比矩阵H(k)的转置矩阵。
6.如权利要求4所述的基于库位跟踪的泊车方法，其特征在于，所述“根据所述系统观测矩阵、所述测量函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及系统状态变量初步估计值，更新车辆当前时刻的系统状态变量估计值；根据所述状态转移函数雅可比矩阵、所述卡尔曼滤波增益矩阵、及所述协方差矩阵初步估计值，更新车辆当前时刻的协方差矩阵估计值”步骤，具体包括以下步骤：
根据所述系统观测矩阵Z(k)、所述测量函数雅可比矩阵H(k)、所述卡尔曼滤波增益矩阵K(k)、及系统状态变量初步估计值
更新车辆当前时刻的系统状态变量估计值如下：
根据所述测量函数雅可比矩阵H(k)、所述卡尔曼滤波增益矩阵K(k)、及所述协方差矩阵初步估计值P(k)-，更新车辆当前时刻的协方差矩阵估计值如下：
P(k)＝(I-K(k)H(k))P(k)-式(9)。
7.如权利要求1所述的基于库位跟踪的泊车方法，其特征在于，所述“基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；控制车辆按照所述圆弧直线泊车轨迹进行泊车操作”步骤，具体包括以下步骤：
基于误差处理后的库位角点信息生成圆弧直线泊车轨迹，并对所述圆弧直线泊车轨迹进行B样条曲线平滑处理；
控制车辆按照B样条曲线平滑处理后的圆弧直线泊车轨迹进行泊车操作。
8.如权利要求7所述的基于库位跟踪的泊车方法，其特征在于，所述“对所述圆弧直线泊车轨迹进行B样条曲线平滑处理”步骤，具体包括以下步骤：
在所述圆弧直线泊车轨迹上选取多个控制点，根据多个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹。
9.如权利要求8所述的基于库位跟踪的泊车方法，其特征在于，所述“在所述圆弧直线泊车轨迹上选取多个控制点，根据多个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹”步骤，具体包括以下步骤：
在所述圆弧直线泊车轨迹上选取多个控制点，所述控制点为7个；
第一控制点为圆弧直线泊车轨迹的初始点，第二控制点为初始点的邻近点，第三控制点为圆弧直线泊车轨迹的终点，第四控制点为终点的领近点，第五控制点和第六控制点为圆弧直线泊车轨迹上圆弧与直线的两个交点，第七控制点为圆弧直线泊车轨迹上圆弧的中点；
根据7个所述控制点、并基于所述圆弧直线泊车轨迹生成B样条泊车轨迹如下：
其中，Bj,3(s)为三次B样条基函数矩阵，
式中，Ci+j为圆弧直线泊车轨迹第i段中第j个控制点，s为参数，s∈[0,1]。
10.一种基于库位跟踪的泊车系统，其特征在于，包括：
库位角点获取模块，用于当检测到车辆进行泊车操作时，获取车辆运行信息及车位的库位角点信息；
误差处理模块，与所述库位角点获取模块通信连接，用于根据所述车辆运行信息，并通过卡尔曼滤波对所述库位角点信息进行误差处理；
泊车轨迹模块，与所述误差处理模块通信连接，用于基于误差处理后的库位角点信息生成圆弧直线泊车轨迹；以及，
泊车操作模块，与所述泊车轨迹模块通信连接，用于控制车辆按照所述圆弧直线泊车轨迹进行泊车操作。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210983628.2A CN115465263A (zh) | 2022-08-16 | 2022-08-16 | 基于库位跟踪的泊车方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210983628.2A CN115465263A (zh) | 2022-08-16 | 2022-08-16 | 基于库位跟踪的泊车方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115465263A true CN115465263A (zh) | 2022-12-13 |
Family
ID=84366040
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210983628.2A Pending CN115465263A (zh) | 2022-08-16 | 2022-08-16 | 基于库位跟踪的泊车方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115465263A (zh) |
-
2022
- 2022-08-16 CN CN202210983628.2A patent/CN115465263A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110271534B (zh) | 自动驾驶车辆的控制方法、装置、计算机设备和存储介质 | |
| CN110136222B (zh) | 虚拟车道线生成方法、装置及系统 | |
| CN110316193B (zh) | 预瞄距离的设置方法、装置、设备及计算机可读存储介质 | |
| CN111123904B (zh) | 路径跟踪方法及终端设备 | |
| CN109795477B (zh) | 消除稳态横向偏差的方法、装置及存储介质 | |
| CN111121777A (zh) | 无人驾驶设备轨迹规划方法、装置、电子设备和存储介质 | |
| CN111177934B (zh) | 参考路径规划的方法、设备和存储介质 | |
| CN111123951B (zh) | 一种双足机器人及其轨迹跟随方法和装置 | |
| CN112148016A (zh) | 基于模型预测控制算法横纵向解耦的车辆控制方法和装置 | |
| CN115635967A (zh) | 一种车辆控制方法、装置、车载终端及存储介质 | |
| CN106364484A (zh) | 用于校正停车导向线的停车转向辅助系统及方法 | |
| JP6972377B2 (ja) | 目標軌跡生成装置、車両制御装置、目標軌跡生成方法および車両制御方法 | |
| CN115465263A (zh) | 基于库位跟踪的泊车方法及系统 | |
| CN115675454B (zh) | 车辆碰撞识别方法、车载终端、车辆及存储介质 | |
| CN117022249A (zh) | 平行车位自动泊车轨迹规划方法、系统、介质及设备 | |
| CN116560370A (zh) | 预瞄距离调整方法及装置 | |
| CN115303288A (zh) | 一种车辆控制方法、控制装置及摄像头装置 | |
| CN116070903A (zh) | 通过障碍物区域的风险确定方法、装置以及电子设备 | |
| CN113156962B (zh) | 运动控制方法、装置、机器人及存储介质 | |
| CN113568403A (zh) | 处理方法、处理装置、车辆及存储介质 | |
| CN113548038B (zh) | 一种泊车曲率控制方法及系统、控制设备、存储介质 | |
| CN114089742B (zh) | 基于路径曲率的agv行驶车速控制方法、装置及介质 | |
| CN115973266B (zh) | 方向盘死区的扭矩补偿方法、装置、计算机设备、介质 | |
| CN116461499B (zh) | 一种泊车控制方法及装置 | |
| CN116990045B (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 |