一种基于雷达障碍物距离动态改变车轮轨迹线方法及系统

摘要

本发明提供一种基于雷达障碍物距离动态改变车轮轨迹线方法及系统，通过在生成轨迹线离散点数据时计算出每一小段的四个顶点数据，在保存轨迹线数据时，仅保存落在四个顶点围成的四边形范围内的数据，由此实现轨迹线的分段显示；通过在绘制轨迹线时，根据雷达障碍物距离，计算出图像坐标系下其所对应的直线方程，来动态调节轨迹线显示的长度，由此实现轨迹线长度动态调节的功能；在保存PAS Distance Warning区域数据时，将每一段的颜色按照预设值设置，在绘制轨迹线时，根据不同的颜色来判断PAS Distance Warning的数据分别对应的区域段，进而控制其是否显示或隐藏或将其修改为需要显示的颜色，由此实现PAS区域的隐藏或显示，及根据障碍物检测的距离动态改变每一段颜色的功能。

说明书

技术领域

本发明涉及激光雷达识别技术领域，尤其是涉及一种基于雷达障碍物距离动态改变车轮轨迹线方法及系统。

背景技术

全景环视系统可视车轮轨迹线是汽车辅助系统的重要组成部分，帮助驾驶者克服汽车盲区的困扰完成行驶、倒车、停车等操作。行驶/倒车轨迹线算法基于相机标定理论，将在世界坐标系下推导的行驶/倒车轨迹方程通过相机坐标系和图像坐标系的坐标变换生成轨迹线数据，再将对应车轮轨迹线数据写入到文件中保存。在显示时由APP根据实际车轮角度读取对应的车轮轨迹线数据文件，将其绘制出来。该算法无法根据雷达障碍物距离动态调整轨迹线的显示长度及颜色，且不支持PAS（Parking Assist System）Distance Warnig样式及根据雷达障碍物距离动态调整其颜色。其次，该算法不支持轨迹线分段显示的样式需求，也不能控制轨迹线和PAS（Parking Assist System）Distance Warnig 的独立显示。

发明内容

针对上述问题，本技术创新地提出了一种基于雷达障碍物距离动态改变车轮轨迹线方法及系统，通过在生成轨迹线离散点数据时计算出每一小段的四个顶点数据，在保存轨迹线数据时，仅保存落在四个顶点围成的四边形范围内的数据，由此实现轨迹线的分段显示。通过在绘制轨迹线时，根据雷达障碍物距离，计算出图像坐标系下其所对应的直线方程，来动态调节轨迹线显示的长度。由此实现轨迹线长度动态调节的功能。

具体的，本发明所述的一种基于雷达障碍物距离动态改变车轮轨迹线方法及系统，包括以下步骤：

S1：根据世界坐标系下的车轮轨迹线运动模型方程，基于相机标定理论转换到图像坐标系下生成轨迹线离散点数据；

S2：根据图像坐标系下的所述离散点数据，对离散点数据剪裁缩放，通过B样条方法进行曲线拟合，生成曲线坐标数据；

S3：对曲线围成的多边形进行颜色填充，并将曲线坐标数据写入数据文件中，生成轨迹线数据文件；

S4：解析所述轨迹线数据文件，获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标，绘制车轮轨迹线，并根据雷达障碍物的距离动态显示对应的PAS DistanceWarning区域的颜色。

其中，所述步骤S1还包括：

S1.1：将轨迹线进行分段显示，根据配置文件内的每一段的长度和间隔长度，计算世界坐标系下离散点数据，同时求出轨迹线要显示的每一小段4个顶点对应的离散点坐标（x0, y0）,（x1, y1）,（x2, y2）,（x3, y3）；

S1.2：根据四段PAS Distance Warning样式，基于配置文件内的四段PASDistance Warning区域的位置获取世界坐标系离散点坐标，并基于相机标定参数投影到图像坐标系下生成四段PAS Distance Warning离散点数据。

进一步的，所述步骤S2还包括：

S2.1：基于图像坐标系下轨迹线离散点数据，经过B样条方法生成对应的曲线坐标数据；

S2.2：基于图像坐标系下的四段PAS Distance Warning离散点数据，经过B样条方法生成对应的曲线坐标数据。

进一步的，所述步骤S3还包括：

S3.1：根据计算出的每一段4个顶点对应的离散点坐标，使用离散点坐标（x0, y0）和离散点坐标（x1, y1）计算出对应的直线方程L1，使用离散点坐标（x2, y2）, 和离散点坐标（x3, y3）计算出对应的直线方程L2；

