CN116859356A - 基于旋转矩阵优化求解的车载4d毫米波雷达自标定方法 - Google Patents
基于旋转矩阵优化求解的车载4d毫米波雷达自标定方法 Download PDFInfo
- Publication number
- CN116859356A CN116859356A CN202311136845.9A CN202311136845A CN116859356A CN 116859356 A CN116859356 A CN 116859356A CN 202311136845 A CN202311136845 A CN 202311136845A CN 116859356 A CN116859356 A CN 116859356A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- self
- coordinate system
- millimeter wave
- rotation matrix
- 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
- 239000011159 matrix material Substances 0.000 title claims abstract description 99
- 238000000034 method Methods 0.000 title claims abstract description 42
- 238000005457 optimization Methods 0.000 title claims abstract description 40
- 238000012545 processing Methods 0.000 claims abstract description 25
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 20
- 238000004364 calculation method Methods 0.000 claims abstract description 14
- 238000003672 processing method Methods 0.000 claims abstract description 14
- 238000012216 screening Methods 0.000 claims abstract description 7
- 230000006870 function Effects 0.000 claims description 20
- 238000005259 measurement Methods 0.000 claims description 15
- 239000013598 vector Substances 0.000 claims description 15
- 238000012937 correction Methods 0.000 claims description 9
- 238000000354 decomposition reaction Methods 0.000 claims description 7
- 238000013507 mapping Methods 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 5
- 230000003068 static effect Effects 0.000 abstract description 2
- 239000002245 particle Substances 0.000 description 18
- 238000010586 diagram Methods 0.000 description 4
- 238000009434 installation Methods 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 230000002068 genetic effect Effects 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
- 230000008447 perception Effects 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 238000002922 simulated annealing Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
- G01S7/40—Means for monitoring or calibrating
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C1/00—Measuring angles
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- 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)
- Mathematical Physics (AREA)
- Mathematical Analysis (AREA)
- Pure & Applied Mathematics (AREA)
- Computational Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Mathematical Optimization (AREA)
- Data Mining & Analysis (AREA)
- Remote Sensing (AREA)
- Algebra (AREA)
- Computer Networks & Wireless Communication (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computing Systems (AREA)
- Radar Systems Or Details Thereof (AREA)
Abstract
本发明涉及一种基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其中，该方法包括：基于4D毫米波雷达根据车辆行驶情况逐帧获取点云信息，并对车辆行驶信息进行自标定算法的条件判断处理；将获取到的满足自标定算法的车辆行驶速度信息进行静止目标点的筛选，并获取有效帧；利用获取到的点云信息构造雷达坐标系点集和车身坐标系点集；根据所述的雷达坐标系点集和车身坐标系点集计算两个点集之间的旋转矩阵；基于获取到的旋转矩阵求解相应的雷达偏角。本发明还涉及一种相应的装置、处理器及其存储介质。采用了本发明的该方法、装置、处理器及其存储介质，无需经历耗时较长的收敛计算过程，在达到计算帧数后的短时间内即可实现准确估计。
Description
技术领域
本发明涉及车载4D毫米波雷达技术领域，具体是指一种基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法、装置、处理器及其计算机可读存储介质。
背景技术
对于车载雷达而言，精准感知是保障驾驶安全的必要前提。然而在实际安装和测试过程中，人工安装可能致使雷达相对车身存在一定的角度误差；此外随着车辆使用过程中的碰撞、摩擦等损耗，雷达或将逐渐偏离初始安装位置。由于雷达的探测距离远，微小的角度偏差将在空间中导致较大的目标位置偏差，从而影响自动驾驶感知性能。因此雷达自标定对于实现精准环境感知极其重要。
现有的雷达自标定方法通常面向3D雷达，只标定方位偏角，对于俯仰偏角校正精度较低；此外现有方法往往需要测试环境中存在强反射的参照物，如长直栏杆、固定位置的角反射器等，这种方法对环境要求较高，在普通场景中效果较差。因此，面向车载4D毫米波雷达，如何在日常行驶环境中实时标定是当前本领域亟待解决的问题。
发明内容
本发明的目的是克服了上述现有技术的缺点，提供了一种测量精准有效的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法、装置、处理器及其计算机可读存储介质。
为了实现上述目的，本发明的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法、装置、处理器及其计算机可读存储介质如下：
该基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其主要特点是，所述的方法包括以下步骤：
（1）基于4D毫米波雷达根据车辆行驶情况逐帧获取点云信息，并对车辆行驶信息进行自标定算法的条件判断处理；
（2）基于当前帧数，将获取到的满足自标定算法的车辆行驶速度信息进行静止目标点的筛选，并获取有效帧；
（3）利用上述步骤获取到的点云信息构造雷达坐标系点集和车身坐标系点集；
（4）根据所述的雷达坐标系点集和车身坐标系点集计算两个点集之间的旋转矩阵；
（5）基于获取到的旋转矩阵求解相应的雷达偏角。
较佳地，所述的步骤（1）具体包括以下步骤：
（1.1）行驶中的车辆基于4D毫米波雷达逐帧获取车辆的目标测量距离r、目标测量多普勒速度、目标测量方位角/>以及目标测量俯仰角/>；
（1.2）初始化点云帧数以及有效帧数/>；
（1.3）基于获取到的车辆行驶信息按照以下条件判断其是否触发所述的自标定算法的处理条件：
；
其中，表示车辆直行速度，/>为车辆直行速度最小值，/>为车辆直行速度最大值，/>为车辆转弯速度，/>表示车辆转弯速度阈值，/>表示求绝对值；
（1.4）如果满足触发条件则进入步骤（2），否则，重复步骤（1）直到满足条件。
较佳地，所述的步骤（2）具体包括以下步骤：
（2.1）在当前第帧内，对所述的4D毫米波雷达探测到的目标点按照以下条件进行静止目标点的筛选：
；
其中，表示可容忍的速度浮动偏差；
（2.2）基于筛选出的静止目标点，若所述的静止目标点的数量大于预设的目标点数量阈值，则将当前帧数记为有效帧，并且有效帧数加1：
；
反之则有效帧数cnt大小不变，当前帧为无效帧；并将点云帧数设置为，继续获取下一帧点云数据。
尤佳地，所述的步骤（3）具体包括以下步骤：
（3.1）将上述筛选出的当前车辆静止目标点的目标测量方位角以及目标测量俯仰角/>，按照以下方式构造映射矩阵/>：
；
其中，表示第i个目标点的方位角，/>表示第i个目标点的俯仰角；
（3.2）将上述筛选出的n个目标点的多普勒速度，按照以下方式构造多普勒速度向量：
；
其中为第i个目标点的多普勒速度；
（3.3）根据所述的多普勒速度向量以及映射矩阵/>构造雷达坐标系下的样本：
；
（3.4）根据车辆行驶速度构造车身坐标系下的样本：
。
尤佳地，所述的步骤（3.4）之后还包括：
判断当前车辆的有效帧数cnt是否满足以下条件：
；
其中，表示帧数阈值，若满足，则进入下一步骤进行偏角计算，若不满足则继续统计下一帧数据，直到满足条件；
根据当前获取到的有效帧数cnt、雷达坐标系下的样本以及车身坐标系下的样本/>构造雷达坐标系目标样本集/>和车身坐标系目标样本集/>；
；
其中，表示第i帧的雷达坐标系样本，/>表示第i帧的车身坐标系样本。
尤佳地，所述的步骤（4）具体包括以下步骤：
（4.1）按照以下方式计算所述的雷达坐标系目标样本集和车身坐标系目标样本集/>之间的协方差矩阵H：
；
（4.2）对所述的协方差矩阵H按照以下方式进行奇异值分解：
；
其中，表示对矩阵进行奇异值分解，S为奇异值矩阵，其对角线为协方差矩阵H的奇异值；U为左奇异矩阵，U的每一列为协方差矩阵H的左奇异向量，W为右奇异矩阵，W的每一列为协方差矩阵H的右奇异向量；
（4.3）按照以下方式计算所述的协方差矩阵H的修正符号d：
；
（4.4）根据计算得到的W、修正符号d以及按照以下方式计算旋转矩阵：
；
其中，表示左奇异矩阵U的转置
；
尤佳地，所述的步骤（5）具体为：
根据计算出的所述的旋转矩阵，按照以下方式求解方位偏角/>、俯仰偏角/>、以及翻滚偏角/>：
；
其中，R（n,m）表示矩阵R中第n行第m列的元素。
更佳地，所述的步骤（5）进一步还包括：将所述的旋转矩阵进行目标函数优化处理，具体为：
构造目标函数：
；
待优化的目标函数为：
；
其中，s.t.表示“受制于”，其后表示、/>、/>需满足的约束条件。
该实现基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理装置，其主要特点是，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
该实现基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理的处理器，其主要特点是，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
该计算机可读存储介质，其主要特点是，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现上述所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
采用了本发明的该基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法、装置、处理器及其计算机可读存储介质，针对车载雷达相对车身存在偏差的问题，相较于常规雷达偏角计算方法只能计算方位偏角而言，本技术方案可同时求解雷达的方位角偏差、俯仰角偏差以及翻滚角偏差；同时，本技术方案对于校准环境的要求较低，校准时不需要参照物，可满足行驶中的实时校准需求。除此之外，本技术方案提出的偏角方法无需经历耗时较长的收敛计算过程，在达到计算帧数后短时间内即可实现准确估计，具有较为突出的应用效果。
附图说明
图1为本发明的该基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的处理流程示意图。
图2为本发明构建坐标系点集的示意图。
图3为本发明针对x-y平面进行校正前的原始点云示意图。
图4为本发明针对x-z平面进行校正前的原始点云示意图。
图5为本发明针对y-z平面进行校正前的原始点云示意图。
图6为本发明针对x-y平面进行校正后的点云示意图。
图7为本发明针对x-z平面进行校正后的点云示意图。
图8为本发明针对y-z平面进行校正后的点云示意图。
具体实施方式
为了能够更清楚地描述本发明的技术内容，下面结合具体实施例来进行进一步的描述。
在详细说明根据本发明的实施例前，应该注意到的是，在下文中，术语“包括”、“包含”或任何其他变体旨在涵盖非排他性的包含，由此使得包括一系列要素的过程、方法、物品或者设备不仅包含这些要素，而且还包含没有明确列出的其他要素，或者为这种过程、方法、物品或者设备所固有的要素。
请参阅图1所示，该基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其中，所述的方法包括以下步骤：
（1）基于4D毫米波雷达根据车辆行驶情况逐帧获取点云信息，并对车辆行驶信息进行自标定算法的条件判断处理；
（2）基于当前帧数，将获取到的满足自标定算法的车辆行驶速度信息进行静止目标点的筛选，并获取有效帧；
（3）利用上述步骤获取到的点云信息构造雷达坐标系点集和车身坐标系点集；
（4）根据所述的雷达坐标系点集和车身坐标系点集计算两个点集之间的旋转矩阵；
（5）基于获取到的旋转矩阵求解相应的雷达偏角。
作为本发明的优选实施方式，所述的步骤（1）具体包括以下步骤：
（1.1）行驶中的车辆基于4D毫米波雷达逐帧获取车辆的目标测量距离r、目标测量多普勒速度、目标测量方位角/>以及目标测量俯仰角/>；
（1.2）初始化点云帧数以及有效帧数/>；
（1.3）基于获取到的车辆行驶信息按照以下条件判断其是否触发所述的自标定算法的处理条件：
；
其中，表示车辆直行速度，/>为车辆直行速度最小值，/>为车辆直行速度最大值，/>为车辆转弯速度，/>表示车辆转弯速度阈值，/>表示求绝对值；
（1.4）如果满足触发条件则进入步骤（2），否则，重复步骤（1）直到满足条件。
作为本发明的优选实施方式，所述的步骤（2）具体包括以下步骤：
（2.1）在当前第帧内，对所述的4D毫米波雷达探测到的目标点按照以下条件进行静止目标点的筛选：
；
其中，表示可容忍的速度浮动偏差；
（2.2）基于筛选出的静止目标点，若所述的静止目标点的数量大于预设的目标点数量阈值，则将当前帧数记为有效帧，并且有效帧数加1：
；
反之则有效帧数cnt大小不变，当前帧为无效帧；并将点云帧数设置为，继续获取下一帧点云数据。
作为本发明的优选实施方式，所述的步骤（3）具体包括以下步骤：
（3.1）将上述筛选出的当前车辆静止目标点的目标测量方位角以及目标测量俯仰角/>，按照以下方式构造映射矩阵/>：
；
其中，表示第i个目标点的方位角，/>表示第i个目标点的俯仰角；
（3.2）将上述筛选出的n个目标点的多普勒速度，按照以下方式构造多普勒速度向量：
；
其中为第i个目标点的多普勒速度；
（3.3）根据所述的多普勒速度向量以及映射矩阵/>构造雷达坐标系下的样本：
；
（3.4）根据车辆行驶速度构造车身坐标系下的样本：
。
作为本发明的优选实施方式，所述的步骤（3.4）之后还包括：
判断当前车辆的有效帧数cnt是否满足以下条件：
；
其中，表示帧数阈值，若满足，则进入下一步骤进行偏角计算，若不满足则继续统计下一帧数据，直到满足条件；
根据当前获取到的有效帧数cnt、雷达坐标系下的样本以及车身坐标系下的样本/>构造雷达坐标系目标样本集/>和车身坐标系目标样本集/>；
；
其中，表示第i帧的雷达坐标系样本，/>表示第i帧的车身坐标系样本。
作为本发明的优选实施方式，所述的步骤（4）具体包括以下步骤：
（4.1）按照以下方式计算所述的雷达坐标系目标样本集和车身坐标系目标样本集/>之间的协方差矩阵H：
；
（4.2）对所述的协方差矩阵H按照以下方式进行奇异值分解：
；
其中，表示对矩阵进行奇异值分解，S为奇异值矩阵，其对角线为协方差矩阵H的奇异值；U为左奇异矩阵，U的每一列为协方差矩阵H的左奇异向量，W为右奇异矩阵，W的每一列为协方差矩阵H的右奇异向量；
（4.3）按照以下方式计算所述的协方差矩阵H的修正符号d：
；
（4.4）根据计算得到的W、修正符号d以及按照以下方式计算旋转矩阵：
；
其中，表示左奇异矩阵U的转置
；
作为本发明的优选实施方式，所述的步骤（5）具体为：
根据计算出的所述的旋转矩阵，按照以下方式求解方位偏角/>、俯仰偏角/>、以及翻滚偏角/>：
；
其中，R（n,m）表示矩阵R中第n行第m列的元素。
除此之外，针对上述旋转矩阵的非线性方程组还可以通过二分法、弦截法、信赖域等方法直接求得数值解。
作为本发明的优选实施方式，所述的步骤（5）进一步还包括：将所述的旋转矩阵进行目标函数优化处理，具体为：
构造目标函数：
；
待优化的目标函数为：
；
其中，s.t.表示“受制于”，其后表示、/>、/>需满足的约束条件。
除此之外，针对目标函数的求解，还可以通过遗传算法、模拟退火、模式搜索、等优化算法进行求解。
下面将结合具体实施例，对本技术方案做进一步详细的说明：
在本发明的一具体实施方式中，选择调频连续波（Frequency Module ContinueWaveform，FMCW）体制的多发多收（Multi-Input Multi-Output，MIMO）雷达进行数据采集，每帧发射信号的周期为100ms。
基于4D毫米波雷达逐帧获取点云信息，其中4D毫米波雷达点云信息包括目标测量距离r，目标测量速度，目标测量方位角/>，目标测量俯仰角/>，并初始化点云帧数及有效帧数/>。
在本发明的一具体实施方式中，首先根据车辆行驶信息判断是否进入自标定算法，若行驶状态满足如下条件则进入计算，反之则不进行计算：
；
其中表示车辆直行速度，/>为车辆直行速度最小值，/>为车辆直行速度最大值，/>表示车辆转弯速度阈值，/>表示求绝对值。实施例中，/>，/>。
在本发明的一具体实施方式中，如果当前车辆行驶速度满足自标定算法触发要求，则在当前第fcnt帧内，对雷达探测到的目标点进行筛选，找出静止目标点。若目标点满足：
；
则认为目标静止，其中表示雷达测得的目标点速度，/>为目标方位角，/>为目标俯仰角，/>表示可容忍的静止目标速度浮动偏差。实施例中/>。
在本发明的一具体实施方式中，若筛选出的目标点数大于，将当前帧记为有效帧，
并且有效帧数加一：
；
其中cnt表示有效帧数。反之cnt大小不变，当前帧为无效帧，，继续获取下一帧点云数据。
在本发明的一具体实施方式中，。
在本发明的一具体实施方式中，基于筛选出的目标点构造雷达坐标系样本和车身坐标系样本/>：
（1）取得筛选出的n个目标点的位置信息，构造映射矩阵：
；
其中表示第i个目标点的方位角，/>表示第i个点的俯仰角。
（2）取得筛选出的n个目标点的多普勒速度，构造多普勒速度向量：
；
其中为第i个目标点的多普勒速度。
（3）基于多普勒速度向量构造雷达坐标系下的样本/>：
；
（4）基于车辆行驶速度构造车身坐标系下的样本：
；
在本发明的一具体实施方式中，判断有效帧数cnt是否达到计算要求：
；
若满足则进行偏角计算，其中表示帧数阈值。实施例中/>。若不满足计算要求则继续统计下一帧数据，/>。
进一步的实施例中，有效帧数满足要求后，根据每帧计算得到的和/>构造车身坐标系目标样本集/>和雷达坐标系目标样本集/>；
；
其中表示第i帧的雷达坐标系样本，/>表示第i帧的车身坐标系样本。
在本发明的一具体实施方式中，估计车身坐标系样本集和雷达坐标系样本集/>之间的旋转矩阵；
（1）计算两个集合之间的协方差矩阵：
；
（2）对H进行奇异值分解：
；
（3）计算修正符号d：
；
其中为求矩阵行列式操作，/>表示取符号操作。
计算旋转矩阵：
；
其中：
；
在本发明的一具体实施方式中，得到R后，可通过R求解方位偏角、俯仰偏角、以及翻滚偏角/>；
构造方程组如下：
；
其中R（n,m）表示矩阵R中第n行第m列的元素。
在本发明的一具体例中，通过牛顿迭代法方法对方程组（16）进行求解：
（1）令，构造向量函数F(x)：
;
（2）设置初始值，计算；k=0；
（3）迭代求解;其中/>是/>的雅格比矩阵，/>为矩阵求逆操作。
（4）计算;
（5）判断是否满足以下迭代停止条件：
；
其中，和/>为允许求解误差，N为最大迭代次数。
在本发明的一具体实施例中，，/>，N=200。
（6）若满足步骤（5）中任意一个条件则停止迭代，输出此时的，得到，，；若不满足，则令k=k+1，并重复步骤（3）～（5）。
在本发明的一具体实施实施方式中，也可将非线性方程求解问题转化为优化问题，首先构造目标函数，令：
；
待优化的目标函数为：
；
可通过优化算法进行求解，包括但不限于遗传算法、模拟退火、模式搜索、粒子群算法等。
作为本发明的一优选实施例，按照以下方式通过粒子群算法对式（20）进行求解；
（1）随机初始化粒子群；其中为粒子群大小，的每一行代
表一个粒子，粒子值为，，可能的取值；初始化迭代速度;迭代次数；
在本发明的一具体实施例中，。
（2）计算粒子群中每个粒子的适应度函数/>，适应度函数即式（18）中的目标函数；
（3）计算当前迭代次数中，粒子群内适应度函数的最小值，及最小值对应的粒子值/>，构造粒子群最优值/>，/>；
（4）对每个粒子计算：在以往次迭代中每个粒子的最优值，记录对
应的粒子值；
（5）更新当前的粒子速度：
；
其中为群体学习因子，/>为自我学习因子。/>
在本发明的一具体实施例中，，/>。
①更新粒子群：
；
②令，判断是否满足迭代停止条件：
；
其中为最大迭代次数。
在本发明的一具体实施例中，。
（8）若满足步骤（7）则停止迭代，输出此时的，取粒子群内适应度函数最小的粒子值，得到/>，/>，/>；若不满足，则重复步骤（2）～（7）。
如图3至5所示为校正前的点云处理结果，可以看出由于安装误差，点云整体在方位向、俯仰向以及翻滚向都出现了偏移，如图6至8所示，可以看出在第192帧（19.2s）完成偏角估计并校正当前帧点云，通过本技术方案所记载的自标定算法进行校正，点云被修正至正确位置。
该实现基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理装置，其中，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
该实现基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理的处理器，其中，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现上述所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
该计算机可读存储介质，其中，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现上述所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。
应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行装置执行的软件或固件来实现。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成的，程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
上述提到的存储介质可以是只读存储器，磁盘或光盘等。
在本说明书的描述中，参考术语“一实施例”、“一些实施例”、“示例”、“具体示例”、或“实施例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。
采用了本发明的该基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法、装置、处理器及其计算机可读存储介质，针对车载雷达相对车身存在偏差的问题，相较于常规雷达偏角计算方法只能计算方位偏角而言，本技术方案可同时求解雷达的方位角偏差、俯仰角偏差以及翻滚角偏差；同时，本技术方案对于校准环境的要求较低，校准时不需要参照物，可满足行驶中的实时校准需求。除此之外，本技术方案提出的偏角方法无需经历耗时较长的收敛计算过程，在达到计算帧数后短时间内即可实现准确估计，具有较为突出的应用效果。
在此说明书中，本发明已参照其特定的实施例作了描述。但是，很显然仍可以作出各种修改和变换而不背离本发明的精神和范围。因此，说明书和附图应被认为是说明性的而非限制性的。
Claims (11)
1.一种基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其特征在于，所述的方法包括以下步骤：
（1）基于4D毫米波雷达根据车辆行驶情况逐帧获取点云信息，并对车辆行驶信息进行自标定算法的条件判断处理；
（2）基于当前帧数，将获取到的满足自标定算法的车辆行驶速度信息进行静止目标点的筛选，并获取有效帧；
（3）利用上述步骤获取到的点云信息构造雷达坐标系点集和车身坐标系点集；
（4）根据所述的雷达坐标系点集和车身坐标系点集计算两个点集之间的旋转矩阵；
（5）基于获取到的旋转矩阵求解相应的雷达偏角。
2.根据权利要求1所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其特征在于，所述的步骤（1）具体包括以下步骤：
（1.1）行驶中的车辆基于4D毫米波雷达逐帧获取车辆的目标测量距离r、目标测量多普勒速度、目标测量方位角/>以及目标测量俯仰角/>；
（1.2）初始化点云帧数以及有效帧数/>；
（1.3）基于获取到的车辆行驶信息按照以下条件判断其是否触发所述的自标定算法的处理条件：
;
其中，表示车辆直行速度，/>为车辆直行速度最小值，/>为车辆直行速度最大值，/>为车辆转弯速度，/>表示车辆转弯速度阈值，/>表示求绝对值；
（1.4）如果满足触发条件则进入步骤（2），否则，重复步骤（1）直到满足条件。
3.根据权利要求2所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其特征在于，所述的步骤（2）具体包括以下步骤：
（2.1）在当前第帧内，对所述的4D毫米波雷达探测到的目标点按照以下条件进行静止目标点的筛选：
；
其中，表示可容忍的速度浮动偏差；
（2.2）基于筛选出的静止目标点，若所述的静止目标点的数量大于预设的目标点数量阈值，则将当前帧数记为有效帧，并且有效帧数加1：
；
反之则有效帧数cnt大小不变，当前帧为无效帧；并将点云帧数设置为，继续获取下一帧点云数据。
4.根据权利要求3所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其特征在于，所述的步骤（3）具体包括以下步骤：
（3.1）将上述筛选出的当前车辆静止目标点的目标测量方位角以及目标测量俯仰角，按照以下方式构造映射矩阵/>：
；
其中，表示第i个目标点的方位角，/>表示第i个目标点的俯仰角；
（3.2）将上述筛选出的n个目标点的多普勒速度，按照以下方式构造多普勒速度向量：
；
其中为第i个目标点的多普勒速度；
（3.3）根据所述的多普勒速度向量以及映射矩阵/>构造雷达坐标系下的样本：
；
（3.4）根据车辆行驶速度构造车身坐标系下的样本：
。
5.根据权利要求4所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其特征在于，所述的步骤（3.4）之后还包括：
判断当前车辆的有效帧数cnt是否满足以下条件：
；
其中，表示帧数阈值，若满足，则进入下一步骤进行偏角计算，若不满足则继续统计下一帧数据，直到满足条件；
根据当前获取到的有效帧数cnt、雷达坐标系下的样本以及车身坐标系下的样本构造雷达坐标系目标样本集/>和车身坐标系目标样本集/>；
；
其中，表示第i帧的雷达坐标系样本，/>表示第i帧的车身坐标系样本。
6.根据权利要求5所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其特征在于，所述的步骤（4）具体包括以下步骤：
（4.1）按照以下方式计算所述的雷达坐标系目标样本集和车身坐标系目标样本集/>之间的协方差矩阵H：
；
（4.2）对所述的协方差矩阵H按照以下方式进行奇异值分解：
；
其中，表示对矩阵进行奇异值分解，S为奇异值矩阵，其对角线为协方差矩阵H的奇异值；U为左奇异矩阵，U的每一列为协方差矩阵H的左奇异向量，W为右奇异矩阵，W的每一列为协方差矩阵H的右奇异向量；
（4.3）按照以下方式计算所述的协方差矩阵H的修正符号d：
；
（4.4）根据计算得到的W、修正符号d以及按照以下方式计算旋转矩阵/>：
；
其中，表示左奇异矩阵U的转置；
。
7.根据权利要求6所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其特征在于，所述的步骤（5）具体为：
根据计算出的所述的旋转矩阵，按照以下方式求解方位偏角/>、俯仰偏角、以及翻滚偏角/>：
；
其中，R（n,m）表示矩阵R中第n行第m列的元素。
8.根据权利要求7所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法，其特征在于，所述的步骤（5）进一步还包括：将所述的旋转矩阵进行目标函数优化处理，具体为：
构造目标函数：
；
待优化的目标函数为：
；
其中，s.t.表示“受制于”，其后表示、/>、/>需满足的约束条件。
9.一种实现基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理装置，其特征在于，所述的装置包括：
处理器，被配置成执行计算机可执行指令；
存储器，存储一个或多个计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～8中任一项所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
10.一种实现基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理的处理器，其特征在于，所述的处理器被配置成执行计算机可执行指令，所述的计算机可执行指令被所述的处理器执行时，实现权利要求1～8中任一项所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
11.一种计算机可读存储介质，其特征在于，其上存储有计算机程序，所述的计算机程序可被处理器执行以实现权利要求1～8中任一项所述的基于旋转矩阵优化求解的车载4D毫米波雷达自标定处理方法的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311136845.9A CN116859356B (zh) | 2023-09-05 | 2023-09-05 | 基于旋转矩阵优化求解的车载4d毫米波雷达自标定方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311136845.9A CN116859356B (zh) | 2023-09-05 | 2023-09-05 | 基于旋转矩阵优化求解的车载4d毫米波雷达自标定方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116859356A true CN116859356A (zh) | 2023-10-10 |
| CN116859356B CN116859356B (zh) | 2023-11-17 |
Family
ID=88219552
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311136845.9A Active CN116859356B (zh) | 2023-09-05 | 2023-09-05 | 基于旋转矩阵优化求解的车载4d毫米波雷达自标定方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116859356B (zh) |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2750214A1 (fr) * | 1996-06-21 | 1997-12-26 | Thomson Csf | Procede de calibration des erreurs de positionnement d'un radar et de la derive en vitesse sol d'une centrale inertielle embarques a bord d'un aeronef |
| US20190353751A1 (en) * | 2018-05-15 | 2019-11-21 | GM Global Technology Operations LLC | Vehicle radar system and method of calibrating the same |
| US20210173055A1 (en) * | 2019-12-09 | 2021-06-10 | Nio Usa, Inc. | Real-time online calibration of coherent doppler lidar systems on vehicles |
| US20210364616A1 (en) * | 2020-05-21 | 2021-11-25 | Hertzwell Pte. Ltd. | Radar system and computer-implemented method for radar target detection |
| CN114325682A (zh) * | 2021-04-29 | 2022-04-12 | 北京易航远智科技有限公司 | 基于车载4d毫米波雷达的车辆速度状态估计方法 |
| CN114706070A (zh) * | 2022-02-22 | 2022-07-05 | 惠州市德赛西威智能交通技术研究院有限公司 | 一种基于4d毫米波雷达的自动泊车车位搜索方法和系统 |
| CN114839611A (zh) * | 2022-04-29 | 2022-08-02 | 上海人工智能创新中心 | 一种毫米波雷达的自标定方法及装置 |
| CN115937325A (zh) * | 2022-09-27 | 2023-04-07 | 上海几何伙伴智能驾驶有限公司 | 一种结合毫米波雷达信息的车端摄像头标定方法 |
| CN116125411A (zh) * | 2023-03-20 | 2023-05-16 | 上海几何伙伴智能驾驶有限公司 | 基于freespace路端4D毫米波雷达实现水平安装角度检测补偿的方法 |
| CN116381649A (zh) * | 2022-12-30 | 2023-07-04 | 烟台艾睿光电科技有限公司 | 联合标定方法、设备和存储介质 |
| CN116559845A (zh) * | 2023-03-22 | 2023-08-08 | 上海人工智能创新中心 | 一种激光雷达自标定方法、系统、设备及存储介质 |
-
2023
- 2023-09-05 CN CN202311136845.9A patent/CN116859356B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2750214A1 (fr) * | 1996-06-21 | 1997-12-26 | Thomson Csf | Procede de calibration des erreurs de positionnement d'un radar et de la derive en vitesse sol d'une centrale inertielle embarques a bord d'un aeronef |
| US20190353751A1 (en) * | 2018-05-15 | 2019-11-21 | GM Global Technology Operations LLC | Vehicle radar system and method of calibrating the same |
| US20210173055A1 (en) * | 2019-12-09 | 2021-06-10 | Nio Usa, Inc. | Real-time online calibration of coherent doppler lidar systems on vehicles |
| US20210364616A1 (en) * | 2020-05-21 | 2021-11-25 | Hertzwell Pte. Ltd. | Radar system and computer-implemented method for radar target detection |
| CN114325682A (zh) * | 2021-04-29 | 2022-04-12 | 北京易航远智科技有限公司 | 基于车载4d毫米波雷达的车辆速度状态估计方法 |
| CN114706070A (zh) * | 2022-02-22 | 2022-07-05 | 惠州市德赛西威智能交通技术研究院有限公司 | 一种基于4d毫米波雷达的自动泊车车位搜索方法和系统 |
| CN114839611A (zh) * | 2022-04-29 | 2022-08-02 | 上海人工智能创新中心 | 一种毫米波雷达的自标定方法及装置 |
| CN115937325A (zh) * | 2022-09-27 | 2023-04-07 | 上海几何伙伴智能驾驶有限公司 | 一种结合毫米波雷达信息的车端摄像头标定方法 |
| CN116381649A (zh) * | 2022-12-30 | 2023-07-04 | 烟台艾睿光电科技有限公司 | 联合标定方法、设备和存储介质 |
| CN116125411A (zh) * | 2023-03-20 | 2023-05-16 | 上海几何伙伴智能驾驶有限公司 | 基于freespace路端4D毫米波雷达实现水平安装角度检测补偿的方法 |
| CN116559845A (zh) * | 2023-03-22 | 2023-08-08 | 上海人工智能创新中心 | 一种激光雷达自标定方法、系统、设备及存储介质 |
Non-Patent Citations (2)
| Title |
|---|
| JURAJ PERŠIĆ: "Extrinsic 6DoF calibration of a radar–LiDAR–camera system enhanced by radar cross section estimates evaluation", 《ROBOTICS AND AUTONOMOUS SYSTEMS》, vol. 114, pages 217 - 230 * |
| 贾子永 等: "基于梯形棋盘格的摄像机和激光雷达标定方法", 《计算机应用》, vol. 37, no. 7, pages 2062 - 2077 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116859356B (zh) | 2023-11-17 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10613227B2 (en) | Method and apparatus for positioning vehicle | |
| US10634777B2 (en) | Radar odometry for vehicle | |
| CN113748357B (zh) | 激光雷达的姿态校正方法、装置和系统 | |
| US11460568B2 (en) | Estimating in-plane velocity from an arbitrary radar return | |
| US20140309836A1 (en) | Position Estimation and Vehicle Control in Autonomous Multi-Vehicle Convoys | |
| CN110889808A (zh) | 一种定位的方法、装置、设备及存储介质 | |
| CN111427022A (zh) | 一种基于极大似然估计的阵列雷达测角方法 | |
| CN112710339A (zh) | 用于校准车辆传感器的方法和设备 | |
| CN109839633B (zh) | 基于最小覆盖空域的机载预警雷达的多帧检测前跟踪方法 | |
| CN115436917A (zh) | Lidar视轴对准误差和主车辆定位误差的协同估计和校正 | |
| CN108507587B (zh) | 一种基于坐标变换的三维车载定位导航方法 | |
| CN116859356B (zh) | 基于旋转矩阵优化求解的车载4d毫米波雷达自标定方法 | |
| EP3907521A1 (en) | Methods and systems for determining alignment parameters of a radar sensor | |
| CN109769206B (zh) | 一种室内定位融合方法、装置、存储介质及终端设备 | |
| CN115469314A (zh) | 一种均匀圆环阵稳健水下目标方位跟踪方法及系统 | |
| CN114740448A (zh) | 用于车载雷达的目标状态估计方法、装置及存储介质 | |
| CN110888104B (zh) | 一种接近信标轨迹下的水下机器人定位方法 | |
| CN112731321A (zh) | 基于mimo认知雷达的移动机器人避障及地图绘制方法 | |
| CN115166701B (zh) | 一种rgb-d相机和激光雷达的系统标定方法及装置 | |
| CN114459484B (zh) | 航迹初始化方法及装置 | |
| US20230168341A1 (en) | Method and System for Estimating a Radar Calibration Matrix | |
| CN114814920A (zh) | 一种无人驾驶定位方法及装置 | |
| Urru et al. | Data Fusion algorithms to improve test range sensors accuracy and precision | |
| CN117970314A (zh) | 基于反馈学习的目标跟踪方法及装置 | |
| CN115327526A (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 |