CN110390697A - 一种基于lm算法的毫米波雷达与摄像头联合标定方法 - Google Patents

Abstract

本发明公开一种基于LM算法的毫米波雷达与摄像头联合标定方法，其首先固定毫米波雷达与摄像头的位置，并测量两者的相对位置关系；接着打开摄像头采集数据，并采用相机内参标定方法标定相机的内参；然后同时打开毫米波雷达和摄像头，放置标定物进行数据采集；再对采集的数据进行融合，并使用LM算法来实现坐标转移矩阵的联合标定；再采集测试数据，并将毫米波雷达数据使用标定后的坐标转移矩阵进行坐标转换，验证联合标定的性能是否满足需求，当不符合要求时，修改联合标定参数，重新采集数据进行标定联合；直到符合要求后输出坐标转移矩阵。该联合标定方法标定了毫米波雷达和摄像头之间的坐标转移矩阵，标定精度高，为两者的数据融合提供了保障。

Description

一种基于LM算法的毫米波雷达与摄像头联合标定方法

技术领域

本发明涉及位置标定领域，尤其涉及一种基于LM算法的毫米波雷达与摄像头联合标定方法。

背景技术

随着多传感器融合技术的迅猛发展，多传感器之间的位置联合标定方法得到了广泛的关注和研究。特别是在智能机器人的视觉导航，及车辆智能驾驶等领域，雷达与摄像头之间的融合感知算法，可以提供更为可靠、精确、稳定的环境感知能力。便于后续机器人与车辆的智能决策与智能驾驶的实现。而联合标定，是实现融合感知算法的前提保障。联合标定的目的是为了实现多传感器之间的数据匹配。

现在的针对雷达与摄像头的联合标定方法有多种主流的选择。一种是分别将雷达的坐标系与摄像头的坐标系相对于一个基准坐标系进行标定后，再通过相应的传递公式，计算两者的相对位置关系；一种是通过测量与设置的方法，直接将雷达与摄像头的相对位置关系进行固定，从而确定两者的相对位置关系；还有一种是通过标定物，将雷达中的点与摄像头中的对应点进行匹配，再根据匹配点来计算两者的相对位置关系。

而如今的雷达与摄像头的联合标定方法主要针对激光雷达与摄像头的联合标定。而在毫米波雷达与摄像头的联合标定过程，由于毫米波的点云成像原理与激光雷达不同，导致点云的分辨率与成像特点都与激光雷达存在差异。由于毫米波雷达的点云成像对目标边缘及大角度目标不敏感，导致毫米波雷达难以提取与激光雷达相同的特征点，因此无法使用基于边缘提取等方式的联合标定方法。这给毫米波雷达与摄像头的联合标定性能带来了极大的挑战。

发明内容

本发明的目的在于针对现有技术的不足，提供一种基于LM算法的毫米波雷达与摄像头联合标定方法，具体技术方案如下：

一种基于LM算法的毫米波雷达与摄像头联合标定方法，其特征在于，该方法包括以下步骤：

S1：固定毫米波雷达与摄像头的位置，并测量两者的相对位置关系；

S2：打开摄像头进行数据采集，并采用相机内参标定方法标定相机的内参；

S3：同时打开毫米波雷达和摄像头，放置标定物进行数据采集；

S4：对采集的数据进行融合，并使用LM算法来实现坐标转移矩阵的联合标定；

S5：选取合适场景及测试物采集测试数据，并将毫米波雷达数据使用标定后的坐标转移矩阵进行坐标转换，当测试物雷达点云在摄像头图片的投影点与摄像头拍摄的测试物图像中的点之间的像素距离不小于雷达分辨率时，重新调整S4的联合标定中的参数设定，重复S1-S5；反之，进入下一步；

S6：输出S4得到的联合标定后的坐标转移矩阵。

进一步地，所述S1具体为：

S1.1：固定毫米波雷达与摄像头的位置，保证毫米波雷达与摄像头之间的相对位置在后续的数据采集过程中不发生变化，包括三个自由度的平移及三个自由度的旋转；

S1.2：在接近毫米波雷达天线位置处选取一点作为雷达坐标系的原点，并根据雷达采集的数据坐标系方向确定雷达坐标系的方向，将两者的坐标系方向进行统一，得到雷达坐标系O_rx_ry_rz_r；在接近摄像头镜头处选取一点作为摄像头坐标系的原点，根据摄像头采集的数据坐标系方向确定摄像头坐标系的方向，将两者的坐标系方向进行统一，得到摄像头坐标系O_cx_cy_cz_c；

