CN117351449B - 基于极坐标加权的道路可通行区域边界优化方法 - Google Patents
基于极坐标加权的道路可通行区域边界优化方法 Download PDFInfo
- Publication number
- CN117351449B CN117351449B CN202311646355.3A CN202311646355A CN117351449B CN 117351449 B CN117351449 B CN 117351449B CN 202311646355 A CN202311646355 A CN 202311646355A CN 117351449 B CN117351449 B CN 117351449B
- Authority
- CN
- China
- Prior art keywords
- fitting
- point
- boundary
- angle
- polar
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000005457 optimization Methods 0.000 title claims abstract description 38
- 238000001914 filtration Methods 0.000 claims abstract description 18
- 238000013500 data storage Methods 0.000 claims abstract description 12
- 125000004122 cyclic group Chemical group 0.000 claims abstract description 10
- 230000006870 function Effects 0.000 claims description 42
- 238000004590 computer program Methods 0.000 claims description 4
- 238000012216 screening Methods 0.000 claims description 4
- 238000001514 detection method Methods 0.000 description 4
- 239000011159 matrix material Substances 0.000 description 4
- 238000007500 overflow downdraw method Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 2
- 238000009499 grossing Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/50—Context or environment of the image
- G06V20/56—Context or environment of the image exterior to a vehicle by using sensors mounted on the vehicle
- G06V20/588—Recognition of the road, e.g. of lane markings; Recognition of the vehicle driving pattern in relation to the road
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
本发明涉及一种基于极坐标加权的道路可通行区域边界优化方法，其中，该方法包括：输入道路可通行区域边界点集；从笛卡尔坐标转换为极坐标，并为每个点设置数据存储结构体；将每一时刻点获取到的边界点的半径存入该角度对应的结构体历史极坐标容器中，并在每一角度上对相应的角度历史极坐标点进行卡尔曼滤波预测；将获取到的极坐标进行极坐标多项式循环拟合；对上述经过多次拟合处理的结果进行投票加权处理，获取拟合极坐标加权结果；设置道路边界点的限制条件，并输出优化后的点集。本发明还涉及一种相应的装置、处理器及其存储介质。采用了本发明的该方法、装置、处理器及其存储介质，在能够剔除边界噪声点的同时大大增加边界拟合的可靠性。
Description
技术领域
本发明涉及自动驾驶技术领域，尤其涉及道路可通行区域检测技术领域，具体是指一种基于极坐标加权的道路可通行区域边界优化方法、装置、处理器及其计算机可读存储介质。
背景技术
在无人驾驶领域中，感知模块需要对环境元素进行识别。道路可通行区域检测能够剔除动静态障碍物、道路边界等环境元素，为车辆提供安全可通行区域。现有道路可通行区域表示方式以极坐标表示为主，在环视360°范围上，提供可通行道路有效边缘点。然而，无论是基于视觉还是雷达的可通行区域检测方法，受感知结果噪声影响极大，在道路边缘处存在频繁的边界跳动现象，这使得最终的可通行区域检测结果无法直接用于后续任务。
基于此，亟需一种能够显著优化可行驶区域边界，并在保留边界细节的同时剔除由噪声引起的边界跳动问题，提升自动驾驶安全保障的技术方案。
发明内容
本发明的目的是克服了上述现有技术的缺点，提供了一种基于极坐标加权的道路可通行区域边界优化方法、装置、处理器及其计算机可读存储介质。
为了实现上述目的，本发明的基于极坐标加权的道路可通行区域边界优化方法如下：
该基于极坐标加权的道路可通行区域边界优化方法，其主要特点是，所述的方法包括以下步骤：
（1）输入道路可通行区域边界点集；
（2）将上述输入点从笛卡尔坐标转换为极坐标，并为每个点设置数据存储结构体；
（3）将每一时刻点获取到的相应边界点的半径存入该角度对应的结构体历史极坐标容器中，并在每一角度上对相应的角度历史极坐标点进行卡尔曼滤波预测处理；
（4）将获取到的各个极坐标进行极坐标多项式循环拟合处理，从而对各个边界点的角度范围进行更新；
（5）对上述经过多次拟合处理的结果进行投票加权处理，获取拟合极坐标加权结果；
（6）设置道路边界点的限制条件，并输出经过优化处理后的可行驶区域边界点集。
较佳地，所述的步骤（1）具体为：
设置道路边界点为z＝0的三维点，其中，设输入的点集合为S，点数为m，第i个点为，则该道路可通行区域边界点集表示为/>。
较佳地，所述的步骤（2）具体包括以下步骤：
（2.1）提取所述的步骤（1）的输入点，将相应的坐标点从笛卡尔坐标转换为极坐标，并保留极坐标系下该点的角度以及半径；
（2.2）为每个点设置数据存储结构体，所述的数据存储结构体包含该点笛卡尔坐标、该点极坐标、初始化该点历史极坐标容器为空，初始化拟合数据容器为空，其中，键值对数据为{误差，拟合所求半径}、初始化拟合极坐标加权结果为0、初始化累积拟合误差为0。
较佳地，所述的步骤（3）具体包括以下步骤：
（3.1）在每一时刻t将该时刻点的半径存入相应角度对应的结构体历史极坐标容器中，且该容器只保留当前时刻前10个历史数据，超过该数据量则动态删除；
（3.2）在每一角度上对该角度历史极坐标点进行卡尔曼滤波处理：
卡尔曼滤波观测输入为当前该点的半径，预测状态为该点极坐标半径值及半径变化速度，卡尔曼滤波结果与当前点极坐标半径的差值的绝对值用来设置该点拟合的显隐属性，其中，差值绝对值大于所设阈值，则该点拟合属性为显性，差值绝对值小于所设阈值，则该点拟合属性为隐性。
尤佳地，所述的步骤（4）包括将获取到的各个极坐标按照角度范围对所述的道路可通行区域边界点集进行拆分处理，具体为：
（4.1）设置单次拟合角度范围、各次拟合之间角度跨度、误差精度限制三个参数，将点集拆分成小的子集，并在所有角度上循环进行多次拟合，其中，所述的单次拟合角度范围为[0°, 360°]，并将角度拟合结束上限设置为360°+拟合角度范围，其大于360°的点选取规则为：该度数%360°的结果所对应的角度点。
更佳地，所述的步骤（4）还包括将经过角度拆分处理后的极坐标按照以下方式进行极坐标多项式拟合处理，具体为：
（4.2.1）使用以下公式作为多项式拟合基础函数模块：
；
其中，为最高项次数为j的幂函数，j的取值范围由拟合的最高项次数决定；
（4.2.2）将上式中的设置为多项式函数f(x):
；
其中，该式包含个未知数，/>为多项式系数，x为极坐标系下点的半径，n为最高项次数；
（4.2.3）设置拟合条件：拟合曲线与原始数据点在y方向上的误差平方最小，即通过以下公式表示为：
；
其中，为误差函数，/>为i点拟合的值，/>为i点半径真值，m为点的数量，n为最高项次数；
（4.2.4）按照以下方式对上式求解最小值，即对a求导等于0：
；
；
；
其中，为第i点的k次幂函数值，/>为第i点的j次幂函数值，/>为i点半径真值，通过求解此方程组，得到拟合函数f(x)，并将公式（6）改写为向量格式：
；
其中，m为点的数量，n为拟合最高项次数，再将公式（7）改写为：
；
其中，
；
其中，m 为点的数量，n为拟合最高项次数，为第m个点半径的n次幂函数，且Y维度为m×1，U维度为m×(n+1)，K维度为(n+1)×1，当m>n+1时，按照以下方式对超定方程求解：
。
较佳地，所述的步骤（4）还包括按照以下方式进行点结构状态更新：
对于每个子集的单次拟合，按照拟合误差阈值进行筛选，大于阈值则不计入本次拟合结果，误差小于阈值且该点拟合属性为显性则将拟合误差与该角度在当前拟合函数的值存储在该角度相应的点结构中，并将该点累积误差与本次拟合误差相加，完成更新；
待更新完成后，进一步判断下一步长所属的角度是否超过角度上限，若否，则按照新的角度范围继续拆分点集，反之，结束极坐标多项式循环拟合过程。
较佳地，所述的步骤（5）具体为：
遍历0-360°内所有点，判断拟合数据容器内参数数量是否大于1，如果小于等于1，则该点的拟合极坐标加权结果为原始半径r；如果参数数量大于1，则按照以下公式计算拟合极坐标加权结果：
；
其中，r表示拟合极坐标加权结果，n表示容器内拟合结果参数个数，E为累积误差，为第k个拟合结果的误差，/>表示第k个拟合结果。
较佳地，所述的步骤（6）具体包括以下步骤：
（6.1）设置道路边界点的限制条件：道路可通行区域最大边界半径为max_r，将大于半径的边界点过滤，对于拟合边界点大于该最大半径的拟合结果，采用最大半径max_r替代；
（6.2）输出经过优化处理后的可行驶区域边界点集。
该实现基于极坐标加权的道路可通行区域边界优化的装置，其主要特点是，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
该实现基于极坐标加权的道路可通行区域边界优化的处理器，其主要特点是，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
该计算机可读存储介质，其主要特点是，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现上述所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
采用了本发明的该基于极坐标加权的道路可通行区域边界优化方法、装置、处理器及其计算机可读存储介质，使用卡尔曼滤波及误差投票加权融合方法，能够对边界点集中的噪声点进行鲁棒的过滤，对边界起伏变化大的连续点进行更好的平滑，在优化的基础上保持更多的原有边界细节，能够显著提升道路可通行边界的准确度；同时，本技术方案中使用的拟合函数及对求解过程重新推导，将最小二乘拟合过程转换为参数矩阵的乘法，这样改进后的函数与实际道路可行驶边界更加贴合，求解过程时间复杂度低，效率更高。除此之外，本技术方案中还提到的点结构体的设置中加入角度范围、步长、精度等控制参数，可根据实际拟合点集进行自适应，使得拟合过程可控。
附图说明
图1为本发明的该基于极坐标加权的道路可通行区域边界优化方法的流程图。
具体实施方式
为了能够更清楚地描述本发明的技术内容，下面结合具体实施例来进行进一步的描述。
在详细说明根据本发明的实施例前，应该注意到的是，在下文中，术语“包括”、“包含”或任何其他变体旨在涵盖非排他性的包含，由此使得包括一系列要素的过程、方法、物品或者设备不仅包含这些要素，而且还包含没有明确列出的其他要素，或者为这种过程、方法、物品或者设备所固有的要素。
本技术方案的该基于极坐标加权的道路可通行区域边界优化方法，其基本处理思路如下：对输入边界点集转换至极坐标下表示，在每一角度间隔上创建点结构体。在每一角度上，根据该角度历史半径，进行卡尔曼滤波预测该点状态。在0＋n°（n=360+角度范围）区间上，按照所设单次拟合角度范围及下次拟合角度跨度步长，进行多次循环拟合。卡尔曼预测状态与当前点状态结合判断，设置单次拟合结果的显隐属性，待拟合完成后，在同一角度下，会存在多次拟合结果，根据拟合误差来投票加权该点所有拟合结果，融合后的结果作为当前点的优化结果。
请参阅图1所示，该基于极坐标加权的道路可通行区域边界优化方法，其中，所述的方法包括以下步骤：
（1）输入道路可通行区域边界点集；
（2）将上述输入点从笛卡尔坐标转换为极坐标，并为每个点设置数据存储结构体；
（3）将每一时刻点获取到的相应边界点的半径存入该角度对应的结构体历史极坐标容器中，并在每一角度上对相应的角度历史极坐标点进行卡尔曼滤波预测处理；
（4）将获取到的各个极坐标进行极坐标多项式循环拟合处理，从而对各个边界点的角度范围进行更新；
（5）对上述经过多次拟合处理的结果进行投票加权处理，获取拟合极坐标加权结果；
（6）设置道路边界点的限制条件，并输出经过优化处理后的可行驶区域边界点集。
作为本发明的优选实施方式，所述的步骤（1）具体为：
设置道路边界点为z＝0的三维点，其中，设输入的点集合为S，点数为m，第i个点为，则该道路可通行区域边界点集表示为/>。
作为本发明的优选实施方式，所述的步骤（2）具体包括以下步骤：
（2.1）提取所述的步骤（1）的输入点，将相应的坐标点从笛卡尔坐标转换为极坐标，并保留极坐标系下该点的角度以及半径；
（2.2）为每个点设置数据存储结构体，所述的数据存储结构体包含该点笛卡尔坐标、该点极坐标、初始化该点历史极坐标容器为空，初始化拟合数据容器为空，其中，键值对数据为{误差，拟合所求半径}、初始化拟合极坐标加权结果为0、初始化累积拟合误差为0。
作为本发明的优选实施方式，所述的步骤（3）具体包括以下步骤：
（3.1）在每一时刻t将该时刻点的半径存入相应角度对应的结构体历史极坐标容器中，且该容器只保留当前时刻前10个历史数据，超过该数据量则动态删除；
（3.2）在每一角度上对该角度历史极坐标点进行卡尔曼滤波处理：
卡尔曼滤波观测输入为当前该点的半径，预测状态为该点极坐标半径值及半径变化速度，卡尔曼滤波结果与当前点极坐标半径的差值的绝对值用来设置该点拟合的显隐属性，其中，差值绝对值大于所设阈值，则该点拟合属性为显性，差值绝对值小于所设阈值，则该点拟合属性为隐性。
作为本发明的优选实施方式，所述的步骤（4）包括将获取到的各个极坐标按照角度范围对所述的道路可通行区域边界点集进行拆分处理，具体为：
（4.1）设置单次拟合角度范围、各次拟合之间角度跨度、误差精度限制三个参数，将点集拆分成小的子集，并在所有角度上循环进行多次拟合，其中，所述的单次拟合角度范围为[0°, 360°]，并将角度拟合结束上限设置为360°+拟合角度范围，其大于360°的点选取规则为：该度数%360°的结果所对应的角度点。
作为本发明的优选实施方式，所述的步骤（4）还包括将经过角度拆分处理后的极坐标按照以下方式进行极坐标多项式拟合处理，具体为：
（4.2.1）使用以下公式作为多项式拟合基础函数模块：
；
其中，为最高项次数为j的幂函数，j的取值范围由拟合的最高项次数决定；
（4.2.2）将上式中的设置为多项式函数f(x):
；
其中，该式包含个未知数，/>为多项式系数，x为极坐标系下点的半径，n为最高项次数；
（4.2.3）设置拟合条件：拟合曲线与原始数据点在y方向上的误差平方最小，即通过以下公式表示为：
；
其中，为误差函数，/>为i点拟合的值，/>为i点半径真值，m为点的数量，n为最高项次数；
（4.2.4）按照以下方式对上式求解最小值，即对a求导等于0：
；
；
；
其中，为第i点的k次幂函数值，/>为第i点的j次幂函数值，/>为i点半径真值，通过求解此方程组，得到拟合函数，并将公式（6）改写为向量格式：
；
其中，m为点的数量，n为拟合最高项次数，再将公式（7）改写为：
；
其中，
；
其中，m 为点的数量，n为拟合最高项次数，为第m个点半径的n次幂函数，且Y维度为m×1，U维度为m×(n+1)，K维度为(n+1)×1，当m>n+1时，按照以下方式对超定方程求解：
。
作为本发明的优选实施方式，所述的步骤（4）还包括按照以下方式进行点结构状态更新：
对于每个子集的单次拟合，按照拟合误差阈值进行筛选，大于阈值则不计入本次拟合结果，误差小于阈值且该点拟合属性为显性则将拟合误差与该角度在当前拟合函数的值存储在该角度相应的点结构中，并将该点累积误差与本次拟合误差相加，完成更新；
待更新完成后，进一步判断下一步长所属的角度是否超过角度上限，若否，则按照新的角度范围继续拆分点集，反之，结束极坐标多项式循环拟合过程。
作为本发明的优选实施方式，所述的步骤（5）具体为：
遍历0-360°内所有点，判断拟合数据容器内参数数量是否大于1，如果小于等于1，则该点的拟合极坐标加权结果为原始半径r；如果参数数量大于1，则按照以下公式计算拟合极坐标加权结果：
；
其中，r表示拟合极坐标加权结果，n表示容器内拟合结果参数个数，E为累积误差，为第k个拟合结果的误差，/>表示第k个拟合结果。
作为本发明的优选实施方式，所述的步骤（6）具体包括以下步骤：
（6.1）设置道路边界点的限制条件：道路可通行区域最大边界半径为max_r，将大于半径的边界点过滤，对于拟合边界点大于该最大半径的拟合结果，采用最大半径max_r替代；
（6.2）输出经过优化处理后的可行驶区域边界点集。
在实际应用当中，该基于极坐标加权的道路可通行区域边界优化方法主要处理步骤如下：
1）道路边界点集输入。
输入所提取的可行驶区域边界的空间点集。在本技术方案中，道路边界点为z＝0的三维点，设输入的点集合为S，点数为m，第i个点为，则该点集可表示为/>。
2）笛卡尔坐标转极坐标。
提取上述输入点，将点的xy坐标转为极坐标系下表示。最终保留极坐标系下点的角度（单位°）与半径（单位m）。在转换中，为每个点设置数据存储结构体，包含该点笛卡尔坐标、该点极坐标、初始化该点历史极坐标容器为空，初始化拟合数据容器为空（键值对数据{误差，拟合所求半径}）、初始化拟合极坐标加权结果为0、初始化累积拟合误差为0。
3）历史点卡尔曼滤波。
在每一时刻t将该时刻点的半径存入该角度对应的结构体历史极坐标容器中，该容器只保留当前时刻前10个历史数据，超过该数据量则动态删除。在每一角度上对该角度历史极坐标点进行卡尔曼滤波。在本技术方案中，卡尔曼滤波观测输入为点的半径，预测状态为该点极坐标半径值及半径变化速度。卡尔曼滤波结果与当前点极坐标半径的差值的绝对值用来设置该点拟合的显隐属性，差值绝对值大于所设阈值，则该点拟合属性为显性，反之为隐性。
4）极坐标多项式循环拟合。
a.按角度范围对点集拆分。
为了能够鲁棒的获取优化结果，避免单次拟合失准造成优化性能下降。本发明通过设置单次拟合角度范围、各次拟合之间角度跨度、误差精度限制三个参数，将点集拆分成小的子集，在所有角度上循环进行多次拟合。为了避免跨360°造成数据截断，设置360°与0°闭合回环；为了在360°及附近角度同样包含多次拟合，将角度拟合结束上限设置为360°+拟合角度范围，其大于360°的点选取规则为：该度数%360的结果所对应的角度点。本发明所设的角度跨度应小于拟合角度范围的一半。
b.极坐标多项式拟合。
本发明所使用的多项式拟合基础函数模块为：
；
公式（1）中，为最高项次数为j的幂函数。j 的取值范围由拟合的最高项次数决定。
多项式函数为:
；
公式（2）中包含个未知数，/>为多项式系数，x 为极坐标系下点的半径，n 为最高项次数。
拟合条件：拟合曲线与原始数据点在y方向上的误差平方最小。即：
；
公式（3）中为误差函数，/>为i点拟合的值，/>为i点半径真值，m为点的数量，n为最高项次数；
对于公式（3）求极小值问题，即对a求导等于0：
；
；
；
为第i点的k次幂函数值，/>为第i点的j次幂函数值，/>为i点半径真值。
解此方程组得系数a0 a1 a2 ... an，得出拟合函数f(x)。公式（6）写成向量格式：
；
公式（7）中，m 为点的数量，n为拟合最高项次数。
为了方便表示，上式写为：
；
其中：
；
公式（9）中，m 为点的数量，n为拟合最高项次数。为第m个点半径的n次幂函数。
在公式（9）中，Y维度为m*1，U维度为m*(n+1)，K维度为(n+1)*1。当m>n+1时，对于超定方程求解：
；
通过上述推导，本技术方案函数拟合过程最终变为矩阵乘法的操作，即公式（10）。
c.点结构状态更新。
对于每个子集的单次拟合，按照拟合误差阈值进行筛选，大于阈值则不计入本次拟合结果，误差小于阈值且该点拟合属性为显性则将拟合误差与该角度在当前拟合函数的值存储在该角度相应的点结构中。并将该点累积误差与本次拟合误差相加，进行更新。
d.最大角度上限判断。
每完成一次子集拟合后，判断下一步长所属的角度是否超过角度上限，若是没有则按照新的角度范围继续拆分点集，重复上述b和c步骤。反之，结束拟合过程。
5）多次拟合结果投票加权。
遍历0-360°所有点，通过上述拟合过程，每个角度点结构体中的拟合数据容器有了新的状态。首先判断该容器内参数数量是否大于1，如果小于等于1，则该点的拟合极坐标加权结果为原始半径r；如果参数数量大于1，则使用公式（11）计算拟合极坐标加权结果。
；
上式中，r表示拟合极坐标加权结果，n表示容器内拟合结果参数个数，E为累积误差，为第k个拟合结果的误差，/>表示第k个拟合结果。
6）边界限制条件。
在本技术方案中，设置道路可通行区域最大边界半径max_r，认为大于半径的边界点在实际中准确的不高，故过滤。对于拟合边界点大于该最大半径的拟合结果，采用最大半径max_r替代。
7）优化后点集输出。
在实际应用当中，本技术方案的该基于极坐标加权的道路可通行区域边界优化方法与现有传统拟合方法相比，在能够剔除边界噪声点的同时大大增加了边界拟合的可靠性；同时，本技术方案的边界拟合结果采用循环投票加权融合方法，可以有效提升拟合边界的准确性，且可以在单次拟合效果不佳的情况下仍保持拟合的准确度；除此之外，其还可以实现自适应拟合过程，可根据所设拟合角度范围、循环步长、自动进行边界拟合调整，并且，由于拟合过程采用矩阵乘法操作，整个拟合过程时间复杂度也是较低的。
在实际应用当中，可以理解的是，如图1所示，“卡尔曼滤波预测”与“极坐标多项式循环拟合”本质上是同步进行的。只是卡尔曼滤波的预测结果会参与点结构体状态的更新，从功能上来讲，卡尔曼滤波做的是每个角度上的点在时间维度上的状态预测，而极坐标多项式循环拟合做的是空间维度上的优化，是通过对每个时刻各个角度上的点之间的空间位置来拟合优化。而在“极坐标多项式循环拟合”任务中，又详细拆分了“按角度范围点集拆分”的前置处理步骤。
该实现基于极坐标加权的道路可通行区域边界优化的装置，其中，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
该实现基于极坐标加权的道路可通行区域边界优化的处理器，其中，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
该计算机可读存储介质，其中，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现上述所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行装置执行的软件或固件来实现。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成的，程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
上述提到的存储介质可以是只读存储器，磁盘或光盘等。
在本说明书的描述中，参考术语“一实施例”、“一些实施例”、“示例”、“具体示例”、或“实施例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
采用了本发明的该基于极坐标加权的道路可通行区域边界优化方法、装置、处理器及其计算机可读存储介质，使用卡尔曼滤波及误差投票加权融合方法，能够对边界点集中的噪声点进行鲁棒的过滤，对边界起伏变化大的连续点进行更好的平滑，在优化的基础上保持更多的原有边界细节，能够显著提升道路可通行边界的准确度；同时，本技术方案中使用的拟合函数及对求解过程重新推导，将最小二乘拟合过程转换为参数矩阵的乘法，这样改进后的函数与实际道路可行驶边界更加贴合，求解过程时间复杂度低，效率更高。除此之外，本技术方案中还提到的点结构体的设置中加入角度范围、步长、精度等控制参数，可根据实际拟合点集进行自适应，使得拟合过程可控。
在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。
Claims (7)
1.一种基于极坐标加权的道路可通行区域边界优化方法，其特征在于，所述的方法包括以下步骤：
（1）输入道路可通行区域边界点集；
（2）将上述输入点从笛卡尔坐标转换为极坐标，并为每个点设置数据存储结构体；
（3）将每一时刻点获取到的相应边界点的半径存入当前角度对应的结构体历史极坐标容器中，并在每一角度上对相应的角度历史极坐标点进行卡尔曼滤波预测处理；
（4）将获取到的各个极坐标进行极坐标多项式循环拟合处理，从而对各个边界点的角度范围进行更新；
（5）对以上经过多次拟合处理的结果进行投票加权处理，获取拟合极坐标加权结果；
（6）设置道路边界点的限制条件，并输出经过优化处理后的可行驶区域边界点集；
所述的步骤（3）具体包括以下步骤：
（3.1）在每一时刻t将该时刻点的半径存入相应角度对应的结构体历史极坐标容器中，且该容器只保留当前时刻前10个历史数据，超过当前数据量则动态删除；
（3.2）在每一角度上对该角度历史极坐标点进行卡尔曼滤波处理：
卡尔曼滤波观测输入为当前该点的半径，预测状态为该点极坐标半径值及半径变化速度，卡尔曼滤波结果与当前点极坐标半径的差值的绝对值用来设置该点拟合的显隐属性，其中，差值绝对值大于所设阈值，则该点拟合属性为显性，差值绝对值小于所设阈值，则该点拟合属性为隐性；
所述的步骤（4）包括将获取到的各个极坐标按照角度范围对所述的道路可通行区域边界点集进行拆分处理，具体为：
（4.1）设置单次拟合角度范围、各次拟合之间角度跨度、误差精度限制三个参数，将点集拆分成小的子集，并在所有角度上循环进行多次拟合，其中，所述的单次拟合角度范围为[0°, 360°]，并将角度拟合结束上限设置为360°+拟合角度范围，其大于360°的点选取规则为：当前度数%360°的结果所对应的角度点；
所述的步骤（4）还包括将经过角度拆分处理后的极坐标按照以下方式进行极坐标多项式拟合处理，具体为：
（4.2.1）使用以下公式作为多项式拟合基础函数模块：
；
其中，为最高项次数为j的幂函数，j的取值范围由拟合的最高项次数决定；
（4.2.2）将上式中的设置为多项式函数f（x）:
；
其中，该式包含个未知数，/>为多项式系数，x为极坐标系下点的半径，n为最高项次数；
（4.2.3）设置拟合条件：拟合曲线与原始数据点在y方向上的误差平方最小，即通过以下公式表示为：
；
其中，为误差函数，/>为i点拟合的值，/>为i点半径真值，m为点的数量，n为最高项次数；
（4.2.4）按照以下方式对上式求解最小值，即对a求导等于0：
；
；
；
其中，为第i点的k次幂函数值，/>为第i点的j次幂函数值，/>为i点半径真值，通过求解此方程组，得到拟合函数f（x），并将公式（6）改写为向量格式：
；
其中，m为点的数量，n为拟合最高项次数，再将公式（7）改写为：
；
其中，
；
其中，m 为点的数量，n为拟合最高项次数，为第m个点半径的n次幂函数，且Y维度为m×1，U维度为m×(n+1)，K维度为(n+1)×1，当m>n+1时，按照以下方式对超定方程求解：
；
所述的步骤（4）还包括按照以下方式进行点结构状态更新：
对于每个子集的单次拟合，按照拟合误差阈值进行筛选，大于阈值则不计入本次拟合结果，误差小于阈值且该点拟合属性为显性则将拟合误差与该角度在当前拟合函数的值存储在该角度相应的点结构中，并将该点累积误差与本次拟合误差相加，完成更新；
待更新完成后，进一步判断下一步长所属的角度是否超过角度上限，若否，则按照新的角度范围继续拆分点集，反之，结束极坐标多项式循环拟合过程；
所述的步骤（5）具体为：
遍历0-360°内所有点，判断拟合数据容器内参数数量是否大于1，如果小于等于1，则该点的拟合极坐标加权结果为原始半径r；如果参数数量大于1，则按照以下公式计算拟合极坐标加权结果：
；
其中，r表示拟合极坐标加权结果，n表示容器内拟合结果参数个数，E为累积误差，为第k个拟合结果的误差，/>表示第k个拟合结果。
2.根据权利要求1所述的基于极坐标加权的道路可通行区域边界优化方法，其特征在于，所述的步骤（1）具体为：
设置道路边界点为z＝0的三维点，其中，设输入的点集合为S，点数为m，第i个点为则该道路可通行区域边界点集表示为/>。
3.根据权利要求2所述的基于极坐标加权的道路可通行区域边界优化方法，其特征在于，所述的步骤（2）具体包括以下步骤：
（2.1）提取所述的步骤（1）的输入点，将相应的坐标点从笛卡尔坐标转换为极坐标，并保留极坐标系下该点的角度以及半径；
（2.2）为每个点设置数据存储结构体，所述的数据存储结构体包含该点笛卡尔坐标、该点极坐标、初始化该点历史极坐标容器为空，初始化拟合数据容器为空，其中，键值对数据为{误差，拟合所求半径}、初始化拟合极坐标加权结果为0、初始化累积拟合误差为0。
4.根据权利要求1所述的基于极坐标加权的道路可通行区域边界优化方法，其特征在于，所述的步骤（6）具体包括以下步骤：
（6.1）设置道路边界点的限制条件：道路可通行区域最大边界半径为max_r，将大于半径的边界点过滤，对于拟合边界点大于该最大半径的拟合结果，采用最大半径max_r替代；
（6.2）输出经过优化处理后的可行驶区域边界点集。
5.一种实现基于极坐标加权的道路可通行区域边界优化的装置，其特征在于，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～4中任一项所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
6.一种实现基于极坐标加权的道路可通行区域边界优化的处理器，其特征在于，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～4中任一项所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
7.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现权利要求1～4中任一项所述的基于极坐标加权的道路可通行区域边界优化方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311646355.3A CN117351449B (zh) | 2023-12-04 | 2023-12-04 | 基于极坐标加权的道路可通行区域边界优化方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311646355.3A CN117351449B (zh) | 2023-12-04 | 2023-12-04 | 基于极坐标加权的道路可通行区域边界优化方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117351449A CN117351449A (zh) | 2024-01-05 |
| CN117351449B true CN117351449B (zh) | 2024-02-09 |
Family
ID=89357873
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311646355.3A Active CN117351449B (zh) | 2023-12-04 | 2023-12-04 | 基于极坐标加权的道路可通行区域边界优化方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117351449B (zh) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104866823A (zh) * | 2015-05-11 | 2015-08-26 | 重庆邮电大学 | 一种基于单目视觉的车辆检测与跟踪方法 |
| CN106842231A (zh) * | 2016-11-08 | 2017-06-13 | 长安大学 | 一种道路边界检测及跟踪方法 |
| CN109684921A (zh) * | 2018-11-20 | 2019-04-26 | 吉林大学 | 一种基于三维激光雷达的道路边界检测与跟踪方法 |
| JP2021086555A (ja) * | 2019-11-29 | 2021-06-03 | 日本電産株式会社 | 自動運転支援装置、プログラムおよび自動運転システム |
| KR20220063433A (ko) * | 2020-11-10 | 2022-05-17 | 성균관대학교산학협력단 | Avm 영상, 차량 데이터 및 지도 정보를 이용한 차선 인식 및 추적 방법 및 장치 |
| WO2023216470A1 (zh) * | 2022-05-11 | 2023-11-16 | 合众新能源汽车股份有限公司 | 一种可行驶区域检测方法、装置及设备 |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| GB2591332B (en) * | 2019-12-19 | 2024-02-14 | Motional Ad Llc | Foreground extraction using surface fitting |
| US20220408019A1 (en) * | 2021-06-17 | 2022-12-22 | Fyusion, Inc. | Viewpoint path modeling |
-
2023
- 2023-12-04 CN CN202311646355.3A patent/CN117351449B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104866823A (zh) * | 2015-05-11 | 2015-08-26 | 重庆邮电大学 | 一种基于单目视觉的车辆检测与跟踪方法 |
| CN106842231A (zh) * | 2016-11-08 | 2017-06-13 | 长安大学 | 一种道路边界检测及跟踪方法 |
| CN109684921A (zh) * | 2018-11-20 | 2019-04-26 | 吉林大学 | 一种基于三维激光雷达的道路边界检测与跟踪方法 |
| JP2021086555A (ja) * | 2019-11-29 | 2021-06-03 | 日本電産株式会社 | 自動運転支援装置、プログラムおよび自動運転システム |
| KR20220063433A (ko) * | 2020-11-10 | 2022-05-17 | 성균관대학교산학협력단 | Avm 영상, 차량 데이터 및 지도 정보를 이용한 차선 인식 및 추적 방법 및 장치 |
| WO2023216470A1 (zh) * | 2022-05-11 | 2023-11-16 | 合众新能源汽车股份有限公司 | 一种可行驶区域检测方法、装置及设备 |
Non-Patent Citations (3)
| Title |
|---|
| Polynomial Based Kalman Filter Result Fitting to Data;Georgiana MAGU 等;《2020 43rd International Conference on Telecommunications and Signal Processing (TSP)》;全文 * |
| Real-time extraction method of road boundary based on three-dimensional lidar;Kunlei Hu 等;《Journal of Physics: Conference Series》;全文 * |
| 空间直线拟合的加权极坐标法;史翔 等;《测绘地理信息》;第44卷(第2期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117351449A (zh) | 2024-01-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20220363259A1 (en) | Method for generating lane changing decision-making model, method for lane changing decision-making of unmanned vehicle and electronic device | |
| CN108803321B (zh) | 基于深度强化学习的自主水下航行器轨迹跟踪控制方法 | |
| US20220004191A1 (en) | Usv formation path-following method based on deep reinforcement learning | |
| CN111142522B (zh) | 一种分层强化学习的智能体控制方法 | |
| CN109885891A (zh) | 一种智能车gpu并行加速轨迹规划方法 | |
| CN109176532B (zh) | 一种机械臂路径规划方法、***及装置 | |
| EP3699698B1 (en) | Method and device for processing control parameter, and storage medium | |
| KR102229859B1 (ko) | 기계 학습 및 전이 학습을 이용한 공정 관리 방법, 장치 및 시스템 | |
| US20210263526A1 (en) | Method and device for supporting maneuver planning for an automated driving vehicle or a robot | |
| WO2021227536A1 (en) | Methods and systems for support policy learning | |
| CN111352419B (zh) | 基于时序差分更新经验回放缓存的路径规划方法及*** | |
| CN109556609B (zh) | 一种基于人工智能的避碰方法及装置 | |
| JP2022189799A (ja) | Ｆｅｗ－ｓｈｏｔ模倣のためのデモンストレーション条件付き強化学習 | |
| CN113625753B (zh) | 一种由专家规则引导神经网络学习无人机机动飞行的方法 | |
| CN117351449B (zh) | 基于极坐标加权的道路可通行区域边界优化方法 | |
| CN114529010A (zh) | 一种机器人自主学习方法、装置、设备及存储介质 | |
| CN114239974A (zh) | 多智能体的位置预测方法、装置、电子设备及存储介质 | |
| CN117387635A (zh) | 一种基于深度强化学习和pid控制器的无人机导航方法 | |
| CN108376283B (zh) | 用于神经网络的池化装置和池化方法 | |
| CN113503885B (zh) | 一种基于采样优化ddpg算法的机器人路径导航方法及*** | |
| CN110533158B (zh) | 模型建构方法、***及非易失性电脑可读取记录介质 | |
| CN114167872A (zh) | 机器人避障方法、***、计算机及机器人 | |
| CN116700015B (zh) | 水下航行器主动增稳控制方法及装置 | |
| CN117055586B (zh) | 基于自适应控制的水下机器人巡游搜索与抓取方法、*** | |
| CN114905505B (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |