空地双模的两栖无人机及其定位方法、装置、存储介质

本发明涉及无人机，特别公开了一种空地双模的两栖无人机及其定位方法、装置、存储介质。

背景技术：

1、空地双模的两栖无人机，是指同时具有地面移动以及空中飞行能力的无人机，其能够解决一些特定场景下的侦查任务。对于这类两栖无人机，其在不同的工作模式下的自定位，特别是在室内环境下的自定位尤为重要，目前常用的定位一般需要依靠视觉辅助设备来实现,如：

2、1.在所给定位标记的前提下，通过视觉算法设计视觉里程计解决两栖无人机定位问题；

3、2.通过安装天线容置腔、定位仪、天线、限位器和防护盒进行定位；

4、3.通过ai视觉辅助进行无人机定位：利用ai视觉观察室内环境实现无人机的自定位，视觉采集到图像后采用多种图像处理方法获取周围环境信息；

5、4.使用动捕系统直接进行精确定位，手持标定杆，在室内动捕相机覆盖范围内绕场，初始化动捕相机，确定坐标系原点，在动捕相机覆盖范围内放置至少四个待测无人机，并在待测无人机上安装用于动捕相机感知的基座；所述基座的几何中心与待测无人机定位测量设备的发射天线的几何中心重合；)开启动捕相机，通过动捕相机感知各基座的位置信息，并将位置信息传输给数据处理计算机，数据处理计算机将位置信息转化为动捕相机坐标系下的坐标；再开启待测无人机对动捕相机坐标系和自定位坐标系下的坐标进行最小二乘拟合实现最终的自定位。

6、此外，定位与地图构建（simultaneous localization and mapping，slam）技术也被广泛用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。它通过传感器数据的获取、处理和分析,实现机器人的位置估计和地图构建。常用的传感器有激光雷达、视觉相机、imu等。根据使用的传感器可以分为激光slam、视觉slam、多传感器融合slam。多传感器融合slam中主流的融合方案为激光-惯性slam（lio-slam）、视觉-惯性slam（vio-slam）。随着机器人应用场景的复杂化，基于单一传感器的slam算法由于传感器的局限性，已经无法满足复杂场景的应用。激光slam使用激光雷达能够获得准确的深度信息，视角大且不受光照影响，但在空旷和特征不明显的区域（广场、长走廊、隧道等）获得的三维特征信息有限，会影响特征匹配，降低精度。视觉slam相机通过图像提取特征点，但尺度不确定性，且受环境影响大，弱光或强光场景相机曝光不足或过度曝光，无法获取有效的图像信息，低纹理或少特征场景画面包含的信息量太少，无法准确估计相机运动。惯性传感器imu通过测得的加速度、角速度经积分变化能够获得自身位姿估计，近距离精度很高，但远距离测量时容易存在零漂和累积误差。激光雷达、视觉相机与imu有较为优良的互补性，imu与激光雷达融合可以对雷达运动产生的畸变点云进行矫正,提高两帧点云之间的特征匹配效率；imu与视觉相机融合可以为视觉相机提供尺度信息，同时能够提高在快速运动时的定位精度。

7、发明人在实施本发明的过程中发现，现有的两栖无人机在地面运动时，z轴（高度方向）的变动通常比较小，然而imu数据可能因为噪声和误差累积导致z轴漂移，而这个漂移很难通过其他方式进行修正，因此lio-slam仍然存在z轴漂移问题。同时kejian j. wu等人通过可观性分析，证明了地面移动平台在匀加速度运动下，vio系统的尺度会变得不可观，无旋转运动时会导致vio系统三个旋转都变为不可观，虽然提出了加入轮速计与视觉-惯性进行融合的方法，但该方法基于平面运动假设，轮速计通过差速仅能获得偏航角用于更新自身在二维平面上的姿态变换，只适用于工厂等地面平坦的场景，并不适用于崎岖路面或具有斜坡等三维场景。

技术实现思路

1、本发明提供了一种空地双模的两栖无人机及其定位方法、装置、存储介质，能至少部分的改善上述问题。

2、为实现上述目的，本发明采用以下技术方案：

3、一种空地双模的两栖无人机的定位方法，其包括：在地面模式下：

4、s101，通过设置在两栖无人机被动轮上的轮速计，获得被动轮的转动角度，通过两栖无人机上的陀螺仪获得两栖无人机的偏航角角速度、俯仰角角速度以及滚动角角速度；

5、s102，获取被动轮的转动角度的变化量，并采用所述俯仰角角速度对被动轮的转动角度的变化量进行补偿，计算得到被动轮的转动角速度；

6、s103，根据两栖无人机的偏航角角速度、俯仰角角速度、滚动角角速度计算所述两栖无人机从odom系到世界系的旋转矩阵；其中，odom系为由轮速计和陀螺仪构成的odom里程计所在的坐标系；

7、s104，基于从odom系到世界系的旋转矩阵以及所述转动角速度进行联合预积分处理，获得所述odom里程计的odom约束；

8、s105，以紧耦合的方式组合imu约束、odom约束、视觉重投影残差约束，根据最大后验估计构建最小二乘问题，并用非线性优化技术求解，得到两栖无人机在地面运动时的定位信息。

9、优选地，在步骤s102中，设两栖无人机具有左被动轮l和右被动轮r，则左被动轮l的转动角速度以及右被动轮r的转动角速度分别表示为：

10、

11、无人机的运动速度为：

12、

13、其中，

14、

15、为上一时刻轮速计获得的左被动轮l和右被动轮r的转动角度；为当前时刻轮速计获得的左被动轮l和右被动轮r的转动角度；为左被动轮l和右被动轮r的转动角度变化量；为被动轮与轮速计之间的传动比；为陀螺仪获得的俯仰角角速度，单位为弧度每秒，为被动轮的半径。

16、优选地，在步骤s103中，满足如下关系：

17、

18、

19、

20、其中，、、分别为偏航角角速度、俯仰角角速度以及滚动角角速度；、、分别为偏航角角速度、俯仰角角速度以及滚动角角速度的增量；、、分别为绕x轴、y轴、z轴旋转的矩阵；

21、为当前时刻imu系到前一时刻imu系的旋转矩阵；imu系为惯性导航单元所在的坐标系；

22、为前一时刻imu系到世界系的旋转矩阵；

23、为imu系到odom系的旋转矩阵，通过外参标定获得；odom系为odom里程计所在的坐标系；

24、为当前时刻odom系到世界系的旋转矩阵。

25、优选地，odom约束表示为第k帧和第k+1帧的预积分残差：

26、

27、其中，为第k帧从odom系到世界系的旋转矩阵；为第k帧无人机体在世界系下的位置向量；为第k帧从无人机体系到世界系的姿态变换；为第k帧陀螺仪的偏转；、为预积分值，分别代表了位置、姿态的变化量；代表四元数的矢量部分，表示四元数乘法，表示待优化的状态量，表示第k帧的odom，，，表示位置、姿态、陀螺仪bias的残差，表示第k帧到第k+1帧的odom测量值。

28、优选地，在步骤s105中，构建最小二乘问题的成本函数如下：

29、

30、其中，为边缘化获得的先验信息；

31、、、分别表示imu约束、odom约束、视觉重投影残差约束；为huber函数，，第k帧到第k+1帧的imu测量值，第 l个特征被第 j个图像观察到的视觉测量值，i表示imu所有测量值的集合，o表示odom所有测量值的集合，c表示当前滑窗中至少观察到两次的特征集合， l表示第 l个特征， j表示第 j个图像，、、表示相应测量值的噪声协方差矩阵，s表示huber函数的自变量，此处代指视觉重投影残差。

32、优选地，还包括：在空中模式下：

33、使用机载激光雷达和飞控内嵌的imu，经激光点云slam算法计算，得到两栖无人机在空中的定位信息。

34、本发明实施例还提供了一种空地双模的两栖无人机的定位装置，其包括：

35、参数获取单元，用于通过设置在两栖无人机被动轮上的轮速计，获得被动轮的转动角度，通过两栖无人机上的陀螺仪获得两栖无人机的偏航角角速度、俯仰角角速度以及滚动角角速度；

36、转动角速度计算单元，用于获取被动轮的转动角度的变化量，并采用所述俯仰角角速度对被动轮的转动角度的变化量进行补偿，计算得到被动轮的转动角速度；

37、姿态变换计算单元，用于根据两栖无人机的偏航角角速度、俯仰角角速度、滚动角角速度计算所述两栖无人机从odom系到世界系的旋转矩阵；其中，odom系为由轮速计和陀螺仪构成的odom里程计所在的坐标系；

38、odom约束计算单元，用于基于从odom系到世界系的旋转矩阵以及所述转动角速度进行联合预积分处理，获得所述odom里程计的odom约束；

39、定位信息计算单元，用于以紧耦合的方式组合imu约束、odom约束、视觉重投影残差约束，根据最大后验估计构建最小二乘问题，并用非线性优化技术求解，得到两栖无人机在地面运动时的定位信息。

40、本发明实施例还提供了一种空地双模的两栖无人机，包括机体以及设置在机体上的被动轮、轮速计、飞控单元、带有陀螺仪的imu、视觉里程计，所述轮速计安装在所述被动轮上，且所述轮速计、惯性导航单元、视觉里程计均与所述飞控单元电连接，所述飞控单元包括存储器以及处理器，所述存储器内存储有计算机程序，所述计算机程序能够被所述处理器执行，以实现如上述的定位方法。

41、本发明实施例还提供了一种计算机可读存储介质，其存储有计算机程序，所述计算机程序能够被所述计算机可读存储介质所在设备的处理器执行，以实现如上述的定位方法。

42、综上所述，本实施例通过轮速计能够直接计算出机体在地面上的移动速度和距离，这不受imu的影响，因此可以用来校正imu的漂移。此外，轮速计计算的z轴移动距离应该和imu积分的结果一致，如果有差距，则说明imu存在漂移，因此通过加入odom约束，对imu的漂移进行修正，可以改善imu数据因为噪声和误差累积导致的z轴漂移，从而得到两栖无人机在地面运动时的精准定位。