本发明涉及机器人，尤其涉及一种四足机器人轨迹生成方法、装置、设备及存储介质。
背景技术：
1、随着机器人技术产业的发展，计算机视觉、传感器技术、人工智能等领域实现了非常大的突破，slam实时定位与建图技术已经成为机器人自主导航、虚拟现实、增强现实等领域的核心技术之一，同时越来越多的机器人在家庭、工业、医疗、军事等领域发光发热。
2、机器人的定位与建图任务是通过其所搭载的传感器对环境的观测信息进而来进行运动状态估计。通常所使用的传感器都是以一定的频率进行信息采样的，因此其需要完成的任务就是用传感器所得到的信息对两次传感器采样时间内的位置姿态变换进行估计。
3、为了满足机器人在不同环境中的定位精度、有效性与鲁棒性，现在主流的方式是使用多传感器信息融合技术来实现更好的定位与建图功能，其中现有的方案里面对激光雷达、摄像头与imu的使用最为广泛，lic(lidar-inertial-camera)定位建图系统因其高精度、强鲁棒的定位效果引起了极大的关注，通过融合三种传感器的信息实现信息融合能够使得lic系统拥有更高的场景适用性。
4、然后，现有的大部分机器人定位与建图系统都是基于离散时间轨迹来做的，这是由于大部分的机器人载体都是使用轮式底盘或者无人机飞行状态，这两种状态下的机器人在平地或者飞行的时候其轨迹都是平滑的曲线，因此使用离散的位姿状态点来描述这个运动过程不会有特别大的影响。但是四足机器人由于其依靠四足仿生运动，即便是在平地上行走，质心运动也是一条弯曲波浪形态的轨迹曲线，使用离散的位姿点对其进行描述的方式会损失许多的轨迹形状信息。在传统的离散时间轨迹描述方法中通常会损失许多运动轨迹信息，在进行多传感器定位信息融合的过程中不可避免地要进行时间戳对齐，从而引入更多的误差值。
技术实现思路
1、本发明提供了一种四足机器人轨迹生成方法、装置、设备及存储介质，用于解决现有的四足机器人轨迹生成方式误差较大的技术问题。
2、本发明提供了一种四足机器人轨迹生成方法，所述四足机器人配置有激光雷达和相机；所述方法包括：
3、采集四足机器人的imu数据；
4、采用所述imu数据生成初始路径；
5、通过所述激光雷达采集连续多帧点云数据；
6、采用所述点云数据进行位姿估算，得到雷达位姿信息；
7、通过所述相机采集连续多帧图像数据；
8、采用所述图像数据进行位姿估算，得到相机位姿信息；
9、采用所述雷达位姿信息、所述相机位姿信息优化所述初始路径，生成所述四足机器人的连续时间轨迹。
10、可选地，所述采用所述imu数据生成初始路径的步骤，包括：
11、依次以所述imu数据中的三个连续位姿信息构建三角形；
12、当所述三角形的面积小于预设阈值时，采用线性插值法对三个所述连续位姿信息进行线性拟合，得到第一拟合线段；
13、当所述三角形的面积不小于所述预设阈值时，采用b样条曲线法对三个所述连续位姿信息进行线性拟合，得到第二拟合线段；
14、连接所有所述第一拟合线段和所有所述第二拟合线段，生成所述四足机器人的初始路径。
15、可选地，所述采用所述点云数据进行位姿估算，得到雷达位姿信息的步骤，包括：
16、将所述点云数据进行去畸变，得到预处理点云数据；
17、对所述预处理点云数据中的每个激光点进行曲率计算，得到各所述激光点的曲率；
18、根据所述曲率计算所述激光点相邻两帧点云数据的特征约束；
19、根据所述特征约束进行位姿优化，得到当前帧点云数据的雷达位姿信息。
20、可选地，所述采用所述点云数据和所述图像数据进行位姿估算，得到相机位姿信息的步骤，包括：
21、提取所述图像数据的特征点；
22、根据所述雷达位姿信息生成所述特征点的深度信息；
23、根据所述特征点及所述特征点的深度信息对所述图像数据进行光流跟踪，得到相机位姿信息。
24、可选地，所述采用所述雷达位姿信息、所述相机位姿信息和所述imu数据构建所述四足机器人的连续时间轨迹的步骤，包括：
25、根据所述雷达位姿信息计算雷达因子；
26、根据所述相机位姿信息计算相机因子；
27、计算imu数据的imu因子和偏置因子；
28、采用所述雷达因子、所述相机因子、所述imu因子和所述偏置因子，通过最小二乘法对所述四足机器人的初始路径进行滑窗优化，生成四足机器人的连续时间轨迹。
29、本发明还提供了一种四足机器人轨迹生成装置，所述四足机器人配置有激光雷达和相机；所述装置包括：
30、imu数据采集模块，用于采集四足机器人的imu数据；
31、初始路径生成模块，用于采用所述imu数据生成初始路径；
32、点云数据采集模块，用于通过所述激光雷达采集连续多帧点云数据；
33、雷达位姿信息获取模块，用于采用所述点云数据进行位姿估算，得到雷达位姿信息；
34、图像数据采集模块，用于通过所述相机采集连续多帧图像数据；
35、相机位姿信息获取模块，用于采用所述图像数据进行位姿估算，得到相机位姿信息；
36、连续时间轨迹生成模块，用于采用所述雷达位姿信息、所述相机位姿信息优化所述初始路径，生成所述四足机器人的连续时间轨迹。
37、可选地，所述初始路径生成模块，包括：
38、三角形构建子模块，用于依次以所述imu数据中的三个连续位姿信息构建三角形；
39、第一拟合线段生成子模块，用于当所述三角形的面积小于预设阈值时，采用线性插值法对三个所述连续位姿信息进行线性拟合，得到第一拟合线段；
40、第二拟合线段生成子模块，用于当所述三角形的面积不小于所述预设阈值时，采用b样条曲线法对三个所述连续位姿信息进行线性拟合，得到第二拟合线段；
41、初始路径生成子模块，用于连接所有所述第一拟合线段和所有所述第二拟合线段，生成所述四足机器人的初始路径。
42、可选地，所述雷达位姿信息获取模块，包括：
43、预处理子模块，用于将所述点云数据进行去畸变，得到预处理点云数据；
44、曲率计算子模块，用于对所述预处理点云数据中的每个激光点进行曲率计算，得到各所述激光点的曲率；
45、特征约束计算子模块，用于根据所述曲率计算所述激光点相邻两帧点云数据的特征约束；
46、雷达位姿信息获取子模块，用于根据所述特征约束进行位姿优化，得到当前帧点云数据的雷达位姿信息。
47、本发明还提供了一种电子设备，所述设备包括处理器以及存储器：
48、所述存储器用于存储程序代码，并将所述程序代码传输给所述处理器；
49、所述处理器用于根据所述程序代码中的指令执行如上任一项所述的四足机器人轨迹生成方法。
50、本发明还提供了一种计算机可读存储介质，所述计算机可读存储介质用于存储程序代码，所述程序代码用于执行如上任一项所述的四足机器人轨迹生成方法。
51、从以上技术方案可以看出，本发明具有以下优点：本发明提供了一种四足机器人轨迹生成方法，具体包括：采集四足机器人的imu数据；采用imu数据生成初始路径；通过激光雷达采集连续多帧点云数据；通过相机采集连续多帧图像数据；采用点云数据进行位姿估算，得到雷达位姿信息；采用图像数据进行位姿估算，得到相机位姿信息；采用雷达位姿信息、相机位姿信息优化初始路径，生成四足机器人的连续时间轨迹。本发明通过imu数据生成随时间变化的初始路径，然后分别采用激光雷达采集的点云数据和相机采集的图像数据生成雷达位姿信息和相机位姿信息。通过雷达位姿信息和相机位姿信息对初始路径进行优化，得到四足机器人随时间变化的连续时间轨迹，相对于离散型的轨迹生成方法，具有更高的定位精度和轨迹准确性。
1.一种四足机器人轨迹生成方法，其特征在于，所述四足机器人配置有激光雷达和相机；所述方法包括：
2.根据权利要求1所述的方法，其特征在于，所述采用所述imu数据生成初始路径的步骤，包括：
3.根据权利要求1所述的方法，其特征在于，所述采用所述点云数据进行位姿估算，得到雷达位姿信息的步骤，包括：
4.根据权利要求1所述的方法，其特征在于，所述采用所述点云数据和所述图像数据进行位姿估算，得到相机位姿信息的步骤，包括：
5.根据权利要求1所述的方法，其特征在于，所述采用所述雷达位姿信息、所述相机位姿信息和所述imu数据构建所述四足机器人的连续时间轨迹的步骤，包括：
6.一种四足机器人轨迹生成装置，其特征在于，所述四足机器人配置有激光雷达和相机；所述装置包括：
7.根据权利要求6所述的装置，其特征在于，所述初始路径生成模块，包括：
8.根据权利要求6所述的装置，其特征在于，所述雷达位姿信息获取模块，包括：
9.一种电子设备，其特征在于，所述设备包括处理器以及存储器：
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于存储程序代码，所述程序代码用于执行权利要求1-5任一项所述的四足机器人轨迹生成方法。
