买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：大连波塞冬汽车电子科技有限公司

摘要：一种自动泊车路径规划装置，属于泊车领域，为了解决摆正车身所增加的控制点和原有控制点相比，斜率变化过大，导致曲线曲率变化过大，从而车辆无法跟上的问题，技术要点是：样条曲线规划模块，在可行范围内选择最优的路径作为样条曲线的逼近路线；在逼近路线上选取若干点作为样条曲线控制点，并规划样条曲线；补齐圆弧模块，在规划的样条曲线的起始点和或终止点补齐一段由车辆最小转弯半径确定的圆弧，使得与样条曲线相接点处，其一阶导数连续；在圆弧段的末端其一阶导数为0；输出轨迹点坐标以完成规划。效果是：本发明保证了整条曲线的起止点位姿，也保证在摆正车身过程中，满足车辆的运动学约束。

主权项：1.一种自动泊车路径规划装置，其特征在于，包括：样条曲线规划模块，在可行性范围内选择最优的路径作为样条曲线的逼近路线；在逼近路线上选取若干点作为样条曲线控制点，并规划样条曲线；补齐圆弧模块，在规划的样条曲线的起始点和或终止点补齐一段由车辆最小转弯半径确定的圆弧，使得与样条曲线相接点处，其一阶导数连续；在圆弧段的末端其一阶导数为0；输出轨迹点坐标以完成规划；所述可行性范围是泊车起始点到终点之间的可行性范围，该范围确定前，泊车起始点计算模块计算泊车起始点范围；所述补齐圆弧模块，在规划的样条曲线的终止点补齐的圆弧为第二圆弧段，在样条曲线的终止点处，车辆位置与姿态分别以Xf_B,Yf_B,Thetaf_B表示，并且，以向右以及向上为X轴和Y轴的正方向，第二圆弧段的起始点和终止点位置和姿态分别用以下公式表示： 根据获得的第二圆弧段的起始点和终止点位置，采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为： 其中：Xi_ARC2：第二段圆弧起始点的X坐标；Yi_ARC2：第二段圆弧起始点的y坐标；Thetai_ARC2：第二段圆弧起始点的姿态角；Xf_ARC2：第二段圆弧终止点的X坐标；Yf_ARC2：第二段圆弧终止点的y坐标；Thetaf_ARC2：第二段圆弧终止点的姿态角；Xf_B：样条曲线终止点的X坐标；Yf_B：样条曲线终止点的y坐标；Thetaf_B：样条曲线终止点的姿态角；Δtheta:采样的角度间隔；X_ARC2i：第二段圆弧中插入第i点x坐标；Y_ARC2i：第二段圆弧中插入第i点y坐标；Theta_ARC2i：第二段圆弧中插入的第i点姿态角；所述补齐圆弧模块，在规划的样条曲线的起始点补齐的圆弧为第一圆弧段，样条曲线的起始点处，车辆位置与姿态分别以Xi_B,Yi_B，Thetai_B，并且，以向右以及向上为X轴和Y轴的正方向，第一圆弧段的起始点和终止点的坐标和位姿由以下公式获得： 根据获得的第一圆弧段的起始点和终止点位置，采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为： 其中：Xi_ARC1：第一段圆弧起始点的X坐标；Yi_ARC1：第一段圆弧起始点的y坐标；Thetai_ARC1：第一段圆弧起始点的姿态角；Xf_ARC1：第一段圆弧终止点的X坐标；Yf_ARC1：第一段圆弧终止点的y坐标；Thetaf_ARC1：第一段圆弧终止点的姿态角；Xi_B：样条曲线起点的X坐标；Yi_B：样条曲线起点的y坐标；Thetai_B：样条曲线起点的姿态角；Δtheta:采样的角度间隔；X_ARC1i：第一段圆弧中插入第i点x坐标；Y_ARC1i：第一段圆弧中插入第i点y坐标；Theta_ARC1i：第一段圆弧中插入的第i点姿态角。

