CN117213475A - 用于双足机器人的数据处理方法、装置及存储介质 - Google Patents

用于双足机器人的数据处理方法、装置及存储介质 Download PDF

Info

Publication number
CN117213475A
CN117213475A CN202311013119.8A CN202311013119A CN117213475A CN 117213475 A CN117213475 A CN 117213475A CN 202311013119 A CN202311013119 A CN 202311013119A CN 117213475 A CN117213475 A CN 117213475A
Authority
CN
China
Prior art keywords
data
moment
pose
camera
kinematic
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202311013119.8A
Other languages
English (en)
Inventor
金子昕
张金霖
甘春标
迟万超
张冲
张晟浩
刘宇真
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang University ZJU
Shenzhen Tencent Computer Systems Co Ltd
Original Assignee
Zhejiang University ZJU
Shenzhen Tencent Computer Systems Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhejiang University ZJU, Shenzhen Tencent Computer Systems Co Ltd filed Critical Zhejiang University ZJU
Priority to CN202311013119.8A priority Critical patent/CN117213475A/zh
Publication of CN117213475A publication Critical patent/CN117213475A/zh
Pending legal-status Critical Current

Links

Landscapes

  • Manipulator (AREA)

Abstract

本申请公开了一种用于双足机器人的数据处理方法、装置及存储介质，先获取双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据，并获取相机拍摄得到的上一时刻图像和当前时刻图像；接着，获取相机在当前时刻的初始位姿；然后，根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，再根据运动学残差、惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿。本申请实施例能够降低图像运动模糊的影响，提高位姿估计的鲁棒性，从而能够得到更为准确的位姿信息。

Description