S1.3：基于Rxyz旋转方式及三维平移来定义坐标系转移参数l＝[θ_x，θ_y，θ_z，t_x，t_y，t_z]^T，上标T表示转置：l中的各个参数通过如下方式得到：

分别沿着及方向，量取O_c相对O_r的相对距离，得到t_x、t_y与t_z，移动雷达坐标系，将雷达坐标系原点与摄像头坐标系重合；旋转雷达坐标系的x_r轴使得其与x_c轴重合，量取旋转的角度，得到θ_x；在上述坐标系转移的基础上，进一步旋转雷达坐标系的y_r轴使得其与y_c轴重合，量取旋转的角度，得到θ_y；在上述坐标系转移的基础上，进一步旋转雷达坐标系的z_r轴使得其与z_c轴重合，量取旋转的角度，得到θ_z；

进一步地，所述S2具体为：

S2.1：设置摄像头的参数，并保持在联合标定过程中摄像头的焦距、分辨率不发生变化；

S2.2：连接摄像头至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)，其中f(x_p，y_p)表示在像素坐标[x_p，y_p]处的像素值；

S2.3：根据选取的摄像头类型，使用合适的相机内参标定方法，标定摄像头的内参矩阵

进一步地，所述S3具体为：

S3.1：选取能够同时被毫米波雷达和摄像头检测到的物体作为标定物，打开毫米波雷达与摄像头，并连接至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)，使毫米波雷达能够按帧输出点云数据P(n)；其中，P(n)为三维点云数据[x，y，z]，n为雷达点索引；

S3.2：初始化联合标定用的数据组数i＝1，将标定物放置于能够同时被毫米波雷达与摄像头检测到的位置，并记录标定物位置；

S3.3：保持标定物的位置不发生改变，同时采集一段时间的毫米波雷达与摄像头数据，并存储放入第i组数据中；

S3.4：如果标定用的数据组数没有达到设定的组数上限I，即i＜I，令i＝i+1，并且改变标定物放置的位置，将标定物放置于能够同时被毫米波雷达与摄像头检测到的位置，并记录标定物位置，且保证该位置不接近第i组之前的数据采集时标定物的放置位置，返回S3.3；如果i≥I，则进入下一步。

进一步地，所述S4具体为：

S4.1：分别对毫米波雷达数据和摄像头数据进行处理，具体如下：

对于毫米波雷达数据，将每一组数据中的多帧数据叠加至一帧，并将雷达点云中的标定物点进行提取，并通过下式计算标定物点的坐标均值：

其中，x(n)、y(n)、z(n)表示当前融合数据组中的第n个标定物点雷达坐标，N_c为总的毫米波标定物点数；

对于摄像头数据，提取每一帧数据中标定物所在的像素坐标，将每一组数据中的多帧数据进行比较，如果存在两帧数据，标定物所在像素重合率不超过80％，则将该组数据舍弃，更新数据总组数I及数据组数索引；反之，则在该组数据中任意选取一帧的摄像头数据，并通过下式计算其中标定物所在的像素坐标均值

其中，x_p(m)、y_p(m)表示当前融合数据组中的第m个标定物点图像坐标，M_c为总的图像标定物点数；

S4.2：使用LM算法来进行坐标转移矩阵的联合标定；首先，构造优化函数L(l)＝L(θ_x，θ_y，θ_z，t_x，t_y，t_z)＝[L₁，L₁，...，L_2I]^T由2I个函数组成，并且满足如下关系：

L_2i-1＝x_p，i-z_i(1)/z_i(3) (1)

L_2i＝y_p，i-z_i(2)/z_i(3) (2)

其中，i＝1，2，...，I；z_i＝[z_i(1)，z_i(2)，z_i(3)]^T，并且z_i满足如下关系：

z_i＝Hx_i (3)

其中，x_i＝[x_i，y_i，z_i，1]^T；H＝AB为坐标转移矩阵；A为摄像头坐标系相对毫米波雷达坐标系的外参矩阵，A＝[R，t]，R为旋转矩阵，t为平移向量，有R＝R_zR_yR_x，t＝[t_x，t_y，t_z]^T，

