CN109849930A - 自动驾驶汽车的相邻车辆的速度计算方法和装置 - Google Patents
自动驾驶汽车的相邻车辆的速度计算方法和装置 Download PDFInfo
- Publication number
- CN109849930A CN109849930A CN201811643341.5A CN201811643341A CN109849930A CN 109849930 A CN109849930 A CN 109849930A CN 201811643341 A CN201811643341 A CN 201811643341A CN 109849930 A CN109849930 A CN 109849930A
- Authority
- CN
- China
- Prior art keywords
- observation point
- cluster
- described cluster
- velocity
- point
- 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.)
- Granted
Links
Abstract
本发明提供一种自动驾驶汽车的相邻车辆的速度计算方法和装置，包括：获取主车周围环境中的观测点集合在主车的坐标系中的运动数据，该观测点集合为一帧图像中的观测点，对观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇，针对每个点簇，根据点簇中的观测点的位置和径向相对速度，确定点簇中的非车轮类观测点和车轮类观测点，根据点簇中非车轮类观测点的径向相对速度、方位角以及主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到点簇的绝对速度，其中，点簇的绝对速度为点簇对应的相邻车辆的绝对速度。该方法基于观测点的径向相对速度、方位角以及主车的横纵向速度，通过最小二乘法拟合得到的相邻车辆的速度更加准确。
Description
技术领域
本发明涉及自动驾驶汽车领域，尤其涉及一种自动驾驶汽车的相邻车辆的速度计算方法和装置。
背景技术
自动驾驶汽车又称无人驾驶汽车、电脑驾驶汽车或轮式移动机器人，是一种通过电脑系统实现无人驾驶的智能汽车。自动驾驶汽车依靠人工智能、视觉计算、雷达、监控装置和全球定位系统协同合作，让电脑可以在没有任何人类主动的操作下，自动安全地操作机动车辆。
自动驾驶汽车在行驶过程中，主要是依靠车辆环境感知系统中的雷达来获取周围环境中物体的测量参数，根据物体的测量参数对周围环境中的相邻车辆进行识别，并获取相邻车辆的二维速度(包括横向速度和纵向速度)以及位置信息等。现有技术中，通过采集同一物体在多帧画面中的观测点的径向速度，基于同一物体的多帧画面的观测点的径向速度计算相邻车辆的二维速度。
但是，由于多帧画面中的观测点的数据的关联性难以确定，从而导致车辆周围的相邻车辆的二维速度的计算不准确。
发明内容
本发明提供一种自动驾驶汽车的相邻车辆的速度计算方法和装置，提高了主车周围的相邻车辆的速度的计算准确度。
本发明第一方面提供一种自动驾驶汽车的相邻车辆的速度计算方法，包括：
获取主车周围环境中的观测点集合在所述主车的坐标系中的运动数据，所述运动数据包括所述观测点集合中的观测点的径向相对速度、径向距离和方位角，所述观测点集合为一帧图像中的观测点；
对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇；
针对每个点簇，根据所述点簇中的观测点的方位角和径向距离得到观测点的位置，根据所述点簇中的观测点的位置和径向相对速度，确定所述点簇中的非车轮类观测点和车轮类观测点；
根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到所述点簇的绝对速度，其中，所述点簇的绝对速度为所述点簇对应的相邻车辆的绝对速度。
可选的，对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇之前，还包括：
滤除所述观测点集合中的静态观测点。
可选的，所述通过多次迭代拟合得到所述点簇的绝对速度之后，还包括：
计算所述点簇的绝对速度的拟合速度方差；
根据所述点簇的绝对速度的拟合速度方差和所述点簇中的非车轮类观测点的个数，得到所述点簇对应的相邻车辆的绝对速度的拟合置信度。
可选的，所述获取主车周围环境中的观测点集合在所述主车所在坐标系中的运动数据，包括：
通过所述主车的四个角上侧向安装的毫米波雷达采集所述主车周围环境中的观测点集合在所述毫米波雷达的坐标系中的运动数据；
通过坐标系转化，将观测点集合在所述毫米波雷达的坐标系中的运动数据转换为所述观测点集合在所述主车的坐标系中的运动数据。
可选的，所述滤除所述观测点集合中的静态观测点，包括：
针对所述观测点集合中的每个观测点，计算所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影；
根据所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影、所述观测点的方位角以及所述观测点的径向相对速度，确定所述观测点的径向速度；
当所述观测点的径向速度的绝对值小于预设的第一阈值时，滤除所述观测点。
可选的，所述对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇，包括：
从所述观测点集合中选择一个未访问的观测点P，遍历所述观测点集合找到与所述观测点P之间的距离小于预设的聚类半径的所有观测点形成点簇M；
如果所述点簇M中的观测点个数大于或等于预设的最小点簇包含的观测点数min，则将所述观测点P归属于所述点簇M形成点簇Mi，将所述观测点P标记为已访问；
依次递归访问所述点簇Mi中的其他未访问的观测点Pi，从所述观测点集合中找到与所述观测点Pi之间的距离小于所述聚类半径的所有其他观测点加入所述点簇Mi，并将所述观测点Pi标记为已访问；
如果所述点簇M中的观测点个数小于所述min，则确定所述观测点P 为噪声点，并将所述观测点P标记为已访问；
重复执行上述步骤，直至所述观测点集合中的所有观测点都被访问。
可选的，所述根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到所述点簇的绝对速度，包括：
根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，拟合得到所述点簇本次迭代对应的绝对速度；
根据所述点簇本次迭代对应的绝对速度，删除所述点簇中的失效观测点，所述点簇中的失效观测点为所述点簇中与所述点簇本次迭代对应的绝对速度的差异大于预设的第二阈值的观测点；
判断本次迭代是否满足预设的迭代结束条件；
如果本次迭代满足所述迭代结束条件，则确定所述点簇本次迭代对应的绝对速度为所述点簇的绝对速度；
如果本次迭代不满足所述迭代结束条件，则根据所述点簇中剩余的观测点，进行下一次迭代。
可选的，所述根据所述点簇本次迭代对应的绝对速度，删除所述点簇中的失效观测点，包括：
根据以下公式计算所述点簇本次迭代对应的绝对速度对应的拟合观测点与所述点簇中所有非车轮类观测点的直线之间的距离：
其中，R'i所述点簇中第i个非车轮类观测点的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度；
确定所述点簇中Di大于所述第二阈值的非车轮类观测点为所述点簇中的失效观测点；
删除所述点簇中的失效观测点。
可选的，所述迭代结束条件包括以下条件中的任意一个：
所述点簇中的非车轮观测点的个数小于预设的第三阈值；
所述点簇的绝对速度的拟合速度方差小于预设的第四阈值；
迭代次数大于预设的第五阈值。
可选的，所述根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，拟合得到所述点簇本次迭代对应的绝对速度，包括：
所述点簇中的第i个观测点在其径向方向上满足如下方程：
其中，R′i所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度，θi为所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的方位角；
对于所述点簇的N非车轮类观测点，存在N组方程，对所述N组方程的交点进行拟合，得到的拟合观测点的速度为所述点簇本次迭代对应的绝对速度。
可选的，所述计算所述点簇的绝对速度的拟合速度方差，包括：
通过下述公式计算所述点簇的绝对速度的拟合速度方差：
其中，为所述点簇本次迭代对应的绝对速度中的纵向速度，为所述点簇本次迭代对应的绝对速度中的横向速度，θi为所述点簇中第i个非车轮类观测点的方位角；
Ci＝R'i+ucosθ+vsinθi；
R′i所述点簇中第i个非车轮类观测点的相对径向速度，v为所述主车的横向速度，u为所述主车的纵向速度。
本发明第二方面提供一种自动驾驶汽车的相邻车辆的速度计算装置，包括：
获取模块，用于获取主车周围环境中的观测点集合在所述主车的坐标系中的运动数据，所述运动数据包括所述观测点集合中的观测点的径向相对速度、径向距离和方位角，所述观测点集合为一帧图像中的观测点；
聚类模块，用于对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇；
确定模块，用于针对每个点簇，根据所述点簇中的观测点的方位角和径向距离得到观测点的位置，根据所述点簇中的观测点的位置和径向相对速度，确定所述点簇中的非车轮类观测点和车轮类观测点；
拟合模块，用于根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到所述点簇的绝对速度，其中，所述点簇的绝对速度为所述点簇对应的相邻车辆的绝对速度。
可选的，还包括：
滤除模块，用于在所述聚类模块对所述观测点集合中的观测点进行基于密度的聚类之前，滤除所述观测点集合中的静态观测点。
可选的，还包括：
计算模块，用于计算所述点簇的绝对速度的拟合速度方差；
所述计算模块，还用于根据所述点簇的绝对速度的拟合速度方差和所述点簇中的非车轮类观测点的个数，得到所述点簇对应的相邻车辆的绝对速度的拟合置信度。
可选的，所述获取模块具体用于：
通过所述主车的四个角上侧向安装的毫米波雷达采集所述主车周围环境中的观测点集合在所述毫米波雷达的坐标系中的运动数据；
通过坐标系转化，将观测点集合在所述毫米波雷达的坐标系中的运动数据转换为所述观测点集合在所述主车的坐标系中的运动数据。
可选的，所述滤除模块具体用于：
针对所述观测点集合中的每个观测点，计算所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影；
根据所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影、所述观测点的方位角以及所述观测点的径向相对速度，确定所述观测点的径向速度；
当所述观测点的径向速度的绝对值小于预设的第一阈值时，滤除所述观测点。
可选的，所述聚类模块具体用于：
从所述观测点集合中选择一个未访问的观测点P，遍历所述观测点集合找到与所述观测点P之间的距离小于预设的聚类半径的所有观测点形成点簇M；
如果所述点簇M中的观测点个数大于或等于预设的最小点簇包含的观测点数min，则将所述观测点P归属于所述点簇M形成点簇Mi，将所述观测点P标记为已访问；
依次递归访问所述点簇Mi中的其他未访问的观测点Pi，从所述观测点集合中找到与所述观测点Pi之间的距离小于所述聚类半径的所有其他观测点加入所述点簇Mi，并将所述观测点Pi标记为已访问；
如果所述点簇M中的观测点个数小于所述min，则确定所述观测点P 为噪声点，并将所述观测点P标记为已访问；
重复执行上述步骤，直至所述观测点集合中的所有观测点都被访问。
可选的，所述拟合模块具体用于：
根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，拟合得到所述点簇本次迭代对应的绝对速度；
根据所述点簇本次迭代对应的绝对速度，删除所述点簇中的失效观测点，所述点簇中的失效观测点为所述点簇中与所述点簇本次迭代对应的绝对速度的差异大于预设的第二阈值的观测点；
判断本次迭代是否满足预设的迭代结束条件；
如果本次迭代满足所述迭代结束条件，则确定所述点簇本次迭代对应的绝对速度为所述点簇的绝对速度；
如果本次迭代不满足所述迭代结束条件，则根据所述点簇中剩余的观测点，进行下一次迭代。
可选的，所述拟合模块具体用于：
根据以下公式计算所述点簇本次迭代对应的绝对速度对应的拟合观测点与所述点簇中所有非车轮类观测点的直线之间的距离：
其中，R'i所述点簇中第i个非车轮类观测点的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度；
确定所述点簇中Di大于所述第二阈值的非车轮类观测点为所述点簇中的失效观测点；
删除所述点簇中的失效观测点。
可选的，所述迭代结束条件包括以下条件中的任意一个：
所述点簇中的非车轮观测点的个数小于预设的第三阈值；
所述点簇的绝对速度的拟合速度方差小于预设的第四阈值；
迭代次数大于预设的第五阈值。
可选的，所述拟合模块具体用于：
假设所述点簇中共有N个非车轮类观测点；
所述点簇中的第i个观测点在其径向方向上满足如下方程：
其中，R′i所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度，θi为所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的方位角；
对于所述点簇的N非车轮类观测点，存在N组方程，对所述N组方程的交点进行拟合，得到的拟合观测点的速度为所述点簇本次迭代对应的绝对速度。
可选的，所述计算模块具体用于：
通过下述公式计算所述点簇的绝对速度的拟合速度方差：
其中，为所述点簇本次迭代对应的绝对速度中的纵向速度，为所述点簇本次迭代对应的绝对速度中的横向速度，θi为所述点簇中第i个非车轮类观测点的方位角；
Ci＝R'i+ucosθ+vsinθi；
R′i所述点簇中第i个非车轮类观测点的相对径向速度，v为所述主车的横向速度，u为所述主车的纵向速度。
本发明第三方面提供一种自动驾驶汽车的相邻车辆的速度计算装置，包括：处理器、存储器和收发器，所述存储器用于存储指令，所述收发器用于和其他设备通信，所述处理器用于执行所述存储器中存储的指令，以使所述装置执行如本发明第一方法以及各可选方式所述的方法。
本发明第四方面提供一种计算机可读存储介质，所述计算机可读存储介质存储有指令，当所述指令被执行时，使得计算机执行如本发明第一方法以及各可选方式所述的方法。
本发明提供的自动驾驶汽车的相邻车辆的速度计算方法和装置，包括：获取主车周围环境中的观测点集合在主车的坐标系中的运动数据，该观测点集合为一帧图像中的观测点，对观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇，针对每个点簇，根据点簇中的观测点的位置和径向相对速度，确定点簇中的非车轮类观测点和车轮类观测点，根据点簇中非车轮类观测点的径向相对速度、方位角以及主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到点簇的绝对速度，其中，点簇的绝对速度为点簇对应的相邻车辆的绝对速度。该方法基于观测点的径向相对速度、方位角以及主车的横纵向速度，通过最小二乘法拟合得到的相邻车辆的速度更加准确。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。
图1为本发明实施例一提供的自动驾驶汽车的相邻车辆的速度计算方法的流程图；
图2为主车的坐标系以及毫米波雷达的坐标系的一种示意图；
图3为左前角的雷达坐标系中观测点的一种示意图；
图4为左前角的雷达坐标系中观测点的又一种示意图；
图5为公共视场中观测点的一种示意图；
图6为点簇中失效观测点的一种示意图；
图7为本发明实施例二提供的自动驾驶汽车的相邻车辆的速度计算方法的流程图；
图8为本发明实施例三提供的自动驾驶汽车的相邻车辆的速度计算装置的结构示意图；
图9为本发明实施例四提供的自动驾驶汽车的相邻车辆的速度计算装置的结构示意图。
通过上述附图，已示出本发明明确的实施例，后文中将有更详细的描述。这些附图和文字描述并不是为了通过任何方式限制本发明构思的范围，而是通过参考特定实施例为本领域技术人员说明本发明的概念。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
图1为本发明实施例一提供的自动驾驶汽车的相邻车辆的速度计算方法的流程图，本实施例的方法由主车的控制装置执行，该控制装置可以是行车电脑或车载单元(OnBoard Unit，简称OBU)等，该控制装置用于控制车辆的自动驾驶，如图1所示，本实施例提供的方法包括以下步骤：
步骤S101、获取主车周围环境中的观测点集合在主车的坐标系中的运动数据。
观测点集合中包括多个观测点，观测点集合的运动数据包括观测点集合中的观测点的径向相对速度、径向距离和方位角，观测点集合中的观测点为一帧图像中的观测点。
本实施例中，在主车的四个角上侧向安装有毫米波雷达，通过毫米波雷达采集周围环境中的观测点的运动数据，采用毫米波雷达采集到的数据更加准确。可以理解的是，在实际应用中，可以根据对精度的要求以及实际的应用场景来选择雷达的类型，本实施例不对此进行限制。
一种示例性的方式中，通过主车的四个角上侧向安装的毫米波雷达采集主车周围环境中的观测点集合在毫米波雷达的坐标系中的运动数据，通过坐标系转化，将观测点集合在毫米波雷达的坐标系中的运动数据转换为观测点集合在主车的坐标系中的运动数据。
图2为主车的坐标系以及毫米波雷达的坐标系的一种示意图，如图2 主车的坐标系的原点为车头的中心点，x轴方向为纵向，向上为正，y轴方向为横向，向左为正。四个毫米波雷达分别安装在主车的四个角：左前角、右前角、左后角、右后角，四个毫米波雷达的视场如图所示，毫米波雷达雷达只能采集到自己视场范围内的物体的观测点的运动数据。相邻毫米波雷达的视场有交集，交集区域成为公共视场。
虚线坐标系为毫米波雷达的坐标系，毫米波雷达的坐标系的原点为雷达的中心点，箭头所指方向为x轴和y轴的正方向。
观测点的径向相对速度是指观测点在径向方向上相对于主车的速度，径向速度一般指物体运动速度在观察者视线方向的速度分量，即速度矢量在视线方向的投影，又称视向速度。观测点的径向方向是指观测点与毫米波雷达的坐标系的原点的连线方向，径向方向为径向速度的方向。
观测点的径向距离是指观测点在径向方向上与主车之间的距离。观测点的方位角是指观测点的径向方向与主车的坐标系的x轴或y轴形成的夹角。
步骤S102、对观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇。
可选的，在步骤S102之前，滤除观测点集合中的静态观测点。主车周围环境中的观测点包括不仅包括主车周围的相邻车辆的观测点，还包括周围环境中一些静态物体的观测点，例如，路边的指示牌、树的观测点等。本实施例的目的是计算主车的相邻车辆的速度，因此，对于主车的周围环境中一些静态物体的观测点需要滤除。
一种示例性的方式中，针对观测点集合中的每个观测点，计算主车的横向速度和纵向速度在该观测点的径向方向上的投影，根据主车的横向速度和纵向速度在该观测点的径向方向上的投影、该观测点的方位角以及该观测点的径向相对速度，确定该观测点的径向速度，当该观测点的径向速度的绝对值小于预设的第一阈值时，滤除该观测点。
其中，主车的横向速度和纵向速度是以主车的坐标系为参考坐标系测量得到的速度。图3为左前角的雷达坐标系中观测点的一种示意图，如图 3所示，安装在主车的左前角的毫米波雷达采集到观测点1，观测点1的方位角为θ1，观测点1的相对径向速度为R′1(远离主车方向为正，靠近主车方向为负)，主车的纵向速度为u，主车的横向速度为v。
主车的纵向速度u在该观测点的径向方向上的投影为ucosθ1，主车的横向速度v在该观测点的径向方向上的投影为vsinθ1，可以根据以下公式计算该观测点的径向速度：
△v1＝R1'+ucosθ1+vsinθ1
计算得到的观测点的径向速度为观测点在径向方向上的绝对径向速度，如果观测点的径向速度的绝对值小于预设的第一阈值，说明观测点可能为静止物体上的观测点，将该观测点删除，观测点的径向速度小于第一阈值。
在滤除观测点集合中的静态观测点后，对观测点集合中的观测点进行基于密度的聚类法(Density-Based Spatial Clustering of Applications with Noise，DBSCAN)，得到至少一个点簇。
其中，基于密度的聚类方法可以在有噪音的数据中发现各种形状和各种大小的簇。其核心思想就是先发现密度较高的点，然后把相近的高密度点逐步都连成一片，进而生成各种簇。
示例性的，通过如下方式进行聚类：
步骤一、从观测点集合中选择一个未访问的观测点P，遍历观测点集合找到与观测点P之间的距离小于预设的聚类半径的所有观测点形成点簇 M。
步骤二、如果点簇M中的观测点个数大于或等于预设的最小点簇包含的观测点数min，则将观测点P归属于点簇M形成点簇Mi，将观测点 P标记为已访问。
依次递归访问点簇Mi中的其他未访问的观测点Pi，从观测点集合中找到与观测点Pi之间的距离小于聚类半径的所有其他观测点加入点簇Mi，并将观测点Pi标记为已访问，从而使得点簇Mi不断扩大。
步骤三、如果点簇M中的观测点个数小于min，则确定观测点P为噪声点，并将观测点P标记为已访问。
重复执行上述步骤一至步骤三，直至观测点集合中的所有观测点都被访问。其中，每次访问观测点集合中一个未访问观测点时，只执行步骤二或步骤三。通过聚类将观测点集合中的所有观测点聚类得到至少一个点簇以及噪声点，噪声点会被删除掉。
步骤S103、针对每个点簇，根据点簇中的观测点的方位角和径向距离得到观测点的位置，根据点簇中的观测点的位置和径向相对速度，确定点簇中的非车轮类观测点和车轮类观测点。
车辆在行驶过程中，车轮相对于车辆的其他部件转速较快，因此，主车的某一相邻车辆的车轮上的观测点一般符合以下特性：在小区域内观测点密集、相互的速度差异大。根据该特性，可以将每个点簇中的所有观测点分为车轮类和非车轮类这两类。
步骤S104、根据点簇中非车轮类观测点的径向相对速度、方位角以及主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到点簇的绝对速度，其中，点簇的绝对速度为点簇对应的相邻车辆的绝对速度。
本实施例中，在拟合得到点簇的绝对速度时，只根据点簇中非车轮类观测点的运动数据进行拟合。示例性，可以通过以下方式进行多次迭代：
步骤一：根据点簇中非车轮类观测点的径向相对速度、方位角以及主车的横向速度和纵向速度，基于最小二乘法，拟合得到点簇本次迭代对应的绝对速度。
步骤二、根据点簇本次迭代对应的绝对速度，删除点簇中的失效观测点，点簇中的失效观测点为点簇中与点簇本次迭代对应的绝对速度的差异大于预设的第二阈值的观测点。
步骤三、判断本次迭代是否满足预设的迭代结束条件。
如果本次迭代满足迭代结束条件，则执行步骤四，如果本次迭代不满足迭代结束条件，则执行步骤五。
步骤四、确定点簇本次迭代对应的绝对速度为点簇的绝对速度。
步骤五、根据点簇中剩余的观测点，进行下一次迭代，即返回执行步骤一，直至满足迭代结束条件。
其中，点簇的绝对速度是指点簇相对于地面的速度，点簇的绝对速度包括点簇的横向速度和纵向速度。
图4为左前角的雷达坐标系中观测点的又一种示意图，如图4所示，安装在主车的左前角的毫米波雷达采集到一组观测点，并聚类为一个点簇，该点簇具有N个非车轮类观测点，图4中仅示出了三个观测点：观测点1、观测点2和观测点3。观测点1的方位角为θ，θ为观测点1与左前角的毫米波雷达的坐标系的原点的连线与主车的X轴的夹角，观测点1的径向相对速度为R'，观测点1的径向相对速度R'在主车的X轴方向的投影为Vx，在主车的Y轴方向的投影为Vy。
图5为公共视场中观测点的一种示意图，如图5所示，一辆相邻车辆 (图中虚线框所示的区域)出现在主车的左前角和左后角的毫米波雷达的公共视场内，观测点2位于公共视场内，观测点1位于左前角的毫米波雷达的视场内，观测点1的方位角为θ1，观测点2的方位角为θ2，观测点1 的相对径向速度为R′1，观测点2的相对径向速度为R′2。
步骤一中可以通过如下方式进行拟合得到点簇本次迭代对应的绝对速度：
假设点簇中共有N个非车轮类观测点；
点簇中的第i个观测点在其径向方向上满足如下方程：
其中，R′i点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的相对径向速度，u为主车的纵向速度，v为主车的横向速度，为点簇中第 i个非车轮类观测点的纵向速度，为点簇中第i个非车轮类观测点的横向速度，θi为点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的方位角。R′i、u、v以及θi是已知量，和是未知量。
对于点簇的N非车轮类观测点，存在N组方程，对N组方程的交点进行拟合，得到的拟合观测点的速度为点簇本次迭代对应的绝对速度。
假设观测点(vx,vy)到N条直线的距离平方之和D2为：
其中，Ci＝R'i+ucosθ+vsinθi。
基于最小二乘法，拟合目标是找到点使得：即
变换形式，得到Rv＝q；
其中，
则v＝R+q
其中，R+为R的为逆矩阵。
在步骤二中可以通过如下方式确定观测点是否为失效点：
根据以下公式计算点簇本次迭代对应的绝对速度对应的拟合观测点与点簇中所有非车轮类观测点的直线之间的距离：
其中，R′i点簇中第i个非车轮类观测点的相对径向速度，v为主车的横向速度，u为主车的纵向速度，为点簇中第i个非车轮类观测点的纵向速度，为点簇中第i个非车轮类观测点的横向速度。
确定点簇中Di大于第二阈值的非车轮类观测点为点簇中的失效观测点。
图6为点簇中失效观测点的一种示意图，如图6所示，删除点簇中的失效观测点后，剩余的观测点包括：直线与以点簇本次迭代对应的绝对速度为圆心、以Rmax为半径的圆没有交点的观测点，Rmax为第二阈值。
可选的，迭代结束条件包括以下条件中的任意一个：点簇中的非车轮观测点的个数小于预设的第三阈值；点簇的绝对速度的拟合速度方差小于预设的第四阈值；迭代次数大于预设的第五阈值。
本实施例中，获取主车周围环境中的观测点集合在主车的坐标系中的运动数据，该观测点集合为一帧图像中的观测点，对观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇，针对每个点簇，根据点簇中的观测点的位置和径向相对速度，确定点簇中的非车轮类观测点和车轮类观测点，根据点簇中非车轮类观测点的径向相对速度、方位角以及主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到点簇的绝对速度，其中，点簇的绝对速度为点簇对应的相邻车辆的绝对速度。该方法基于观测点的径向相对速度、方位角以及主车的横纵向速度，通过最小二乘法拟合得到的相邻车辆的速度更加准确。
图7为本发明实施例二提供的自动驾驶汽车的相邻车辆的速度计算方法的流程图，如图7所示，本实施例提供的方法包括以下步骤：
步骤S201、通过主车的四个角上侧向安装的毫米波雷达采集主车周围环境中的观测点集合在毫米波雷达的坐标系中的运动数据。
步骤S202、通过坐标系转化，将观测点集合在毫米波雷达的坐标系中的运动数据转换为观测点集合在主车的坐标系中的运动数据。
步骤S203、滤除观测点集合中的静态观测点。
步骤S204、对观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇。
步骤S205、针对每个点簇，根据点簇中的观测点的位置和径向相对速度，确定点簇中的非车轮类观测点和车轮类观测点。
其中，观测点的位置是根据点簇中的观测点的方位角和径向距离得到的。
步骤S206、根据点簇中非车轮类观测点的径向相对速度、方位角以及主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到点簇的绝对速度。
步骤S201-S206的具体实现方式参照实施例一的相关描述，这里不再赘述。
步骤S207、计算点簇的绝对速度的拟合速度方差。
示例性的，可以通过下述公式计算点簇的绝对速度的拟合速度方差：
其中，为点簇本次迭代对应的绝对速度中的纵向速度，为点簇本次迭代对应的绝对速度中的横向速度，θi为点簇中第i个非车轮类观测点的方位角；
Ci＝R'i+ucosθ+vsinθi；
R′i点簇中第i个非车轮类观测点的相对径向速度，v为主车的横向速度， u为主车的纵向速度。
步骤S208、根据点簇的绝对速度的拟合速度方差和点簇中的非车轮类观测点的个数，得到点簇对应的相邻车辆的绝对速度的拟合置信度。
其中，拟合速度方差越小，拟合置信度越高，点簇中的非车轮类观测点的个数越多，拟合置信度越高。如果拟合置信度大于或等于预设的第六阈值，则确定拟合得到点簇对应的相邻车辆的绝对速度可信，能够用于后续的目标跟踪等操作。如果拟合置信度小于第六阈值，则确定拟合得到的点簇对应的相邻车辆的绝对速度不可信。
图8为本发明实施例三提供的自动驾驶汽车的相邻车辆的速度计算装置的结构示意图，该装置可以集成在车辆的行车电脑或者其他用于控制车辆自动驾驶的控制装置中，如图8所示，本实施例的装置，包括：
获取模块11，用于获取主车周围环境中的观测点集合在所述主车的坐标系中的运动数据，所述运动数据包括所述观测点集合中的观测点的径向相对速度、径向距离和方位角，所述观测点集合为一帧图像中的观测点；
聚类模块12，用于对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇；
确定模块13，用于针对每个点簇，根据所述点簇中的观测点的方位角和径向距离得到观测点的位置，根据所述点簇中的观测点的位置和径向相对速度，确定所述点簇中的非车轮类观测点和车轮类观测点；
拟合模块14，用于根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到所述点簇的绝对速度，其中，所述点簇的绝对速度为所述点簇对应的相邻车辆的绝对速度。
可选的，还包括：
滤除模块15，用于在所述聚类模块对所述观测点集合中的观测点进行基于密度的聚类之前，滤除所述观测点集合中的静态观测点。
可选的，还包括：
计算模块16，用于计算所述点簇的绝对速度的拟合速度方差；
所述计算模块16，还用于根据所述点簇的绝对速度的拟合速度方差和所述点簇中的非车轮类观测点的个数，得到所述点簇对应的相邻车辆的绝对速度的拟合置信度。
可选的，所述获取模块11具体用于：
通过所述主车的四个角上侧向安装的毫米波雷达采集所述主车周围环境中的观测点集合在所述毫米波雷达的坐标系中的运动数据；
通过坐标系转化，将观测点集合在所述毫米波雷达的坐标系中的运动数据转换为所述观测点集合在所述主车的坐标系中的运动数据。
可选的，所述滤除模块15具体用于：
针对所述观测点集合中的每个观测点，计算所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影；
根据所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影、所述观测点的方位角以及所述观测点的径向相对速度，确定所述观测点的径向速度；
当所述观测点的径向速度的绝对值小于预设的第一阈值时，滤除所述观测点。
可选的，所述聚类模块12具体用于：
从所述观测点集合中选择一个未访问的观测点P，遍历所述观测点集合找到与所述观测点P之间的距离小于预设的聚类半径的所有观测点形成点簇M；
如果所述点簇M中的观测点个数大于或等于预设的最小点簇包含的观测点数min，则将所述观测点P归属于所述点簇M形成点簇Mi，将所述观测点P标记为已访问；
依次递归访问所述点簇Mi中的其他未访问的观测点Pi，从所述观测点集合中找到与所述观测点Pi之间的距离小于所述聚类半径的所有其他观测点加入所述点簇Mi，并将所述观测点Pi标记为已访问；
如果所述点簇M中的观测点个数小于所述min，则确定所述观测点P 为噪声点，并将所述观测点P标记为已访问；
重复执行上述步骤，直至所述观测点集合中的所有观测点都被访问。
可选的，所述拟合模块14具体用于：
根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，拟合得到所述点簇本次迭代对应的绝对速度；
根据所述点簇本次迭代对应的绝对速度，删除所述点簇中的失效观测点，所述点簇中的失效观测点为所述点簇中与所述点簇本次迭代对应的绝对速度的差异大于预设的第二阈值的观测点；
判断本次迭代是否满足预设的迭代结束条件；
如果本次迭代满足所述迭代结束条件，则确定所述点簇本次迭代对应的绝对速度为所述点簇的绝对速度；
如果本次迭代不满足所述迭代结束条件，则根据所述点簇中剩余的观测点，进行下一次迭代。
可选的，所述拟合模块14具体用于：
根据以下公式计算所述点簇本次迭代对应的绝对速度对应的拟合观测点与所述点簇中所有非车轮类观测点的直线之间的距离：
其中，R'i所述点簇中第i个非车轮类观测点的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度；
确定所述点簇中Di大于所述第二阈值的非车轮类观测点为所述点簇中的失效观测点；
删除所述点簇中的失效观测点。
可选的，所述迭代结束条件包括以下条件中的任意一个：
所述点簇中的非车轮观测点的个数小于预设的第三阈值；
所述点簇的绝对速度的拟合速度方差小于预设的第四阈值；
迭代次数大于预设的第五阈值。
可选的，所述拟合模块14具体用于：
假设所述点簇中共有N个非车轮类观测点；
所述点簇中的第i个观测点在其径向方向上满足如下方程：
其中，R′i所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度，θi为所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的方位角；
对于所述点簇的N非车轮类观测点，存在N组方程，对所述N组方程的交点进行拟合，得到的拟合观测点的速度为所述点簇本次迭代对应的绝对速度。
可选的，所述计算模块16具体用于：
通过下述公式计算所述点簇的绝对速度的拟合速度方差：
其中，为所述点簇本次迭代对应的绝对速度中的纵向速度，为所述点簇本次迭代对应的绝对速度中的横向速度，θi为所述点簇中第i个非车轮类观测点的方位角；
Ci＝R'i+ucosθ+vsinθi；
R′i所述点簇中第i个非车轮类观测点的相对径向速度，v为所述主车的横向速度，u为所述主车的纵向速度。
图9为本发明实施例四提供的自动驾驶汽车的相邻车辆的速度计算装置的结构示意图，如图9所示，本实施例提供的装置包括处理器21、存储器 22、收发器23，存储器22、收发器23通过总线与处理器21连接并通信，所述存储器22用于存储指令，所述收发器23用于和其他设备通信，所述处理器21用于执行所述存储器22中存储的指令，以使所述装置执行如实施例一所述的方法，这里不再赘述；
其中，该处理器21可以是微控制单元(Microcontroller Unit，简称MCU)， MCU又称单片微型计算机(Single Chip Microcomputer)或者单片机，该处理器21还可以是中央处理器(Central Process Unit，简称CPU)、数字信号处理器(digital signal processor，简称DSP)、专用集成电路(application specific integrated circuit，简称ASIC)、现成可编程门阵列(field programmable gate array，简称FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件。
存储器22可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(Static Random-Access Memory，简称SRAM)，电可擦除可编程只读存储器(Electrically Erasable Programmable read only memory，简称EEPROM)，可擦除可编程只读存储器(Erasable Programmable Read Only Memory，简称EPROM)，可编程只读存储器(Programmable Read-Only Memory，简称PROM)，只读存储器(Read-Only Memory，简称ROM)，磁存储器，快闪存储器，磁盘或光盘。
收发器23可以和其他设备之间建立有线或无线通信链路，如WiFi，2G、3G、4G或者5G链路，或它们的组合。
总线可以是工业标准体系结构(Industry Standard Architecture，ISA)总线、外部设备互连(Peripheral Component，PCI)总线或扩展工业标准体系结构(ExtendedIndustry Standard Architecture，EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，本申请附图中的总线并不限定仅有一根总线或一种类型的总线。
可选的，中控设备还包括显示屏44和/或音频器件45，
图9仅示出了与本发明相关的一些组件，该装置还可以包括更多的组件，本发明不对此进行限制。
本发明实施例五提供一种计算机可读存储介质，所述计算机可读存储介质存储有指令，当所述指令被执行时，使得计算机执行如实施例一所述方法。
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
Claims (24)
1.一种自动驾驶汽车的相邻车辆的速度计算方法，其特征在于，包括：
获取主车周围环境中的观测点集合在所述主车的坐标系中的运动数据，所述运动数据包括所述观测点集合中的观测点的径向相对速度、径向距离和方位角，所述观测点集合为一帧图像中的观测点；
对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇；
针对每个点簇，根据所述点簇中的观测点的方位角和径向距离得到观测点的位置，根据所述点簇中的观测点的位置和径向相对速度，确定所述点簇中的非车轮类观测点和车轮类观测点；
根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到所述点簇的绝对速度，其中，所述点簇的绝对速度为所述点簇对应的相邻车辆的绝对速度。
2.根据权利要求1所述的方法，其特征在于，对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇之前，还包括：
滤除所述观测点集合中的静态观测点。
3.根据权利要求2所述的方法，其特征在于，所述通过多次迭代拟合得到所述点簇的绝对速度之后，还包括：
计算所述点簇的绝对速度的拟合速度方差；
根据所述点簇的绝对速度的拟合速度方差和所述点簇中的非车轮类观测点的个数，得到所述点簇对应的相邻车辆的绝对速度的拟合置信度。
4.根据权利要求1所述的方法，其特征在于，所述获取主车周围环境中的观测点集合在所述主车所在坐标系中的运动数据，包括：
通过所述主车的四个角上侧向安装的毫米波雷达采集所述主车周围环境中的观测点集合在所述毫米波雷达的坐标系中的运动数据；
通过坐标系转化，将观测点集合在所述毫米波雷达的坐标系中的运动数据转换为所述观测点集合在所述主车的坐标系中的运动数据。
5.根据权利要求2所述的方法，其特征在于，所述滤除所述观测点集合中的静态观测点，包括：
针对所述观测点集合中的每个观测点，计算所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影；
根据所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影、所述观测点的方位角以及所述观测点的径向相对速度，确定所述观测点的径向速度；
当所述观测点的径向速度的绝对值小于预设的第一阈值时，滤除所述观测点。
6.根据权利要求1所述的方法，其特征在于，所述对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇，包括：
从所述观测点集合中选择一个未访问的观测点P，遍历所述观测点集合找到与所述观测点P之间的距离小于预设的聚类半径的所有观测点形成点簇M；
如果所述点簇M中的观测点个数大于或等于预设的最小点簇包含的观测点数min，则将所述观测点P归属于所述点簇M形成点簇Mi，将所述观测点P标记为已访问；
依次递归访问所述点簇Mi中的其他未访问的观测点Pi，从所述观测点集合中找到与所述观测点Pi之间的距离小于所述聚类半径的所有其他观测点加入所述点簇Mi，并将所述观测点Pi标记为已访问；
如果所述点簇M中的观测点个数小于所述min，则确定所述观测点P为噪声点，并将所述观测点P标记为已访问；
重复执行上述步骤，直至所述观测点集合中的所有观测点都被访问。
7.根据权利要求1所述的方法，其特征在于，所述根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到所述点簇的绝对速度，包括：
根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，拟合得到所述点簇本次迭代对应的绝对速度；
根据所述点簇本次迭代对应的绝对速度，删除所述点簇中的失效观测点，所述点簇中的失效观测点为所述点簇中与所述点簇本次迭代对应的绝对速度的差异大于预设的第二阈值的观测点；
判断本次迭代是否满足预设的迭代结束条件；
如果本次迭代满足所述迭代结束条件，则确定所述点簇本次迭代对应的绝对速度为所述点簇的绝对速度；
如果本次迭代不满足所述迭代结束条件，则根据所述点簇中剩余的观测点，进行下一次迭代。
8.根据权利要求7所述的方法，其特征在于，所述根据所述点簇本次迭代对应的绝对速度，删除所述点簇中的失效观测点，包括：
根据以下公式计算所述点簇本次迭代对应的绝对速度对应的拟合观测点与所述点簇中所有非车轮类观测点的直线之间的距离：
其中，R'i所述点簇中第i个非车轮类观测点的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度；
确定所述点簇中Di大于所述第二阈值的非车轮类观测点为所述点簇中的失效观测点；
删除所述点簇中的失效观测点。
9.根据权利要求7所述的方法，其特征在于，所述迭代结束条件包括以下条件中的任意一个：
所述点簇中的非车轮观测点的个数小于预设的第三阈值；
所述点簇的绝对速度的拟合速度方差小于预设的第四阈值；
迭代次数大于预设的第五阈值。
10.根据权利要求7-9任一项所述的方法，其特征在于，所述根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，拟合得到所述点簇本次迭代对应的绝对速度，包括：
假设所述点簇中共有N个非车轮类观测点；
所述点簇中的第i个观测点在其径向方向上满足如下方程：
其中，R′i所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度，θi为所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的方位角；
对于所述点簇的N非车轮类观测点，存在N组方程，对所述N组方程的交点进行拟合，得到的拟合观测点的速度为所述点簇本次迭代对应的绝对速度。
11.根据权利要求3所述的方法，其特征在于，所述计算所述点簇的绝对速度的拟合速度方差，包括：
通过下述公式计算所述点簇的绝对速度的拟合速度方差：
其中，为所述点簇本次迭代对应的绝对速度中的纵向速度，为所述点簇本次迭代对应的绝对速度中的横向速度，θi为所述点簇中第i个非车轮类观测点的方位角；
Ci＝R'i+ucosθ+vsinθi；
R′i所述点簇中第i个非车轮类观测点的相对径向速度，v为所述主车的横向速度，u为所述主车的纵向速度。
12.一种自动驾驶汽车的相邻车辆的速度计算装置，其特征在于，包括：
获取模块，用于获取主车周围环境中的观测点集合在所述主车的坐标系中的运动数据，所述运动数据包括所述观测点集合中的观测点的径向相对速度、径向距离和方位角，所述观测点集合为一帧图像中的观测点；
聚类模块，用于对所述观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇；
确定模块，用于针对每个点簇，根据所述点簇中的观测点的方位角和径向距离得到观测点的位置，根据所述点簇中的观测点的位置和径向相对速度，确定所述点簇中的非车轮类观测点和车轮类观测点；
拟合模块，用于根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到所述点簇的绝对速度，其中，所述点簇的绝对速度为所述点簇对应的相邻车辆的绝对速度。
13.根据权利要求12所述的装置，其特征在于，还包括：
滤除模块，用于在所述聚类模块对所述观测点集合中的观测点进行基于密度的聚类之前，滤除所述观测点集合中的静态观测点。
14.根据权利要求13所述的装置，其特征在于，还包括：
计算模块，用于计算所述点簇的绝对速度的拟合速度方差；
所述计算模块，还用于根据所述点簇的绝对速度的拟合速度方差和所述点簇中的非车轮类观测点的个数，得到所述点簇对应的相邻车辆的绝对速度的拟合置信度。
15.根据权利要求12所述的装置，其特征在于，所述获取模块具体用于：
通过所述主车的四个角上侧向安装的毫米波雷达采集所述主车周围环境中的观测点集合在所述毫米波雷达的坐标系中的运动数据；
通过坐标系转化，将观测点集合在所述毫米波雷达的坐标系中的运动数据转换为所述观测点集合在所述主车的坐标系中的运动数据。
16.根据权利要求13所述的装置，其特征在于，所述滤除模块具体用于：
针对所述观测点集合中的每个观测点，计算所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影；
根据所述主车的横向速度和纵向速度在所述观测点的径向方向上的投影、所述观测点的方位角以及所述观测点的径向相对速度，确定所述观测点的径向速度；
当所述观测点的径向速度的绝对值小于预设的第一阈值时，滤除所述观测点。
17.根据权利要求12所述的装置，其特征在于，所述聚类模块具体用于：
从所述观测点集合中选择一个未访问的观测点P，遍历所述观测点集合找到与所述观测点P之间的距离小于预设的聚类半径的所有观测点形成点簇M；
如果所述点簇M中的观测点个数大于或等于预设的最小点簇包含的观测点数min，则将所述观测点P归属于所述点簇M形成点簇Mi，将所述观测点P标记为已访问；
依次递归访问所述点簇Mi中的其他未访问的观测点Pi，从所述观测点集合中找到与所述观测点Pi之间的距离小于所述聚类半径的所有其他观测点加入所述点簇Mi，并将所述观测点Pi标记为已访问；
如果所述点簇M中的观测点个数小于所述min，则确定所述观测点P为噪声点，并将所述观测点P标记为已访问；
重复执行上述步骤，直至所述观测点集合中的所有观测点都被访问。
18.根据权利要求12所述的装置，其特征在于，所述拟合模块具体用于：
根据所述点簇中非车轮类观测点的径向相对速度、方位角以及所述主车的横向速度和纵向速度，基于最小二乘法，拟合得到所述点簇本次迭代对应的绝对速度；
根据所述点簇本次迭代对应的绝对速度，删除所述点簇中的失效观测点，所述点簇中的失效观测点为所述点簇中与所述点簇本次迭代对应的绝对速度的差异大于预设的第二阈值的观测点；
判断本次迭代是否满足预设的迭代结束条件；
如果本次迭代满足所述迭代结束条件，则确定所述点簇本次迭代对应的绝对速度为所述点簇的绝对速度；
如果本次迭代不满足所述迭代结束条件，则根据所述点簇中剩余的观测点，进行下一次迭代。
19.根据权利要求18所述的装置，其特征在于，所述拟合模块具体用于：
根据以下公式计算所述点簇本次迭代对应的绝对速度对应的拟合观测点与所述点簇中所有非车轮类观测点的直线之间的距离：
其中，R'i所述点簇中第i个非车轮类观测点的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度；
确定所述点簇中Di大于所述第二阈值的非车轮类观测点为所述点簇中的失效观测点；
删除所述点簇中的失效观测点。
20.根据权利要求18所述的装置，其特征在于，所述迭代结束条件包括以下条件中的任意一个：
所述点簇中的非车轮观测点的个数小于预设的第三阈值；
所述点簇的绝对速度的拟合速度方差小于预设的第四阈值；
迭代次数大于预设的第五阈值。
21.根据权利要求18-20任一项所述的装置，其特征在于，所述拟合模块具体用于：
假设所述点簇中共有N个非车轮类观测点；
所述点簇中的第i个观测点在其径向方向上满足如下方程：
其中，R′i所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的相对径向速度，u为所述主车的纵向速度，v为所述主车的横向速度，为所述点簇中第i个非车轮类观测点的纵向速度，为所述点簇中第i个非车轮类观测点的横向速度，θi为所述点簇中第i个非车轮类观测点在毫米波雷达所在坐标系中的方位角；
对于所述点簇的N非车轮类观测点，存在N组方程，对所述N组方程的交点进行拟合，得到的拟合观测点的速度为所述点簇本次迭代对应的绝对速度。
22.根据权利要求14所述的装置，其特征在于，所述计算模块具体用于：
通过下述公式计算所述点簇的绝对速度的拟合速度方差：
其中，为所述点簇本次迭代对应的绝对速度中的纵向速度，为所述点簇本次迭代对应的绝对速度中的横向速度，θi为所述点簇中第i个非车轮类观测点的方位角；
Ci＝R'i+ucosθ+vsinθi；
R′i所述点簇中第i个非车轮类观测点的相对径向速度，v为所述主车的横向速度，u为所述主车的纵向速度。
23.一种自动驾驶汽车的相邻车辆的速度计算装置，其特征在于，包括：处理器、存储器和收发器，所述存储器用于存储指令，所述收发器用于和其他设备通信，所述处理器用于执行所述存储器中存储的指令，以使所述装置执行如权利要求1-11任一项所述的方法。
24.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有指令，当所述指令被执行时，使得计算机执行如权利要求1-11任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811643341.5A CN109849930B (zh) | 2018-12-29 | 2018-12-29 | 自动驾驶汽车的相邻车辆的速度计算方法和装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811643341.5A CN109849930B (zh) | 2018-12-29 | 2018-12-29 | 自动驾驶汽车的相邻车辆的速度计算方法和装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109849930A true CN109849930A (zh) | 2019-06-07 |
| CN109849930B CN109849930B (zh) | 2020-08-04 |
Family
ID=66893377
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811643341.5A Active CN109849930B (zh) | 2018-12-29 | 2018-12-29 | 自动驾驶汽车的相邻车辆的速度计算方法和装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109849930B (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112505680A (zh) * | 2019-09-13 | 2021-03-16 | 动态Ad有限责任公司 | 使用雷达的扩展对象跟踪 |
| CN112689775A (zh) * | 2020-04-29 | 2021-04-20 | 华为技术有限公司 | 雷达点云聚类方法和装置 |
| WO2021218346A1 (zh) * | 2020-04-30 | 2021-11-04 | 华为技术有限公司 | 一种聚类的方法和装置 |
| CN113850995A (zh) * | 2021-09-14 | 2021-12-28 | 华设设计集团股份有限公司 | 一种基于隧道雷视数据融合的事件检测方法、装置及系统 |
| CN116299302A (zh) * | 2023-05-19 | 2023-06-23 | 南京隼眼电子科技有限公司 | 交通工具本体速度确定方法、雷达系统和存储介质 |
| CN117092636A (zh) * | 2023-10-12 | 2023-11-21 | 深圳安智杰科技有限公司 | 一种毫米波雷达多目标跟踪虚假航迹识别系统及方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105467393A (zh) * | 2016-01-06 | 2016-04-06 | 北京无线电测量研究所 | 一种毫米波云雷达三维风场反演方法及系统 |
| CN107831496A (zh) * | 2017-11-01 | 2018-03-23 | 驭势科技（北京）有限公司 | 一种智能汽车自动获取前车速度的方法及系统 |
| WO2018114352A1 (en) * | 2016-12-23 | 2018-06-28 | Robert Bosch Gmbh | Method and system for determining vehicle speed using vertical and longitudinal acceleration sensors |
| CN108394410A (zh) * | 2017-02-08 | 2018-08-14 | 现代自动车株式会社 | Ecu、包括该ecu的自动驾驶车辆以及确定该车辆的行驶车道的方法 |
| EP3493177A1 (en) * | 2016-07-29 | 2019-06-05 | Pioneer Corporation | Measurement device, measurement method, and program |
-
2018
- 2018-12-29 CN CN201811643341.5A patent/CN109849930B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105467393A (zh) * | 2016-01-06 | 2016-04-06 | 北京无线电测量研究所 | 一种毫米波云雷达三维风场反演方法及系统 |
| EP3493177A1 (en) * | 2016-07-29 | 2019-06-05 | Pioneer Corporation | Measurement device, measurement method, and program |
| WO2018114352A1 (en) * | 2016-12-23 | 2018-06-28 | Robert Bosch Gmbh | Method and system for determining vehicle speed using vertical and longitudinal acceleration sensors |
| CN108394410A (zh) * | 2017-02-08 | 2018-08-14 | 现代自动车株式会社 | Ecu、包括该ecu的自动驾驶车辆以及确定该车辆的行驶车道的方法 |
| CN107831496A (zh) * | 2017-11-01 | 2018-03-23 | 驭势科技（北京）有限公司 | 一种智能汽车自动获取前车速度的方法及系统 |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112505680A (zh) * | 2019-09-13 | 2021-03-16 | 动态Ad有限责任公司 | 使用雷达的扩展对象跟踪 |
| CN112689775A (zh) * | 2020-04-29 | 2021-04-20 | 华为技术有限公司 | 雷达点云聚类方法和装置 |
| CN112689775B (zh) * | 2020-04-29 | 2022-06-14 | 华为技术有限公司 | 雷达点云聚类方法和装置 |
| WO2021218346A1 (zh) * | 2020-04-30 | 2021-11-04 | 华为技术有限公司 | 一种聚类的方法和装置 |
| CN113850995A (zh) * | 2021-09-14 | 2021-12-28 | 华设设计集团股份有限公司 | 一种基于隧道雷视数据融合的事件检测方法、装置及系统 |
| CN113850995B (zh) * | 2021-09-14 | 2022-12-27 | 华设设计集团股份有限公司 | 一种基于隧道雷视数据融合的事件检测方法、装置及系统 |
| CN116299302A (zh) * | 2023-05-19 | 2023-06-23 | 南京隼眼电子科技有限公司 | 交通工具本体速度确定方法、雷达系统和存储介质 |
| CN117092636A (zh) * | 2023-10-12 | 2023-11-21 | 深圳安智杰科技有限公司 | 一种毫米波雷达多目标跟踪虚假航迹识别系统及方法 |
| CN117092636B (zh) * | 2023-10-12 | 2024-01-02 | 深圳安智杰科技有限公司 | 一种毫米波雷达多目标跟踪虚假航迹识别系统及方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109849930B (zh) | 2020-08-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109849930A (zh) | 自动驾驶汽车的相邻车辆的速度计算方法和装置 | |
| CN111307162B (zh) | 用于自动驾驶场景的多传感器融合定位方法 | |
| CN109459734B (zh) | 一种激光雷达定位效果评估方法、装置、设备及存储介质 | |
| CN110458854A (zh) | 一种道路边缘检测方法和装置 | |
| CN108931245A (zh) | 移动机器人的局部自定位方法及设备 | |
| CN110287276A (zh) | 高精地图更新方法、装置及存储介质 | |
| CN111947672B (zh) | 用于检测环境变化的方法、装置、设备和介质 | |
| CN113147738A (zh) | 一种自动泊车定位方法和装置 | |
| EP3598144A1 (en) | Motion detection method, motion detection apparatus, and medium | |
| CN109885046B (zh) | 一种基于粒子滤波的移动机器人定位加速方法 | |
| CN112329789B (zh) | 点云的提取方法、装置、计算机设备和存储介质 | |
| CN114111774B (zh) | 车辆的定位方法、系统、设备及计算机可读存储介质 | |
| CN114730472A (zh) | 车载相机的外部参数的标定方法及相关装置 | |
| CN115205803A (zh) | 自动驾驶环境感知方法、介质及车辆 | |
| WO2022078342A1 (zh) | 动态占据栅格估计方法及装置 | |
| CN114485698A (zh) | 一种交叉路口引导线生成方法及系统 | |
| CN115546781A (zh) | 一种点云数据的聚类方法以及装置 | |
| US20230251095A1 (en) | Vehicle positioning method and apparatus | |
| CN116467848B (zh) | 一种毫米波雷达点云仿真的方法和装置 | |
| CN109769206B (zh) | 一种室内定位融合方法、装置、存储介质及终端设备 | |
| CN113034538B (zh) | 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 | |
| CN114663615A (zh) | 电子地图显示方法及装置、电子设备 | |
| CN115236672A (zh) | 障碍物信息生成方法、装置、设备及计算机可读存储介质 | |
| CN112797981A (zh) | 一种基于正态分布的粒子滤波定位方法 | |
| CN116502479B (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 |