CN105197010A - 辅助泊车系统以及辅助泊车控制方法 - Google Patents

Abstract

本发明公开了一种辅助泊车系统泊车路径规划方法和泊车运动控制方法。基于B样条理论的泊车路径规划方法设计出多阶平滑的泊车路径满足车辆避障约束、最小转弯半径约束、最高转向速度等多个非线性约束。路径曲率连续性避免了原地转向现象降低了轮胎磨损和转向电机负荷，路径的多阶平滑性易于车辆跟踪。本发明提出的泊车运动控制策略，降低了泊车系统对驾驶员泊车速度要求，驾驶员只需控制车辆低速行驶即可。针对泊车过程中因车速过高等因素导致车辆偏离目标路径而不能泊车入位现象，提出基于定点跟踪控制的车辆位姿调整策略，增加了泊车成功率，并采用定点跟踪控制使车辆与车位平行停放，实现了车辆停放规范性。

Description

辅助泊车系统以及辅助泊车控制方法

技术领域

本发明涉及一种停车泊位的装置和方法，特别涉及一种辅助泊车系统以及辅助泊车控制方法。

背景技术

随着车辆数量的增加使城市有限的空间变得逐渐拥挤，泊车空间的缩小增加了泊车困难，因此人们对辅助泊车系统的需求日益增加。

现有泊车入位控制方法主要有：(1)基于经验知识的方法；(2)基于路径规划的方法。

基于经验知识的方法采用模糊控制、神经网络、遗传算法等模拟驾驶员泊车操作行为，该方法样本数据获取较困难，控制逻辑结构不易获取，控制规则不易训练。

基于路径规划方法由路径规划和路径跟踪控制组成。路径规划方法主要采用圆弧-直线法规划泊车路径。由于圆弧和直线组成的泊车路径曲率不连续，泊车过程中需停车转向。停车转向现象导致轮胎磨损加速转向控制电机工作负荷增大。泊车过程中，车辆跟踪圆弧直线路径到达圆弧和直线连接点或圆弧和圆弧连接点处时，需驾驶员降低车速甚至停车配合方向盘转向动作。车速过高或未完全停车，会导致泊车系统失去最佳转向时机，车辆偏离目标路径，最终导致泊车入位失败，泊车过程中对驾驶员车速控制要求高。

实际泊车操作中，当一次不能泊车入位时，驾驶员将车辆向前行驶小段距离后继续后退泊车，针对一次不能泊车入位情况，现有泊车系统没有补救措施。

针对车辆最后停放未与车位平行现象，现有泊车系统没有停放调整措施。

发明内容

本发明所要解决的技术问题是提供一种辅助泊车系统以及辅助泊车控制方法，

本发明为解决上述问题，而采用以下技术方案，提供一种辅助泊车系统，包括路径存储器，泊车系统感应单元，泊车运动控制器，转向执行机构，泊车系统人机交互系统，所述路径存储器，用于存储根据车辆结构参数，采取B样条曲线方式确定最小泊车位的泊车路径数据；所述泊车系统感应单元，用于感应车辆相对于车位的具体位置，并将感应数据传输给所述泊车运动控制器；所述泊车运动控制器，用于根据路径存储器提供的数据以及泊车系统感知单元提供的所需泊车的具体位置，确定运动控制策略并计算方向盘转角值。所述转向执行机构，用于接收该泊车运动控制器的信号，驱动车辆转向系统，实现泊车运动控制器计算的方向盘转角值；所述泊车系统人机交互系统，用于将车位检测结果、换挡操作示、周围障碍物存在情况向驾驶员展示，并将驾驶员操作指令传递给泊车系统。所述泊车系统人机交互系统通过CAN通信方式与泊车系统通信，接收泊车系统发送的提示信息并以图像形式展现出来，通过触摸屏获取驾驶员操作指令，并将指令通过CAN通信方式发送给控制器。

为解决现有技术中存在的问题，本申请还提供了一种辅助泊车控制方法，包括以下步骤：

步骤一，泊车路径规划；根据车辆结构参数确定车辆所需最小泊车位空间，建立全局坐标系，采用B样条曲线设计车辆所需最小泊车位的泊车路径；该最小泊车位的泊车路径为路径曲线，将该路径曲线以数据形式固化于泊车路径存储器。

步骤二，车位感应提示；开启泊车系统，泊车系统感应单元开始检测右侧车位，并通过显示屏幕车辆与右侧车位的侧向距离。当检测到右侧车位满足最小车位需求，泊车系统根据车辆与车位侧向距离判断车位是否有效，对于有效车位，在车辆尾端与车位起始点距离大于7m时，人机交互屏幕显示已检测到车位并提示停车。

步骤三，泊车运动控制；泊车系统读入泊车路径，然后进入运动控制；在泊车运动控制中首先进入路径跟踪控制状态，当车辆X坐标大于x_flag时，泊车系统一直采取路径跟踪控制，当X坐标等于x_flag时，泊车系统根据车辆与目标路径的偏差判断启动过定点控制或继续采用路径跟踪控制，当跟踪偏差均小于设定值，则继续采用路径跟踪控制，当跟踪偏差大于设定值，则启动跟踪定点控制。其中，x_flag＝l_c-K_flag·(L_r+L)，K_flag取值范围为：0.4～0.7，实际车位越小，K_flag取值应越小。

步骤四，车辆停放调整；车辆进入车位后，若车辆未与车位平行，泊车系统采用过定点控制率调整车辆与车位平行。

进一步，所述步骤一包括以下步骤：

(1)根据车辆结构参数确定车辆所需最小车位长度

$l_{c\min }=L_r+\sqrt{{(R_{\min }+L_k/2)}^2+{(L+L_f)}^2-{(R_{\min }-L_k/2)}^2}+l_{\mathrm{csafe}}$

以及最小车位宽度；

$l_{k\min }=L_k/2+\sqrt{{(R_{\min }+L_k/2)}^2+L_r^2}-R_{\min }+l_{\mathrm{ksafe}}$

其中：l_cmin为所需最小车位长度，l_kmin为所需最小车位宽度，L_r为车辆后轴到车辆尾端距离，R_min为车辆后轴中点最小转弯半径，L_k为车辆宽度，L为车辆轴距，L_f为车辆前轴到车辆前端距离，l_csafe为车位长度安全余量，l_ksafe为车位宽度安全余量。

(2)以车位外侧起始点为坐标原点建立全局坐标系，选择4次5阶B样条控制点的样条曲线设计泊车路径，设定约束条件，选取控制点个数，通过优化算法计算出B样条曲线曲率最小化路径曲线。

进一步，所述约束条件包括：1)车辆沿最小化路径曲线行驶与停车位三侧面不发生碰撞；2)车辆到达路径终点时完全进入车位且与车位平行；3)曲线最大曲率不大于1/R_min；4)车辆以v_max车速沿路径曲线行驶时，前轴等效转角转速ω不大于ω_max，v_max为泊车过程允许最高车速，取值0.5～1.5m/s，ω_max为车辆转向轴最高转向角速度。

进一步，所述步骤二中侧向距离保持在0.5～1.5m。

进一步，所述步骤三中的目标路径偏差包括侧向距离偏差和航向角偏差，当车辆与目标路径侧向距离偏差|y-y_r|≤Δy_o且航向角偏差|θ-θ_r|≤Δθ_o，泊车控制策略继续采用路径跟踪控制，若车辆与目标路径侧向距离偏差|y-y_r|>Δy_o或航向角偏差|θ-θ_r|>Δθ_o，泊车控制策略采用车辆过定点控制，调整车辆位姿；其中：y为车辆后轴中点Y轴坐标，y_r为过车辆后轴中点且与X坐标轴相垂直的直线与路径交点处的Y轴坐标，Δy_o最大侧向距离偏差阀值，θ为车辆对称线与坐标系X轴夹角，θ_r为过车辆后轴中点且与X坐标轴相垂直的直线与路径相交点处的路径切线与X坐标轴夹角，Δθ_o为航向角偏差阀值。

进一步，所述侧向距离偏差阀值：Δy_o＝(l_c-l_cmin)/K₂，K₂取值范围为：10～100，l_c越小取值应越大；航向角偏差阀值：Δθ_o＝(l_c-l_cmin)/K₃，K₃取值范围：50～100，l_c越小取值应越大；式中l_c为实际车位长度，l_cmin为车辆所需最小车位长度。

进一步所述路径跟踪控制，其车辆控制率为：

式中x为车辆101后轴中点106的X坐标，y为车辆101后轴中点106的Y轴坐标，θ为车辆对称线与坐标系X轴的夹角，即车辆航向角，y_r为车辆x坐标对应目标路径上点P_r的Y轴坐标，θ_r为P_r点处切线与坐标系X轴夹角，即车辆目标航向角，ρ_r为泊车路径301在P_r点的曲率，L为车辆101后轴到前轴距离，k₁为路径跟踪侧向距离偏差系数，k₂为路径跟踪航向角偏差系数，车辆前进行驶时k₁、k₂均取负值，车辆后退行驶时k₁取负值，k₂取正值，为前轴等效转角，泊车控制器根据转向执行结构转向传动比得到方向盘转向角，由转向执行机构实现转向，为正值方向盘右转，负值方向盘左转。