S4.3：将S1.3中得到的量测数据作为迭代初值l₀；初始化迭代次数k＝1，并计算优化函数的雅可比矩阵根据联合标定的实际情况，设定迭代次数上限K、下降梯度步长c及容忍系数σ的初值；对迭代优化函数值进行初始化赋值F＝L(l₀)，对迭代优化函数雅可比矩阵进行初始化赋值J_F＝J(l₀)，对下降梯度步长进行初始化赋值D＝diag(J^H(l₀)J(l₀))，其中diag(·)表示提取方阵的对角元素形成新的方阵，J^H是J的共轭转置；

S4.4：当时，令LM参数λ＝0，计算变化量其中上标+表示矩阵的伪逆；当时，根据需求设定合适的LM参数λ，并通过求解如下的最小二乘问题得到变化量p：

S4.5：首先计算变化后的优化函数值F_p＝L(l_k-1+p)，然后通过下式计算线性度ρ，来评估下降步长：

S4.6：当ρ＞0.0001时，令l_k＝l_k-1+p；否则，令l_k＝l_k-1；更新迭代优化函数值F＝L(l_k)，更新迭代优化函数雅可比矩阵J_F＝J(l_k)；

S4.7：当ρ≤0.25时，缩小下降梯度步长c；当0.25＜ρ＜0.75且λ＝0时，或者ρ≥0.75时，扩大下降梯度步长c；

S4.8：通过下式更新步长梯度：

其中，max{·，·}表示将两个同尺度矩阵中，相同位置的每个元素进行比较，并将较大值赋值给新矩阵的同一个位置。

S4.9：当k＜K时，令k＝k+1，返回S4.4；否则，进入下一步；

S4.10：根据式(3)，将l_k作为l赋值给H，得到坐标转移矩阵输出。

进一步地，所述的S5具体为：

S5.1：选取合适场景及测试物进行测试数据的采集，打开毫米波雷达与摄像头，并连接至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)。使毫米波雷达能够按帧输出点云数据P(n)；其中，P(n)为三维点云数据[x，y，z]，n为雷达点索引。

S5.2：将毫米波雷达的帧数据与摄像头帧数据进行时间同步，并提取时间同步帧的数据；

S5.3：从时间同步帧的毫米波数据中提取测试物的点云，并根据式(3)进行投影，当测试物雷达点云在摄像头图片的投影点与摄像头拍摄的测试物图像中的点之间的像素距离不小于雷达分辨率时，重新调整S4的联合标定中的迭代次数上限K、下降梯度步长c及容忍系数σ，重复S1-S5.3；反之，则进入下一步。

进一步地，所述的相机内参标定方法具体为张正友标定法。

进一步地，所述的S4.4中，当时，所述的LM参数λ根据Hebden迭代算法确定。

本发明的有益效果如下：

基于LM算法的毫米波雷达与摄像头联合标定方法，研究根据毫米波雷达的点云成像特点，选取合适的标定物实现毫米波雷达与摄像头匹配点的提取。相比于其他的大部分联合标定方法，在毫米波雷达与摄像头联合标定场景下，由于毫米波雷达成像特点造成的标定性能损失。本方法采用的数据采集及融合方式更能适应在毫米波雷达与摄像头联合标定下，数据匹配点的提取需求。且基于LM算法的联合标定坐标转移矩阵的参数迭代优化设计，具有较高的精度及收敛速度，从而提供更好的参数估计性能。标定物及测试物的选取更为方便，也能提供更好的联合标定性能保障。

附图说明

图1是基于LM算法的毫米波雷达与摄像头联合标定方法设计流程图；

图2是本发明的联合标定方法坐标转移矩阵估计收敛性能图；

图3是本发明的联合标定方法性能验证图。

具体实施方式

下面根据附图和优选实施例详细描述本发明，本发明的目的和效果将变得更加明白，应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

一种基于LM算法的毫米波雷达与摄像头联合标定方法，如图1所示，该方法包括以下步骤：

S1：固定毫米波雷达与摄像头的位置，并测量两者的相对位置关系：

S1.1：固定毫米波雷达与摄像头的位置，保证毫米波雷达与摄像头之间的相对位置在后续的数据采集过程中不发生变化，包括三个自由度的平移及三个自由度的旋转。当发生相对位置变化时，需要重新进行本发明的联合标定算法，并对坐标转移矩阵进行更新。

S1.2：在接近毫米波雷达天线位置处选取一点作为雷达坐标系的原点，并根据雷达采集的数据坐标系方向确定雷达坐标系的方向，将两者的坐标系方向进行统一，得到雷达坐标系O_rx_ry_rz_r；在接近摄像头镜头处选取一点作为摄像头坐标系的原点，根据摄像头采集的数据坐标系方向确定摄像头坐标系的方向，将两者的坐标系方向进行统一，得到摄像头坐标系O_cx_cy_cz_c；

S1.3：基于Rxyz旋转方式及三维平移来定义坐标系转移参数l＝[θ_x，θ_y，θ_z，t_x，t_y，t_z]^T，上标T表示转置：l中的各个参数通过如下方式得到：

分别沿着及方向，量取O_c相对O_r的相对距离，得到t_x、t_y与t_z，移动雷达坐标系，将雷达坐标系原点与摄像头坐标系重合；旋转雷达坐标系的x_r轴使得其与x_c轴重合，量取旋转的角度，得到θ_x；在上述坐标系转移的基础上，进一步旋转雷达坐标系的y_r轴使得其与y_c轴重合，量取旋转的角度，得到θ_y；在上述坐标系转移的基础上，进一步旋转雷达坐标系的z_r轴使得其与z_c轴重合，量取旋转的角度，得到θ_z；

S2：打开摄像头进行数据采集，并采用相机内参标定方法标定相机的内参：

S2.1：设置摄像头的参数，并保持在联合标定过程中摄像头的焦距、分辨率不发生变化；

S2.2：连接摄像头至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)，其中f(x_p，y_p)表示在像素坐标[x_p，y_p]处的像素值；

S2.3：根据选取的摄像头类型，使用合适的相机内参标定方法，标定摄像头的内参矩阵

S3：同时打开毫米波雷达和摄像头，放置标定物进行数据采集：

S3.1：选取能够同时被毫米波雷达和摄像头检测到的物体作为标定物，打开毫米波雷达与摄像头，并连接至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)，使毫米波雷达能够按帧输出点云数据P(n)；其中，P(n)为三维点云数据[x，y，z]，n为雷达点索引；

S3.2：初始化联合标定用的数据组数i＝1，将标定物放置于能够同时被毫米波雷达与摄像头检测到的位置，并记录标定物位置；

S3.3：保持标定物的位置不发生改变，同时采集一段时间的毫米波雷达与摄像头数据，并存储放入第i组数据中；

S3.4：如果标定用的数据组数没有达到设定的组数上限I，即i＜I，令i＝i+1，并且改变标定物放置的位置，将标定物放置于能够同时被毫米波雷达与摄像头检测到的位置，并记录标定物位置，且保证该位置不接近第i组之前的数据采集时标定物的放置位置，返回S3.3；如果i≥I，则进入下一步。

S4：对采集的数据进行融合，并使用LM算法来实现坐标转移矩阵的联合标定：

S4.1：分别对毫米波雷达数据和摄像头数据进行处理，具体如下：

对于毫米波雷达数据，将每一组数据中的多帧数据叠加至一帧，并将雷达点云中的标定物点进行提取，并通过下式计算标定物点的坐标均值：

其中，x(n)、y(n)、z(n)表示当前融合数据组中的第n个标定物点雷达坐标，N_c为总的毫米波标定物点数；

对于摄像头数据，提取每一帧数据中标定物所在的像素坐标，将每一组数据中的多帧数据进行比较，如果存在两帧数据，标定物所在像素重合率不超过80％，则将该组数据舍弃，更新数据总组数I及数据组数索引；反之，则在该组数据中任意选取一帧的摄像头数据，并通过下式计算其中标定物所在的像素坐标均值

其中，x_p(m)、y_p(m)表示当前融合数据组中的第m个标定物点图像坐标，M_c为总的图像标定物点数；

S4.2：使用LM算法来进行坐标转移矩阵的联合标定；首先，构造优化函数L(l)＝L(θ_x，θ_y，θ_z，t_x，t_y，t_z)＝[L₁，L₁，...，L_2I]^T由2I个函数组成，并且满足如下关系：

L_2i-1＝x_p，i-z_i(1)/z_i(3) (1)

L_2i＝y_p，i-z_i(2)/z_i(3) (2)

其中，i＝1，2，...，I；z_i＝[z_i(1)，z_i(2)，z_i(3)]^T，并且z_i满足如下关系：

z_i＝Hx_i (3)

其中，x_i＝[x_i，y_i，z_i，1]^T；H＝AB为坐标转移矩阵；A为摄像头坐标系相对毫米波雷达坐标系的外参矩阵，A＝[R，t]，R为旋转矩阵，t为平移向量，有R＝R_zR_yR_x，t＝[t_x，t_y，t_z]^T，

S4.3：将S1.3中得到的量测数据作为迭代初值l₀；初始化迭代次数k＝1，并计算优化函数的雅可比矩阵根据联合标定的实际情况，调整并设定迭代次数上限K、下降梯度步长c及容忍系数σ；对迭代优化函数值进行初始化赋值F＝L(l₀)，对迭代优化函数雅可比矩阵进行初始化赋值J_F＝J(l₀)，对下降梯度步长进行初始化赋值D＝diag(J^H(l₀)J(l₀))，其中diag(·)表示提取方阵的对角元素形成新的方阵，J^H是J的共轭转置；

S4.4：当时，令LM参数λ＝0，计算变化量其中上标+表示矩阵的伪逆；当时，根据需求设定合适的LM参数λ，并通过求解如下的最小二乘问题得到变化量p：

S4.5：首先计算变化后的优化函数值F_p＝L(l_k-1+p)，然后通过下式计算线性度ρ，来评估下降步长：

S4.6：当ρ＞0.0001时，令l_k＝l_k-1+p；否则，令l_k＝l_k-1；更新迭代优化函数值F＝L(l_k)更新迭代优化函数雅可比矩阵J_F＝J(l_k)；

S4.7：当ρ≤0.25时，缩小下降梯度步长c；当0.25＜ρ＜0.75且λ＝0时，或者ρ≥0.75时，扩大下降梯度步长c；

S4.8：通过下式更新步长梯度：

其中，max{·，·}表示将两个同尺度矩阵中，相同位置的每个元素进行比较，并将较大值赋值给新矩阵的同一个位置。

S4.9：当k＜K时，令k＝k+1，返回S4.4；否则，进入下一步；

S4.10：根据式(3)，将l_k作为l赋值给H，得到坐标转移矩阵。

S5：选取合适场景及测试物采集测试数据，并将毫米波雷达数据使用标定后的坐标转移矩阵进行坐标转换，当测试物雷达点云在摄像头图片的投影点与摄像头拍摄的测试物图像中的点之间的像素距离不小于雷达分辨率时，重新调整S4的联合标定中的参数设定，重复S1-S5；反之，进入下一步；

S5.1：选取合适场景及测试物进行测试数据的采集，打开毫米波雷达与摄像头，并连接至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)。使毫米波雷达能够按帧输出点云数据P(n)；其中，P(n)为三维点云数据[x，y，z]，n为雷达点索引。

S5.2：将毫米波雷达的帧数据与摄像头帧数据进行时间同步，并提取时间同步帧的数据；

S5.3：从时间同步帧的毫米波数据中提取测试物的点云，并根据式(3)进行投影，当测试物雷达点云在摄像头图片的投影点与摄像头拍摄的测试物图像中的点之间的像素距离不小于雷达分辨率时，重新调整S4的联合标定中的迭代次数上限K、下降梯度步长c及容忍系数σ，重复S1-S5.3；反之，则进入下一步。

S6：输出S4得到的联合标定后的坐标转移矩阵。

优选地，所述的相机内参标定方法具体为张正友标定法。该标定法标定物较为简单，实验易于实现，标定精度高。

优选地，所述的S4.4中，当时，所述的LM参数λ根据Hebden迭代算法确定。

图2是本发明的联合标定方法坐标转移矩阵估计收敛性能图。图3是本发明的联合标定方法性能验证图，A点为经过联合标定后的投影点，B点为测量得到的毫米波雷达与摄像头的相对位置计算得到的坐标转移矩阵形成的投影点。从图2和图3可以看出，本发明的联合标定方法能够实现毫米波雷达与摄像头的联合标定，并具有较好的标定性能。

本领域普通技术人员可以理解，以上所述仅为发明的优选实例而已，并不用于限制发明，尽管参照前述实例对发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实例记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在发明的精神和原则之内，所做的修改、等同替换等均应包含在发明的保护范围之内。

Claims (8)

1.一种基于LM算法的毫米波雷达与摄像头联合标定方法，其特征在于，该方法包括以下步骤：

S1：固定毫米波雷达与摄像头的位置，并测量两者的相对位置关系；

S2：打开摄像头进行数据采集，并采用相机内参标定方法标定相机的内参；

S3：同时打开毫米波雷达和摄像头，放置标定物进行数据采集；

S4：对采集的数据进行融合，并使用LM算法来实现坐标转移矩阵的联合标定；

S5：选取合适场景及测试物采集测试数据，并将毫米波雷达数据使用标定后的坐标转移矩阵进行坐标转换，当测试物雷达点云在摄像头图片的投影点与摄像头拍摄的测试物图像中的点之间的像素距离不小于雷达分辨率时，重新调整S4的联合标定中的参数设定，重复S1-S5；反之，进入下一步；

S6：输出S4得到的联合标定后的坐标转移矩阵。

2.根据权利要求1所述的联合标定方法，其特征在于，所述S1具体为：

S1.1：固定毫米波雷达与摄像头的位置，保证毫米波雷达与摄像头之间的相对位置在后续的数据采集过程中不发生变化，包括三个自由度的平移及三个自由度的旋转；

S1.2：在接近毫米波雷达天线位置处选取一点作为雷达坐标系的原点，并根据雷达采集的数据坐标系方向确定雷达坐标系的方向，将两者的坐标系方向进行统一，得到雷达坐标系O_rx_ry_rz_r；在接近摄像头镜头处选取一点作为摄像头坐标系的原点，根据摄像头采集的数据坐标系方向确定摄像头坐标系的方向，将两者的坐标系方向进行统一，得到摄像头坐标系O_cx_cy_cz_c。

S1.3：基于Rxyz旋转方式及三维平移来定义坐标系转移参数l＝[θ_x，θ_y，θ_z，t_x，t_y，t_z]^T，上标T表示转置：l中的各个参数通过如下方式得到：

分别沿着及方向，量取O_c相对O_r的相对距离，得到t_x、t_y与t_z，移动雷达坐标系，将雷达坐标系原点与摄像头坐标系重合；旋转雷达坐标系的x_r轴使得其与x_c轴重合，量取旋转的角度，得到θ_x；在上述坐标系转移的基础上，进一步旋转雷达坐标系的y_r轴使得其与y_c轴重合，量取旋转的角度，得到θ_y；在上述坐标系转移的基础上，进一步旋转雷达坐标系的z_r轴使得其与z_c轴重合，量取旋转的角度，得到θ_z。

3.根据权利要求2所述的联合标定方法，其特征在于，所述S2具体为：

S2.1：设置摄像头的参数，并保持在联合标定过程中摄像头的焦距、分辨率不发生变化；

S2.2：连接摄像头至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)，其中f(x_p，y_p) 表示在像素坐标[x_p，y_p]处的像素值；

S2.3：根据选取的摄像头类型，使用合适的相机内参标定方法，标定摄像头的内参矩阵

4.根据权利要求3所述的联合标定方法，其特征在于，所述S3具体为：

S3.1：选取能够同时被毫米波雷达和摄像头检测到的物体作为标定物，打开毫米波雷达与摄像头，并连接至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)，使毫米波雷达能够按帧输出点云数据P(n)；其中，P(n)为三维点云数据[x，y，z]，n为雷达点索引；

S3.2：初始化联合标定用的数据组数i＝1，将标定物放置于能够同时被毫米波雷达与摄像头检测到的位置，并记录标定物位置；

S3.3：保持标定物的位置不发生改变，同时采集一段时间的毫米波雷达与摄像头数据，并存储放入第i组数据中；

S3.4：如果标定用的数据组数没有达到设定的组数上限I，即i＜I，令i＝i+1，并且改变标定物放置的位置，将标定物放置于能够同时被毫米波雷达与摄像头检测到的位置，并记录标定物位置，且保证该位置不接近第i组之前的数据采集时标定物的放置位置，返回S3.3；如果i≥I，则进入下一步。

5.根据权利要求4所述的联合标定方法，其特征在于，所述S4具体为：

S4.1：分别对毫米波雷达数据和摄像头数据进行处理，具体如下：

对于毫米波雷达数据，将每一组数据中的多帧数据叠加至一帧，并将雷达点云中的标定物点进行提取，并通过下式计算标定物点的坐标均值：

其中，x(n)、y(n)、z(n)表示当前融合数据组中的第n个标定物点雷达坐标，N_c为总的毫米波标定物点数；