全文数据：一种自动泊车路径规划装置技术领域本发明属于泊车领域，涉及一种自动泊车路径规划装置。背景技术现有技术是在规划B样条路径的时候，在最后一个控制点的结尾处，同一水平线上再增加4个控制节点，以保持终止点姿态与泊车位底边平行。该方法存在以下缺点：1、在额外增加控制点后，无法保证整条样条曲线的曲率都满足车辆的最小半径约束。2、初始位置处无法保证车身的姿态。在自动泊车控制系统中，路径规划作为其中的关键核心技术，一直是众多研究人员关注的重点和热点。良好的路径规划不仅可以保证车辆的避障功能，而且可以减轻控制执行系统的压力。针对平行泊车位，双圆弧拼接的方法是最为基础的方法。该方法通过车辆与泊车位的相对关系，计算出泊车位的起始位置，然后通过两段最小转弯半径所构成的圆弧，将车辆摆进停车位。放宽要求后，两段圆弧的半径可以扩展到满足车辆运动学、避障要求，相切约束的任意半径。但是基于圆弧拼接的泊车路径规划方法，有一个无法克服的困难，就是曲率不连续。这就要求车辆在某些特定位置，在极端的时间内，将方向盘转过一个较大的角度，从而对车辆造成损害，且对乘车人的舒适性造重大影响。在此基础上，研发人员开发了基于样条曲线的泊车路径规划算法。该算法最大的优点就在于保证了车辆在行驶过程中曲率的连续性。本专利正是在此基础之上发明的来的。现有技术请参见宋金泽博士论文《自主泊车系统关键技术研究》，第三章融合微分平坦、样条理论的轨迹生成方法，《国防科学技术大学》,2009。根据以上原理，在进行研究分析及仿真时，发现，在曲线的后半段部分，车辆并不能根据规划好的点进行跟踪。原因在于其为了摆正车身所增加的控制点和原有控制点相比，斜率变化过大，导致曲线曲率变化过大，从而车辆无法跟上。发明内容为了解决摆正车身所增加的控制点和原有控制点相比，斜率变化过大，导致曲线曲率变化过大，从而车辆无法跟上的问题，本发明提供的技术方案是：一种自动泊车路径规划装置，包括：样条曲线规划模块，在可行范围内选择最优的路径作为样条曲线的逼近路线；在逼近路线上选取若干点作为样条曲线控制点，并规划样条曲线；补齐圆弧模块，在规划的样条曲线的起始点和或终止点补齐一段由车辆最小转弯半径确定的圆弧，使得与样条曲线相接点处，其一阶导数连续；在圆弧段的末端其一阶导数为0；输出轨迹点坐标以完成规划。有益效果：本发明在规划好样条曲线后，在该曲线的起止点位置补齐一段由最小转弯半径构成的圆弧。其主要的优点为：1.保证了起止点的车身姿态。2.满足车辆的运动学约束。3.在样条曲线弧长一定的情况下，其所增加的满足要求的圆弧弧长最小。用于平行泊车位的自动泊车，实现平滑有效、规范停车，本技术方案是在规划好样条曲线后，在原有曲线的起止位置点，补上一段以最小半径约束的圆弧路径，圆弧段与样条曲线相接处保持一阶导数连续，而末端姿态保持斜率为0。这样保证了整条曲线的起止点位姿，也保证在摆正车身过程中，满足车辆的运动学约束。原有技术没有考虑车辆起点位置的姿态约束，也并没有考虑到终止点摆正过程中的车辆运动学约束.本发明保证了整条曲线的起止点位姿，也保证在摆正车身过程中，满足车辆的运动学约束。附图说明图1为实施例1中的自动泊车路径规划方法的流程示意图。具体实施方式实施例1：在自动泊车控制系统中，路径规划作为其中的关键核心技术，一直是众多研究人员关注的重点和热点。良好的路径规划不仅可以保证车辆的避障功能，而且可以减轻控制执行系统的压力。针对平行泊车位，双圆弧拼接的方法是最为基础的方法。该方法通过车辆与泊车位的相对关系，计算出泊车位的起始位置，然后通过两段最小转弯半径所构成的圆弧，将车辆摆进停车位。放宽要求后，两段圆弧的半径可以扩展到满足车辆运动学、避障要求，相切约束的任意半径。但是基于圆弧拼接的泊车路径规划方法，有一个无法克服的困难，就是曲率不连续。这就要求车辆在某些特定位置，在极端的时间内，将方向盘转过一个较大的角度，从而对车辆造成损害，且对乘车人的舒适性造重大影响。在此基础上，研发人员开发了基于样条曲线的泊车路径规划算法。该算法最大的优点就在于保证了车辆在行驶过程中曲率的连续性。本专利正是在此基础之上发明的来的。现有技术请参见宋金泽博士的论文《自主泊车系统关键技术研究》，第三章融合微分平坦、样条理论的轨迹生成方法，《国防科学技术大学》,2009。根据以上原理，在进行研究分析及仿真时，发明人发现，在曲线的后半段部分，车辆并不能根据规划好的点进行跟踪。原因在于其为了摆正车身所增加的控制点和原有控制点相比，斜率变化过大，导致曲线曲率变化过大，从而车辆无法跟上。基于此，本实施例记载了一种自动泊车路径规划方法，在可行范围内选择最优的路径作为样条曲线的逼近路线；在逼近路线上选取若干点作为样条曲线控制点，并规划样条曲线；其特征在于，在规划的样条曲线的起始点和或终止点补齐一段由车辆最小转弯半径确定的圆弧，使得与样条曲线相接点处，其一阶导数连续；在圆弧段的末端其一阶导数为0，即车辆姿态与泊车位的底边平行；输出轨迹点坐标以完成规划。作为一种实施例：所述可行性范围是泊车起始点到终点之间的可行性范围，该范围确定前，具有计算泊车起始点范围的步骤。而该实施例中，计算泊车起始点范围的步骤之前，又具有参数输入步骤，所述参数包括：车身尺寸参数、泊车位尺寸参数、车辆与泊车位位置关系参数。作为另一种实施例：在规划的样条曲线的终止点补齐的圆弧为第二圆弧段，在样条曲线的终止点处，车辆位置即局部坐标系下的x、y坐标与姿态分别以Xf_B,Yf_B,Thetaf_B表示，并且，以向右以及向上为X轴和Y轴的正方向，那么，在样条曲线的终点处，第二圆弧段的起始点和终止点位置和姿态分别用以下公式表示：根据获得的起始点和终止点的位置，采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为：其中：Xi_ARC2：第二段圆弧起始点的X坐标；Yi_ARC2：第二段圆弧起始点的y坐标；Thetai_ARC2：第二段圆弧起始点的姿态角。Xf_ARC2：第二段圆弧终止点的X坐标；Yf_ARC2：第二段圆弧终止点的y坐标；Thetaf_ARC2：第二段圆弧终止点的姿态角。Xf_B：样条曲线终点的X坐标；Yf_B：样条曲线终点的y坐标；Thetaf_B：样条曲线终点点的姿态角。Δtheta:采样的角度间隔。X_ARC2i：第二段圆弧中插入第i点x坐标；Y_ARC2i：第二段圆弧中插入第i点y坐标；Theta_ARC2i：第二段圆弧中插入的第i点姿态角。作为另一种实施例：在规划的样条曲线的起始点补齐的圆弧为第一圆弧段，样条曲线的起始点处，车辆位置与姿态分别以Xi_B,Yi_B，Thetai_B，并且，以向右以及向上为X轴和Y轴的正方向，第一圆弧段的起始点和终止点的坐标和位姿由以下公式获得：根据获得的起始点和终止点位置，可以采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为：其中：Xi_ARC1：第一段圆弧起始点的X坐标；Yi_ARC1：第一段圆弧起始点的y坐标；Thetai_ARC1：第一段圆弧起始点的姿态角。Xf_ARC1：第一段圆弧终止点的X坐标；Yf_ARC1：第一段圆弧终止点的y坐标；Thetaf_ARC1：第一段圆弧终止点的姿态角。Xi_B：样条曲线起点的X坐标；Yi_B：样条曲线起点的y坐标；Thetai_B：样条曲线起点的姿态角。Δtheta:采样的角度间隔。X_ARC1i：第一段圆弧中插入第i点x坐标；Y_ARC1i：第一段圆弧中插入第i点y坐标；Theta_ARC1i：第一段圆弧中插入的第i点姿态角。本实施例的关键点在于，规划好样条曲线后，在该曲线的起止点位置补齐一段圆弧，使得圆弧的其起始点位置或者终止点位置斜率为0。该圆弧的半径为车辆的最小转弯半径。实施例2：一种在自动泊车路径规划中保证车辆起点位置姿态约束的方法，其特征在于，在样条曲线的起始点位置补齐一段由车辆最小半径确定的圆弧，该圆弧与样条曲线的相接点处，其一阶导数连续，该圆弧起始点位置其一阶导数为0。在规划的样条曲线的起始点补齐的圆弧为第一圆弧段，样条曲线的起始点处，车辆位置与姿态分别以Xi_B,Yi_B，Thetai_B，并且，以向右以及向上为X轴和Y轴的正方向，第一圆弧段的起始点和终止点的坐标和位姿由以下公式获得：根据获得的起始点和终止点位置，可以采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为：其中：Xi_ARC1：第一段圆弧起始点的X坐标；Yi_ARC1：第一段圆弧起始点的y坐标；Thetai_ARC1：第一段圆弧起始点的姿态角。Xf_ARC1：第一段圆弧终止点的X坐标；Yf_ARC1：第一段圆弧终止点的y坐标；Thetaf_ARC1：第一段圆弧终止点的姿态角。Xi_B：样条曲线起点的X坐标；Yi_B：样条曲线起点的y坐标；Thetai_B：样条曲线起点的姿态角。Δtheta:采样的角度间隔。X_ARC1i：第一段圆弧中插入第i点x坐标；Y_ARC1i：第一段圆弧中插入第i点y坐标；Theta_ARC1i：第一段圆弧中插入的第i点姿态角.实施例3：一种在自动泊车路径规划中摆正车身时保证车辆运动学约束的方法，其特征在于，在样条曲线的终止点位置补齐一段由车辆最小半径确定的圆弧，该圆弧与样条曲线的相接点处，其一阶导数连续，该圆弧终止点位置其一阶导数为0。在规划的样条曲线的终止点补齐的圆弧为第二圆弧段，在样条曲线的终止点处，车辆位置即局部坐标系下的x、y坐标与姿态分别以Xf_B,Yf_B,Thetaf_B表示，并且，以向右以及向上为X轴和Y轴的正方向，那么，在样条曲线的终点处，第二圆弧段的起始点与样条曲线相接的点和终止点位置和姿态分别用以下公式表示：根据获得的起始点和终止点的位置，采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为：其中：Xi_ARC2：第二段圆弧起始点的X坐标；Yi_ARC2：第二段圆弧起始点的y坐标；Thetai_ARC2：第二段圆弧起始点的姿态角。Xf_ARC2：第二段圆弧终止点的X坐标；Yf_ARC2：第二段圆弧终止点的y坐标；Thetaf_ARC2：第二段圆弧终止点的姿态角。Xf_B：样条曲线终点的X坐标；Yf_B：样条曲线终点的y坐标；Thetaf_B：样条曲线终点点的姿态角。Δtheta:采样的角度间隔。X_ARC2i：第二段圆弧中插入第i点x坐标；Y_ARC2i：第二段圆弧中插入第i点y坐标；Theta_ARC2i：第二段圆弧中插入的第i点姿态角。实施例4：一种自动泊车路径规划装置，包括：样条曲线规划模块，在可行范围内选择最优的路径作为样条曲线的逼近路线；在逼近路线上选取若干点作为样条曲线控制点，并规划样条曲线；补齐圆弧模块，在规划的样条曲线的起始点和或终止点补齐一段由车辆最小转弯半径确定的圆弧，使得与样条曲线相接点处，其一阶导数连续；在圆弧段的末端其一阶导数为0；输出轨迹点坐标以完成规划。所述可行性范围是泊车起始点到终点之间的可行性范围，该范围确定前，泊车起始点计算模块计算泊车起始点范围。计算泊车起始点范围之前，参数输入模块输入参数，所述参数包括：车身尺寸参数、泊车位尺寸参数、车辆与泊车位位置关系参数。所述补齐圆弧模块，在规划的样条曲线的终止点补齐的圆弧为第二圆弧段，在样条曲线的终止点处，车辆位置与姿态分别以Xf_B,Yf_B,Thetaf_B表示，并且，以向右以及向上为X轴和Y轴的正方向，第二圆弧段的起始点和终止点位置和姿态分别用以下公式表示：根据获得的起始点和终止点的位置，采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为：其中：Xi_ARC2：第二段圆弧起始点的X坐标；Yi_ARC2：第二段圆弧起始点的y坐标；Thetai_ARC2：第二段圆弧起始点的姿态角；Xf_ARC2：第二段圆弧终止点的X坐标；Yf_ARC2：第二段圆弧终止点的y坐标；Thetaf_ARC2：第二段圆弧终止点的姿态角；Xf_B：样条曲线终点的X坐标；Yf_B：样条曲线终点的y坐标；Thetaf_B：样条曲线终点点的姿态角；Δtheta:采样的角度间隔；X_ARC2i：第二段圆弧中插入第i点x坐标；Y_ARC2i：第二段圆弧中插入第i点y坐标；Theta_ARC2i：第二段圆弧中插入的第i点姿态角；所述补齐圆弧模块，在规划的样条曲线的起始点补齐的圆弧为第一圆弧段，样条曲线的起始点处，车辆位置与姿态分别以Xi_B,Yi_B，Thetai_B，并且，以向右以及向上为X轴和Y轴的正方向，第一圆弧段的起始点和终止点的坐标和位姿由以下公式获得：根据获得的起始点和终止点位置，可以采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为：其中：Xi_ARC1：第一段圆弧起始点的X坐标；Yi_ARC1：第一段圆弧起始点的y坐标；Thetai_ARC1：第一段圆弧起始点的姿态角；Xf_ARC1：第一段圆弧终止点的X坐标；Yf_ARC1：第一段圆弧终止点的y坐标；Thetaf_ARC1：第一段圆弧终止点的姿态角；Xi_B：样条曲线起点的X坐标；Yi_B：样条曲线起点的y坐标；Thetai_B：样条曲线起点的姿态角；Δtheta:采样的角度间隔；X_ARC1i：第一段圆弧中插入第i点x坐标；Y_ARC1i：第一段圆弧中插入第i点y坐标；Theta_ARC1i：第一段圆弧中插入的第i点姿态角；实施例5：一种在自动泊车路径规划中保证车辆起点位置姿态约束的装置，在样条曲线的起始点位置补齐一段由车辆最小半径确定的圆弧，该圆弧与样条曲线的相接点处，其一阶导数连续，该圆弧起始点位置其一阶导数为0。实施例6：一种在自动泊车路径规划中摆正车身时保证车辆运动学约束的装置，其特征在于，在样条曲线的终止点位置补齐一段由车辆最小半径确定的圆弧，该圆弧与样条曲线的相接点处，其一阶导数连续，该圆弧终止点位置其一阶导数为0。以上所述，仅为本发明创造较佳的具体实施方式，但本发明创造的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明创造披露的技术范围内，根据本发明创造的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明创造的保护范围之内。

权利要求：1.一种自动泊车路径规划装置，其特征在于，包括：样条曲线规划模块，在可行范围内选择最优的路径作为样条曲线的逼近路线；在逼近路线上选取若干点作为样条曲线控制点，并规划样条曲线；补齐圆弧模块，在规划的样条曲线的起始点和或终止点补齐一段由车辆最小转弯半径确定的圆弧，使得与样条曲线相接点处，其一阶导数连续；在圆弧段的末端其一阶导数为0；输出轨迹点坐标以完成规划。2.如权利要求1所述的自动泊车路径规划装置，其特征在于，所述可行性范围是泊车起始点到终点之间的可行性范围，该范围确定前，泊车起始点计算模块计算泊车起始点范围。3.如权利要求2所述的自动泊车路径规划装置，其特征在于，计算泊车起始点范围之前，参数输入模块输入参数，所述参数包括：车身尺寸参数、泊车位尺寸参数、车辆与泊车位位置关系参数。4.如权利要求1所述的自动泊车路径规划装置，其特征在于，所述补齐圆弧模块，在规划的样条曲线的终止点补齐的圆弧为第二圆弧段，在样条曲线的终止点处，车辆位置与姿态分别以Xf_B,Yf_B,Thetaf_B表示，并且，以向右以及向上为X轴和Y轴的正方向，第二圆弧段的起始点和终止点位置和姿态分别用以下公式表示：根据获得的起始点和终止点的位置，采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为：其中：Xi_ARC2：第二段圆弧起始点的X坐标；Yi_ARC2：第二段圆弧起始点的y坐标；Thetai_ARC2：第二段圆弧起始点的姿态角；Xf_ARC2：第二段圆弧终止点的X坐标；Yf_ARC2：第二段圆弧终止点的y坐标；Thetaf_ARC2：第二段圆弧终止点的姿态角；Xf_B：样条曲线终点的X坐标；Yf_B：样条曲线终点的y坐标；Thetaf_B：样条曲线终点点的姿态角；Δtheta:采样的角度间隔；X_ARC2i：第二段圆弧中插入第i点x坐标；Y_ARC2i：第二段圆弧中插入第i点y坐标；Theta_ARC2i：第二段圆弧中插入的第i点姿态角。5.如权利要求1所述的自动泊车路径规划装置，其特征在于，所述补齐圆弧模块，在规划的样条曲线的起始点补齐的圆弧为第一圆弧段，样条曲线的起始点处，车辆位置与姿态分别以Xi_B,Yi_B，Thetai_B，并且，以向右以及向上为X轴和Y轴的正方向，第一圆弧段的起始点和终止点的坐标和位姿由以下公式获得：根据获得的起始点和终止点位置，可以采取插值法生成中间路径点，假定要生成圆弧上的n个路径点，则第i个路径点的位姿为：其中：Xi_ARC1：第一段圆弧起始点的X坐标；Yi_ARC1：第一段圆弧起始点的y坐标；Thetai_ARC1：第一段圆弧起始点的姿态角；Xf_ARC1：第一段圆弧终止点的X坐标；Yf_ARC1：第一段圆弧终止点的y坐标；Thetaf_ARC1：第一段圆弧终止点的姿态角；Xi_B：样条曲线起点的X坐标；Yi_B：样条曲线起点的y坐标；Thetai_B：样条曲线起点的姿态角；Δtheta:采样的角度间隔；X_ARC1i：第一段圆弧中插入第i点x坐标；Y_ARC1i：第一段圆弧中插入第i点y坐标；Theta_ARC1i：第一段圆弧中插入的第i点姿态角。6.一种在自动泊车路径规划中保证车辆起点位置姿态约束的装置，其特征在于，在样条曲线的起始点位置补齐一段由车辆最小半径确定的圆弧，该圆弧与样条曲线的相接点处，其一阶导数连续，该圆弧起始点位置其一阶导数为0。7.一种在自动泊车路径规划中摆正车身时保证车辆运动学约束的装置，其特征在于，在样条曲线的终止点位置补齐一段由车辆最小半径确定的圆弧，该圆弧与样条曲线的相接点处，其一阶导数连续，该圆弧终止点位置其一阶导数为0。

百度查询： 大连波塞冬汽车电子科技有限公司 一种自动泊车路径规划装置