用于双足机器人的数据处理方法、装置及存储介质
技术领域
本申请涉及人工智能技术领域，尤其涉及一种用于双足机器人的数据处理方法、装置及存储介质。
背景技术
基于视觉的同步定位与地图构建(Visual Simultaneous Localization AndMapping，VSLAM)在移动机器人执行任务过程中发挥着举重若轻的作用，如移动机器人通过图像传感器识别障碍物的相对位置和方向，防止本体与物体接触碰撞；通过感知且估计本体所在空间位姿信息，以保证移动机器人构建的地图与场景环境的全局一致性。
目前，由于双足机器人的图像传感器设置于头部，且双足机器人的双足运动模式容易导致图像采集过程中出现运动模糊，从而降低图像匹配效果，同时降低VSLAM过程中识别的稳定性，干扰位姿估计，导致双足机器人的空间位姿信息的准确性差。
发明内容
以下是对本文详细描述的主题的概述。本概述并非是为了限制权利要求的保护范围。
本申请实施例提供了一种用于双足机器人的数据处理方法、装置及存储介质，能够降低图像运动模糊的影响，提高位姿估计的鲁棒性，从而能够得到更为准确的位姿信息。
一方面，本申请实施例提供了一种用于双足机器人的数据处理方法，所述双足机器人的头部位置设置有相机，所述方法包括以下步骤：
获取所述双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据；
获取所述相机拍摄得到的上一时刻图像和当前时刻图像；
获取所述相机在当前时刻的初始位姿；
根据所述上一时刻运动学数据、所述当前时刻运动学数据和所述初始位姿，计算得到所述相机的运动学残差；
根据所述运动学残差、所述惯性测量数据、所述上一时刻图像和所述当前时刻图像，计算得到所述相机在当前时刻的目标位姿。
另一方面，本申请实施例还提供了一种用于双足机器人的数据处理装置，所述双足机器人的头部位置设置有相机，所述数据处理装置包括：
数据获取单元，用于获取所述双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据；
图像获取单元，用于获取所述相机拍摄得到的上一时刻图像和当前时刻图像；
位姿获取单元，用于获取所述相机在当前时刻的初始位姿；
残差计算单元，用于根据所述上一时刻运动学数据、所述当前时刻运动学数据和所述初始位姿，计算得到所述相机的运动学残差；
位姿计算单元，用于根据所述运动学残差、所述惯性测量数据、所述上一时刻图像和所述当前时刻图像，计算得到所述相机在当前时刻的目标位姿。
可选地，所述残差计算单元还用于：
根据所述上一时刻运动学数据和所述当前时刻运动学数据，计算得到所述相机的运动增量数据；
根据所述运动增量数据和所述初始位姿，计算得到所述相机的运动学残差。
可选地，所述残差计算单元还用于：
获取所述相机在上一时刻的历史位姿；
根据所述历史位姿和所述初始位姿计算得到所述相机的位姿变化量；
根据所述运动增量数据和所述位姿变化量，计算得到所述相机的运动学残差。
可选地，所述残差计算单元还用于：
将所述位移增量数据和所述位移变化数据进行差值计算，得到位移变化值；
根据所述旋转增量数据和所述旋转变化数据进行对数映射，得到姿态变化值；
根据所述位移变化值和所述姿态变化值，构建得到所述相机的运动学残差。
可选地，所述残差计算单元还用于：
根据所述上一时刻足部受力数据确定上一时刻支撑足，并根据所述当前时刻足部受力数据确定当前时刻支撑足；
根据从所述上一时刻支撑足至所述双足机器人的腰部的第一运动链，对所述上一时刻下肢运动数据进行数据转换，得到上一时刻下肢转换数据；
根据从所述当前时刻支撑足至所述双足机器人的腰部的第二运动链，对所述当前时刻下肢运动数据进行数据转换，得到当前时刻下肢转换数据；
根据从所述双足机器人的腰部至所述相机的第三运动链，对所述上一时刻躯干运动数据和所述当前时刻躯干运动数据分别进行数据转换，得到上一时刻躯干转换数据和当前时刻躯干转换数据；
根据所述上一时刻下肢转换数据、所述当前时刻下肢转换数据、所述上一时刻躯干转换数据和所述当前时刻躯干转换数据，计算得到所述相机的运动增量数据。
可选地，所述残差计算单元还用于：
根据所述上一时刻支撑足和所述当前时刻支撑足数据确定足部位姿变换数据；
根据所述上一时刻下肢转换数据和所述上一时刻躯干转换数据进行矩阵转置，得到上一时刻位姿转置数据；
将所述双足位姿变换数据、所述上一时刻位姿转置数据、所述当前时刻下肢转换数据和所述当前时刻躯干转换数据进行相乘，得到所述相机的运动增量数据。
可选地，所述位姿获取单元还用于：
对所述惯性测量数据进行积分处理，得到腰部初始位姿数据；
根据从所述双足机器人的腰部至所述相机的第四运动链，对所述当前时刻躯干运动数据进行数据转换，得到躯干初始转换数据；
根据所述腰部初始位姿数据和所述躯干初始转换数据，计算得到所述相机在当前时刻的初始位姿。
可选地，所述位姿获取单元还用于：
对所述当前时刻下肢运动数据和所述当前时刻躯干运动数据进行位姿预测，得到所述相机在当前时刻的初始位姿。
可选地，所述位姿计算单元还用于：
对所述惯性测量数据进行预积分处理，得到惯性数据残差；
将所述上一时刻图像和所述当前时刻图像进行特征提取和特征匹配处理，得到视觉重投影误差；
根据所述运动学残差、所述惯性数据残差和所述视觉重投影误差，计算得到所述相机在当前时刻的目标位姿。
可选地，所述用于双足机器人的数据处理装置还包括：
二次残差计算单元，用于若所述当前时刻图像被确认为关键图像，根据所述上一时刻运动学数据、所述当前时刻运动学数据和所述目标位姿，计算得到所述相机的运动学二次残差。
历史信息获取单元，用于获取历史关键图像和所述历史关键图像对应时刻的历史惯性测量数据；
数据局部优化单元，用于根据所述运动学二次残差、所述惯性测量数据、所述历史惯性测量数据、所述历史关键图像和所述当前时刻图像，对所述目标位姿以及所述当前时刻图像对应的地图点进行优化处理，得到优化后的关键位姿和关键地图点。
可选地，所述二次残差计算单元还用于：
获取所述历史关键图像对应时刻的历史关键位姿，根据所述历史关键位姿和所述目标位姿计算得到所述相机的关键位姿变换量；
根据所述上一时刻运动学数据和所述当前时刻运动学数据，计算得到所述相机的关键运动增量数据；
根据所述关键运动增量数据和所述关键位姿变换量，计算得到所述相机的运动学二次残差。
另一方面，本申请实施例还提供了一种用于双足机器人的数据处理装置，包括：
至少一个处理器；
至少一个存储器，用于存储至少一个程序；
当至少一个所述程序被至少一个所述处理器执行时实现如前面所述的用于双足机器人的数据处理方法。
另一方面，本申请实施例还提供了一种计算机可读存储介质，其中存储有处理器可执行的计算机程序，所述处理器可执行的计算机程序被处理器执行时用于实现如前面所述的用于双足机器人的数据处理方法。
另一方面，本申请实施例还提供了一种计算机程序产品，包括计算机程序或计算机指令，所述计算机程序或所述计算机指令存储在计算机可读存储介质中，用于双足机器人的数据处理装置的处理器从所述计算机可读存储介质读取所述计算机程序或所述计算机指令，所述处理器执行所述计算机程序或所述计算机指令，使得所述用于双足机器人的数据处理装置执行如前面所述的用于双足机器人的数据处理方法。
本申请实施例至少包括以下有益效果：在获取得到双足机器人上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据以及当前时刻的初始位姿后，可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，由于双足机器人的双足步态具有相应的摆动轨迹以及足部撞击地面而产生的冲击力，容易造成图像运动模糊，因此，通过上一时刻与当前时刻双足机器人运动状态的变化，确定出相机位姿在由运动学数据计算得出的数据(即实际观测变换量)与初始位姿计算得出的数据(即估计变换量)之间的偏差，从而可以有助于校正相机位姿，降低图像运动模糊的影响。另外，还可以获取相机拍摄得到的上一时刻图像和当前时刻图像，然后可以根据运动学残差、上一时刻至当前时刻的惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿，通过结合惯性测量数据和多个时刻的运动学数据之间的运动关系，以及上一时刻图像与当前时刻图像之间的视觉投影关系，对初始位姿进行优化调整，能够实现多源信息的紧耦合，从而能够提高位姿估计的鲁棒性，得到更为准确的双足机器人的位姿数据。
本申请的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本申请而了解。本申请的目的和其他优点可通过在说明书以及附图中所特别指出的结构来实现和获得。
附图说明
附图用来提供对本申请技术方案的进一步理解，并且构成说明书的一部分，与本申请的实施例一起用于解释本申请的技术方案，并不构成对本申请技术方案的限制。
图1是本申请实施例提供的一种实施环境的示意图；
图2是本申请实施例提供的另一种实施环境的示意图；
图3是本申请实施例提供的一种用于双足机器人的数据处理方法的流程图；
图4是本申请实施例提供的双足机器人的结构示意图；
图5是本申请实施例提供的初始位姿计算方式的示意图；
图6是本申请实施例提供的运动状态预测的示意图；
图7是本申请实施例提供的运动增量数据计算方式的示意图；
图8是本申请实施例提供的计算运动学残差的示意图；
图9是相关技术中的一种数据处理方法的流程示意图；
图10是本申请实施例提供的位姿变化的示意图；
图11是本申请实施例提供的双足机器人在不同支撑状态的示意图；
图12是本申请实施例提供的双足机器人的运动学数据解算的示意图；
图13是本申请实施例提供的双足机器人的运动坐标系的转换关系的示意图；
图14是本申请实施例提供的用于双足机器人的数据处理方法的实际应用场景的示意图；
图15是本申请一个具体例子提供的用于双足机器人的数据处理方法的整体流程图；
图16是本申请实施例提供的一种用于双足机器人的数据处理装置的示意图；
图17是本申请实施例提供的另一种用于双足机器人的数据处理装置的示意图。
具体实施方式
下面结合说明书附图和具体的实施例对本申请进行进一步的说明。所描述的实施例不应视为对本申请的限制，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在以下的描述中，涉及到“一些实施例”，其描述了所有可能实施例的子集，但是可以理解，“一些实施例”可以是所有可能实施例的相同子集或不同子集，并且可以在不冲突的情况下相互结合。
除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中所使用的术语只是为了描述本申请实施例的目的，不是旨在限制本申请。
对本申请实施例进行进一步详细说明之前，对本申请实施例中涉及的名词和术语进行说明，本申请实施例中涉及的名词和术语适用于如下的解释。
1)计算机视觉技术(Computer Vision，CV)，计算机视觉是一门研究如何使机器“看”的科学，更进一步的说，就是指用摄影机和电脑代替人眼对目标进行识别和测量等机器视觉，并进一步做图形处理，使电脑处理成为更适合人眼观察或传送给仪器检测的图像。作为一个科学学科，计算机视觉研究相关的理论和技术，试图建立能够从图像或者多维数据中获取信息的人工智能***。计算机视觉技术通常包括图像处理、图像识别、图像语义理解、图像检索、OCR、视频处理、视频语义理解、视频内容/行为识别、三维物体重建、3D技术、虚拟现实、增强现实、同步定位与地图构建等技术，还包括常见的人脸识别、指纹识别等生物特征识别技术。
2)人工智能(Artificial Intelligence，AI)是利用数字计算机或者数字计算机控制的机器模拟、延伸和扩展人的智能，感知环境、获取知识并使用知识获得最佳结果的理论、方法、技术及应用***。换句话说，人工智能是计算机科学的一个综合技术，它企图了解智能的实质，并生产出一种新的能以人类智能相似的方式做出反应的智能机器。人工智能也就是研究各种智能机器的设计原理与实现方法，使机器具有感知、推理与决策的功能。人工智能技术是一门综合学科，涉及领域广泛，既有硬件层面的技术也有软件层面的技术。人工智能基础技术一般包括如传感器、专用人工智能芯片、云计算、分布式存储、大数据处理技术、预训练模型技术、操作/交互***、机电一体化等。其中，预训练模型又称大模型、基础模型，经过微调后可以广泛应用于人工智能各大方向下游任务。人工智能软件技术主要包括计算机视觉技术、语音处理技术、自然语言处理技术以及机器学习/深度学习等几大方向。
3)基于视觉的同步定位与建图(Visual Simulatneous Localization AndMapping，VSLAM)是指运动物体根据传感器的信息，其中包括相机等视觉传感器，通过获取环境图像信息，利用图像处理计算自身位置并构建环境地图的过程，解决机器人等在未知环境下运动时的定位与地图构建问题。VSLAM可应用于机器人，其用途包括传感器自身的定位，以及后续的路径规划、运动性能、场景理解。
4)位姿，是描述某个对象(如坐标)在指定坐标系下的位置和姿态，如常用位姿来描述机器人在空间坐标系的位置与姿态；其中，位置是指刚体在空间里的定位，刚体的位置可以用一个3×1的矩阵来表示，即刚体坐标系中心在基坐标系中的位置；而姿态是指刚体在空间里的朝向，刚体的姿态可以用一个3×3的矩阵来表示，即刚体坐标系在基坐标系中的姿态。
目前，基于视觉的同步定位与地图构建在移动机器人执行任务过程中发挥着举重若轻的作用，如移动机器人通过视觉传感器识别障碍物的相对位置和方向，防止本体与物体接触碰撞；通过感知且估计本体所在空间位姿信息，以保证移动机器人构建的地图与场景环境的全局一致性。
由于双足机器人的图像传感器设置于头部，而双足机器人的双足运动模式与相关技术中采用轮式或履带式的移动机器人的运动模式不同，双足步态所指定的摆动轨迹以及足部撞击地面产生的地面冲击反作用力，容易在图像传感器采集图像的过程中产生图像运动模糊。另外，由于双足机器人的双足运动模式相较于三足及以上的足式机器人的多足运动模式的稳定性差，导致头部的图像传感器晃动幅度更大，因此，双足机器人在图像采集过程中更为容易引起图像运动模糊，从而降低双足机器人的图像匹配效果，同时降低VSLAM过程中识别的稳定性，干扰位姿估计，导致双足机器人的空间位姿信息的准确性差。
为了能够获得双足机器人更为准确的位姿，以改善提高图像匹配效果，降低图像运动模糊对图像匹配效果的影响，提高VSLAM***构建地图的准确性，本申请实施例提供了一种用于双足机器人的数据处理方法、用于双足机器人的数据处理装置、计算机可读存储介质以及计算机程序产品，先获取双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据，还可以获取设置于双足机器人头部位置的相机所拍摄得到的上一时刻图像和当前时刻图像，另外还可以获取相机在当前时刻的初始位姿，从而可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，因此，可以计算出双足机器人在相邻时刻的运动变化，确定出相机位姿在运动学数据计算得出的数据(即实际观测变换量)与初始位姿计算得出的数据(即估计变换量)之间的偏差，进而有助于校正相机位姿，降低图像运动模糊的影响。然后，可以根据运动学残差、上一时刻至当前时刻的惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿，通过结合惯性测量数据和多个时刻的运动学数据之间的运动关系，以及上一时刻图像与当前时刻图像之间的视觉投影关系，对初始位姿进行优化调整，能够实现多源信息的紧耦合，从而能够提高位姿估计的鲁棒性，得到更为准确的双足机器人的位姿数据。
参照图1，图1是本申请实施例提供的一种实施环境的示意图。该实施环境可以包括第一机器人终端110和数据处理服务器120，第一机器人终端110和数据处理服务器120可以通过有线或无线通信方式进行直接或间接的连接。其中，第一机器人终端110和数据处理服务器120可以为区块链中的一个节点服务器，本实施例对此并不作具体限定。
第一机器人终端110可以是智能手机、平板电脑、笔记本电脑、台式计算机、智能音箱、智能手表、车载终端等，但并不局限于此。其中，第一机器人终端110可以与设置在双足机器人中的关节编码器、压力传感器等测量双足机器人运动学数据的运动学传感器、设置在双足机器人的腰部位置的惯性测量单元，以及设置在双足机器人的头部位置的相机通过有线或无线通信方式进行直接或间接的连接，从而能够接收得到双足机器人的运动学数据、惯性测量数据以及相机拍摄的图像。
数据处理服务器120可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式***，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN(Content Delivery Network，内容分发网络)以及大数据和人工智能平台等基础云计算服务的云服务器。其中，数据处理服务器120能够获取相机在当前时刻的初始位姿，根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差；并且能够根据运动学残差、上一时刻至当前时刻的惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿。
参照图1所示，在一应用场景中，假设第一机器人终端110是电脑，并且第一机器人终端110可以与安装于双足机器人的关节编码器、惯性测量单元以及相机通过无线通信方式进行连接。第一机器人终端110可以作为双足机器人的数据采集器，获取得到双足机器人在运动过程中各个时刻关节编码器所记录测量的运动学数据、由惯性测量单元测量的惯性测量数据，以及相机拍摄得到的图像，第一机器人终端110可以将双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据，以及相机拍摄得到的上一时刻图像和当前时刻图像发送至数据处理服务器120，此时，数据处理服务器120可以通过由第一机器人终端110发送的数据获取得到双足机器人在当前时刻的初始位姿，也可以通过利用位姿预测模型进行位姿预测得到双足机器人在当前时刻的初始位姿，接着，数据处理服务器120可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，然后根据运动学残差、上一时刻至当前时刻的惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿，进而数据处理服务器120可以利用当前时刻的目标位姿以及当前时刻图像进行双足机器人的定位及地图构建，然后数据处理服务器120可以向第一机器人终端110返回构建后的地图，使得第一机器人终端110可以显示构建后的地图。
参照图2，图2是本申请实施例提供的另一种实施环境的示意图。该实施环境可以包括第二机器人终端210、第一处理服务器220和第二处理服务器230，第一处理服务器220可以分别与第二处理服务器230和第二机器人终端210通过有线或无线通信方式进行直接或间接的连接。其中，第二机器人终端210、第一处理服务器220和第二处理服务器230可以为区块链中的节点，本实施例对此并不作具体限定。
第二机器人终端210可以是智能手机、平板电脑、笔记本电脑、台式计算机、智能音箱、智能手表、车载终端等，但并不局限于此。其中，第二机器人终端110可以与设置在双足机器人中的关节编码器、压力传感器等测量双足机器人运动学数据的运动学传感器、设置在双足机器人的腰部位置的惯性测量单元，以及设置在双足机器人的头部位置的相机通过有线或无线通信方式进行直接或间接的连接，从而能够接收得到双足机器人的运动学数据、惯性测量数据以及相机拍摄的图像。
第一处理服务器220和第二处理服务器230可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式***，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN(ContentDelivery Network，内容分发网络)以及大数据和人工智能平台等基础云计算服务的云服务器。其中，第二处理服务器230可以接收由第一处理服务器220发送的相机在当前时刻的初始位姿、双足机器人的上一时刻运动学数据和当前时刻运动学数据，并根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，进而向第一处理服务器220返回运动学残差；而第一处理服务器220能够获取相机在当前时刻的初始位姿，并向第二处理服务器230发送相机在当前时刻的初始位姿，还可以利用由第二处理服务器230发送的运动学残差、由第二机器人终端210发送的上一时刻至当前时刻的惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿。
参照图2所示，在另一应用场景中，假设第二机器人终端210是电脑，并且第二机器人终端210可以与安装于双足机器人的关节编码器、惯性测量单元以及相机通过无线通信方式进行连接。第二机器人终端210可以作为双足机器人的数据采集器，获取得到双足机器人在运动过程中各个时刻关节编码器所记录测量的运动学数据、由惯性测量单元测量的惯性测量数据，以及相机拍摄得到的图像，第二机器人终端210可以将双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据，以及相机拍摄得到的上一时刻图像和当前时刻图像发送至第一处理服务器220，此时，第一处理服务器220可以通过由第二机器人终端210发送的数据获取得到双足机器人在当前时刻的初始位姿，也可以通过利用位姿预测模型进行位姿预测得到双足机器人在当前时刻的初始位姿，接着，第一处理服务器220可以将初始位姿发送至第二处理服务器230，而第二处理服务器230可以通过第二机器人终端210获取上一时刻运动学数据和当前时刻运动学数据，从而第二处理服务器230可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差；或者，第一处理服务器220可以将初始位姿发送至第二处理服务器230的同时将来自于第二机器人终端210的上一时刻运动学数据和当前时刻运动学数据发送至第二处理服务器230，使得第二处理服务器230可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差。接着，第二处理服务器230可以将相机的运动学残差返回至第一处理服务器220，然后第一处理服务器220可以根据运动学残差、上一时刻至当前时刻的惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿，进而第一处理服务器220可以利用当前时刻的目标位姿以及当前时刻图像进行双足机器人的定位及地图构建，然后第一处理服务器220可以向第二机器人终端210返回构建后的地图，使得第二机器人终端210可以显示构建后的地图。
图3是本申请实施例提供的一种用于双足机器人的数据处理方法的流程图，该用于双足机器人的数据处理方法可以由服务器执行。参照图3，该用于双足机器人的数据处理方法可以包括但不限于步骤310至步骤350。
步骤310：获取双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据。
在一实施例中，双足机器人的运动学数据可以是指用于描述双足机器人动作的数据，由于双足机器人主要依靠关节控制自身的组成部分之间的相对运动，例如，双足机器人的机械腿可以通过关节与双足机器人的腰部连接，从而在驱动装置的驱动下，双足机器人的机械腿可以相对于腰部进行旋转，使得机械腿与腰部产生相对运动，进而实现双足机器人的移动。因此，可以通过控制双足机器人中的关节来控制双足机器人完成相应的动作，而为了精确控制各个关节的运动和位置，需要精确测量驱动装置的两个输出轴的转速并根据测量的转速调整每个输出轴的转速，通常采用编码器来测量驱动装置的两个输出轴的转速，关节编码器是一种通过磁性或光学测量原理将两个输出轴的转速信号转化为正弦波的装置，通过获取关节编码器的编码数据则可以解读出双足机器人的动作姿态，所以，双足机器人的运动学数据可以是指双足机器人中用于控制双足机器人运动的关节所对应的关节编码器的编码数据，从而可以利用双足机器人的运动学数据确定出双足机器人各部位的相对位置，进而确定出双足机器人的动作姿态。其中，双足机器人中的关节编码器可以与机器人终端通过有线或无线通信的方式进行直接或间接的连接，从而机器人终端可以获取双足机器人的运动学数据。
在一实施例中，双足机器人的运动学数据除了关节编码器的编码数据之外，还可以是双足机器人的组成部分的受力数据、位移数据、速度数据等，而相应的测量装置可以设置于双足机器人的压力传感器、直线移动传感器、速度传感器等，本申请实施例并不对此作具体限定。
在一实施例中，惯性测量数据可以是指描述双足机器人在空间坐标系下的三轴姿态角以、速度及加速度的数据，惯性测量数据可以用于描述双足机器人在空间姿态，其中，惯性测量数据可以是通过惯性测量单元(Inertial Measurement Unit，IMU)测量得到，惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置，惯性测量单元通常包括三个单轴的加速度计和对应三个单轴的陀螺仪，加速度计可以检测物体在载体坐标***独立三轴的加速度信号，而陀螺仪可以检测载体相对于导航坐标系的角速度信号，而双足机器人的惯性测量单元可以设置于腰部位置，从而可以利用惯性测量单元测量双足机器人在以腰部位置为坐标系中心的三维空间中的角速度和加速度，进而可以利用惯性测量数据解算出双足机器人的姿态。需要说明的是，惯性测量单元可以与机器人终端通过有线或无线通信的方式进行直接或间接的连接，因此，机器人终端可以获取双足机器人的惯性测量数据。
参照图4，图4示出了本申请一实施例提供的双足机器人的结构示意图。可见，双足机器人设置有多个关节编码器，如设置在头部、腰部410、胯部、膝盖和足部的关节编码器，关节编码器可以测量各个关节的运动和位置，通过关节编码器测量的运动学数据可以将各个关节的位置通过在以该关节编码器为中心建立的坐标系下的具体坐标进行，而各个关节的运动则可以通过同一坐标系下的具体向量进行表示。具体地，如图4所示的双足机器人的左足420，位于左足420的关节编码器可以测量左足420关节的旋转角度如X轴的自转角度，以及X轴相对于Y轴的转动角度，进而可以通过左足420关节的连杆长度，计算出左足420的位置以及左腿膝盖的位置。因此，利用双足机器人各个关节部位的运动学数据可以判断双足机器人各个关节部位的位置姿态，从而可以判断出双足机器人的运动状态，进而有助于计算出更为准确的相机姿态。
步骤320：获取相机拍摄得到的上一时刻图像和当前时刻图像。
在一实施例中，双足机器人在头部设置有相机，利用相机可以拍摄得到双足机器人当前所处环境的环境图像，由于在双足机器人的运动过程中，相机所拍摄得到的图像会发生变化，因此，可以通过相邻图像间的相似性或差异性，来估计相机的运动轨迹，从而可以确定出图像中各个像素的空间点的位置，进而可以得到双足机器人周围环境的地图。因此，可以通过双足机器人头部设置的相机即视觉传感器拍摄得到的图像建立周围环境的路标点地图，进而确定双足机器人相对路标点的位置，也可以对双足机器人的周围环境进行重建。
其中，双足机器人在头部设置的相机可以是单目摄像头、双目摄像头或者深度图像摄像头中的至少一种。
在一实施例中，相机拍摄图像的时间间隔即频率可以与双足机器人的运动学数据和惯性测量数据的测量频率相等，从而可以确定出以时间维度建立运动学数据、惯性测量数据和图像的对应关系，进而可以根据三者的对应关系，确定出同一时刻的运动学数据、惯性测量数据和图像，减少因信息在时间维度上的不对齐而引起的误差，有助于提高位姿估计的准确性。另外，通过相机可以拍摄实时视频流数据，并传输至机器人终端，机器人终端可以根据与运动学数据相同的采样频率对实时视频流数据进行抽帧，获取对应时刻的图像。
步骤330：获取相机在当前时刻的初始位姿。
在一实施例中，相机在当前时刻的初始位姿可以是指在当前时刻针对相机进行估计预测的位姿，初始位姿可以表示出在相机在三维空间的初始位置和初始姿态，利用相机在连续多个时刻的初始位姿可以估计出相机的运动，进而有助于对相机位姿进行优化，提高相机位姿估计的精度。相机在当前时刻的初始位姿可以通过从上一个相机的运动状态进行估计，也可以通过全局重定位进行估计，例如，在上一时刻成功获取相机的位姿之后，可以假设相机在上一时刻至当前时刻进行恒速运动，从而可以利用恒速运动模型来估计当前时刻的初始位姿，具体地，可以利用相机在上一时刻的历史位姿与上一时刻的位移速度来估计相机在当前时刻的初始位姿，其中，上一个相机的位姿可以是指上一时刻相机的历史位姿，还可以是指上一个关键图像所对应的历史关键位姿；而在对当前时刻图像进行特征提取和特征匹配处理后得到的特征匹配点的数量小于预设阈值，说明当前时刻图像识别失败，则可以通过全局重定位进行估计当前时刻的初始位姿，具体地，可以利用上一个关键图像所对应的地图点进行迭代优化，预测得到当前时刻图像的预测地图点并根据预测地图点进行初始位姿的估计。
在一实施例中，初始位姿的估计过程可以在机器人终端中执行，也可以在服务器中执行，当机器人终端执行初始位姿的估计过程，机器人终端可以在接收得到双足机器人中各个传感器(包括运动学传感器、惯性测量传感器、相机等)传输的观测数据之后，可以利用观测数据(如运动学数据、惯性测量数据、图像等)进行相机在当前时刻的初始位姿的估计，并对观测数据整理后，将上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据、上一时刻图像、当前时刻图像和初始位姿发送至服务器，以使服务器对初始位姿进行优化得到目标位姿；当服务器执行初始位姿的估计过程，机器人终端可以在接收得到双足机器人中各个传感器(包括运动学传感器、惯性测量传感器、相机等)传输的观测数据之后，可以将上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据、上一时刻图像和当前时刻图像至服务器，以使服务器进行初始位姿的估计，并对估计得到的初始位姿进行优化得到目标位姿；其中，对初始位姿的估计过程和对目标位姿的计算过程可以分别由不同的服务器中独立执行。因此，可以根据机器人终端以及服务器的处理能力分配执行，提高数据处理的速率。
在一实施例中，相机在当前时刻的初始位姿可以通过当前时刻运动学数据和惯性测量数据进行估计得到，具体地，当前时刻运动学数据包括双足机器人的当前时刻躯干运动数据，即双足机器人的腰部至头部相机处各个组成部分的运动数据，因此，可以先对上一时刻至当前时刻的惯性测量数据进行积分处理，得到腰部初始位姿数据；接着，根据从双足机器人的腰部至相机的第四运动链，对当前时刻躯干运动数据进行数据转换，得到躯干初始转换数据；然后，根据腰部初始位姿数据和躯干初始转换数据，计算得到相机在当前时刻的初始位姿。
由于惯性测量单元安装于双足机器人的腰部位置，因此，可以利用惯性测量单元测量得到上一时刻至当前时刻的腰部加速度以及角速度等惯性测量数据，对上一时刻至当前时刻的惯性测量数据进行积分，得到在腰部位置在上一时刻至当前时刻的腰部平均加速度和腰部平均角速度，进而可以利用腰部平均加速度、腰部平均角速度以及上一时刻的腰部历史位姿数据，可以估计出当前时刻的腰部初始位姿数据。
双足机器人的腰部至头部相机的运动链，可以是指从双足机器人的腰部到头部相机之间的多个关节等运动***，每个相连的关节对应的关节坐标系之间的转换关系可以通过该关节对应的关节坐标系下的编码器数据和连杆长度计算得到，从而可以根据双足机器人的腰部至头部相机的运动链的传递顺序，以及每个相连的关节对应的关节坐标系之间的转换关系，构建得出双足机器人腰部坐标系与头部相机坐标系的转换关系，并计算得到躯干初始转换数据，其中，躯干初始转换数据可以表征为双足机器人腰部坐标系与头部相机坐标系的转换关系。由于腰部初始位姿数据可以以矩阵数据进行表示，因此躯干初始转换数据可以为转换矩阵，所以可以利用腰部初始位姿数据与躯干初始转换数据进行矩阵相乘计算，得到相机在当前时刻的初始位姿，即相机坐标系在腰部坐标系下的位姿。由于传感器的测量值存在大量噪声信息，尤其是当双足机器人落地时的冲击与震荡等产生的尖峰噪声，导致估计位姿存在大量的噪声信息，精度较低；而在相关技术中，可以将双足机器人身上的惯性测量单元采集得到的惯性测量数据输入至预设的预测模型预测出相机的运动状态，进而实现对相机位姿的估计，但这种实现方式的精度主要依赖于预测模型的准确性，当预测模型与实际的相机轨迹偏离较多时，相机位姿的估计会随着时间的积累出现漂移现象，精度自然也会受到影响。所以，通过融合由惯性测量数据积分预测得到的腰部初始位姿，以及通过运动学数据运动学解算得到的躯干初始转换数据的方式得到相机的初始位姿，不仅能够避免仅采用惯性测量数据估计位姿时出现的漂移现象，也能够避免仅采用运动学解算时出现的噪声，从而提高相机在当前时刻的初始位姿的估计精度。
参照图5，图5示出了本申请一实施例提供的初始位姿计算方式的示意图。可以看出，双足机器人的腰部410至头部相机510的运动链，包括腰部410关节、头部关节以及腰部410关节与头部关节之间的躯干部分，因此，可以根据腰部410关节的关节编码器测量得到的腰部410坐标系下的腰部运动学数据(如图5所示的躯干部分相对于腰部410坐标系下的Y轴的摆动角度)、头部关节的关节编码器测量得到的头部(即相机)坐标系下的头部运动学数据(如图5所示的躯干部分相对于相机坐标下Y轴的摆动角度)，以及躯干部分的长度，可以构建得到相机坐标系与腰部坐标系下的转换关系，并计算得到躯干初始转换数据。腰部410关节除了可以设置有关节编码器之外，还设置有惯性测量单元，惯性测量单元可以测量以双足机器人的腰部410为中心的双足机器人的移动速度加速度和角速度。因此，通过惯性测量单元测量得到的上一时刻至当前时刻的惯性测量数据可以估计出当前时刻的腰部初始位姿数据，然后利用腰部初始位姿数据与躯干初始转换数据进行相乘，得到相机在当前时刻的初始位姿，即相机坐标系在腰部坐标系下的初始位姿。
在一实施例中，相机在当前时刻的初始位姿可以通过对当前时刻运动学数据进行估计得到，具体地，当前时刻运动学数据包括当前时刻下肢运动数据和当前时刻躯干运动数据，其中，下肢运动数据可以包括腰部至足部之间的运动学数据，而躯干运动数据可以包括腰部至头部之间的运动学数据，因此，可以对当前时刻下肢运动数据和当前时刻躯干运动数据进行位姿预测，得到相机在当前时刻的初始位姿。在双足机器人中设置有多个运动学传感器，利用双足机器人的运动学数据可以确定出双足机器人各部位的相对位置，进而确定出双足机器人的动作姿态，因此，可以利用双足机器人的历史运动学数据建立运动学模型，从而可以将当前时刻下肢运动数据和当前时刻躯干运动数据输入至运动学模型进行预测，得到双足机器人中的头部相机位置处的运动状态，进而实现对相机在当前时刻的初始位姿估计。
参照图6，图6示出了本申请一实施例提供的运动状态预测的示意图。如图6所示，通过获取双足机器人各个关节的编码器数据，即躯干运动数据和下肢运动数据，接着将躯干运动数据和下肢运动数据输入至双足机器人的运动学预测模型进行运动状态的预测，运动学预测模型是利用历史数据(包括历史躯干运动数据、历史下肢运动数据、历史运动状态、历史位姿等)进行训练生成，因此，可以得到当前输入的躯干运动数据和下肢运动数据所对应的双足机器人的运动状态，从而可以判断出头部相机的运动状态，进而可以对相机在当前时刻的初始位姿估计。如图6所示，利用当前时刻的躯干运动数据和下肢运动数据预测得到的运动状态是双足机器人的左足抬起，右足支撑的状态，从而可以根据双足机器人的双足步态的摆动轨迹判断出头部相机的运动状态，进而计算出初始位姿。
步骤340：根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差。
在一实施例中，运动学残差可以是指相机利用实际观察值计算得出的位姿与估计拟合的位姿之间的位姿偏差，即通过上一时刻运动学数据和当前时刻运动学数据计算得出的相机运动，与利用初始位姿估计拟合得出的相机运动之间的差异，具体地，可以通过上一时刻运动学数据和当前时刻运动学数据计算得出相机的实际运动，而通过上一时刻的历史位姿和当前时刻的初始位姿计算得出相机的预测运动，进而计算相机的实际运动与预测运动之间的偏差，即可以利用相机的运动学数据确定出相机的位姿偏差。因此，通过运动学残差可以发现相机实际位姿与预测位姿的误差，从而可以根据运动学残差，对初始位姿进行优化，降低运动学残差以改善相机位姿估计的准确性。另外，通过计算运动学残差可以识别出在进行估计相机运动过程中的异常情况，当运动学残差超过预设残差阈值，可以认为相机定位出现问题，如运动学数据或者初始位姿出现错误，从而及时进行数据回退或重新初始化的操作，提高VSLAM***的鲁棒性。同时，运动学残差可以用可视化调试VSLAM***，通过观察运动学残差的变化情况，可以了解相机位姿估计的质量和准确性，从而有助于快速识别VSLAM***中存在的异常情况。
在一实施例中，在根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差的过程中，可以先根据上一时刻运动学数据和当前时刻运动学数据，计算得到相机的运动增量数据；然后根据运动增量数据和初始位姿，计算得到相机的运动学残差。通过比较上一时刻运动学数据和当前时刻运动学数据之间的数值偏差，即运动增量数据，进而利用运动增量数据进行运动学解算，估计出相机在上一时刻至当前时刻的运动变化，再确定出相机在当前时刻的运动姿态，接着，将相机的运动姿态和初始位姿进行比较，计算得到相机的运动学残差。或者，通过对上一时刻运动学数据进行运动学解算，得出相机在上一时刻的运动姿态，而通过对当前时刻运动学数据进行运动学解算，可以得出相机在当前时刻的运动姿态，然后将相机在上一时刻的运动姿态与当前时刻的运动姿态进行差值计算，可以得出相机在上一时刻至当前时刻的运动增量数据，即相机在上一时刻至当前时刻的运动姿态的变化量。通过优先计算运动增量数据再利用运动增量数据和初始位姿进行运动学残差的计算方式，可以提高实际观察值计算得出的位姿的准确性，进而可以提高运动学残差的准确性。
参照图7，图7示出了本申请一实施例提供的运动增量数据的示意图。对上一时刻运动学数据进行计算，得到双足机器人在上一时刻的运动姿态，以及相机在上一时刻的运动姿态，而对当前时刻运动学数据进行计算，可以得到双足机器人在当前时刻的运动姿态，以及相机在当前时刻的运动姿态，如图7所示的虚线部分表示双足机器人在上一时刻的运动姿态，实线部分表示双足机器人在当前时刻的运动姿态，同时，双足机器人头部位置的坐标系{c}表示为相机坐标系，双足机器人腰部位置的坐标系{b}表示为腰部坐标系，双足机器人足部位置的坐标系{s}表示为支撑足坐标系，通过比较双足机器人两个时刻的运动姿态可以看出双足机器人在上一时刻至当前时刻的运动姿态的变化量，而相应地，通过比较相机在上一时刻的运动姿态和当前时刻的运动姿态，可以得出相机在上一时刻至当前时刻的运动增量数据，运动增量数据可以通过表示，其中，(fk)可以表示为运动学解算的方式得出，i时刻可以表示上一时刻，i+1时刻可以表示为当前时刻，因此，/>即运动增量数据可以表示为在上一时刻至当前时刻通过运动学数据解算得到在相机坐标系下的相机的增量运动。
在一实施例中，在根据运动增量数据和初始位姿，计算得到相机的运动学残差的过程中，可以先获取相机在上一时刻的历史位姿；接着，根据历史位姿和初始位姿计算得到相机的位姿变化量；然后根据运动增量数据和位姿变化量，计算得到相机的运动学残差。其中，相机在上一时刻的历史位姿可以存储于机器人终端中，也可以存储于服务器中；相机在上一时刻的历史位姿可以是指相机在上一时刻所对应的初始位姿，也可以是指上一个关键图像对应时刻的初始位姿，还可以是指相机在上一时刻所对应的目标位姿，也可以是指上一个关键图像对应时刻的目标位姿。通过比较相机在上一时刻的历史位姿和在当前时刻的初始位姿，可以计算得到估计拟合的位姿在上一时刻至当前时刻的位姿变化量，即可以确定出预测估计的相机增量运动。因此，通过比较由运动学数据解算得到的相机增量运动与通过预测估计的相机增量运动之间的偏差，即利用运动增量数据和位姿变化量计算得出运动学残差，有助于更为准确地确定出由运动学数据解算得到的相机运动(即实际观测到的相机运动)与通过运动模型和初始位姿预测的相机运动之间的差异，进而能够对初始位姿进行优化得到更为准确的目标位姿。
参照图8，图8示出了本申请一实施例提供的计算运动学残差的示意图。如图8中的左侧子图所示，根据上一时刻运动学数据和当前时刻运动学数据，可以计算得到相机的运动增量数据。如图8中的右侧子图所示，通过比较相机在上一时刻的历史位姿和在当前时刻的初始位姿，可以计算得到估计拟合的位姿在上一时刻至当前时刻的位姿变化量。然后，如图8中的中间子图所示，通过比较由运动学数据解算得到的运动增量数据与通过预测估计的位姿变化量之间的偏差，可以计算得到运动学残差。
步骤350：根据运动学残差、惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿。
在一实施例中，结合上一时刻至当前时刻的惯性测量数据可以确定出双足机器人以腰部为中心的运动状态，而结合上一时刻图像和当前时刻图像可以捕捉到图像对应的特征点在视觉上的运动，进而可以通过分析图像对应的特征点匹配、运动轨迹等信息，获得相机的视觉运动增量数据，然后通过融合运动学残差、惯性测量数据以及相机拍摄得到的图像多种观测数据的方式可以提高位姿估计的准确性和鲁棒性。利用运动学传感器测量得到的运动学数据、惯性测量单元测量得到的惯性测量数据可以推断出相机在短时间内的快速运动，所以通过惯性测量数据或者运动学残差均可以实现相机的位姿更新，而由于运动学残差反映了实际观测的位姿与估计的位姿之间的偏差，将运动学残差与惯性测量数据进行融合，可以对位姿估计的误差进行校正，提高相机位姿估计的精度。但利用运动学残差与惯性测量数据进行位姿估计，是依赖于相邻时刻之间的关系完成的，这种依赖局部约束且不停地链式进行的算法会导致误差逐渐累积，产生较大的误差漂移；而利用视觉传感器的数据即相机拍摄得到的图像可以推断出相机的运动，不容易产生误差漂移，因此，通过将运动学残差、惯性测量数据以及拍摄得到的图像进行融合，从而能够实现短时间的快速运动的位姿估计，还能够减少由运动学残差与惯性测量数据行位姿估计所产生的误差，进而达到提高相机位姿的精度和稳定性的效果，可以得到更为准确的目标位姿。
在一实施例中，在计算相机在当前时刻的目标位姿的过程中，可以先对惯性测量数据进行预积分处理，得到惯性数据残差；接着，将上一时刻图像和当前时刻图像进行特征提取和特征匹配处理，得到视觉重投影误差；然后根据运动学残差、惯性数据残差和视觉重投影误差，计算得到相机在当前时刻的目标位姿。具体地，可以采用片段式积分方法，对上一时刻至当前时刻的惯性测量数据进行积分并构建最小二乘残差，作为惯性数据残差，通过预积分处理，可以提高相机姿态估计的准确性和稳定性，修正惯性测量单元的积分误差，并减小由于不准确姿态估计造成的累积误差。其中，还可以对上一时刻至当前时刻之间的所有惯性测量数据进行积分并构建最小二乘残差，作为惯性数据残差。
而针对视觉重投影误差的计算，可以先对上一时刻图像和当前时刻图像进行特征提取，可以使用基于角点的快速特征检测算法((Features from Accelerated SegmentTest,FAST)与特征点描述算法(Binary Robust Independent Elementary Features，BRIEF)融合的定向快速特征检测与特征点描述算法(Oriented Fast and Rotated Brief，ORB)来检测图像中的关键点，并计算关键点的描述符，用于进行后续的特征匹配，接着，根据上一时刻图像中的上一时刻关键点描述符以及当前时刻图像中的当前时刻关键点描述符，采用特征匹配算法(如最近邻匹配算法、最佳匹配算法等)确定出上一时刻关键点描述符与当前时刻关键点描述符之间的对应关系，从而找到相匹配的特征点，然后，通过计算匹配对应的特征点之间的像素坐标差异，得到视觉重投影误差，视觉重投影误差可以直接观测得出关于图像的姿态变化，所以可以视觉重投影误差对相机位姿进行调整校正，提高相机位姿的准确性。
在得到运动学残差、惯性数据残差和视觉重投影误差之后，可以构建最小二乘问题，并利用迭代求函数极值的算法进行求解，得到相机在当前时刻的目标位姿。通过使用运动学残差、惯性数据残差和视觉重投影误差，可以计算相机在当前时刻的目标位姿。这些残差量代表了不同来源的误差信息，通过融合多个观测数据的误差信息，实现多源观测数据的紧耦合，可以提高位姿估计的准确性和稳定性，得到更为准确的目标位姿。
其中，可以利用运动学残差、惯性数据残差和视觉重投影误差构建残差函数，具体残差函数如下所示：
其中，rkin表示运动学残差，rimu表示惯性数据残差，rcam表示视觉重投影误差，xi表示为相机位姿中需要优化的状态量，具体包括如下状态量：
其中，为相机的位置，/>为相机的姿态，/>为惯性测量单元测量得到的移动速度，/>为惯性测量单元中加速度计的测量偏置数据，/>为惯性测量单元中陀螺仪的测量偏置数据。
需要注意的是，在实际应用中，预积分处理和图像特征提取和特征匹配处理的具体方法可能因具体的算法、传感器和应用场景有所不同。因此，在实际的应用场景中，可以根据具体情况进行调整和优化。
参照图9，图9示出了相关技术中的一种数据处理方法的流程示意图。相关技术中，基于一种稳健的深度图像密集SLAM框架，增加机器人运动状态估计模块(StateEstimation Robot Walking，SEROW)，该机器人运动状态估计模块可以通过惯性测量单元测量的数据、关节编码器测量的数据以及受力传感器测量的数据进行状态估计，具体流程如图9所示。首先利用视觉里程计对深度图像进行机器人的位姿估计，然后将得到的位姿输入至机器人运动状态估计模块对估计的位姿进行修正优化，接着，将优化后的位姿输入至局部建图模块进行地图构建，但在机器人运动状态估计模块中是采用后处理方式利用惯性测量单元测量的数据、关节编码器测量的数据以及受力传感器测量的数据对位姿进行处理，各项数据均会累积误差，导致得到的位姿的准确度差。而本申请实施例提供的方法，先获取双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据，还可以获取设置于双足机器人头部位置的相机所拍摄得到的上一时刻图像和当前时刻图像，另外还可以获取相机在当前时刻的初始位姿，从而可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，因此，可以计算出双足机器人在相邻时刻的运动变化，确定出相机位姿在由运动学数据计算得出的数据(即实际观测变换量)与初始位姿计算得出的数据(即估计变换量)之间的偏差，进而有助于校正相机位姿，降低图像运动模糊的影响。然后，可以根据运动学残差、上一时刻至当前时刻的惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿，通过结合惯性测量数据和多个时刻的运动学数据之间的运动关系，以及上一时刻图像与当前时刻图像之间的视觉投影关系，对初始位姿进行优化调整，能够实现多源信息的紧耦合，从而能够提高位姿估计的鲁棒性，得到更为准确的双足机器人的位姿数据。
在一实施例中，由于运动增量数据和位姿变化量均可以用于描述相机在上一时刻至当前时刻的位姿变化，而位姿变化包括相机的位置变化和相机的姿态变化，因此，在运动增量数据中包括有用于描述由运动学数据解算得出相机的位置变化的位移增量数据，以及用于描述由运动学解算得出相机的姿态变化的旋转增量数据；相应地，在位姿变化量中包括有用于描述由初始位姿估计得出相机的位置变化的位移变化数据，以及用于描述由初始位姿估计得出相机的姿态变化的旋转变化数据。所以，在计算相机的运动学残差的过程中，可以先将位移增量数据和位移变化数据进行差值计算，得到位移变化值，并根据旋转增量数据和旋转变化数据进行对数映射，得到姿态变化值；然后，根据位移变化值和姿态变化值，构建得到相机的运动学残差。
参照图10，图10示出了本申请一实施例提供的位姿变化的示意图。相机在上一时刻至当前时刻的位姿变化包括有相机510的位置变化和相机510的姿态变化，如图10所示，虚线部分表示相机510和腰部410在上一时刻的位置和姿态，而实线部分表示相机510在当前时刻的位置和姿态，以腰部坐标系为基准通过比较上一时刻的相机510位置和当前时刻的相机510位置，可以得到上一时刻至当前时刻相机510的位置变化。相应地，可以以腰部坐标系为基准比较上一时刻的相机510姿态和当前时刻的相机510姿态，得到上一时刻至当前时刻相机510的姿态变化。通过相机510的位置变化和相机510的姿态变化，可以确定出以腰部坐标系为基准相机510的位姿变化情况，换句话说，在计算相机510的位姿变化可以对相机510的位置变化和姿态变化分别求解计算，从而可以简化数据处理量，提高数据处理效率。
位移增量数据可以是通过运动学数据求解得出相机在上一时刻至当前时刻的位置变化向量，而位移变化量可以是通过初始位姿和历史位姿计算得出相机在上一时刻至当前时刻的位置变化向量，因此，通过将位移增量数据和位移变化数据进行差值计算，可以得到实际观测的位移量与估计拟合得到的位移量的偏差，即位移变化值。相应地，旋转增量数据可以是通过运动学数据求解得出相机在上一时刻至当前时刻姿态变换的姿态旋转矩阵，而旋转变化数据可以是通过初始位姿和历史位姿计算得出相机在上一时刻至当前时刻姿态变换的姿态旋转矩阵，因此，根据旋转增量数据和旋转变化数据进行对数映射，可以将李群转化为李代数，得到姿态变化值，接着利用姿态变化值和位移变化值进行矩阵构建，得到运动学残差，从而能够实现将非线性的运算转化为线性运算，简化最小二乘问题的求解和位姿优化过程，同时转化为李代数的计算效率高且数值稳定性好。
运动学残差的具体公式如下所示：
其中，表示上一时刻至当前时刻的位移增量数据，/>表示上一时刻至当前时刻的位移变化量，/>表示上一时刻至当前时刻的旋转增量数据，/>表示上一时刻至当前时刻的旋转变化量，LogSO3函数为对数映射函数。
在一实施例中，上一时刻运动学数据包括上一时刻足部受力数据、上一时刻下肢运动数据和上一时刻躯干运动数据，当前时刻运动学数据包括当前时刻足部受力数据、当前时刻下肢运动数据和当前时刻躯干运动数据；在计算得到相机的运动增量数据的过程中，可以先根据上一时刻足部受力数据确定上一时刻支撑足，并根据当前时刻足部受力数据确定当前时刻支撑足；接着，根据从上一时刻支撑足至双足机器人的腰部的第一运动链，对上一时刻下肢运动数据进行数据转换，得到上一时刻下肢转换数据，并根据从当前时刻支撑足至双足机器人的腰部的第二运动链，对当前时刻下肢运动数据进行数据转换，得到当前时刻下肢转换数据；还可以根据从双足机器人的腰部至相机的第三运动链，对上一时刻躯干运动数据和当前时刻躯干运动数据分别进行数据转换，得到上一时刻躯干转换数据和当前时刻躯干转换数据；然后根据上一时刻下肢转换数据、当前时刻下肢转换数据、上一时刻躯干转换数据和当前时刻躯干转换数据，计算得到相机的运动增量数据。
通过判断出双足机器人的支撑足，利用相应时刻的支撑足至腰部的运动链，求解出相应时刻的下肢转换数据，从而可以构建腰部坐标系与支撑足坐标系的转换关系，进而可以根据不同支撑足来计算出不同的转换数据，减小数据误差。而相机的位姿不仅受到下肢运动的影响，还受到躯干运动的影响，利用腰部至头部相机之间的运动链，求解出相应时刻的躯干转换数据，从而可以构建腰部坐标系与相机坐标系的转换关系。在得到躯干转换数据和下肢转换数据之后，可以将下肢运动和躯干运动在支撑足坐标系中进行联合考虑，得到相机坐标系在支撑足坐标下的位姿描述，进而能够更为准确描述相机的运动情况。所以，通过上一时刻下肢转换数据和上一时刻躯干转换数据可以得到相机坐标系在上一时刻的支撑足坐标下的相机位姿描述，而通过当前时刻下肢转换数据和当前时刻躯干转换数据可以得到相机坐标系在当前时刻的支撑足坐标下的相机位姿描述，进而可以根据两个时刻的相机位姿描述，计算得到相机的运动增量数据。由于双足机器人的双足步态会使得足部产生摆动轨迹，同时足部撞击地面而产生的冲击力会引起图像模糊，而支撑足是双足机器人在步态运动中与地面直接接触的部位，支撑足的运动与双足机器人整体平衡性和稳定性密切相关。因此，选择支撑足对应的运动链能够更准确地捕捉双足机器人的运动特征，提高运动增量数据的可靠性和精度。
在一实施例中，参照图11，图11示出了本申请一实施例提供的双足机器人在不同支撑状态的示意图。足部受力数据可以通过设置在双足机器人足部的压力传感器测量得到，下肢运动数据可以通过设置在双足机器人腰部至足部之间的关节编码器测量得到，躯干运动数据可以通过设置在双足机器人腰部至头部相机之间的关节编码器测量得到。通过双足机器人的足部受力数据可以判断出双足机器人的支撑状态，双足机器人的支撑状态包括左足单足支撑状态、右足单足支撑状态以及左右足同时支撑状态，当双足机器人的左足420的足部受力数据超过预设受力数值，且右足的足部受力数据未超过预设受力数值，说明双足机器人处于左足单足支撑状态，对应的支撑足为左足420，如图11中的左侧子图所示，双足机器人中的实线部分表示左足420为支撑足；当双足机器人的右的足部受力数据超过预设受力数值，且左的足部受力数据未超过预设受力数值，说明双足机器人处于右足单足支撑状态，对应的支撑足为右足430，如图11中的中间子图所示，双足机器人中的实线部分表示右足430为支撑足；当双足机器人的左足420和右足430的足部受力数据均超过预设受力数值，说明双足机器人处于左右足同时支撑状态，对应的支撑足为左足420和右足430，如图11中的右侧子图所示，双足机器人中的实线部分表示左足420和右足430均为支撑足。
相应地，当支撑足确定为左足，则第一运动链或第二运动链可以是双足机器人的左足至腰部之间的多个关节等运动***；而当支撑足确定为右足，则第一运动链或第二运动链则可以是双足机器人的右足至腰部之间的多个关节等运动***；当支撑足确定为左右双足，则第一运动链或第二运动链可以是双足机器人的左右双足至腰部之间的多个关节等运动***，或者是双足机器人的左足至腰部之间的多个关节等运动***，还可以是右足至腰部之间的多个关节等运动***。第三运动链是指双足机器人的腰部至头部相机之间的多个关节等运动***。在步态运动中，切换支撑足会导致双足机器人整体运动产生跳变，若保持采用同一支撑足的坐标系等固定坐标系，则会容易使得数据转换出现误差，影响运动增量数据的准确性，因此，根据不同时刻的支撑足而采用对应的运动链进行数据转换可以在支撑足切换之间保持数据的连续性，减小运动学数据的不连续性对相机运动估计的影响，提高运动增量数据的准确性。
参照图12，图12示出了本申请一实施例提供的双足机器人的运动学数据解算的示意图。运动学数据包括腰部至相机之间的关节编码器的编码数据(即躯干运动数据)、下肢关节编码器的编码数据(即下肢运动数据)以及足部受力数据，通过对足部受力数据进行分析，可以判断出支撑足，进而根据支撑足对下肢运动数据进行运动学解算，得到下肢转换数据。而针对躯干运动数据可以直接进行运动学解算，得到躯干转换数据。
在一实施例中，在计算相机的运动增量数据的过程中，可以先根据上一时刻支撑足和当前时刻支撑足数据确定足部位姿变换数据，并根据上一时刻下肢转换数据和上一时刻躯干转换数据进行矩阵转置，得到上一时刻位姿转置数据；然后将双足位姿变换数据、上一时刻位姿转置数据、当前时刻下肢转换数据和当前时刻躯干转换数据进行相乘，得到相机的运动增量数据。
在一实施例中，足部位姿变换数据可以用于表征上一时刻与当前时刻支撑足的运动变化，上一时刻位姿转置数据可以用于表征上一时刻相机坐标系在上一时刻支撑足对应的支撑足坐标系下的相机位姿，而当前时刻下肢转换数据和当前时刻躯干转换数据进行相乘后可以得到用于表征当前时刻相机坐标系在当前时刻支撑足对应的支撑足坐标系下的相机位姿的数据。因此，通过将双足位姿变换数据、上一时刻位姿转置数据、当前时刻下肢转换数据和当前时刻躯干转换数据进行相乘得到机的运动增量数据的方式，能够综合考虑双足机器人在双足步态运动时支撑足变换以及躯干运动对相机位姿的影响，能够更为全面地描述相机的位姿变化，提高位姿估计的准确性。
参照图13，图13示出了本申请一实施例提供的双足机器人的运动坐标系的转换关系的示意图。利用躯干转换数据可以将相机坐标系下的相机位姿转换得到相机坐标系在腰部坐标系下的相机位姿，而利用下肢转换数据可以将相机坐标系在腰部坐标系下的相机位姿转换至相机坐标系在支撑足坐标系下的相机位姿。当上一时刻支撑足与当前时刻支撑足相同，说明支撑足坐标系固定，从而可以利用上一时刻的相机位姿与当前时刻的相机位姿，计算出相机的运动变化；当上一时刻支撑足与当前时刻支撑足不相同，则可以利用双足位姿变换数据，将相机坐标系在非支撑足坐标系下的相机位姿转换至相机坐标系在同一支撑足坐标系下的相机位姿，从而可以准确计算出相机的运动变化。
在一实施例中，可以先将上一时刻下肢转换数据与上一时刻躯干转换数据进行矩阵相乘，得到上一时刻位姿转换数据，然后将上一时刻位姿转换数据进行矩阵转置，得到上一时刻位姿转置数据，相机的运动增量数据的具体计算公式如下：
其中，(fk)可以表示为运动学解算的方式得出，i时刻可以表示上一时刻，i+1时刻可以表示为当前时刻，因此，即运动增量数据可以表示为在上一时刻至当前时刻通过运动学数据解算得到在相机坐标系下的相机的增量运动。/>可以表示上一时刻腰部坐标系在支撑足坐标系下的腰部位姿；/>可以表示上一时刻相机坐标系在腰部坐标系下的相机位姿；/>可以表示双足位姿变换数据；/>可以表示当前时刻腰部坐标系在支撑足坐标系下的腰部位姿；/>可以表示当前时刻相机坐标系在腰部坐标系下的相机位姿。
因此，将上一时刻位姿转换数据进行矩阵转置能够保持数据在计算过程中的一致性，便于将双足位姿变换数据、上一时刻位姿转置数据、当前时刻下肢转换数据和当前时刻躯干转换数据相乘，可以判断出上一时刻与当前时刻的相机的运动变化，得到相机的运动增量数据，提高运动增量数据的计算系效率和准确性。
在一实施例中，足部位姿变换数据可以用于表征上一时刻与当前时刻支撑足的运动变化，因此，当上一时刻支撑足与当前时刻支撑足相同，说明双足机器人的支撑状态未发生变化，即在上一时刻至当前时刻，双足机器人的非支撑足的足部未与地面撞击，且双足步态的摆动轨迹对相机的位姿变化无影响，可以采用单位矩阵作为足部位姿变换数据；当上一时刻支撑足与当前时刻支撑足不相同，则说明双足机器人的支撑状态发生变化，即在上一时刻至当前时刻，双足机器人的足部与地面发生撞击，且双足步态的摆动轨迹对相机的位姿变化有影响，由于双足机器人的双足步态是在左右足同时支撑状态、左足单足支撑状态和右足单足支撑状态中进行切换，因此，可以获取双足机器人在左右足同时支撑状态下的下肢运动数据，根据双足机器人的双足之间的运动链，对在左右足同时支撑状态下的下肢运动数据、当前时刻下肢运动数据和上一时刻下肢运动数据中至少一种下肢运动数据进行计算，得到足部位姿变换数据，从而可以判断出相对于在左右足同时支撑状态下双足机器人的足部，当前时刻双足机器人的足部的运动变化。
当上一时刻支撑足为左右双足，当前时刻支撑足为右足，则说明双足机器人的支撑状态发生变化，根据上一时刻至当前时刻双足机器人的双足之间的运动链，可以利用在左右足同时支撑状态下的下肢运动数据进行计算，得到足部位姿变换数据，或者可以根据上一时刻至当前时刻双足机器人的双足之间的运动链，对在左右足同时支撑状态下的下肢运动数据和当前时刻下肢运动数据进行计算，得到足部位姿变换数据；当上一时刻支撑足为左足，当前时刻支撑足为左右双足，可以根据上一时刻至当前时刻双足机器人的双足之间的运动链，利用在左右足同时支撑状态下的下肢运动数据进行计算，得到足部位姿变换数据，或者可以根据上一时刻至当前时刻双足机器人的双足之间的运动链，对在左右足同时支撑状态下的下肢运动数据和上一时刻下肢运动数据进行计算，得到足部位姿变换数据；当上一时刻支撑足为左足，当前时刻支撑足为右足，可以根据上一时刻至当前时刻双足机器人的双足之间的运动链，可以利用在左右足同时支撑状态下的下肢运动数据进行计算，得到足部位姿变换数据，或者可以根据上一时刻至当前时刻双足机器人的双足之间的运动链，对在左右足同时支撑状态下的下肢运动数据、上一时刻下肢运动数据和当前时刻下肢运动数据进行计算，得到足部位姿变换数据。
在一实施例中，在计算得到相机在当前时刻的目标位姿之后，用于双足机器人的数据处理方法还包括：
若当前时刻图像被确认为关键图像，根据上一时刻运动学数据、当前时刻运动学数据和目标位姿，计算得到相机的运动学二次残差；
获取历史关键图像和历史关键图像对应时刻的历史惯性测量数据；
根据运动学二次残差、惯性测量数据、历史惯性测量数据、历史关键图像和当前时刻图像，对目标位姿以及当前时刻图像对应的地图点进行优化处理，得到优化后的关键位姿和关键地图点。
在一实施例中，在计算得到相机在当前时刻的目标位姿之后，可以对当前时刻图像进行检测，判断当前时刻图像是否可以作为关键图像。关键图像是指在某段时长对应的多个图像中具有代表性的一个图像，由于相机拍摄图像的频率很高，时间上相邻的两个图像之间信息冗余度高，同时相机拍摄得到的图像的数据量庞大，若对所有图像均提取特征点进行地图构建则导致数据处理时间长，而检测关键图像是取局部时间上的多个图像中具有代表性的一个图像，从而可以降低信息冗余度，缩短数据处理时间。关键图像主要用于在局部地图线程和闭环检测线程对误差进行优化，并用于在局部地图线程中进行地图点的标记和生成局部地图，而关键图像的图像质量对于误差优化、地图构建等任务的影响力大，因此，需要判断图像是否满足预设的检测条件，预设的检测条件可以包括以下情况的一种或多种：已确定的关键图像的数量小于预设图像数目、距离上一个关键图像的时间间隔大于预设时间间隔、当前时刻图像进行特征匹配后得到的三维点数目大于预设三维点阈值等。当当前时刻图像满足预设的检测条件，则可以将当前时刻图像确认为关键图像。同时，可以通过对关键图像进行特征提取和特征匹配，确定出关键图像中的地图点。
在一实施例中，运动学二次残差可以是指相机利用实际观察值计算得出的位姿与目标位姿之间的位姿偏差，即通过上一时刻运动学数据和当前时刻运动学数据计算得出的相机运动，与利用目标位姿计算得出的相机运动之间的差异，具体地，可以通过上一时刻运动学数据和当前时刻运动学数据计算得出相机的实际运动，而通过上一时刻的目标历史位姿和当前时刻的目标位姿计算得出相机的预测运动，进而计算相机的实际运动与预测运动之间的偏差，即可以二次利用相机的运动学数据和目标位姿确定出相机的位姿偏差。因此，通过运动学二次残差可以发现相机实际位姿与调整后的目标位姿的误差，从而可以根据运动学二次残差，对目标位姿进行二次优化，降低运动学二次残差以改善相机位姿估计的准确性。
在一实施例中，历史关键图像、历史关键图像对应时刻的历史惯性测量数据可以存储于机器人终端中，也可以存储于服务器中；历史关键图像和历史惯性测量数据一一对应，可以仅获取相邻的历史关键图像即上一个关键图像，则相应地获取上一个关键图像对应时刻的历史惯性测量数据，同样地，结合历史惯性测量数据和惯性测量数据可以确定出双足机器人以腰部为中心的关键运动状态，而结合历史图像和当前时刻图像可以捕捉到关键图像对应的特征点在视觉上的运动，进而可以通过分析关键图像对应的特征点匹配、运动轨迹等信息，获得相机的关键视觉运动增量数据，然后通过融合运动学二次残差、历史惯性测量数据以及关键图像多种观测数据的方式再次对目标位姿进行调整，可以提高位姿估计的准确性和鲁棒性。
在一实施例中，在根据运动学二次残差、惯性测量数据、历史惯性测量数据、历史关键图像和当前时刻图像，对目标位姿以及当前时刻图像对应的地图点进行优化处理的过程中，可以先根据历史惯性测量数据和惯性测量数据进行预积分处理，得到关键惯性数据残差；接着，将历史关键图像和当前时刻图像进行特征提取和特征匹配处理，得到关键视觉重投影误差；然后根据运动学二次残差、关键惯性数据残差和关键视觉重投影误差，对目标位姿以及当前时刻图像对应的地图点进行优化处理。
在一实施例中，可以同时获取多个历史关键图像，则相应地获取各个历史关键图像所对应的所有历史惯性测量数据，通过对惯性测量数据和多个历史惯性测量数据进行预积分处理，得到关键惯性数据残差。
在一实施例中，在对目标位姿以及当前时刻图像对应的地图点进行优化处理的过程中，可以获取历史关键图像对应时刻的历史关键位姿，并对历史关键图像进行特征提取和特征匹配得到历史关键地图点，进而利用历史关键位姿、历史关键地图点和历史惯性测量数据等构建先验残差项，先验残差项可以是指先验信息和先验估计值之间的差异，先验信息可以是指在当前局部地图线程中提供的先前知识或预先存在的信息，用于引导和约束估计，如历史关键位姿、历史关键地图点和历史惯性测量数据，而先验估计值可以通过对历史关键位姿、历史关键地图点和历史惯性测量数据进行估计得到，例如通过机器学习、构建最小二乘法、最大似然估计算法来估计得到。通过将先验信息与先验估计值进行相减，得到先验残差，然后将先验残差的雅可比矩阵，与由各个关键图像对应的目标位姿和地图点构建得到的待优化状态矩阵进行相乘，得到待处理残差矩阵，接着，将先验残差与待处理残差矩阵进行相减得到先验残差项。在得到先验残差项、运动学二次残差、关键惯性数据残差和关键视觉重投影误差之后可以得到二次残差公式，并构建最小二乘问题，具体的二次残差公式如下所示：
其中，rkin表示运动学二次残差，rimu表示惯性数据残差，rcam表示视觉重投影误差，χ表示为目标位姿和对应的地图点，具体包括如下状态量：
χ＝[xn，xn+1，…，xn+N，λm，λm+1，…，λm+M]
其中，xn表示在第n个目标位姿且为滑动窗口中的起始位姿，N表示滑动窗口中的目标位姿的数量，为相机的位置，/>为相机的姿态，/>为惯性测量单元测量得到的移动速度，/>为惯性测量单元中加速度计的测量偏置数据，/>为惯性测量单元中陀螺仪的测量偏置数据。而λm表示第m个地图点，且为被滑动窗口内所有关键图像所观测到的起始地图点，M是被滑动窗口内所有关键图像所观测到的地图点数量。rp为先验残差，Jp为先验残差对应的雅可比矩阵。
通过采用基于滑动窗口的边缘化方式对惯性测量数据、运动学数据、关键图像进行紧耦合，利用历史惯性测量数据、惯性测量数据、历史关键图像和当前时刻图像计算关键惯性数据残差和关键视觉重投影残差，然后将运动学二次残差、关键惯性数据残差和关键视觉重投影残差合并至滑动窗口中，对目标位姿和当前时刻图像对应的地图点进行优化。随着时间的增加，惯性测量数据、运动学数据和关键图像的数据量逐渐增加，导致计算复杂度越来越高，因此，可以采用边缘化的方式进行化处理，在移除关键图像对应的历史关键位姿和历史关键地图点时，利用滑动窗口将相邻关键图像关联的约束关系作为先验项加入位姿和地图点的优化问题中，从而能够避免丢失重要信息，当需要丢弃历史数据，则可以用先验残差项代替需丢弃的化数据，以完成历史关键图像对应的关键信息的传递。
在构建最小二乘问题之后，可以利用迭代求函数极值的算法进行求解，得到相机在当前时刻的目标位姿。通过使用先验残差项、运动学残差、惯性数据残差和视觉重投影误差，对目标位姿以及当前时刻图像对应的地图点进行优化处理，可以得到优化后的关键位姿和关键地图点。由于这些残差量代表了不同来源的误差信息，通过融合多源信息的误差信息，可以提高位姿估计以及地图点估计的准确性和稳定性，得到更为准确的关键位姿和关键地图点，进而能够提高地图构建的准确性。
需要注意的是，在实际应用中，预积分处理和图像特征提取和特征匹配处理的具体方法可能因具体的算法、传感器和应用场景有所不同。因此，在实际的应用场景中，可以根据具体情况进行调整和优化。
在一实施例中，在计算相机的运动学二次残差的过程中，可以先获取历史关键图像对应时刻的历史关键位姿，根据历史关键位姿和目标位姿计算得到相机的关键位姿变换量；接着，根据上一时刻运动学数据和当前时刻运动学数据，计算得到相机的关键运动增量数据；然后根据关键运动增量数据和关键位姿变换量，计算得到相机的运动学二次残差。
在一实施例中，历史关键位姿可以是历史关键图像对应时刻的初始位姿，也可以是历史关键图像对应时刻的目标位姿。通过比较相机在历史关键位姿和在当前时刻的目标位姿，可以计算得到调整后的位姿在两个关键图像的间隔时长所产生的关键位姿变化量，由于关键图像是指局部时间上具有代表性的图像，因此，利用相邻两个关键图像确定出的关键位姿变化量能够得到更为准确的相机增量运动。
在一实施例中，上一时刻运动学数据可以是指上一个非关键图像所对应的运动学数据，也可以是指上一个关键图像所对应的运动学数据。通过比较上一时刻运动学数据和当前时刻运动学数据之间的数值偏差，即关键运动增量数据，进而通过比较由运动学数据解算得到的相机增量运动与通过调整后的目标位置预测估计的相机增量运动之间的偏差，即利用关键运动增量数据和关键位姿变换量计算得出运动学二次残差，有助于更为准确地确定出由运动学数据解算得到的相机运动(即实际观测到的相机运动)与通过运动模型和目标位姿预测的相机运动之间的差异，进而能够对目标位姿和当前时刻图像对应的地图点进行优化得到更为准确的关键位姿和关键地图点。
参照图14，图14示出了本申请一实施例的用于双足机器人的数据处理方法的实际应用场景的示意图。在数据输入阶段中获取运动学数据、惯性测量数据和图像，还可以获取上一时刻的历史位姿。然后进入实时检测阶段，可以先对地图进行初始化，然后实时检测相机拍摄得到的图像，对上一时刻图像和当前时刻图像进行特征提取和特征匹配，得到视觉重投影误差；同时还可以对惯性测量数据进行积分处理，得到腰部初始位姿，然后利用腰部初始位姿和运动学数据，计算得到初始位姿，接着利用初始位姿、历史位姿、上一时刻运动学数据和当前时刻运动学数据进行计算，得到运动学残差。并且，惯性测量数据进行预积分处理，得到惯性数据残差。然后，可以根据运动学残差、惯性数据残差和视觉重投影误差对初始位姿进行联合优化，得到目标位姿。另外，在对图像进行实时检测时，还会判断当前时刻图像是否可以被确认为关键图像，若当前时刻图像被确认为关键图像，则可以将当前时刻的目标位姿、当前时刻图像对应的地图点与历史关键位姿、历史关键图像对应的地图点进行局部捆绑优化，接着对惯性测量单元(IMU)进行初始化，保证惯性测量数据的准确性，并持续进行尺度优化，尺度是指当前场景中真实物体的物理尺寸与它在图像中多对应的像素距离之间的比例关系，以建立准确的空间位置。因此，通过融合运动学数据、惯性测量数据和图像，对关键图像的目标位姿和地图点进行优化得到关键位姿和关键地图点，能够提高关键位姿和关键地图点的准确性，进而能够有助于在后续的闭环线程、全局优化过程中得到更为准确的位姿和地图点，以构建更为准确的地图。
下面以具体的例子对本申请实施例所提供的用户生成内容的处理方法进行详细的说明。
参照图15所示，图15是一个具体例子提供的用户生成内容的处理方法的整体流程图。在图15中，该用户生成内容的处理方法可以包括但不限于步骤1501至步骤1513。
步骤1501：获取双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据，并获取相机拍摄得到的上一时刻图像和当前时刻图像。
在本步骤中，上一时刻运动学数据可以包括上一时刻足部受力数据、上一时刻下肢运动数据和上一时刻躯干运动数据；当前时刻运动学数据包括当前时刻足部受力数据、当前时刻下肢运动数据和当前时刻躯干运动数据。
步骤1502：获取相机在当前时刻的初始位姿。
本步骤中，获取相机在当前时刻的初始位姿，可以是通过双足机器人的数据(如运动学数据和惯性测量数据等)进行计算得到或者通过相机在历史时刻的历史位姿进行预测得到，此处不作具体限定。例如，可以对上一时刻至当前时刻的惯性测量数据进行积分处理，得到腰部初始位姿数据，接着，根据从双足机器人的腰部至相机的第四运动链，对当前时刻运动学数据中的当前时刻躯干运动数据进行数据转换，得到躯干初始转换数据；然后根据腰部初始位姿数据和躯干初始转换数据，计算得到相机在当前时刻的初始位姿；又如，当前时刻运动学数据可以包括当前时刻下肢运动数据和当前时刻躯干运动数据，从而可以利用由相机在历史时刻的历史位姿训练得到的位姿预测模型，对当前时刻下肢运动数据和当前时刻躯干运动数据进行位姿预测，得到相机在当前时刻的初始位姿。另外，在本步骤结束后，可以执行步骤1503，也可以同时执行步骤1503和步骤1510。
步骤1503：根据上一时刻足部受力数据确定上一时刻支撑足，并当前时刻足部受力数据确定当前时刻支撑足。
本步骤中，双足机器人的支撑状态包括左足单足支撑状态、右足单足支撑状态以及左右足同时支撑状态，因此，双足机器人在左足单足支撑状态下的支撑足为左足，在右足单足支撑状态下双足机器人的支撑足为右足，而在左右足同时支撑状态下双足机器人的支撑足为左右双足。其中，上一时刻支撑足与当前时刻支撑足可以相同或者不同。
步骤1504：根据从上一时刻支撑足至双足机器人的腰部的第一运动链，对上一时刻下肢运动数据进行数据转换，得到上一时刻下肢转换数据。
步骤1505：根据从当前时刻支撑足至双足机器人的腰部的第二运动链，对当前时刻下肢运动数据进行数据转换，得到当前时刻下肢转换数据。
步骤1506：根据从双足机器人的腰部至相机的第三运动链，对上一时刻躯干运动数据和当前时刻躯干运动数据分别进行数据转换，得到上一时刻躯干转换数据和当前时刻躯干转换数据。
步骤1507：根据上一时刻支撑足和当前时刻支撑足数据确定足部位姿变换数据。
步骤1508：根据上一时刻下肢转换数据和上一时刻躯干转换数据进行矩阵转置，得到上一时刻位姿转置数据。
步骤1509：将双足位姿变换数据、上一时刻位姿转置数据、当前时刻下肢转换数据和当前时刻躯干转换数据进行相乘，得到相机的运动增量数据。
步骤1510：获取相机在上一时刻的历史位姿，并根据历史位姿和初始位姿计算得到相机的位姿变化量。
步骤1511：根据运动增量数据和位姿变化量，计算得到相机的运动学残差。
在本步骤中，相机的运动增量数据包括位移增量数据和旋转增量数据，而相机的位姿变化量包括位移变化数据和旋转变化数据，从而运动学残差可以通过以下步骤得到：先将位移增量数据和位移变化数据进行差值计算，得到位移变化值；接着根据旋转增量数据和旋转变化数据进行对数映射，得到姿态变化值；然后根据位移变化值和姿态变化值构建矩阵，得到相机的运动学残差。
步骤1512：对惯性测量数据进行预积分处理得到惯性数据残差，并将上一时刻图像和当前时刻图像进行特征提取和特征匹配处理得到视觉重投影误差。
步骤1513：根据运动学残差、惯性数据残差和视觉重投影误差，计算得到相机在当前时刻的目标位姿。
步骤1514：判断当前时刻图像是否为关键图像？若是，执行步骤1515，若否，执行步骤1519。
在本步骤中，关键图像可以通过判断预设的检测条件是否满足进行确认，预设的检测条件可以包括以下情况的一种或多种：已确定的关键图像的数量小于预设图像数目、距离上一个关键图像的时间间隔大于预设时间间隔、当前时刻图像进行特征匹配后得到的三维点数目大于预设三维点阈值等。
步骤1515：获取历史关键图像和历史关键图像对应时刻的历史运动学数据、历史惯性测量数据和历史关键位姿。
在本步骤中，历史关键图像、历史关键图像对应时刻的历史运动学数据、历史惯性测量数据和历史关键位姿可以存储于机器人终端中，也可以存储于服务器中。当历史关键图像、历史关键图像对应时刻的历史运动学数据、历史惯性测量数据和历史关键位姿均存储于机器人终端中，则机器人终端可以将对应数据发送至服务器中。
步骤1516：根据上一时刻运动学数据和当前时刻运动学数据，计算得到相机的关键运动增量数据。
步骤1517：根据关键运动增量数据和关键位姿变换量，计算得到相机的运动学二次残差。
步骤1518：根据运动学二次残差、历史运动学数据、历史惯性测量数据、历史关键图像和当前时刻图像，对目标位姿以及当前时刻图像对应的地图点进行优化处理，得到优化后的关键位姿和关键地图点。
步骤1519：结束步骤流程。
本实施例中，通过包括前面步骤1501至步骤1519的用于双足机器人的数据处理方法，通过先获取双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据，还可以获取设置于双足机器人头部位置的相机所拍摄得到的上一时刻图像和当前时刻图像，另外还可以获取相机在当前时刻的初始位姿，从而可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，因此，可以计算出双足机器人在相邻时刻的运动变化，确定出相机位姿在由运动学数据计算得出的数据(即实际观测变换量)与初始位姿计算得出的数据(即估计变换量)之间的偏差，进而有助于校正相机位姿，降低图像运动模糊的影响。然后，可以根据运动学残差、惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿，通过结合惯性测量数据和多个时刻的运动学数据之间的运动关系，以及上一时刻图像与当前时刻图像之间的视觉投影关系，对初始位姿进行优化调整，能够实现多源信息的紧耦合，从而能够提高位姿估计的鲁棒性，得到更为准确的双足机器人的位姿数据。
下面以一些实际例子说明本申请实施例的应用场景。
需要说明的是，本申请实施例提供的用于双足机器人的数据处理方法，可以应用于双足机器人进行环境探索、双足机器人进行物流配送等不同的应用场景，下面以对双足机器人进行环境探索的场景和对双足机器人进行物流配送的场景为例进行说明。
场景一
本申请实施例提供的用于双足机器人的数据处理方法，可以应用于双足机器人进行环境探索的场景，例如，假设双足机器人在地下岩洞等严酷恶劣环境进行探索作业，双足机器人在运动过程中可以将自身的关节编码器、压力传感器以及惯性测量传感器等传感器测量得到的传感器数据，以及相机拍摄的图像返回至机器人终端，机器人终端则可以将采集得到的传感器数据和图像进行整理后，将双足机器人的上一时刻运动学数据、当前时刻运动学数据和上一时刻至当前时刻的惯性测量数据发送至服务器，还可以将相机拍摄得到的上一时刻图像和当前时刻图像发送至服务器，从而服务器还可以通过机器人终端发送的数据获取得到相机在当前时刻的初始位姿，接着，服务器可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，然后根据运动学残差、惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿，在服务器得到目标位姿之后，可以利用目标位姿和相机拍摄得到的图像，建立双足机器人所处环境的环境地图，并且机器人终端可以在接收得到服务器发送的环境地图后可以对环境地图进行显示，此时，探索作业人员可以根据机器人终端显示的环境地图完成对双足机器人所处环境的环境探索工作和地图测绘工作。
场景二
本申请实施例提供的用于双足机器人的数据处理方法，还可以应用于双足机器人进行物流配送的场景，例如，假设双足机器人通过货物仓库中进行货物运输，双足机器人在运动过程中可以将将自身的关节编码器、压力传感器以及惯性测量传感器等传感器测量得到的传感器数据，以及相机拍摄的图像返回至机器人终端，机器人终端则可以将采集得到的传感器数据和图像进行整理后，将双足机器人的上一时刻运动学数据、当前时刻运动学数据和上一时刻至当前时刻的惯性测量数据发送至服务器，还可以将相机拍摄得到的上一时刻图像和当前时刻图像发送至服务器，从而服务器还可以通过机器人终端发送的数据获取得到相机在当前时刻的初始位姿，接着，服务器可以根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差，然后根据运动学残差、惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿，在服务器得到目标位姿之后，可以利用目标位姿和相机拍摄得到的图像，建立双足机器人所处货物仓库的环境地图，进而服务器可以将环境地图发送至机器人终端并使得机器人终端显示环境地图，此时，物流人员可以根据机器人终端显示的环境地图指引双足机器人将指定货物运输至指定地点，实现双足机器人的物流配送工作。
可以理解的是，虽然上述各个流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本实施例中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，上述流程图中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时间执行完成，而是可以在不同的时间执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
参照图16，本申请实施例还公开了一种用于双足机器人的数据处理装置，该用于双足机器人的数据处理装置1600能够实现前面实施例中的用于双足机器人的数据处理方法，该双足机器人的头部位置设置有相机，该数据处理装置1600包括：
数据获取单元1610，用于获取双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据；
图像获取单元1620，用于获取相机拍摄得到的上一时刻图像和当前时刻图像；
位姿获取单元1630，用于获取相机在当前时刻的初始位姿；
残差计算单元1640，用于根据上一时刻运动学数据、当前时刻运动学数据和初始位姿，计算得到相机的运动学残差；
位姿计算单元1650，用于根据运动学残差、惯性测量数据、上一时刻图像和当前时刻图像，计算得到相机在当前时刻的目标位姿。
在一实施例中，残差计算单元1640还用于：
根据上一时刻运动学数据和当前时刻运动学数据，计算得到相机的运动增量数据；
根据运动增量数据和初始位姿，计算得到相机的运动学残差。
在一实施例中，残差计算单元1640还用于：
获取相机在上一时刻的历史位姿；
根据历史位姿和初始位姿计算得到相机的位姿变化量；
根据运动增量数据和位姿变化量，计算得到相机的运动学残差。
在一实施例中，残差计算单元1640还用于：
将位移增量数据和位移变化数据进行差值计算，得到位移变化值；
根据旋转增量数据和旋转变化数据进行对数映射，得到姿态变化值；
根据位移变化值和姿态变化值，构建得到相机的运动学残差。
在一实施例中，残差计算单元1640还用于：
根据上一时刻足部受力数据确定上一时刻支撑足，并根据当前时刻足部受力数据确定当前时刻支撑足；
根据从上一时刻支撑足至双足机器人的腰部的第一运动链，对上一时刻下肢运动数据进行数据转换，得到上一时刻下肢转换数据；
根据从当前时刻支撑足至双足机器人的腰部的第二运动链，对当前时刻下肢运动数据进行数据转换，得到当前时刻下肢转换数据；
根据从双足机器人的腰部至相机的第三运动链，对上一时刻躯干运动数据和当前时刻躯干运动数据分别进行数据转换，得到上一时刻躯干转换数据和当前时刻躯干转换数据；
根据上一时刻下肢转换数据、当前时刻下肢转换数据、上一时刻躯干转换数据和当前时刻躯干转换数据，计算得到相机的运动增量数据。
在一实施例中，残差计算单元1640还用于：
根据上一时刻支撑足和当前时刻支撑足数据确定足部位姿变换数据；
根据上一时刻下肢转换数据和上一时刻躯干转换数据进行矩阵转置，得到上一时刻位姿转置数据；
将双足位姿变换数据、上一时刻位姿转置数据、当前时刻下肢转换数据和当前时刻躯干转换数据进行相乘，得到相机的运动增量数据。
在一实施例中，位姿获取单元1630还用于：
对惯性测量数据进行积分处理，得到腰部初始位姿数据；
根据从双足机器人的腰部至相机的第四运动链，对当前时刻躯干运动数据进行数据转换，得到躯干初始转换数据；
根据腰部初始位姿数据和躯干初始转换数据，计算得到相机在当前时刻的初始位姿。
在一实施例中，位姿获取单元1630还用于：
对当前时刻下肢运动数据和当前时刻躯干运动数据进行位姿预测，得到相机在当前时刻的初始位姿。
在一实施例中，位姿计算单元1650还用于：
对惯性测量数据进行预积分处理，得到惯性数据残差；
将上一时刻图像和当前时刻图像进行特征提取和特征匹配处理，得到视觉重投影误差；
根据运动学残差、惯性数据残差和视觉重投影误差，计算得到相机在当前时刻的目标位姿。
在一实施例中，用于双足机器人的数据处理装置1600还包括：
二次残差计算单元，用于若当前时刻图像被确认为关键图像，根据上一时刻运动学数据、当前时刻运动学数据和目标位姿，计算得到相机的运动学二次残差。
历史信息获取单元，用于获取历史关键图像和历史关键图像对应时刻的历史惯性测量数据；
数据局部优化单元，用于根据运动学二次残差、历史运动学数据、历史惯性测量数据、历史关键图像和当前时刻图像，对目标位姿以及当前时刻图像对应的地图点进行优化处理，得到优化后的关键位姿和关键地图点。
在一实施例中，二次残差计算单元还用于：
获取历史关键图像对应时刻的历史关键位姿，根据历史关键位姿和目标位姿计算得到相机的关键位姿变换量；
根据上一时刻运动学数据和当前时刻运动学数据，计算得到相机的关键运动增量数据；
根据关键运动增量数据和关键位姿变换量，计算得到相机的运动学二次残差。
需要说明的是，由于本实施例的用于双足机器人的数据处理装置1600能够实现如前面实施例的用于双足机器人的数据处理方法，因此本实施例的用于双足机器人的数据处理装置1600与前面实施例的用于双足机器人的数据处理方法，具有相同的技术原理以及相同的有益效果，为了避免内容重复，此处不再赘述。
参照图17，本申请实施例还公开了一种用于双足机器人的数据处理装置，该用于双足机器人的数据处理装置1700包括：
至少一个处理器1701；
至少一个存储器1702，用于存储至少一个程序；
当至少一个程序被至少一个处理器1701执行时，实现如前面的用于双足机器人的数据处理方法。
本申请实施例还公开了一种计算机可读存储介质，其中存储有处理器可执行的计算机程序，处理器可执行的计算机程序被处理器执行时，用于实现如前面的用于双足机器人的数据处理方法。
本申请实施例还公开了一种计算机程序产品，包括计算机程序或计算机指令，计算机程序或计算机指令存储在计算机可读存储介质中，用于双足机器人的数据处理装置的处理器从计算机可读存储介质读取计算机程序或计算机指令，处理器执行计算机程序或计算机指令，使得用于双足机器人的数据处理装置执行如前面的用于双足机器人的数据处理方法。
本申请的说明书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例例如能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、***、产品或装置不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或装置固有的其他步骤或单元。
应当理解，在本申请中，“至少一个(项)”是指一个或者多个，“多个”是指两个或两个以上。“和/或”，用于描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：只存在A，只存在B以及同时存在A和B三种情况，其中A，B可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。“以下至少一项(个)”或其类似表达，是指这些项中的任意组合，包括单项(个)或复数项(个)的任意组合。例如，a，b或c中的至少一项(个)，可以表示：a，b，c，“a和b”，“a和c”，“b和c”，或“a和b和c”，其中a，b，c可以是单个，也可以是多个。
在本申请所提供的几个实施例中，应该理解到，所揭露的***、装置和方法，可以通过其他的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口、装置或单元的间接耦合或通信连接，可以是电性、机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机装置(可以是个人计算机、服务器或者网络装置等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，简称ROM)、随机存取存储器(Random Access Memory，简称RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
对于上述方法实施例中的步骤编号，仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

Claims (15)

1.一种用于双足机器人的数据处理方法，其特征在于，所述双足机器人的头部位置设置有相机，所述方法包括以下步骤：
获取所述双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据；
获取所述相机拍摄得到的上一时刻图像和当前时刻图像；
获取所述相机在当前时刻的初始位姿；
根据所述上一时刻运动学数据、所述当前时刻运动学数据和所述初始位姿，计算得到所述相机的运动学残差；
根据所述运动学残差、所述惯性测量数据、所述上一时刻图像和所述当前时刻图像，计算得到所述相机在当前时刻的目标位姿。
2.根据权利要求1所述的方法，其特征在于，所述根据所述上一时刻运动学数据、所述当前时刻运动学数据和所述初始位姿，计算得到所述相机的运动学残差，包括：
根据所述上一时刻运动学数据和所述当前时刻运动学数据，计算得到所述相机的运动增量数据；
根据所述运动增量数据和所述初始位姿，计算得到所述相机的运动学残差。
3.根据权利要求2所述的方法，其特征在于，所述根据所述运动增量数据和所述初始位姿，计算得到所述相机的运动学残差，包括：
获取所述相机在上一时刻的历史位姿；
根据所述历史位姿和所述初始位姿计算得到所述相机的位姿变化量；
根据所述运动增量数据和所述位姿变化量，计算得到所述相机的运动学残差。
4.根据权利要求3所述的方法，其特征在于，所述运动增量数据包括位移增量数据和旋转增量数据，所述位姿变化量包括位移变化数据和旋转变化数据；
所述根据所述运动增量数据和所述位姿变化量，计算得到所述相机的运动学残差，包括：
将所述位移增量数据和所述位移变化数据进行差值计算，得到位移变化值；
根据所述旋转增量数据和所述旋转变化数据进行对数映射，得到姿态变化值；
根据所述位移变化值和所述姿态变化值，构建得到所述相机的运动学残差。
5.根据权利要求2所述的方法，其特征在于，所述上一时刻运动学数据包括上一时刻足部受力数据、上一时刻下肢运动数据和上一时刻躯干运动数据，所述当前时刻运动学数据包括当前时刻足部受力数据、当前时刻下肢运动数据和当前时刻躯干运动数据；
所述根据所述上一时刻运动学数据和所述当前时刻运动学数据，计算得到所述相机的运动增量数据，包括：
根据所述上一时刻足部受力数据确定上一时刻支撑足，并根据所述当前时刻足部受力数据确定当前时刻支撑足；
根据从所述上一时刻支撑足至所述双足机器人的腰部的第一运动链，对所述上一时刻下肢运动数据进行数据转换，得到上一时刻下肢转换数据；
根据从所述当前时刻支撑足至所述双足机器人的腰部的第二运动链，对所述当前时刻下肢运动数据进行数据转换，得到当前时刻下肢转换数据；
根据从所述双足机器人的腰部至所述相机的第三运动链，对所述上一时刻躯干运动数据和所述当前时刻躯干运动数据分别进行数据转换，得到上一时刻躯干转换数据和当前时刻躯干转换数据；
根据所述上一时刻下肢转换数据、所述当前时刻下肢转换数据、所述上一时刻躯干转换数据和所述当前时刻躯干转换数据，计算得到所述相机的运动增量数据。
6.根据权利要求5所述的方法，其特征在于，所述根据所述上一时刻下肢转换数据、所述当前时刻下肢转换数据、所述上一时刻躯干转换数据和所述当前时刻躯干转换数据，计算得到所述相机的运动增量数据，包括：
根据所述上一时刻支撑足和所述当前时刻支撑足数据确定足部位姿变换数据；
根据所述上一时刻下肢转换数据和所述上一时刻躯干转换数据进行矩阵转置，得到上一时刻位姿转置数据；
将所述双足位姿变换数据、所述上一时刻位姿转置数据、所述当前时刻下肢转换数据和所述当前时刻躯干转换数据进行相乘，得到所述相机的运动增量数据。
7.根据权利要求1所述的方法，其特征在于，所述当前时刻运动学数据包括当前时刻躯干运动数据，所述获取所述相机在当前时刻的初始位姿，包括：
对所述惯性测量数据进行积分处理，得到腰部初始位姿数据；
根据从所述双足机器人的腰部至所述相机的第四运动链，对所述当前时刻躯干运动数据进行数据转换，得到躯干初始转换数据；
根据所述腰部初始位姿数据和所述躯干初始转换数据，计算得到所述相机在当前时刻的初始位姿。
8.根据权利要求1所述的方法，其特征在于，所述当前时刻运动学数据包括当前时刻下肢运动数据和当前时刻躯干运动数据，所述获取所述相机在当前时刻的初始位姿，包括：
对所述当前时刻下肢运动数据和所述当前时刻躯干运动数据进行位姿预测，得到所述相机在当前时刻的初始位姿。
9.根据权利要求1所述的方法，其特征在于，所述根据所述运动学残差、所述惯性测量数据、所述上一时刻图像和所述当前时刻图像，计算得到所述相机在当前时刻的目标位姿，包括：
对所述惯性测量数据进行预积分处理，得到惯性数据残差；
将所述上一时刻图像和所述当前时刻图像进行特征提取和特征匹配处理，得到视觉重投影误差；
根据所述运动学残差、所述惯性数据残差和所述视觉重投影误差，计算得到所述相机在当前时刻的目标位姿。
10.根据权利要求1所述的方法，其特征在于，所述根据所述运动学残差、所述惯性测量数据、所述上一时刻图像和所述当前时刻图像，计算得到所述相机在当前时刻的目标位姿之后，所述方法还包括：
若所述当前时刻图像被确认为关键图像，根据所述上一时刻运动学数据、所述当前时刻运动学数据和所述目标位姿，计算得到所述相机的运动学二次残差；
获取历史关键图像和所述历史关键图像对应时刻的历史惯性测量数据；
根据所述运动学二次残差、所述惯性测量数据、所述历史惯性测量数据、所述历史关键图像和所述当前时刻图像，对所述目标位姿以及所述当前时刻图像对应的地图点进行优化处理，得到优化后的关键位姿和关键地图点。
11.根据权利要求10所述的方法，其特征在于，所述根据所述上一时刻运动学数据、所述当前时刻运动学数据和所述目标位姿，计算得到所述相机的运动学二次残差，包括：
获取所述历史关键图像对应时刻的历史关键位姿，根据所述历史关键位姿和所述目标位姿计算得到所述相机的关键位姿变换量；
根据所述上一时刻运动学数据和所述当前时刻运动学数据，计算得到所述相机的关键运动增量数据；
根据所述关键运动增量数据和所述关键位姿变换量，计算得到所述相机的运动学二次残差。
12.一种用于双足机器人的数据处理装置，其特征在于，所述双足机器人的头部位置设置有相机，所述数据处理装置包括：
数据获取单元，用于获取所述双足机器人的上一时刻运动学数据、当前时刻运动学数据、上一时刻至当前时刻的惯性测量数据；
图像获取单元，用于获取所述相机拍摄得到的上一时刻图像和当前时刻图像；
位姿获取单元，用于获取所述相机在当前时刻的初始位姿；
残差计算单元，用于根据所述上一时刻运动学数据、所述当前时刻运动学数据和所述初始位姿，计算得到所述相机的运动学残差；
位姿计算单元，用于根据所述运动学残差、所述惯性测量数据、所述上一时刻图像和所述当前时刻图像，计算得到所述相机在当前时刻的目标位姿。
13.一种用于双足机器人的数据处理装置，其特征在于，包括：
至少一个处理器；
至少一个存储器，用于存储至少一个程序；
当至少一个所述程序被至少一个所述处理器执行时实现如权利要求1至11任意一项所述的用于双足机器人的数据处理方法。
14.一种计算机可读存储介质，其特征在于，其中存储有处理器可执行的计算机程序，所述处理器可执行的计算机程序被处理器执行时用于实现如权利要求1至11任意一项所述的用于双足机器人的数据处理方法。
15.一种计算机程序产品，包括计算机程序或计算机指令，其特征在于，所述计算机程序或所述计算机指令存储在计算机可读存储介质中，用于双足机器人的数据处理装置的处理器从所述计算机可读存储介质读取所述计算机程序或所述计算机指令，所述处理器执行所述计算机程序或所述计算机指令，使得所述用于双足机器人的数据处理装置执行如权利要求1至11任意一项所述的用于双足机器人的数据处理方法。
CN202311013119.8A 2023-08-11 2023-08-11 用于双足机器人的数据处理方法、装置及存储介质 Pending CN117213475A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311013119.8A CN117213475A (zh) 2023-08-11 2023-08-11 用于双足机器人的数据处理方法、装置及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311013119.8A CN117213475A (zh) 2023-08-11 2023-08-11 用于双足机器人的数据处理方法、装置及存储介质

Publications (1)

Publication Number Publication Date
CN117213475A true CN117213475A (zh) 2023-12-12

Family

ID=89037846

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311013119.8A Pending CN117213475A (zh) 2023-08-11 2023-08-11 用于双足机器人的数据处理方法、装置及存储介质

Country Status (1)

Country Link
CN (1) CN117213475A (zh)

Similar Documents

Publication Publication Date Title
CN110125928A (zh) 一种基于前后帧进行特征匹配的双目惯导slam***
Howard Real-time stereo visual odometry for autonomous ground vehicles
Hu et al. A sliding-window visual-IMU odometer based on tri-focal tensor geometry
CN107193279A (zh) 基于单目视觉和imu信息的机器人定位与地图构建***
CN109166149A (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与***
CN112219087A (zh) 位姿预测方法、地图构建方法、可移动平台及存储介质
CN116205947B (zh) 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质
CN110717927A (zh) 基于深度学习和视惯融合的室内机器人运动估计方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN111795686A (zh) 一种移动机器人定位与建图的方法
CN111354043A (zh) 一种基于多传感器融合的三维姿态估计方法及装置
Campos et al. Fast and robust initialization for visual-inertial SLAM
CN114234967B (zh) 一种基于多传感器融合的六足机器人定位方法
CN113899364B (zh) 定位方法及装置、设备、存储介质
JP2003271975A (ja) 平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置
CN112101160A (zh) 一种面向自动驾驶场景的双目语义slam方法
Bloesch State estimation for legged robots-kinematics, inertial sensing, and computer vision
CN115962773A (zh) 一种移动机器人同步定位与地图构建方法、装置和设备
Steder et al. Learning maps in 3d using attitude and noisy vision sensors
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及***
US10977810B2 (en) Camera motion estimation
CN116577801A (zh) 一种基于激光雷达和imu的定位与建图方法及***
CN117213475A (zh) 用于双足机器人的数据处理方法、装置及存储介质
CN113847907A (zh) 定位方法及装置、设备、存储介质
Li et al. A stereo-based visual-inertial odometry for slam

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication