CN119408531A 一种泊车apa规划控制方法及装置 （纽劢科技(上海)有限公司） - 副本

本发明提供一种泊车APA规划控制方法及装现了低算力的泊车规划以及较强泊车路径规划2步骤S1、获取泊车所需要的外部环境信息，根据所述外步骤S3、将所述泊车路径传输至规控交互处理器进步骤S3中，所述规控交互处理器包括泊车规划信息处理处理模块的状态包括NoOp、Execute；所述泊车控制模块的状态包括Pending、Active和车控制模块状态为Pending，当形成泊车路径时，所述泊车规划信息处理模块存储所述所需要的外部环境信息，根据所述外部环境信息生成世界坐标系下的泊车起点位姿start步骤S102、根据所述泊车车位类型、所述车位坐标步骤S103、对感兴趣区域内外的障碍物进行处理并对动态3步骤S102d、将所述车位坐标系下的泊车预终点位姿和泊车终点位姿进行坐标系转换步骤S201、通过HybirdAStar算法对所述泊车起点位姿startpose和所述泊车预终步骤S202、通过直线对所述泊车预终点位姿preendpose和所述泊车终点位姿endCosttotal=Wl*Costl+Wre化后泊车路径相对原始路径偏移量代价的权重，Wsmooth表示优化后泊车路径平滑性代价的泊车路径相对原始泊车路径偏移量代价，Costsmooth表示优化后泊车路径平滑性代价，;4i表示世界坐标系下优化后第i个轨迹点的横坐标，yi表示世界坐标;;;;;。步骤S303、所述泊车规划信息处理模块获取当前泊车步骤S304、当所述泊车控制模块接收到当前泊车轨迹段512.根据权利要求1所述的一种泊车AP步骤S309、当泊车过程中出现暂停后并继续进行泊车泊车路径生成模块，用于根据世界坐标系下的所述泊车起6[0005]本发明提供一种泊车APA规划控制方法及装置，以解决现有技术中存在的泊车路系下的泊车起点位姿startpose、泊车预终点位姿preendpose和泊车终点位姿end7车位角点坐标值和障碍物角点坐标值，形成所述世界坐标系下的泊车终点位姿endpose需根据所述泊车车位类型、所述车位坐标系和所述世界坐标系下的泊车车位角点坐标值、障碍物角点坐标值和限位块角点坐标值，形成所述世界坐标系下的泊车终点位姿endpose；物进行处理并对动态障碍物进行实时监测，形成所述世界坐标系下的泊车预终点位姿pre误差不同确定世界坐标系下的泊车起点位姿startpose。块角点坐标值和所述泊车车位类型，形成车位坐标系下的泊车预终点位姿和泊车终点位步骤S102d、将所述车位坐标系下的泊车预终点位姿和泊车终点位姿进行坐标系8步骤S201、通过HybirdAStar算法对所述泊车起点位姿startpose和所述泊车步骤S202、通过直线对所述泊车预终点位姿preendpose和所述泊车终点位姿Costtotal=Wl*Costl+Wre优化后泊车路径长度代价的权重，Wref表示优化后泊车路径相对原始路径偏移量代价的权化后泊车路径长度代价，Costref表示优化后泊车路径相对原始泊车路径偏移量代价，迹优化前的轨迹点，优化轨迹点表示进行轨迹优化后的轨迹点）位置分别为(x0-ref,y0-ref),…,(xN-ref,yN-ref)，所述N+1个原始轨迹点在世界坐标系下的位置可以表示为{x0-;;i-ref表示世界坐标系下原始第i个轨迹点的横坐标，yi-ref表示世界坐标系;9;其中，lowerbound-i表示世界坐标系下第i个轨迹点的最大下边界变化值，;;。泊车控制模块用于下发控车指令和将反馈状态传输至泊车规划信息[0019]在本申请中，所述泊车控制模块的状态包括Pending、Active和Pending表示悬挂状态，该状态下泊车控制模块等待接收泊车规划信息处理模块传输的泊态为Active，所述泊车控制模块下发控制指令并对泊车过程根据当前泊车轨迹段进行跟息生成泊车起点位姿startpose、泊车预终点位姿preendpose和泊车终点位姿end述泊车预终点位姿preendpose和所述泊车终点位姿endpose，进行泊车路径规划处理，泊车位姿生成第三子模块，用于对roi内外的障碍物进行处理并对动态障碍物进第二孙模块，用于根据所述车位坐标系下的车位角点坐标值和障碍物角点坐标标系转换处理，形成世界坐标系下的泊车预终点位姿preendpose和泊车终点位姿end泊车路径规划第一子模块，用于通过HybirdAStar算法对所述泊车起点位姿泊车路径规划第三子模块，用于根据所述第一个泊车轨迹和所述第二个泊车轨Costtotal=Wl*Costl+Wre优化后泊车路径长度代价的权重，Wref表示优化后泊车路径相对原始路径偏移量代价的权化后泊车路径长度代价，Costref表示优化后泊车路径相对原始泊车路径偏移量代价，所述N+1个原始轨迹点在世界坐标系下的位置可以表示为{x0-refx1-ref…xN-ref}、{y0-;;i-ref表示世界坐标系下原始第i个轨迹点的横坐标，yi-ref表示世界坐标系;;其中，lowerbound-i表示世界坐标系下第i个轨迹点的最大下边界变化值，;;。泊车规控交互第三子模块，用于所述泊车规划信息处理模块获取当前泊车轨迹泊车规控交互第四子模块，用于当所述泊车控制模块接收到当前泊车轨迹段后，泊车轨迹段进行跟踪，在对当前泊车轨迹段完成跟踪后，所述泊车控制模块状态为泊车规控交互第五子模块，用于所述泊车规划信息处理模块接收到状态Success径传输至规控交互处理器进行处理，通过所述规控交互处理器完成所述泊车路径的控制，所述泊车规控交互第五子模块和所述泊车规控交互第六子模块完成对新的泊车路径的控暂停重启泊车规控交互模块，用于当泊车过程中出现暂停后并继续进行泊车时，计算机程序被处理器执行时实现本申请第一方面中任一项所述的一种泊车APA规划控制方本发明提供一种泊车APA规划控制方法及装置，解决了现有技术中存在的泊车路[0046]图2是本发明优选实施例的一种泊车APA规划控制方法中正常泊车场景规控交互[0047]图3是本发明优选实施例的一种泊车APA规划控制方法中重规划场景规控交互处[0048]图4是本发明优选实施例的一种泊车APA规划控制方法中泊车暂停并继续场景规系下的泊车起点位姿startpose、泊车预终点位姿preendpose和泊车终点位姿end车位角点坐标值和障碍物角点坐标值，形成所述世界坐标系下的泊车终点位姿endpose，块角点坐标值和所述泊车车位类型，形成车位坐标系下的泊车预终点位姿和泊车终点位[0058]步骤S102d、将所述车位坐标系下的泊车预终点位姿和泊车终点位姿进行坐标系[0060]步骤S104、获取泊车使用的定位源，确定世界坐标系下的泊车起点位姿start步骤S201、通过HybirdAStar算法对所述泊车起点位姿startpose和所述泊车[0063]步骤S202、通过直线对所述泊车预终点位姿preendpose和所述泊车终点位姿[0066]通过步骤S201-步骤S203实现泊车路径规划处理，相较于直接从泊车起点通过Costtotal=Wl*Costl+Wre优化后泊车路径长度代价的权重，Wref表示优化后泊车路径相对原始路径偏移量代价的权化后泊车路径长度代价，Costref表示优化后泊车路径相对原始泊车路径偏移量代价，所述N+1个原始轨迹点在世界坐标系下的位置可以表示为{x0-refx1-ref…xN-ref}、{y0-;;i-ref表示世界坐标系下原始第i个轨迹点的横坐标，yi-ref表示世界坐标系;;其中，lowerbound-i表示世界坐标系下第i个轨迹点的最大下边界变化值，;;。规控交互处理器还肩负着处理重规划与意外暂停等特殊问题发生后的规划控制之间交互态为Active，所述泊车控制模块下发控制指令并对泊车过程根据当前泊车轨迹段进行跟息生成泊车起点位姿startpose、泊车预终点位姿preendpose和泊车终点位姿end第二孙模块，用于根据所述车位坐标系下的车位角点坐标值和障碍物角点坐标标系转换处理，形成世界坐标系下的泊车预终点位姿preendpose和泊车终点位姿end[0087]泊车位姿生成第三子模块，用于对roi内外的障碍物进行处理并对动态障碍物进泊车路径规划第一子模块，用于通过HybirdAStar算法对所述泊车起点位姿[0092]泊车路径规划第三子模块，用于根据所述第一个泊车轨迹和所述第二个泊车轨Costtotal=Wl*Costl+Wre优化后泊车路径长度代价的权重，Wref表示优化后泊车路径相对原始路径偏移量代价的权化后泊车路径长度代价，Costref表示优化后泊车路径相对原始泊车路径偏移量代价，所述N+1个原始轨迹点在世界坐标系下的位置可以表示为{x0-refx1-ref…xN-ref}、{y0-;;i-ref表示世界坐标系下原始第i个轨迹点的横坐标，yi-ref表示世界坐标系;;其中，lowerbound-i表示世界坐标系下第i个轨迹点的最大下边界变化值，;;。[0099]泊车规控交互第三子模块，用于所述泊车规划信息处理模块获取当前泊车轨迹[0100]泊车规控交互第四子模块，用于当所述泊车控制模块接泊车轨迹段进行跟踪，在对当前泊车轨迹段完成跟踪后，所述泊车控制模块状态为[0101]泊车规控交互第五子模块，用于所述泊车规划信息处理模块接收