1.一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，该方法包括以下步骤：

1)对定位用的摄像头进行相机标定，获得该摄像头的参数；

2)在需要用到April Tag图像进行定位的载体平面上布置一个April Tag图，使用摄像头在其上方进行拍摄，不同载体上布置的April Tag图的类型、方向一致，ID号码不同；

3)对摄像头拍摄到的April Tag图像进行预处理；

4)分析预处理后的April Tag图，对图像进行标记；

5)对图像进行初步姿态解算，使用P4P算法处理拍摄的April Tag图的信息，得到正交化后的旋转矩阵和平移向量；

6)对平移向量以及正交化后的旋转矩阵使用LM算法进行迭代，获得新的旋转矩阵和平移向量；

7)构造三维坐标系，用以表示摄像头姿态，在图像上绘制一个箭头，并显示箭头与摄像头的偏转方向。

2.根据权利要求1所述的一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，所述步骤1)包括以下子步骤：

11)在平面上布置一张角点个数为6×4棋盘格的相机标定板；

12)在相机标定板的上方使用摄像头拍摄20张图像，存储在指定路径下，摄像头的拍摄高度为April Tag的定位高度；

13)构建棋盘格点在世界坐标系下的三维坐标矩阵以及其对应的二维坐标；

14)读取指定路径下拍摄的一系列棋盘格图像，对图像进行处理分析；

15)基于分析处理的信息，进行相机标定。

3.根据权利要求2所述的一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，步骤14)中，灰度图像的处理分析过程如下：设置角点检测的阈值标准；

通过cv2.cvtColor函数将图像转换为灰度图像；

循环查找灰度图像中的棋盘格角点，直至角点的数量满足要求；

基于灰度图像中的角点位置以及检测阈值，使用cv2.cornerSubPix函数精确细化角点位置；

获取灰度图像中检测到的各角点的二维坐标及其三维坐标，分别整合到数组中；

在灰度图像上绘制检测到的角点并调整灰度图像的尺寸；

步骤15)中，基于得到的三维坐标数组、二维坐标数组以及灰度图像的尺寸，使用cv2.calibrateCamera函数获取摄像头的内参数矩阵和畸变系数。

4.根据权利要求3所述的一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，所述步骤3)中，包括以下子步骤：

31)初始化April Tag检测器，通过pl.Detector函数指定待检测的April Tag种类为“tag36h11”；

32)使用cv2.VideoCapture函数打开摄像头，循环拍摄捕获摄像头视频画面；

33)对拍摄到的图像进行去畸变处理，去畸变的过程如下：获取拍摄图像的大小；

使用cv2.getOptimalNewCameraMatrix函数对摄像头内参数矩阵K和畸变系数dist进行处理，获得新的不畸变的相机参数Knew和尺寸roi；

通过K、dist和Knew，使用cv2.undistort函数对图像进行去畸变，得到去畸变的图像；

根据roi参数对去畸变的图像进行裁剪，得到最终的去畸变图像；

34)将去畸变的图像进行灰度化处理；

35)通过detector.detect函数识别检测灰度化图像中的April Tag图像。

5.根据权利要求4所述的一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，所述步骤4)中，包括以下子步骤：

41)获取April Tag的编码的ID号码tag_id，使用cv2.putText函数使其以指定字体和颜色显示在图像中指定位置；

42)获取April Tag图像的四个角点以及中心点的像素坐标，使用cv2.circle函数分别以这些点为圆心，绘制不同颜色的圆圈来标记各个角点；

43)使用cv2.line函数绘制绿色的直线连接两两相邻的角点，用以标记整个April Tag图。

6.根据权利要求5所述的一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，所述步骤5)中，包括以下子步骤：

51)根据布置的April Tag图的大小设置四个角点的三维坐标pst3d，获取角点的二维像素坐标pst2d；

52)基于pst3d、pst2d、相机参数K和畸变系数dist，获得3×3的旋转矩阵R和3×1的平移向量t，方法如下：April Tag四个角点的坐标pst3d为 i为角点个数，此处取i＝1,…,4；则有对应的像素坐标为Mi＝RXi+t；

图像中特征点坐标与像素坐标之间的关系记为 s为定义的一个未知像素坐标常量，则可以由上述式子得出 式中，ri为旋转矩阵R的第i列；

‑1 ‑1 H H ‑1

由于K由相机标定获得，则可得(r1  r2  t)＝K H＝sK UY (YY ) ，式中，式中，带上标H表示为该矩阵的共轭转置矩阵；

由于r3＝r1×r2，则可以根据外差公式求得r3，进而求得旋转矩阵R＝[r1 r2 r3]；

53)对旋转矩阵R进行施密特正交化以提高其正交性，方法如下：对旋转矩阵R＝[r1 r2 r3]进行正交化，则β1＝r1，再对其进行归一化，有 则得到正交化后的旋转矩阵R＝[γ1,γ2,γ3]。

7.根据权利要求6所述的一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，所述步骤6)中，包括以下子步骤：

61)设置收敛阈值tolerance为10‑6，最大迭代次数max_iters为100次，初始阻尼因子λ为0.01；

62)对旋转矩阵R、平移向量t、相机内参K、April Tag角点的三维坐标pst3d和二维坐标pst2d处理，获得Jacobi矩阵和残差err：Jacobi矩阵的定义为 其中，f为误差向量函数，t为平移向量；

又有 其中 为角点三维坐标pst3d在相机坐标系下的相机变化；

其中，ki,j是相机内参

数矩阵的第i行第j列的元素； 是角点的实际二维像素坐标；i与图像角点的数量有关，取值为i＝1,…,4；

又有 其中，I为单位矩阵，R为旋转矩阵，S为角点的三维坐标，t为平移向量；

′ ′ ′ ′ ′ ′

对于残差err的定义，有 其中，f2i‑1＝ziui‑zi ui，f2i＝zivi‑zivi，i＝1,…,4； K即相机内参数矩阵， 即由角点在平面上的坐标、R、t计算得到的投影像素坐标；

63)对Jacobi矩阵、阻尼因子λ和残差err做处理，计算获得参数增量Δθ：‑1 T T

Δθ＝(H+λI) J r，其中，J为Jacobi矩阵，λ即阻尼因子，r即残差，H由H＝J J计算获得，此处上标T为矩阵倒置；

64)由参数增量计算获得旋转矩阵的增量ΔR和平移向量的增量Δt，并以此更新获得新的参数Rnew和tnew；

由于 参数增量可写作 其中，ΔS、Δρ都是三维列向量，将′

参数增量映射为矩阵ΔT，与T相乘，得到更新后的矩阵T＝ΔTT，有令 则有

式中，a为长度为1的方向向量，a^为a的反

对称矩阵；

则可获得新的参数Rnew＝ΔR·R，tnew＝ΔR·t+V·ΔS；

65)一次迭代结束后，对本次迭代结果进行收敛判断；

66)若参数增量Δθ的值小于收敛阈值tolerance，则本次迭代结果收敛，结束迭代；

67)若不收敛，则使用Rnew和tnew计算新的残差errnew，与本次迭代之前的残差err比较，以此动态调整阻尼因子λ的值：若errnew的值大于err，则将λ增大10倍，反之则将λ调整为1/10，并使用Rnew和tnew作为下一次迭代的输入值，直到参数增量小于收敛阈值或迭代次数达到max_iters设置的次数。

8.根据权利要求7所述的一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，所述步骤7)中，构造三维坐标系的方法如下：

711)设定一组共三个3D点axis，存储三个点的三维坐标，用于表示坐标轴在三维空间中的坐标信息；

712)基于迭代后的旋转向量和平移向量以及摄像头的内参数矩阵和畸变系数，将三维坐标投影成二维像素坐标：通过cv2.Rodrigues函数将旋转矩阵Rnew转换回对应的旋转向量rnew，通过cv2.projectPoints函数对旋转向量rnew、平移向量tnew以及相机参数K和dist做处理，获得axis在平面上的投影坐标组imgpts；

713)选定April Tag的角点坐标pst2d中的第一个角点corner作为坐标系的原点，使用cv2.line函数分别用不同颜色的直线连接corner与imgpts中的三个投影点坐标，绘制出坐标系。

9.根据权利要求8所述的一种基于LM算法的April Tag定位及姿态解算方法，其特征在于，所述步骤7)中，在图像上绘制一个箭头的方法如下：

721)分别计算用以表示姿态坐标系的Y轴的投影点和原点的横坐标差值与纵坐标差值：选择坐标系中绘制Y轴的投影点坐标imgpts[1]，计算dx＝imgpts[1][0]‑corner[0]，dy＝imgpts[1][1]‑corner[1]；其中，imgpts[1][0]和imgpts[1][1]分别为该投影点的x坐标值和y坐标值；corner[0]和corner[1]分别为原点的x坐标值和y坐标值，dx和dy分别为横坐标差值与纵坐标差值；

722)基于两个坐标差值，通过反正切函数计算偏转角度，并将弧度转换为角度，若横坐标差值为零时，判断角度为90°或270°：使用math.atan2函数计算计算偏转角度， 计算数值，并使用math.degrees函数将弧度转为角度angle；

723)以April Tag的中心为起点，绘制一条与摄像头拍摄画面的正方向同向的箭头，将偏转角度显示在图像中箭头的附近：计算两相邻角点的横坐标差值Δx和纵坐标差值Δy,使用cv2.arrowedLine从April Tag图的中心坐标(cX,cY)处出发，到(cX+Δx，cY+Δy)终止，绘制一个箭头，并将偏转角度angle显示在箭头附近。