本发明提供了一种自动泊车控制系统中的轨迹计算方法、设备及存储设备，通过密集化，将规划得到的弧长分为n份，同时将曲率信息中回旋曲线段以n份线性插值，直线段和固定圆弧以固定曲率。根据密集化数据以及起始点坐标和航向角，计算整个泊车路段的密集轨迹点，并找到折返点和目标点；然后去密集化，得到整个泊车规划路径的最终轨迹点信息。本发明的有益效果是：本发明充分利用规划的轨迹信息计算出符合控制要求的近似等间距的轨迹点信息，并采用直线‑>螺旋曲线‑>固定圆弧方式进行处理，处理前后不引入路程、折返点和终点误差，消除了超小弧长段，提高轨迹处理精度。
1.一种自动泊车控制系统中的轨迹计算方法，自动泊车控制系统包括感知模块和规划模块，所述感知模块用于规划泊车路径和确定起点坐标，所述规划模块用于获取泊车规划路径的起点至终点的弧长信息及曲率信息和初始航向角；其特征在于：该轨迹计算方法包括以下步骤：
S1：根据获取的泊车起点至泊车终点的弧长信息和曲率信息，将泊车规划路径划分为若干个区间，每一个区间都对应有该区间的弧长信息和曲率信息；所述曲率信息具体分为直线段、回旋曲线段和固定圆弧段三类；
S2：将每个区间对应的弧长分为n份，回旋曲线段的区间采用线性插值法分为n份，n为大于0的正整数，直线段和固定圆弧段的曲率为步骤S1中对应获得的曲率信息，从而得到密集化的弧长信息和密集化的曲率信息；
S3：根据起点坐标和初始航向角，计算整个泊车路段的密集轨迹点，得到折返点和目标点；所述折返点为前进段结束准备后退的点，所述目标点指最后一个轨迹点；
S4：根据自动泊车控制系统中设定的间距，在所述密集轨迹点中每隔一定距离进行采样取点，得到去密集化后的一系列新的轨迹点对应的坐标和航向角，进而得到整个泊车规划路径的最终轨迹点信息。
2.如权利要求1所述的一种自动泊车控制系统中的轨迹计算方法，其特征在于：步骤S1中，根据曲率的变化将泊车规划路径划分为若干个区间。
3.如权利要求1所述的一种自动泊车控制系统中的轨迹计算方法，其特征在于：步骤S1中，所述直线段和固定圆弧段的曲率是固定的，回旋曲线段的曲率是变化的。
4.如权利要求1所述的一种自动泊车控制系统中的轨迹计算方法，其特征在于：步骤S2中，直线段和固定圆弧段的区间对应的弧长被均分为n份。
5.如权利要求1所述的一种自动泊车控制系统中的轨迹计算方法，其特征在于：步骤S3中，根据起点坐标、初始航向角和对应弧长及曲率，计算得到整个泊车路段的每个密集轨迹点的坐标和航向角的过程如下：
xk＝xk‑1+sk‑1cos(θk‑1)，yk＝yk‑1+sk‑1sin(θk‑1)，得到：
其中，(xk‑1,yk‑1)为前一个轨迹点的坐标， 为前一个轨迹点对应的航向角，Lk‑1为前一个轨迹点至当前轨迹点间对应的弧长，Kk‑1为前一个轨迹点至当前轨迹点间对应的曲率，(xk,yk)为当前轨迹点的坐标， 为当前轨迹点对应的航向角，θk‑1为经过前一个轨迹点与当前轨迹点的连线与X轴的夹角，sk‑1为前一个轨迹点与当前轨迹点的连线长度；
第一个轨迹点为泊车规划路径的起点，根据起点的起点坐标、初始航向角和对应弧长及曲率，通过上述公式，依次得到所有轨迹点的坐标和航向角。
6.如权利要求1所述的一种自动泊车控制系统中的轨迹计算方法，其特征在于：步骤S4中，在所述密集轨迹点中每隔一定距离进行采样取点的过程如下：根据自动泊车控制系统中设定的间距unit，在密集化轨迹中每隔m＝unit/min_l个点采样取第index个点，得到去密集化的轨迹点，min_l表示前n‑1份的弧长；
a、若第index个点靠近折返点或者目标点，则直接取折返点或目标点；
b、若第index个点远离折返点或者目标点，则每隔固定m个点采样；
c、根据第index个点取值的间距，将对应密集化弧长信息dense_L table中的元素取和，作为去密集化后的新的轨迹点的弧长，进而得到新的轨迹点的坐标；
d、根据第index个点取值的间距，将对应密集化曲率信息dense_Ro table中的元素取平均，作为去密集化后的新的轨迹点的曲率，进而得到新的轨迹点的航向角；
根据上述操作，最终得到去密集化后的一系列新的轨迹点对应的坐标和航向角，进而得到整个泊车规划路径的最终轨迹点信息。
7.一种存储设备，其特征在于：所述存储设备存储指令及数据用于实现权利要求1～6任一项所述的自动泊车控制系统中的轨迹计算方法。
8.一种自动泊车控制系统中的轨迹计算设备，其特征在于：包括：处理器及存储设备；
所述处理器加载并执行所述存储设备中的指令及数据用于实现权利要求1～6任一项所述的自动泊车控制系统中的轨迹计算方法。
自动泊车控制系统中的轨迹计算方法、设备及存储设备
技术领域
[0001] 本发明涉及自动泊车领域，尤其涉及一种自动泊车控制系统中的轨迹计算方法、设备及存储设备。
背景技术
[0002] 自动泊车感知决策规划出泊车路径信息，根据曲率表中的曲率值(Ro table)的变化来将整个泊车路径划分为若干个区间。各区间的弧长相差较大且极有可能远小于控制间
距，该控制间距为控制算法进行轨迹跟踪时设置的两个轨迹点之间的最小距离；
[0003] 而控制模块需要使用一定间距且近似相等的轨迹点的坐标和航向角等信息，以作为自动泊车控制系统中的输入参数进行计算，得到其他有效数据，现有的计算方法通常是：
首先根据弧长表和曲率表计算间距较大的轨迹点该间距即若干个区间的区间长度，即先计
算间隔长度较大的那个区间的轨迹点，然后对这些轨迹点进行拟合，最后在拟合曲线上采
样获得等分的轨迹点。这样的处理将引入较大的误差。
发明内容
[0004] 为了解决上述问题，本发明提供了一种自动泊车控制系统中的轨迹计算方法、设备及存储设备，自动泊车控制系统包括感知模块和规划模块，所述感知模块用于规划泊车
路径和确定起点坐标，所述规划模块用于获取泊车规划路径的起点至终点的弧长信息及曲
率信息和初始航向角；一种自动泊车控制系统中的轨迹计算方法，主要包括以下步骤：
[0005] S1：根据获取的泊车起点至泊车终点的弧长信息和曲率信息，将泊车规划路径划分为若干个区间，每一个区间都对应有该区间的弧长信息和曲率信息；所述曲率信息具体
分为直线段、回旋曲线段和固定圆弧段三类；
[0006] S2：将每个区间对应的弧长分为n份，回旋曲线段的区间采用线性插值法分为n份，n为大于0的正整数，直线段和固定圆弧段的曲率为步骤S1中对应获得的曲率信息，从而得
到密集化的弧长信息和密集化的曲率信息；
[0007] S3：根据起点坐标和初始航向角，计算整个泊车路段的密集轨迹点，得到折返点和目标点；所述折返点为前进段结束准备后退的点，所述目标点指最后一个轨迹点；
[0008] S4：根据自动泊车控制系统中设定的间距，在所述密集轨迹点中每隔一定距离进行采样取点，得到去密集化后的一系列新的轨迹点对应的坐标和航向角，进而得到整个泊
车规划路径的最终轨迹点信息。
[0009] 进一步地，根据曲率的变化将泊车规划路径划分为若干个区间。
[0010] 进一步地，所述直线段和固定圆弧段的曲率是固定的，回旋曲线段的曲率是变化的。
[0011] 进一步地，直线段和固定圆弧段的区间对应的弧长被均分为n份。
[0012] 进一步地，根据起点坐标、初始航向角和对应弧长及曲率，计算得到整个泊车路段的每个密集轨迹点的坐标和航向角的过程如下：
[0013] 通过如下所示的公式：
[0014]
[0015]
[0016] xk＝xk‑1+sk‑1cos(θk‑1)，
[0017] yk＝yk‑1+sk‑1sin(θk‑1)，
[0018]
[0019] 得到：
[0020]
[0021] 其中，(xk‑1,yk‑1)为前一个轨迹点的坐标， 为前一个轨迹点对应的航向角，Lk‑1为前一个轨迹点至当前轨迹点间对应的弧长，Kk‑1为前一个轨迹点至当前轨迹点间对应的
曲率，(xk,yk)为当前轨迹点的坐标， 为当前轨迹点对应的航向角，θk‑1为经过前一个轨迹
点与当前轨迹点的连线与X轴的夹角；
[0022] 第一个轨迹点为泊车规划路径的起点，根据起点的起点坐标、初始航向角和对应弧长及曲率，通过上述公式，依次得到所有轨迹点的坐标和航向角。
[0023] 进一步地，在所述密集轨迹点中每隔一定距离进行采样取点的过程如下：
[0024] 根据自动泊车控制系统中设定的间距unit，在密集化轨迹中每隔m＝unit/min_l个点采样取第index个点，得到去密集化的轨迹点，min_l表示前n‑1份的弧长；
[0025] a、若第index个点靠近折返点或者目标点，则直接取折返点或目标点；
[0026] b、若第index个点远离折返点或者目标点，则每隔固定m个点采样；
[0027] c、根据第index个点取值的间距，将对应密集化弧长信息dense_L table中的元素取和，作为去密集化后的新的轨迹点的弧长，进而得到新的轨迹点的坐标；
[0028] d、根据第index个点取值的间距，将对应密集化曲率信息dense_Ro table中的元素取平均，作为去密集化后的新的轨迹点的曲率，进而得到新的轨迹点的航向角；
[0029] 根据上述操作，最终得到去密集化后的一系列新的轨迹点对应的坐标和航向角，进而得到整个泊车规划路径的最终轨迹点信息。
[0030] 一种存储设备，所述存储设备存储指令及数据用于实现一种自动泊车控制系统中的轨迹计算方法。
[0031] 一种自动泊车控制系统中的轨迹计算设备，包括：处理器及存储设备；所述处理器加载并执行所述存储设备中的指令及数据用于实现一种自动泊车控制系统中的轨迹计算
方法。
[0032] 本发明提供的技术方案带来的有益效果是：本发明充分利用规划的轨迹信息计算出符合控制要求的近似等间距的轨迹点信息，并采用直线‑>螺旋曲线‑>固定圆弧方式进行
处理，处理前后不引入路程、折返点和终点误差，消除规划信息中的超小弧长段。提高了轨
迹处理的精度。
附图说明
[0033] 下面将结合附图及实施例对本发明作进一步说明，附图中：
[0034] 图1是本发明实施例中一种自动泊车控制系统中的轨迹计算方法的流程图；
[0035] 图2是本发明实施例中根据表1数据进行分区的示意图；
[0036] 图3是本发明实施例中密集化后的示意图；
[0037] 图4是本发明实施例中采用AVM坐标系标识的参数示意图；
[0038] 图5是本发明实施例中硬件设备工作的示意图。
具体实施方式
[0039] 为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。
[0040] 本发明的实施例提供了一种自动泊车控制系统中的轨迹计算方法、设备及存储设备。自动泊车控制系统包括感知模块和规划模块，所述感知模块用于规划泊车路径和确定
起点坐标，所述规划模块用于获取泊车规划路径的起点至终点的弧长信息及曲率信息和初
始航向角。
[0041] 请参考图1‑4，图1是本发明实施例中一种自动泊车控制系统中的轨迹计算方法的流程图，图2是本发明实施例中根据表1数据进行分区的示意图，图3是本发明实施例中密集
化后的示意图，图4是本发明实施例中采用AVM坐标系标识的参数示意图，具体包括如下步
骤：
[0042] S1：根据获取的泊车起点至泊车终点的弧长信息和曲率信息，将泊车规划路径划分为若干个区间，每一个区间都对应有该区间的弧长信息和曲率信息，如图2所示；所述曲
率信息具体分为直线段、回旋曲线段和固定圆弧段三类；根据曲率的变化将泊车规划路径
划分为若干个区间。所述直线段和固定圆弧段的曲率是固定的，回旋曲线段的曲率是变化
的。
[0043] 表1为得到的弧长和曲率信息(L table和Ro table)，这些数据是一段完整的泊车轨迹信息，是由规划模块提供的，本发明的目的就是将这些数据转换成自动泊车控制系统
内部要使用的轨迹点信息。
[0044] 表1 L table和Ro table：
[0045] L table 0.750.0001 0.481285 2.40.41285 ‑0.000012 ‑4.941387 ‑0.000012 ‑1.995792 ‑0.0001Ro table 0 0 0 0.20.2 0 ‑0.199148 ‑0.199148 0 0
[0046] S2：如图3所示，将每个区间对应的弧长分为n份，回旋曲线段的区间采用线性插值法分为n份，n为大于0的正整数，直线段和固定圆弧段的曲率为步骤S1中对应获得的曲率信
息，从而得到密集化的弧长信息和密集化的曲率信息；直线段和固定圆弧段的区间对应的
弧长被均分为n份。
[0047] S3：根据起点坐标和初始航向角，计算整个泊车路段的密集轨迹点，得到折返点和目标点；所述折返点为前进段结束准备后退的点，所述目标点指最后一个轨迹点；
[0048] 一次完整的泊车过程会经历前进再倒退，或者是倒退再前进等泊车情况。以前进再倒退为例，折返点指的就是前进段结束，准备开始倒退的那个轨迹点，目标点指的就是倒
退结束准备停车的那个轨迹点。这两个点都是根据表1中的信息来计算得到的，上表中，正
号表示前进路段，负号表示后退路段，数值表示要走多长的距离，折返点表示弧长由正变为
负的点，目标点表示轨迹点的最后一个点。
[0049] 根据起点坐标、初始航向角和相邻两个轨迹点间对应弧长及曲率，计算得到整个泊车路段的每个密集轨迹点的坐标和航向角的过程如下：
[0050] 通过如下所示的公式：
[0051]
[0052]
[0053] xk＝xk‑1+sk‑1cos(θk‑1)，
[0054] yk＝yk‑1+sk‑1sin(θk‑1)，
[0055]
[0056] 得到：
[0057]
[0058] 如图4所示，(xk‑1,yk‑1)为前一个轨迹点的坐标， 为前一个轨迹点对应的航向角，Lk‑1为前一个轨迹点至当前轨迹点间对应的弧长，Kk‑1为前一个轨迹点至当前轨迹点间
对应的曲率，(xk,yk)为当前轨迹点的坐标， 为当前轨迹点对应的航向角，θk‑1为经过前一
个轨迹点与当前轨迹点的连线与X轴的夹角；
[0059] 第一个轨迹点为泊车规划路径的起点，根据起点的起点坐标、初始航向角和对应弧长及曲率，通过上面的公式，依次得到每一个轨迹点的坐标和航向角。
[0060] S4：根据自动泊车控制系统中设定的间距，在所述密集轨迹点中每隔一定距离进行采样取点，得到去密集化后的一系列新的轨迹点对应的坐标和航向角，进而得到整个泊
车规划路径的最终轨迹点信息。
[0061] LTable和Ro Table密集化的具体操作如下：
[0062] 1)将LTable和Ro Table采用“直线段‑‑>回旋曲线段‑‑>固定圆弧段‑‑>回旋曲线段‑‑>固定圆弧‑‑>直线段”进行分区。首先将L table分为正负两段，每一段都是根据Ro
table的变化来区分的。例如：L table值0.75对应的Ro table值是0，它的下一个数值
0.0001对应的Ro table的值也是0，这说明在0.75‑0.0001这段距离Ro table的值没有发生
变化且数值为0，所以这一段属于直线段；而L table值0.481285的下一个数值2.4所对应的
Ro table的值为0.2，这说明在0.481285‑2.4这段距离Ro table的值发生了变化，从0‑0.2，
所以这段属于回旋曲线段，类似的L table值2.4的下一个值0.41285对应的Ro table也为
0.2，这说明在2.4‑0.41285这段距离Ro table的值一直为0.2，所以这段属于固定圆弧段，
后面的划分以此类推
[0063] 2)对L table的每个区间段，将其分为n份，其中前n‑1份为弧长为min_l，例如0.1m，第n份为此区间弧长对min_l的余，得到密集化的弧长表dense_L。
[0064] 3)直线段和固定圆弧段，每段密集化的弧长对应的密集化曲率为原始L table段对应的曲率，例如0.75直线段密集化的曲率为0；0.481285固定圆弧段对应的密集化曲率为
0.2。
[0065] 4)回旋曲线段，对曲率进行线性插值，每段密集化的弧长对应的密集化曲率为插值大小。得到密集化的曲率表dense_Ro。
[0066] 在所述密集轨迹点中每隔一定距离进行采样取点的过程如下：
[0067] 根据自动泊车控制系统中设定的间距unit，在密集化轨迹中每隔m＝unit/min_l个点采样取第index个点，得到去密集化的轨迹点，min_l表示前n‑1份的弧长；
[0068] a、若第index个点靠近折返点或者目标点，则直接取折返点或目标点；
[0069] b、若第index个点远离折返点或者目标点，则每隔固定m个点采样；
[0070] c、根据第index个点取值的间距，将对应密集化弧长信息dense_L table中的元素取和，作为去密集化后的新的轨迹点的弧长，进而得到新的轨迹点的坐标；
[0071] d、根据第index个点取值的间距，将对应密集化曲率信息dense_Ro table中的元素取平均，作为去密集化后的新的轨迹点的曲率，进而得到新的轨迹点的航向角；
[0072] 根据上述操作，最终得到去密集化后的一系列新的轨迹点对应的坐标和航向角，进而得到整个泊车规划路径的最终轨迹点信息。
[0073] 请参见图5，图5是本发明实施例的硬件设备工作示意图，所述硬件设备具体包括：一种自动泊车控制系统中的轨迹计算设备401、处理器402及存储设备403。
[0074] 一种自动泊车控制系统中的轨迹计算设备401：所述一种自动泊车控制系统中的轨迹计算设备401实现所述一种自动泊车控制系统中的轨迹计算方法。
[0075] 处理器402：所述处理器402加载并执行所述存储设备403中的指令及数据用于实现所述一种自动泊车控制系统中的轨迹计算方法。
[0076] 存储设备403：所述存储设备403存储指令及数据；所述存储设备403用于实现所述一种自动泊车控制系统中的轨迹计算方法。
[0077] 本发明的有益效果是：本发明充分利用规划的轨迹信息计算出符合控制要求的近似等间距的轨迹点信息，并采用直线‑>螺旋曲线‑>固定圆弧方式进行处理，处理前后不引
入路程、折返点和终点误差，消除规划信息中的超小弧长段。提高了轨迹处理的精度。
[0078] 以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
