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

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

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

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

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

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

步骤5）包括以下子步骤：

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

52）基于 、 、相机参数K和畸变系数 ，获得3×3的旋转矩阵 和3×1的平移向量 ，方法如下：April Tag四个角点的坐标 为 ，为角点个数，此处取 则有对应的像素坐标为 ；

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

由于K由相机标定获得，则可得 ，式中，， ，式中，带上标H表示为该矩阵的共轭转置矩阵；

由于 ，则根据外差公式求得r3，进而求得旋转矩阵 ；

53）对旋转矩阵 进行施密特正交化以提高其正交性，方法如下：对旋转矩阵 进行正交化，则 ， ，；

再对其进行归一化，有 ，则得到正交化后的旋转矩阵；

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）中，灰度图像的处理分析过程如下：设置角点检测的阈值标准；

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

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

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

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

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

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

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

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

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

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

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

通过 、 和 ，使用 函数对图像进行去畸变，得到去畸变的图像；

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

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

35）通过 函数识别检测灰度化图像中的April Tag图像。

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

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

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

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

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

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

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

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

， ，其中， 是相机内参

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

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

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

63）对 矩阵、阻尼因子 和残差 做处理，计算获得参数增量 ：，其中，J为Jacobi矩阵，λ即阻尼因子，r即残差，H由 计算获得，此处上标T为矩阵倒置；

64）由参数增量计算获得旋转矩阵的增量 和平移向量的增量 ，并以此更新获得新的参数 和 ；

由于 ，参数增量可写作 ，其中， 都是三维列向量，将参数增量映射为矩阵 ，与T相乘，得到更新后的矩阵 ，有 ；

令 ，则有 ，

，式中，为长度为1的方向向量， 为 的反对称矩阵；

则可获得新的参数 ， ；

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

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

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

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

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

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

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

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

721）分别计算用以表示姿态坐标系的Y轴的投影点和原点的横坐标差值与纵坐标差值：选择坐标系中绘制Y轴的投影点坐标 ，计算， ；其中，

和 分别为该投影点的x坐标值和y坐标值； 和分别为原点的x坐标值和y坐标值， 和 分别为横坐标差值与纵坐标差值；

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

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