[发明专利]一种基于毫米波雷达的本车车速和本车转向角计算方法

权利要求书

1.一种基于毫米波雷达的本车车速和本车转向角计算方法，其特征在于，包括：

S10：获取毫米波雷达输出的点云图以及所述点云图中各个点云的点云信息，所述点云信息包括点云的径向距离、径向速度、水平方位角和俯仰方位角；

S20：根据每个所述点云的径向速度、水平方位角和俯仰方位角，计算每个所述点云的Y轴投影速度；

S30：根据每个所述点云的Y轴投影速度，确定本车车速；

S40：根据本车车速与Y轴投影速度，确定所有对地静止的静止点云；

S50：根据所有静止点云的径向距离、径向速度和本车车速，计算本车转向角。

2.如权利要求1所述的基于毫米波雷达的本车车速和本车转向角计算方法，其特征在于，所述S30包括：

对每个所述Y轴投影速度取整，得到多个投影整数速度，确定出现频次最高的投影整数速度；

确定投影整数速度与出现频次最高的投影整数速度相同的所有第一点云，计算所有所述第一点云的所述Y轴投影速度的平均值，所述平均值的相反数为本车车速。

3.如权利要求2所述的基于毫米波雷达的本车车速和本车转向角计算方法，其特征在于，计算每个所述点云的Y轴投影速度的公式为：

Vy= V/（cosα*cosβ）

其中，Vy为Y轴投影速度，V为径向速度，α为水平方位角，β为俯仰方位角。

4.如权利要求2所述的基于毫米波雷达的本车车速和本车转向角计算方法，其特征在于，对所述Y轴投影速度取整的取整方式为四舍五入。

5.如权利要求1所述的基于毫米波雷达的本车车速和本车转向角计算方法，其特征在于，所述S40包括：

计算所述本车车速与所述点云的Y轴投影速度的和，当速度和的绝对值小于预设阈值时，判定该点云为对地静止的静止点云，确定所有静止点云。

6.如权利要求5所述的基于毫米波雷达的本车车速和本车转向角计算方法，其特征在于，所述预设阈值的范围是：0~0.5m/s。

7.如权利要求1所述的基于毫米波雷达的本车车速和本车转向角计算方法，其特征在于，所述S50包括：

根据静止点云的径向速度和本车车速，计算X轴方向的线速度，其中，所述径向速度为X轴方向的线速度与本车车速的矢量之和；

根据X轴方向的线速度和径向距离，计算静止点云对应的转向角；

计算所有静止点云对应的转向角，对所有转向角求取平均值，所述平均值为本车转向角。

8.如权利要求7所述的基于毫米波雷达的本车车速和本车转向角计算方法，其特征在于，计算静止点云对应的转向角的公式为：

Vx=Ω*r

其中，Vx为X轴方向的线速度，r为径向距离，Ω为转向角。