进一步，所述跟踪定点控制，其车辆跟踪定点转向控制率为：

式中，x为车辆后轴中点X坐标，y为车辆后轴中点Y轴坐标，θ为车辆轴线与X轴夹角即车辆航向角，x_p、y_p为跟踪目标定点的位置，θ_p为目标定点处航向角。L为车辆轴距，k₃为定点跟踪位置偏差系数，k₄为定点跟踪航向角偏差系数，车辆前进行驶时，k₃取正值，k₄取正值，车辆后退行驶时，k₃取正直，k₄取负值，为前轴104等效转角，为正值方向盘右转，负值方向盘左转。

本发明根据上述技术方案可以得到如下技术效果：

(1)采用样条曲线规划多阶光滑的泊车路径方法避免停车转向现象，降低了轮胎磨损和电机工作负荷。

(2)基于非时间参考的路径跟踪控制方法对车速控制要求低，要求驾驶员不高于v_max的速度平稳行驶即可，更容易操作，缓解了泊车过程中驾驶员紧张心理。

(3)针对车辆偏离目标路径，一次不能泊车入位情况，采用过定点控制策略调整车辆位姿，使车辆重新沿预定路径行驶，提高了泊车成功率。

(4)针对车辆未与车位平行停放现象，采用过定点控制策略调整车辆位姿，实现车辆与车位平行停放

附图说明

图1是本发明的待泊车辆结构及参数图

图2是基于B样条曲线的泊车路径规划示意图

图3是车辆跟踪泊车路径转向控制率分析图

图4是车辆跟踪定点转向控制率分析图

图5是对一次不能成功泊车入位车辆调整策略示意图

图6是辅助泊车系统示意图

图7是辅助泊车系统工作流程图

图8是泊车运动控制流程图

图9是车辆停放调整控制流程图

其中：101、待停车辆；102、方向盘；103、转向系统；104、车辆前轴：105、车辆后轴；106、后轴中点；107、车辆对称轴线；201～212、B样条曲线控制点；301、泊车路径；401、车辆实际行驶路径；402、车辆行驶方向标示；A、车位后方物体；B、道路；C、车位前方物体；D车位边界线。

具体实施方式

下面结合附图进一步描述该辅助泊车控制方法，附图7示出了该辅助泊车控制方法的工作流程图。首先，将泊车路径规划曲线以数据形式固化于泊车路径存储器中，开启泊车系统，泊车系统感应单元开始检测如附图2示出的由车位边界线D围成的右侧车位，并通过显示屏幕车辆与右侧车位的侧向距离。当检测到右侧车位满足泊车路径存储器中存储的最小车位需求时，泊车系统根据车辆与车位侧向距离判断车位是否有效，当侧向距离为0.3～2m，在车辆尾端与车位起始点距离大于7m时，人机交互屏幕显示已检测到车位并提示停车。接着，进行泊车运动控制；将车辆驶入车位。如果车辆与车位平行，则泊车控制结束，否则，启动车辆停放调整，泊车系统采用过定点控制率调整车辆与车位平行。

附图1、2示出了该控制方法的泊车路径规划。首先要根据附图1的车辆参数，确定待停车辆所需要的最小车位。根据车辆结构参数确定车辆所需最小车位长度

$l_{c\min }=L_r+\sqrt{{(R_{\min }+L_k/2)}^2+{(L+L_f)}^2-{(R_{\min }-L_k/2)}^2}+l_{\mathrm{csafe}}$

以及最小车位宽度；

$l_{k\min }=L_k/2+\sqrt{{(R_{\min }+L_k/2)}^2+L_r^2}-R_{\min }+l_{\mathrm{ksafe}}$

其中：式中L_k为车辆101宽度，L_r为车辆后轴105到尾部长度，L为车辆101前轴104到后轴105距离，L_f为车辆101前轴104到前端距离，R_min为后轴105中点106最小转弯半径，为前轴最大等效转角，ω_max为前轴等效转角最高转速，v_max为设定的泊车过程最高车速，v_max根据前轴最高转速确定泊车最高车速，取值范围0.1～1.5m/s，l_ksafe为最小车位宽度安全余量，取值大于等于0.1m。

其次，如图2所示，以车位外侧起始点为坐标原点建立全局坐标系，选择4次5阶B样条控制点的样条曲线设计泊车路径，控制点个数n个，n取值范围：12～15，路径控制点记为：Q₁，Q₂，…，Q_n。由n个控制点Q₁～Q_n的坐标和根据公式(a)可得到泊车路径曲线。由n个控制点Q₁～Q_n的坐标联合公式(a)、(b)、(e)、(f)可得到车辆沿泊车路径行驶时，车辆轮廓4个顶点的轨迹。由n个控制点Q₁～Q_n的坐标可联合(a)、(b)、(e)得到车辆沿泊车路径行驶时车身在路径各点处的航向角。路径曲线曲率根据控制点Q₁～Q_n的坐标并结合公式(a)、(b)、(c)、(g)得到。车辆以车速v_max沿路径行驶时，前轴等效转角转向角速度ω根据控制点Q₁～Q_n的坐标和公式(a)、(b)、(c)、(d)、(h)得到。

本发明选择12个控制点规划泊车路径，如图2所示。采用遗传算法以控制点201～212坐标为优化变量，以B样条曲线曲率最小化为目标，以以下约束对泊车路径进行优化：1)车辆101沿路径行驶与周围物体A、B、C不发生碰撞；2)车辆到达路径终点时完全进入车位且与车位平行3)曲线最大曲率不大于1/R_min；4)车辆以v_max车速沿泊车路径行驶时，车辆前轴等效转角转速不大于车辆转向系统最高转速ω_max。ω_max为前轴105转向轮最高转速，v_max为设定的泊车过程中最高车速，取值0.5～1.5m/s，L为车辆轴距，ρ为泊车路径曲率。

为降低泊车控制器路径规划计算量，将规划的B样条泊车路径存储到控制系统中，当检测到车位满足车辆最小车位需求时，泊车控制系统读取目标路径并控制车辆跟踪目标路径。

$G=\left[\begin{array}{ccccc}1& -4& 6& -4& 1\\ -4& 12& -12& 4& 0\\ 6& -6& -6& 6& 0\\ -4& -12& 12& 4& 0\\ 1& 11& 11& 1& 0\end{array}\right]$

Q＝[Q_iQ_i+1Q_i+2Q_i+3Q_i+4]′,i＝1,2,n-4

i表示第i段B样条曲线，n为控制点个数，Q_i～Q_i+4,为产生第i段B样条曲线的5制点坐标，n个控制点生成的B样条路径是由n-4段B样条平滑连接组成。

u∈[0,1]，为每段B样条曲线的无量纲变量。

$\left[\begin{array}{cc}x& y\end{array}\right]=\frac{1}{24}\left[\begin{array}{ccccc}{u}^4& u^3& u^2& u& 1\end{array}\right]\·G\·Q---\left(a\right)$

式(a)中x为样条曲线在X轴坐标，y为样条曲线在Y轴坐标，u为B样条曲线变量，Q为B样条曲线控制点。

$\left[\begin{array}{cc}\stackrel{\·}{x}& \stackrel{\·}{y}\end{array}\right]=\frac{1}{24}\left[\begin{array}{ccccc}{4u}^3& 3u^2& 2u& 1& 0\end{array}\right]\·G\·Q---\left(b\right)$

式(b)中为路径X轴坐标对u的一阶导数，为路径Y轴坐标对u的一阶导数，u为B样条曲线变量，Q为B样条曲线控制点。

$\left[\begin{array}{cc}\stackrel{\·\·}{x}& \stackrel{\·\·}{y}\end{array}\right]=\frac{1}{24}\left[\begin{array}{ccccc}12u^2& 6u& 2& 0& 0\end{array}\right]\·G\·Q---\left(c\right)$

式(c)中为路径X轴坐标对u的二阶导数，为路径Y轴坐标对u的二阶导数，u为B样条曲线变量，Q为B样条曲线控制点。

$\left[\begin{array}{cc}\stackrel{\·\·\·}{x}& \stackrel{\·\·\·}{y}\end{array}\right]=\frac{1}{24}\left[\begin{array}{ccccc}24u& 6& 0& 0& 0\end{array}\right]\·G\·Q---\left(d\right)$

式(d)中为路径X轴坐标对u的三阶导数，为路径Y轴坐标对u的三阶导数，u为B样条曲线变量，Q为B样条曲线控制点。

$\mathrm{\θ}=\arctan \left(\frac{\stackrel{\·}{y}}{\stackrel{\·}{x}}\right)---\left(e\right)$

式(e)中，θ为B样条曲线切线与X轴夹角，为路径X轴坐标的一阶导数，为路径Y轴坐标的一阶导数

$\left\{\begin{array}{c}{x}_{\mathrm{zq}}=x+(L_f+L)\·\cos \mathrm{\θ}-L_k/2\·\sin \mathrm{\θ};\\ y_{\mathrm{zq}}=x+(L_f+L)\·\sin \mathrm{\θ}+L_k/2\·\cos \mathrm{\θ};\\ x_{\mathrm{yq}}=x+(L_f+L)\·\cos \mathrm{\θ}+L_k/2\·\sin \mathrm{\θ};\\ y_{\mathrm{yq}}=x+(L_f+L)\·\sin \mathrm{\θ}-L_k/2\·\cos \mathrm{\θ};\\ x_{\mathrm{yh}}=x-L_r\·\cos \mathrm{\θ}+L_k/2\·\sin \mathrm{\θ};\\ y_{\mathrm{yh}}=y-L_r\·\sin \mathrm{\θ}-L_k/2\·\cos \mathrm{\θ};\\ x_{\mathrm{zh}}=x-L_r\·\cos \mathrm{\θ}-L_k/2\·\sin \mathrm{\θ};\\ y_{\mathrm{zh}}=y-L_r\·\sin \mathrm{\θ}+L_k/2\·\cos \mathrm{\θ};\end{array}\right.---\left(f\right)$

式(f)中，x为样条曲线在X轴坐标，y为样条曲线在Y轴坐标，θ为样条曲线切线与X轴夹角，L为车辆轴距，L_f为车辆前轴到前端距离，L_r为车辆后轴到车辆后端距离，L_k为车辆宽度，x_zq为车辆左前端X轴坐标，y_zq为车辆左前端Y轴坐标，x_yq为车辆右前端X轴坐标，y_yq为车辆右前端Y轴坐标，x_yh为车辆右后端X轴坐标，y_yh为车辆右后端Y轴坐标，x_zh为车辆左后端X轴坐标，y_zh为车辆左后端Y轴坐标。

$\mathrm{\ρ}=\frac{\stackrel{\·}{x}\stackrel{\·\·}{y}-\stackrel{\·\·}{x}\stackrel{\·}{y}}{{({\stackrel{\·}{x}}^2+{\stackrel{\·}{y}}^2)}^{3/2}}---\left(g\right)$

式(g)中，ρ为路径曲率，路径X轴坐标一阶导数，为路劲Y轴坐标一阶导数为路径X轴坐标二阶导数，为路径Y轴坐标二阶导数。

$\mathrm{\ω}=\frac{v_{\max }(L(\stackrel{\·}{x}\·\stackrel{\·\·\·}{y}-\stackrel{\·\·\·}{x}\·\stackrel{\·}{y})({\stackrel{\·}{x}}^2+{\stackrel{\·}{y}}^2)-3L(\stackrel{\·}{x}\stackrel{\·\·}{y}-\stackrel{\·\·}{x}\stackrel{\·}{y})(\stackrel{\·}{x}\stackrel{\·\·}{x}+\stackrel{\·}{y}\stackrel{\·\·}{y}))}{{({\stackrel{\·}{x}}^2+{\stackrel{\·}{y}}^2)}^3+L^2{(\stackrel{\·}{x}\stackrel{\·}{y}-\stackrel{\·\·}{x}\stackrel{\·}{y})}^2}---\left(h\right)$

式(h)中，v_max为泊车最高车速，ω为车辆以v_max的车速沿样条曲线行驶时前轴等效转角转速，路径X轴坐标一阶导数，为路劲Y轴坐标一阶导数为路径X轴坐标二阶导数，为路径Y轴坐标二阶导数。

最后，将完成的路径曲线以数据形式固化在泊车路径存储器中。

附图3-5、8示出了该方法的步骤三的泊车运动控制。如附图3所示，泊车系统运动控制率选择参考量：x_flag＝l_c-K_flag·(L_r+L)，K_flag取值范围为：0.4～0.7，实际车位越小，K_flag取值应越小。当车辆X坐标大于x_flag时，泊车系统一直采取路径跟踪控制，当X坐标等于x_flag时，泊车系统根据车辆与目标路径的偏差判断启动过定点控制或继续采用路径跟踪控制，当跟踪偏差均小于设定值，则继续采用路径跟踪控制，否则，启动跟踪定点控制。目标路径偏差包括侧向距离偏差和航向角偏差，当车辆与目标路径侧向距离偏差|y-y_r|≤Δy_o且航向角偏差|θ-θ_r|≤Δθ_o，泊车控制策略继续采用路径跟踪控制，若车辆与目标路径侧向距离偏差|y-y_r|>Δy_o或航向角偏差|θ-θ_r|>Δθ_o，泊车控制策略采用车辆过定点控制，调整车辆位姿；其中：y为车辆后轴中点Y轴坐标，y_r为过车辆后轴中点且与X坐标轴相垂直的直线与路径交点处的Y轴坐标，Δy_o侧向距离偏差阀值，θ为车辆对称线与坐标系X轴夹角，θ_r为过车辆后轴中点且与X坐标轴相垂直的直线与路径相交点处的路径切线与X坐标轴夹角，Δθ_o为航向角偏差阀值。

车辆跟踪偏差设定如下：

侧向距离偏差阀值：Δy_o＝(l_c-l_cmin)/K₂，K₂取值范围为：10～100，l_c越小取值应越大；

航向角偏差阀值：Δθ_o＝(l_c-l_cmin)/K₃，K₃取值范围：50～100，l_c越小取值应越大；

式中l_c为实际车位长度，l_cmin为车辆所需最小车位长度。

车辆从泊车起始点后退行驶，当车辆101后轴中点106在X轴坐标大于x_flag时，泊车控制器进行路径跟踪控制，跟踪偏差如图3所示，车辆运动控制率为：

式中x为车辆101后轴中点106的X坐标，y为车辆101后轴中点106的Y轴坐标，θθ为车辆对称线与坐标系X轴的夹角，即车辆航向角，y_r为车辆X坐标对应目标路径上点P_r的Y轴坐标，θ_r为P_r点处切线与坐标系X轴夹角，即车辆目标航向角，ρ_r为泊车路径301在P_r点的曲率，L为车辆101后轴105到前轴104距离，k₁为路径跟踪位置偏差系数，k₂为路径跟踪航向角偏差系数，车辆前进行驶时k₁、k₂均取负值，车辆后退行驶时k₁取负值，k₂取正值，为前轴05等效转角，泊车控制器根据转向执行结构103转向传动比得到方向盘102转向角，由转向执行机构103实现转向，为正值方向盘右转，负值方向盘左转；

图4为车辆定点跟踪偏差示意图，P点为泊车系统启动定点跟踪控制后要跟踪的点。跟踪定点转向控制率为：

式中，x为车辆后轴中点X坐标，y为车辆后轴中点Y轴坐标，θ为车辆轴线与X轴夹角即车辆航向角，x_p、y_p为跟踪目标定点的位置，θ_p为目标定点处航向角。L为车辆101轴距，k₃为定点跟踪位置偏差系数，k₄为定点跟踪航向角偏差系数，车辆前进行驶时，k₃取正值，k₄取正值，车辆后退行驶时，k₃取正直，k₄取负值，为前轴104等效转角，为正值方向盘右转，负值方向盘左转。

如图5所示。记目标路径上X轴坐标等于x_flag的点为P₂，P₂点切线上选取一点记为P₁，P₂点处位姿表示为(x₂,_y2,θ₂)，P₁点位姿为(x₂+K₁·cosθ₁,y₂+K₁·sinθ₁,θ₂),K₁取值范围：0.5～2。

记目标路径终点为P₃，P₄为P₃正前方一点，且P₄点距物体C不小于L+L_f+Δ_safe，Δ_safe为车位内泊车安全距离，根据泊车系统前后倒车雷达最小量程和测量精度确定安全值。车辆101后轴中点106X轴坐标等于x_flag时，若车辆位置偏差|y-y_r|≤Δy_o且|θ-θ_r|≤Δθ_o继续采用路径跟踪控制，否则泊车系统通过屏幕提示驾驶员前进行驶，泊车系统控制车辆跟踪定点P₁，当车辆X轴坐标大于P₁点X轴坐标或车辆Y轴坐标大于P₁点Y轴坐标时，泊车系统提示驾驶员后退行驶，泊车系统控制车辆跟踪P₂点，并在车辆X轴坐标等于x_flag时根据跟踪偏差判断采用路径跟踪控制还是定点跟踪控制。

如附图9所示，车辆沿目标路径到达P₃点或后方物体A距离小于Δ_safe时，若车辆未能平行，泊车系统提示驾驶员前进行驶，并控制车辆跟踪P₄点，当车辆X坐标大于P₄点X坐标或车辆前端与物体C距离小于Δ_safe时，若车辆仍未与道路C平行，泊车系统提示驾驶员后退行驶，控制车辆跟踪P₃点。泊车系统通过跟踪P₃和P₄点调整车辆101航向角，使车辆101与道路B平行。

本发明辅助泊车系统如图6所示，包括路径存储器，泊车系统感应单元，泊车运动控制器，转向执行机构，泊车系统人机交互系统，所述路径存储器，用于存储根据车辆结构参数，采取B样条曲线方式确定最小泊车位的泊车路径数据；所述泊车系统感应单元，用于感应车辆相对于车位的具体位置，并将感应数据传输给所述泊车运动控制器；所述泊车运动控制器，用于根据路径存储器提供的数据以及泊车系统感知单元提供的所需泊车的具体位置，判断车辆行驶方向和计算方向盘转角值。所述转向执行机构，用于接收该泊车运动控制器的信号，驱动车辆转向系统实现方向盘转角值；所述泊车系统人机交互系统，用于将车位检测结果、换挡操作示、周围障碍物存在情况向驾驶员展示，并将驾驶员操作指令传递给泊车系统。所述泊车系统人机交互系统通过CAN通信方式与泊车系统通信，接收泊车系统发送的提示信息并以图像形式展现出来，通过触摸屏获取驾驶员操作指令，并将指令通过CAN通信方式发送给控制器。

本说明书中所述的只是本发明的具体实施例，以上实施例仅用以说明本发明的技术方案而非对本发明的限制。凡本领域技术人员依本发明的构思通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在本发明的范围之内。

Claims (9)

1.一种辅助泊车控制方法，其特征在于：包括以下步骤

步骤一，泊车路径规划；根据车辆结构参数确定车辆所需最小泊车位空间，建立全局坐标系，采用B样条曲线设计车辆所需最小泊车位的泊车路径；该最小泊车位的泊车路径为路径曲线，将该路径曲线以数据形式固化于泊车路径存储器；

步骤二，车位感应提示；开启泊车系统，泊车系统感应单元开始检测右侧车位，并通过显示屏幕车辆与右侧车位的侧向距离。当检测到右侧车位满足最小车位需求，泊车系统根据车辆与车位侧向距离判断车位是否有效，对于有效车位，在车辆尾端与车位起始点距离大于7m时，人机交互屏幕显示已检测到车位并提示停车。

步骤三，泊车运动控制；泊车系统读入泊车路径，然后进入运动控制；在泊车运动控制中首先进入路径跟踪控制状态，当车辆X坐标大于x_flag时，泊车系统一直采取路径跟踪控制，当X坐标等于x_flag时，泊车系统根据车辆与目标路径的偏差判断启动过定点控制或继续采用路径跟踪控制，当跟踪偏差均小于设定值，则继续采用路径跟踪控制，否则，启动跟踪定点控制。其中，x_flag＝l_c-K_flag·(L_r+L)，K_flag取值范围为：0.4～0.7，实际车位越小，K_flag取值应越小。

步骤四，车辆停放调整；车辆进入车位后，若车辆未与车位平行，泊车系统采用过定点控制率调整车辆与车位平行。

2.根据权利要求1所述的一种辅助泊车控制方法，其特征在于：所述步骤一包括以下步骤：

(1)根据车辆结构参数确定车辆所需最小车位长度

$l_{c\min }=L_r+\sqrt{{(R_{\min }+L_k/2)}^2+{(L+L_f)}^2-{(R_{\min }-L_k/2)}^2}+l_{\mathrm{csafe}}$

以及最小车位宽度；

$l_{k\min }=L_k/2+\sqrt{{(R_{\min }+L_k/2)}^2+L_r^2}-R_{\min }+l_{\mathrm{ksafe}}$

其中：l_cmin为所需最小车位长度，l_kmin为所需最小车位宽度，L_r为车辆后轴到车辆尾端距离，R_min为车辆后轴中点最小转弯半径，L_k为车辆宽度，L为车辆轴距，L_f为车辆前轴到车辆前端距离，l_csafe为车位长度安全余量，l_ksafe为车位宽度安全余量。

(2)以车位外侧起始点为坐标原点建立全局坐标系，选择4次5阶B样条控制点的样条曲线设计泊车路径，设定约束条件，选取控制点个数，通过优化算法计算出B样条曲线曲率最小化路径曲线。

3.根据权利要求2所述的一种辅助泊车控制方法，其特征在于：所述约束条件包括：

1)车辆沿最小化路径曲线行驶与停车位三侧面不发生碰撞；