S3.2：在保存轨迹线曲线坐标数据时，将每个像素坐标（X，Y）中的X值代入直线方程L1和L2中，求出对应的Y1和Y2，并判断Y值是否位于Y1和Y2之间，若位于则将该轨迹线曲线坐标数据保存至数据文件中，否则不保存；

S3.3：同时，将每一段PAS Distance Warning区域的颜色做有规律的修改，所述四段PAS Distance Warning区域中的每一段的颜色设置为根据雷达障碍物距离进行动态调整，在写入数据文件时，对每一段所述PAS Distance Warning区域的颜色RGB值进行设置，进而保存至数据文件中。

进一步的，所述S3.3还包括：所述四段PAS Distance Warning区域中的每一段的颜色RGB中对应的R值分别累加1，通过颜色区分每一段PAS Distance Warning区域的数据。

进一步的，所述步骤S4还包括：

S4.1：获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标，根据相机标定参数将其转换至图像坐标系下离散点坐标（x4, y4）和离散点坐标（x5, y5），并求出对应的直线方程L3；

S4.2：将轨迹线数据坐标（X’,Y’）的X’值代入方程L3中，求出对应的Y3；判断Y’值是否大于Y3，若大于则显示，否则不显示；

S4.3：同时，根据PAS Distance Warning区域数据每一小段的颜色，判断其具体属于哪一小段，再将其修改为实际需要的颜色，再进行显示；

S4.4：通过汽车中控显示屏进行呈现。

进一步的，所述步骤S4还包括：在绘制轨迹线时，根据所述四段PAS DistanceWarning区域中的每一段的颜色判断PAS Distance Warning的数据分别对应为所述区域中的相同颜色段，进而控制该颜色是否显示，或隐藏，或将其修改为需要显示的颜色。

作为另一优选的，本发明还提供了一种基于雷达障碍物距离动态改变车轮轨迹线系统，包括：

轨迹线生成单元，根据世界坐标系下的车轮轨迹线运动模型方程，基于相机标定理论转换到图像坐标系下生成轨迹线离散点数据；根据图像坐标系下的所述离散点数据，通过B样条方法进行曲线拟合，生成曲线坐标数据；将曲线坐标数据写入数据文件中，生成轨迹线数据文件；解析所述轨迹线数据文件，获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标，绘制车轮轨迹线，并根据雷达障碍物的距离动态显示对应的PAS DistanceWarning区域的颜色；

显示单元，根据雷达障碍物距离，计算出图像坐标系下其所对应的直线方程，来动态调节轨迹线显示的长度；根据障碍物检测的距离，动态显示所述四段PAS DistanceWarning区域中的每一段的颜色。

其中，所述轨迹线生成单元，包括轨迹线数据文件生成单元，具体包括以下步骤：

步骤1：将轨迹线进行分段显示，根据配置文件内的每一段的长度和间隔长度，计算世界坐标系下离散点数据，同时求出轨迹线要显示的每一小段4个顶点对应的离散点坐标（x0, y0）,（x1, y1）,（x2, y2）,（x3, y3）；

步骤2：根据四段PAS Distance Warning样式，基于配置文件内的四段PASDistance Warning区域的位置获取世界坐标系离散点坐标，并基于相机标定参数投影到图像坐标系下生成四段PAS Distance Warning离散点数据；

步骤3：基于图像坐标系下轨迹线离散点数据，对离散点数据剪裁缩放，经过B样条方法生成对应的曲线坐标数据；基于图像坐标系下的四段PAS Distance Warning离散点数据，经过B样条方法生成对应的曲线坐标数据；对曲线围成的多边形进行颜色填充，将曲线坐标数据写入数据文件中，生成轨迹线数据文件；

步骤4：判断轨迹线数据是否位于所述四段PAS Distance Warning区域的每一小段对应的区域内，若是，则保存至数据文件中；否则，不保存；同时，对每一段所述PASDistance Warning区域的颜色RGB值进行设置，所述四段PAS Distance Warning区域中的每一段的颜色RGB中对应的R值分别累加1。

进一步的，所述轨迹线生成单元，还包括轨迹线绘制单元，具体还包括以下步骤：

步骤5：解析所述轨迹线数据文件，获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标；

步骤6：获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标，根据相机标定参数将其转换至图像坐标系下离散点坐标（x4, y4）和离散点坐标（x5, y5），并求出对应的直线方程L3；

步骤7：将轨迹线数据坐标（X’,Y’）的X’值代入方程L3中，求出对应的Y3；判断Y’值是否大于Y3，若大于则显示，否则不显示；

步骤8：根据PAS Distance Warning区域数据每一段颜色，判断PAS DistanceWarning的数据分别对应为所述区域中的相同颜色段，进而控制该颜色是否显示，或隐藏，或将其修改为需要显示的颜色。

综上所述，本发明提供一种基于雷达障碍物距离动态改变车轮轨迹线方法及系统，通过在生成轨迹线离散点数据时计算出每一小段的四个顶点数据，在保存轨迹线数据时，仅保存落在四个顶点围成的四边形范围内的数据，由此实现轨迹线的分段显示；通过在绘制轨迹线时，根据雷达障碍物距离，计算出图像坐标系下其所对应的直线方程，来动态调节轨迹线显示的长度，由此实现轨迹线长度动态调节的功能；在保存PAS DistanceWarning区域数据时，将每一段的颜色按照预设值设置，在绘制轨迹线时，根据不同的颜色来判断PAS Distance Warning的数据分别对应的区域段，进而控制其是否显示或隐藏或将其修改为需要显示的颜色，由此实现PAS 区域的隐藏或显示，及根据障碍物检测的距离动态改变每一段颜色的功能。

附图说明

图1为本发明所述的一种基于雷达障碍物距离动态改变车轮轨迹线方法流程图。

图2为图1中的PAS 区域显示样式图。

具体实施方式

下面将结合具体实施例及附图对本发明的一种基于雷达障碍物距离动态改变车轮轨迹线方法及系统，作进一步详细描述。

如图1所示，为本发明所述的一种基于雷达障碍物距离动态改变车轮轨迹线方法流程图，其中，本发明所述的一种基于雷达障碍物距离动态改变车轮轨迹线方法主要包括生成轨迹线数据文件和轨迹线绘制两大部分。

其中，生成轨迹线数据文件流程主要包含如下几个部分：

S1：根据世界坐标系下的车轮轨迹线运动模型方程，基于相机标定理论转换到图像坐标系下生成轨迹线离散点数据；

其中，所述步骤S1还包括：

S1.1：将轨迹线进行分段显示，根据配置文件内的每一段的长度和间隔长度，计算世界坐标系下离散点数据，同时求出轨迹线要显示的每一小段4个顶点对应的离散点坐标（x0, y0）,（x1, y1）,（x2, y2）,（x3, y3）；

S1.2：根据四段PAS Distance Warning样式，基于配置文件内的四段PASDistance Warning区域的位置获取世界坐标系离散点坐标，并基于相机标定参数投影到图像坐标系下生成四段PAS Distance Warning离散点数据。

S2：根据图像坐标系下的所述离散点数据，对离散点数据剪裁缩放，通过B样条方法进行曲线拟合，生成曲线坐标数据；

进一步的，所述步骤S2还包括：

S2.1：基于图像坐标系下轨迹线离散点数据，经过B样条方法生成对应的曲线坐标数据；

S2.2：基于图像坐标系下的四段PAS Distance Warning离散点数据，经过B样条方法生成对应的曲线坐标数据。

S3：对曲线围成的多边形进行颜色填充，将曲线坐标数据写入数据文件中，生成轨迹线数据文件；

进一步的，所述步骤S3还包括：

S3.1：根据计算出的每一段4个顶点对应的离散点坐标，使用离散点坐标（x0, y0）和离散点坐标（x1, y1）计算出对应的直线方程L1，使用离散点坐标（x2, y2）, 和离散点坐标（x3, y3）计算出对应的直线方程L2；

S3.2：在保存轨迹线曲线坐标数据时，将每个像素坐标（X，Y）中的X值代入直线方程L1和L2中，求出对应的Y1和Y2，并判断Traj的数据是否位于每一小段对应的区域内，即判断Y值是否位于Y1和Y2之间，若位于则将该轨迹线曲线坐标数据保存至数据文件中，否则不保存；

S3.3：同时，将每一段PAS Distance Warning区域的颜色做有规律的修改，即对所述四段PAS Distance Warning区域中的每一段的颜色设置为根据雷达障碍物距离进行动态调整，在写入数据文件时，对每一段所述PAS Distance Warning区域的颜色RGB值进行设置，进而保存至数据文件中，这样通过颜色即可区分每一段PAS Distance Warning区域的数据。

进一步的，所述S3.3还包括：所述四段PAS Distance Warning区域中的每一段的颜色RGB中对应的R值分别累加1，通过颜色区分每一段PAS Distance Warning区域的数据。

作为另一优选的，所述轨迹线绘制流程主要包含：解析所述轨迹线数据文件，获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标，绘制车轮轨迹线，并根据雷达障碍物的距离动态显示对应的PAS Distance Warning区域的颜色。

进一步的，轨迹线绘制流程包括以下步骤：

S4.1：获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标，根据相机标定参数将其转换至图像坐标系下离散点坐标（x4, y4）和离散点坐标（x5, y5），并求出对应的直线方程L3；

S4.2：将轨迹线数据坐标（X’,Y’）的X’值代入方程L3中，求出对应的Y3；判断轨迹线数据是否位于L3的下方，即判断Y’值是否大于Y3，若大于则显示，否则不显示。

S4.3：同时，根据PAS Distance Warning区域数据每一小段的颜色，判断其具体属于哪一小段，再将其修改为实际需要的颜色，再进行显示。

S4.4：通过汽车中控显示屏进行呈现。

其中，如图2所示，其中，1-长度根据雷达距离动态变化，2-PAS Distance Warning区域,通过不同的颜色段进行显示。在绘制轨迹线时，根据所述四段PAS Distance Warning区域中的每一段的颜色判断PAS Distance Warning的数据分别对应为所述区域中的相同颜色段，进而控制该颜色是否显示，或隐藏，或将其修改为需要显示的颜色。

作为另一优选的，本发明还提供了一种基于雷达障碍物距离动态改变车轮轨迹线系统，包括：

轨迹线生成单元，根据世界坐标系下的车轮轨迹线运动模型方程，基于相机标定理论转换到图像坐标系下生成轨迹线离散点数据；根据图像坐标系下的所述离散点数据，通过B样条方法进行曲线拟合，生成曲线坐标数据；将曲线坐标数据写入数据文件中，生成轨迹线数据文件；解析所述轨迹线数据文件，获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标，绘制车轮轨迹线，并根据雷达障碍物的距离动态显示对应的PAS DistanceWarning区域的颜色；

显示单元，根据雷达障碍物距离，计算出图像坐标系下其所对应的直线方程，来动态调节轨迹线显示的长度；根据障碍物检测的距离，动态显示所述四段PAS DistanceWarning区域中的每一段的颜色。

其中，所述轨迹线生成单元，包括轨迹线数据文件生成单元，包括步骤：

步骤1：将轨迹线进行分段显示，根据配置文件内的每一段的长度和间隔长度，计算每一段4个顶点对应的离散点坐标（x0, y0）,（x1, y1）,（x2, y2）,（x3, y3）；

步骤2：根据四段PAS Distance Warning样式，基于配置文件内的四段PASDistance Warning区域的位置获取世界坐标系离散点坐标，并基于相机标定参数转换到图像坐标系下生成四段PAS Distance Warning离散点数据；

步骤3：基于图像坐标系下轨迹线离散点数据，经过B样条方法生成对应的曲线坐标数据；基于图像坐标系下的四段PAS Distance Warning离散点数据，经过B样条方法生成对应的曲线坐标数据；将曲线坐标数据写入数据文件中，生成轨迹线数据文件；

步骤4：判断轨迹线数据是否位于所述四段PAS Distance Warning区域的每一小段对应的区域内，若是，则保存至数据文件中；否则，不保存；同时，对每一段所述PASDistance Warning区域的颜色RGB值进行设置，所述四段PAS Distance Warning区域中的每一段的颜色RGB中对应的R值分别累加1。

进一步的，所述轨迹线生成单元，还包括轨迹线绘制单元，具体包括：

步骤5：解析所述轨迹线数据文件，获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标；

步骤6：获取世界坐标系下轨迹线位于障碍物距离处的离散点坐标，根据相机标定参数将其转换至图像坐标系下离散点坐标（x4, y4）和离散点坐标（x5, y5），并求出对应的直线方程L3；

步骤7：将轨迹线数据坐标（X’,Y’）的X’值代入方程L3中，求出对应的Y3；判断Y’值是否大于Y3，若大于则显示，否则不显示；

步骤8：根据PAS Distance Warning区域数据每一段颜色，判断PAS DistanceWarning的数据分别对应为所述区域中的相同颜色段，进而控制该颜色是否显示，或隐藏，或将其修改为需要显示的颜色。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。