对于摄像头数据，提取每一帧数据中标定物所在的像素坐标，将每一组数据中的多帧数据进行比较，如果存在两帧数据，标定物所在像素重合率不超过80％，则将该组数据舍弃，更新数据总组数I及数据组数索引；反之，则在该组数据中任意选取一帧的摄像头数据，并通过下式计算其中标定物所在的像素坐标均值

其中，x_p(m)、y_p(m)表示当前融合数据组中的第m个标定物点图像坐标，M_c为总的图像标定物点数；

S4.2：使用LM算法来进行坐标转移矩阵的联合标定；首先，构造优化函数L(l)＝L(θ_x，θ_y，θ_z，t_x，t_y，t_z)＝[L₁，L₁，...，L_2I]^T由2I个函数组成，并且满足如下关系：

L_2i-1＝x_p，i-z_i(1)/z_i(3) (1)

L_2i＝y_p，i-z_i(2)/z_i(3) (2)

其中，i＝1，2，...，I；z_i＝[z_i(1)，z_i(2)，z_i(3)]^T，并且z_i满足如下关系：

z_i＝Hx_i (3)

其中，x_i＝[x_i，y_i，z_i，1]^T；H＝AB为坐标转移矩阵；A为摄像头坐标系相对毫米波雷达坐标系的外参矩阵，A＝[R，t]，R为旋转矩阵，t为平移向量，有R＝R_zR_yR_x，t＝[t_x，t_y，t_z]^T，

S4.3：将S1.3中得到的量测数据作为迭代初值l₀；初始化迭代次数k＝1，并计算优化函数的雅可比矩阵根据联合标定的实际情况，设定迭代次数上限K、下降梯度步长c及容忍系数σ的初值；对迭代优化函数值进行初始化赋值F＝L(l₀)，对迭代优化函数雅可比矩阵进行初始化赋值J_F＝J(l₀)，对下降梯度步长进行初始化赋值D＝diag(J^H(l₀)J(l₀))，其中diag(·)表示提取方阵的对角元素形成新的方阵，J^H是J的共轭转置；

S4.4：当时，令LM参数λ＝0，计算变化量其中上标+表示矩阵的伪逆；当时，根据需求设定合适的LM参数λ，并通过求解如下的最小二乘问题得到变化量p：

S4.5：首先计算变化后的优化函数值F_p＝L(l_k-1+p)，然后通过下式计算线性度ρ，来评估下降步长：

S4.6：当ρ＞0.0001时，令l_k＝l_k-1+p；否则，令l_k＝l_k-1；更新迭代优化函数值F＝L(l_k)，更新迭代优化函数雅可比矩阵J_F＝J(l_k)；

S4.7：当ρ≤0.25时，缩小下降梯度步长c；当0.25＜ρ＜0.75且λ＝0时，或者ρ≥0.75时，扩大下降梯度步长c；

S4.8：通过下式更新步长梯度：

其中，max{·，·}表示将两个同尺度矩阵中，相同位置的每个元素进行比较，并将较大值赋值给新矩阵的同一个位置。

S4.9：当k＜K时，令k＝k+1，返回S4.4；否则，进入下一步；

S4.10：根据式(3)，将l_k作为l赋值给H，得到坐标转移矩阵。

6.根据权利要求5所述的基于LM算法的毫米波雷达与摄像头联合标定方法，其特征在于，所述的S5具体为：

S5.1：选取合适场景及测试物进行测试数据的采集，打开毫米波雷达与摄像头，并连接至数据处理器，使摄像头能够按帧输出图片数据f(x_p，y_p)。使毫米波雷达能够按帧输出点云数据P(n)；其中，P(n)为三维点云数据[x，y，z]，n为雷达点索引。

S5.2：将毫米波雷达的帧数据与摄像头帧数据进行时间同步，并提取时间同步帧上的数据；

S5.3：从时间同步帧的毫米波数据中提取测试物的点云，并根据式(3)进行投影，当测试物雷达点云在摄像头图片的投影点与摄像头拍摄的测试物图像中的点之间的像素距离不小于雷达分辨率时，重新调整S4的联合标定中的迭代次数上限K、下降梯度步长c及容忍系数σ，重复S1-S5.3；反之，则进入下一步。

7.根据权利要求1所述的基于LM算法的毫米波雷达与摄像头联合标定方法，其特征在于，所述的相机内参标定方法具体为张正友标定法。

8.根据权利要求5所述的基于LM算法的毫米波雷达与摄像头联合标定方法，其特征在于，所述的S4.4中，当时，所述的LM参数λ根据Hebden迭代算法确定。