详情

原文传递自动驾驶汽车的相邻车辆的速度计算方法和装置

专利名称：	自动驾驶汽车的相邻车辆的速度计算方法和装置
摘要：	本发明提供一种自动驾驶汽车的相邻车辆的速度计算方法和装置，包括：获取主车周围环境中的观测点集合在主车的坐标系中的运动数据，该观测点集合为一帧图像中的观测点，对观测点集合中的观测点进行基于密度的聚类，得到至少一个点簇，针对每个点簇，根据点簇中的观测点的位置和径向相对速度，确定点簇中的非车轮类观测点和车轮类观测点，根据点簇中非车轮类观测点的径向相对速度、方位角以及主车的横向速度和纵向速度，基于最小二乘法，通过多次迭代拟合得到点簇的绝对速度，其中，点簇的绝对速度为点簇对应的相邻车辆的绝对速度。该方法基于观测点的径向相对速度、方位角以及主车的横纵向速度，通过最小二乘法拟合得到的相邻车辆的速度更加准确。
专利类型：	发明专利
国家地区组织代码：	北京;11
申请人：	百度在线网络技术(北京)有限公司
发明人：	黄彬;张智华;周全赟
专利状态：	有效
申请日期：	2018-12-29T00:00:00+0800
发布日期：	2019-06-07T00:00:00+0800
申请号：	CN201811643341.5
公开号：	CN109849930A
代理机构：	北京同立钧成知识产权代理有限公司
代理人：	祝乐芳;刘芳
分类号：	B60W40/105(2012.01);B;B60;B60W;B60W40
申请人地址：	100085 北京市海淀区上地十街10号百度大厦三层
主权项：	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任一项所述的方法。
所属类别：	发明专利