[发明专利]一种融合惯导测量和ArUco标记的定位与导航方法

权利要求书

1.一种融合惯导测量和ArUco标记的定位与导航方法，其特征在于：采用惯导元件和相机，包括以下步骤：

步骤1：建立ArUco地图；

步骤2：根据任务需要建立路径，路径由一系列坐标点表示，并设定任务目标点P_set；

步骤3：固定惯导元件和相机的相对位姿，计算得到惯导元件坐标系与相机坐标系之间的旋转关系；

步骤4：惯导元件开始解算姿态；

步骤5：机器人按固定的角速度旋转，直到相机观测到ArUco标记，并成功获得相机在世界坐标系下的姿态，设定当前时刻为k时刻，记录当前相机在世界坐标系中的姿态和惯导元件的自解算姿态；

步骤6：根据步骤5所得到的相机在世界坐标系中的姿态和惯导元件的自解算姿态，计算惯导元件在世界坐标系中的姿态；

步骤7：设定下一时刻为k+1时刻，根据步骤6所得到的惯导元件在世界坐标系中的姿态，计算得到k+1时刻惯导元件在世界坐标系中的姿态；

步骤8：根据步骤7所得到的k+1时刻惯导元件在世界坐标系中的姿态，计算得到k+1时刻相机在世界坐标系中的姿态；

步骤9：通过ArUco标记得到机器人在世界坐标系下的位置P_current，并计算得到设定路径上距离机器人最近的一点P_nearest，P_nearest沿设定路径向P_set方向的第n个点记为P_aim；

步骤10：计算P_nearest到P_aim的向量V_aim，V_aim即为机器人的方向向量；

步骤11：根据t+Δt时刻机器人的姿态向量V_robot和方向向量V_aim，计算机器人的偏航角α和俯仰角β；根据α和β的大小，控制机器人的运动角速度，根据P_aim和设定点P_set之间的距离控制机器人的运动线速度；

步骤12：判断器人的当前位置P_current是否等于P_set，若不相等则跳转步骤7，若相等则结束导航；

在步骤3中；

惯导元件和相机采用正交固定的方法，能够直接推导出相机坐标系和惯导坐标系之间的旋转关系；

惯导元件和相机采用联合标定的方法，能够标定出惯导元件和相机之间的具体坐标关系；

在步骤4中，

惯导元件开始解算的姿态的四元数初值一般设定为[1000]；

在步骤5中，相机通过识别ArUco标记获得在世界坐标系下的姿态后，取一段时间内的姿态平均数据，以增加定位精度；

在步骤6中，根据公式(1)，计算惯导元件在世界坐标系中的姿态：

式中，q为四元数，为四元数表示旋转时的运算，k表示相机通过识别ArUco标记获得在世界坐标系下的姿态的时刻，/为惯导元件在初始时刻即0时刻在世界坐标系中的姿态，/为k时刻相机在世界坐标系中的姿态，/为k时刻相机相对惯导元件0时刻坐标系的旋转；

根据公式(2)计算

式中，为惯导元件k时刻的解算姿态，/为惯导坐标系和相机坐标系之间的旋转关系。

2.根据权利要求1所述的融合惯导测量和ArUco标记的定位与导航方法，其特征在于：在步骤7中，根据公式(3)，计算k+1时刻惯导元件在世界坐标系中的姿态：

式中为k+1时刻惯导元件在世界坐标系中的姿态，/为惯导元件在k+1时刻的解算姿态。

3.根据权利要求1所述的融合惯导测量和ArUco标记的定位与导航方法，其特征在于：在步骤8中，根据公式(4)，计算k+1时刻相机在世界坐标系中的姿态：

式中为k+1时刻相机在世界坐标系中的姿态，此时/的计算不再依赖k+1时刻的ArUco标记的识别结果。/