2)车辆到达路径终点时完全进入车位且与车位平行；

3)曲线最大曲率不大于1/R_min；

4)车辆以v_max车速沿路径曲线行驶时，前轴等效转角转速ω不大于ω_max，v_max为泊车过程允许最高车速，取值0.5～1.5m/s，ω_max为车辆转向轴最高转向角速度。

4.根据权利要求1所述的一种辅助泊车控制方法，其特征在于：所述步骤二中侧向距离保持在0.3～2m。

5.根据权利要求3所述的一种辅助泊车控制方法，其特征在于：所述步骤三中的目标路径偏差包括侧向距离偏差和航向角偏差，当车辆与目标路径侧向距离偏差｜y-y_r|≤Δy_o且航向角偏差|θ-θ_r|≤Δθ_o，泊车控制策略继续采用路径跟踪控制，若车辆与目标路径侧向距离偏差|y-y_r|>Δy_o或航向角偏差|θ-θ_r|>Δθ_o，泊车控制策略采用车辆过定点控制，调整车辆位姿；其中：y为车辆后轴中点Y轴坐标，y_r为过车辆后轴中点且与X坐标轴相垂直的直线与路径交点处的Y轴坐标，Δy_o侧向距离偏差阀值，θ为车辆对称线与坐标系X轴夹角，θ_r为过车辆后轴中点且与X坐标轴相垂直的直线与路径相交点处的路径切线与X坐标轴夹角，Δθ_o为航向角偏差阀值。

6.根据权利要求5所述的一种辅助泊车控制方法，其特征在于：所述侧向距离偏差：Δy_o＝(l_c-l_cmin)/K₂，K₂取值范围为：10～100，l_c越小取值应越大；航向角偏差：Δθ_o＝(l_c-l_cmin)/K₃，K₃取值范围：50～100，l_c越小取值应越大；式中l_c为实际车位长度，l_cmin为车辆所需最小车位长度。

7.根据权利要求6所述的一种辅助泊车控制方法，其特征在于：所述路径跟踪控制，其车辆控制率为：

式中x为车辆101后轴中点106的X坐标，y为车辆101后轴中点106的Y轴坐标，θ为车辆对称线与坐标系X轴的夹角，即车辆航向角，y_r为车辆x坐标对应目标路径上点P_r的Y轴坐标，θ_r为P_r点处切线与坐标系X轴夹角，即车辆目标航向角，ρ_r为泊车路径301在P_r点的曲率，L为车辆101后轴到前轴距离，k₁为路径跟踪侧向距离偏差系数，k₂为路径跟踪航向角偏差系数，车辆前进行驶时k₁、k₂均取负值，车辆后退行驶时k₁取负值，k₂取正值，为前轴等效转角，泊车控制器根据转向执行结构转向传动比得到方向盘转向角，由转向执行机构实现转向，为正值方向盘右转，负值方向盘左转。

8.根据权利要求7所述的一种辅助泊车控制方法，其特征在于：所述跟踪定点控制，其车辆跟踪定点转向控制率为：

式中，x为车辆后轴中点X坐标，y为车辆后轴中点Y轴坐标，θ为车辆轴线与X轴夹角即车辆航向角，x_p、y_p为跟踪目标定点的位置，θ_p为目标定点处航向角。L为车辆轴距，k₃定点跟踪位置偏差系数，k₄为定点跟踪航向角偏差系数，车辆前进行驶时，k₃取正值，k₄取正值，车辆后退行驶时，k₃取正直，k₄取负值，为前轴104等效转角，由前轴等效转角根据车辆转向系统角传动比计算得到方向盘转角，为正值方向盘右转，负值方向盘左转。

9.一种运行如权利要求1所述的辅助泊车控制方法的辅助泊车系统，包括路径存储器，泊车系统感应单元，泊车运动控制器，转向执行机构，泊车系统人机交互系统，其特征于：

所述路径存储器，用于存储根据车辆结构参数，采取B样条曲线方式确定最小泊车位的泊车路径数据；

所述泊车系统感应单元，用于感应车辆相对于车位的具体位置，并将感应数据传输给所述泊车运动控制器；

所述泊车运动控制器，用于根据路径存储器提供的数据以及泊车系统感知单元提供的所需泊车的具体位置，选择运动控制策略并计算方向盘转角值。

所述转向执行机构，用于接收该泊车运动控制器的信号，驱动车辆转向系统实现泊车运动控制器计算的方向盘转角值；

所述泊车系统人机交互系统，用于将车位检测结果、换挡操作示、周围障碍物存在情况向驾驶员展示，并将驾驶员操作指令传递给泊车系统。所述泊车系统人机交互系统通过CAN通信方式与泊车系统通信，接收泊车系统发送的提示信息并以图像形式展现出来，通过触摸屏获取驾驶员操作指令，并将指令通过CAN通信方式发送给控制器。