CN116394265B - 一种姿态传感器标定方法、装置、设备及存储介质 - Google Patents

Abstract

本申请实施例属于遥操作技术领域，涉及一种姿态传感器标定方法，以得到关节坐标系与姿态传感器坐标系之间的转换关系，包括下述步骤：设定关节坐标系；获取关节在预设姿态下，姿态传感器在关节坐标系的标定投影矩阵；对投影矩阵进行处理，以得到满足正交关系的与投影矩阵最接近的正交矩阵。本申请实施例还涉及标定装置、设备及存储介质。本申请的技术方案可以降低对部分姿态传感器安装精度要求。

Description

一种姿态传感器标定方法、装置、设备及存储介质

技术领域

本申请涉及遥操作技术领域，尤其涉及一种姿态传感器标定方法、装置、设备及存储介质。

背景技术

随着科技的发展，机器人应用领域越发广泛，在一些复杂和危险环境中，需要机器人灵活性更高，具有更高的仿人工作的能力，为此，遥操作机器人应运而生。遥操作机器人是通过在主端操作者身上安装姿态传感器，比如，惯性传感器（Inertial MeasurementUnit IMU），操作人员在另一真实或者虚拟场景下进行目标任务操作，通过IMU捕捉操作人员在操作过程中的动作并采集相应的运动数据发送给控制器，控制器根据运动数据生成运动控制指令等，以控制机器人完成目标任务，从而实现遥操作的目的。

然而，现有的遥操作系统，往往需要将IMU按照出厂设置固定在操作者预设位置，这造成穿戴调试过程中需要浪费很多时间；另外，针对现有的一些基于IMU的传感器标定方法，往往标定精度又不够准确。

发明内容

本申请实施例的目的在于提出一种姿态传感器标定方法、装置、设备及存储介质，以降低对部分姿态传感器安装精度要求。

第一方面，本申请实施例提供一种姿态传感器标定方法，采用了如下所述的技术方案：

一种姿态传感器标定方法，以得到关节坐标系与姿态传感器坐标系之间的转换关系，所述方法包括下述步骤：

设定关节坐标系；

求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵；

对所述投影矩阵进行处理，以得到满足正交关系的与所述投影矩阵最接近的正交矩阵；其中，

所述预设姿态包括：第一预设姿态、第二预设姿态、第三预设姿态、第四预设姿态和第五预设姿态；

所述第一预设姿态为双臂位于身体两侧下垂状态；

所述第二预设姿态为双臂位于身体两侧且向前伸状态;

所述第三预设姿态为双臂向身体两侧张开状态；

所述第四预设姿态为双臂位于身体两侧并拢且弯曲状态；

所述第五预设姿态为双臂位于身体两侧下垂且手腕面向身体前胸转动后的状态。

进一步的，所述对所述投影矩阵进行处理，以得到满足正交关系的与所述投影矩阵最接近的矩阵包括下述步骤：

对所述投影矩阵进行奇异值分解，得到U,V矩阵；

将U乘以V的转置得到所述正交矩阵。

进一步的，所述关节坐标系包括肩关节坐标系和肘关节坐标系；以及

所述姿态传感器包括：上姿态传感器和下姿态传感器；其中，

所述上姿态传感器设置在肘部；所述下姿态传感器设置在腕部。

进一步的，所述关节坐标系还包括肩部的基座坐标系；以及

所述姿态传感器还包括：基座姿态传感器；所述基座姿态传感器设置在人体胸部或背部；且所述基座姿态传感器与所述肩关节坐标系对齐。

进一步的，所述求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵包括下述步骤：

求取在所述第一预设姿态和所述第二预设姿态下，上姿态传感器的第一读数差值；求取在所述第一预设姿态和所述第三预设姿态下，所述上姿态传感器的第二读数差值；基于所述第一读数差值和所述第二读数差值求取所述上姿态传感器在肩关节坐标系下的上标定投影矩阵；和/或

求取在所述第一预设姿态和所述第二预设姿态下，下姿态传感器的第三读数差值；求取在所述第一预设姿态和所述第三预设姿态下，所述下姿态传感器的第四读数差值；基于所述第三读数差值和所述第四读数差值求取所述下姿态传感器在肩关节坐标系下的第一下标定投影矩阵；和/或

求取在所述第一预设姿态和所述第四预设姿态下，所述下姿态传感器的第五读数差值；求取在所述第一预设姿态和所述第五预设姿态下，所述下姿态传感器的第六读数差值；基于所述第五读数差值和所述第六读数差值求取所述下姿态传感器在肘部关节坐标系下的第二下标定投影矩阵。

进一步的，v当所述标定投影矩阵包括所述第二下标定投影矩阵，所述求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵还包括下述步骤：

基于所述第二下标定投影矩阵和所述肩关节坐标系在所述下姿态传感器坐标系的投影矩阵；求取所述肩关节坐标系在所述肘关节坐标系的肩部标定投影矩阵；和/或

所述求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵还包括下述步骤：

基于基座传感器坐标系和所述肩关节坐标系，求取所述基座传感器坐标系在所述肩关节坐标系的基座投影矩阵；

获取所述基座传感器的基座姿态读数；

基于所述基座姿态读数和所述基座投影矩阵，求取所述基座姿态传感器坐标系在所述肩关节坐标系下的基座标定投影矩阵。

第二方面，本申请实施例提供一种姿态传感器标定装置，以得到关节坐标系与姿态传感器坐标系之间的转换关系，所述装置包括：

坐标设定模块，用于设定关节坐标系；

投影求取模块，用于获取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵；

结果修正模块，用于对所述投影矩阵进行处理，以得到满足正交关系的与所述投影矩阵最接近的正交矩阵。

第三方面，本申请实施例提供一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器执行所述计算机程序时实现上面任一项所述的姿态传感器标定方法的步骤。

第四方面，本申请实施例提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上面任一项所述的姿态传感器标定方法的步骤。

与现有技术相比，本申请实施例主要有以下有益效果：

本申请实施例通过设定关节坐标系；求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵；对投影矩阵进行处理，以得到满足正交关系的与所述投影矩阵最接近的正交矩阵，从而得到姿态传感器进行标定，免去了对上姿态传感器和下姿态传感器安装精度要求，提高姿态传感器标定效率。

附图说明

为了更清楚地说明本申请中的方案，下面将对本申请实施例描述中所需要使用的附图作一个简单介绍，显而易见地，下面描述中的附图是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1A是本申请可以应用于其中的示例性系统架构图；

图1B为本申请可以应用于其中的遥操作系统的示例性系统架构图；

图2A是本申请第一预设姿态的一个实施例的示意图；

图2B是本申请第二预设姿态的一个实施例的示意图；

图2C是本申请第三预设姿态的一个实施例的示意图；

图2D是本申请第四预设姿态的一个实施例的示意图；

图2E是本申请第五预设姿态的一个实施例的示意图；

图3是本申请的姿态传感器标定方法的一个实施例的流程示意图；

图4 是本申请的姿态传感器标定装置的一个实施例的结构示意图；

图5是本申请的计算机设备的一个实施例的结构示意图。

具体实施方式

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同；本文中在申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请；本申请的说明书和权利要求书及上述附图说明中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。本申请的说明书和权利要求书或上述附图中的术语“第一”、“第二”等是用于区别不同对象，而不是用于描述特定顺序。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。

为了使本技术领域的人员更好地理解本申请方案，下面将结合附图，对本申请实施例中的技术方案进行清楚、完整地描述。

如图1A所示，图1A是本申请可以应用于其中的示例性系统架构图。

如图1A所示，系统架构100可以包括终端设备101，网络104和服务器105；以及传感器（有关传感器的描述可以参见后面实施例中基于遥操作的训练系统中的从端传感器的描述）。网络104用以在终端设备101和服务器105之间提供通信链路的介质。网络104可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。

用户可以使用终端设备101通过网络104与服务器105交互，以接收或发送消息等。终端设备101上可以安装有各种通讯客户端应用，例如网页浏览器应用、购物类应用、搜索类应用、即时通信工具、邮箱客户端、社交平台软件等。

终端设备101可以是具有显示屏并且支持网页浏览的各种电子设备，包括但不限于智能手机、平板电脑、电子书阅读器、MP3播放器( Moving Picture Experts GroupAudio Layer III，动态影像专家压缩标准音频层面3 )、MP4( Moving Picture ExpertsGroup Audio Layer IV，动态影像专家压缩标准音频层面4 )播放器、膝上型便携计算机和台式计算机等等。

服务器105可以是提供各种服务的服务器，例如对终端设备101上显示的页面提供支持的后台服务器。

需要说明的是，本申请实施例所提供的姿态传感器标定方法一般由服务器/终端设备执行，相应地，姿态传感器标定装置一般设置于服务器/终端设备中。

应该理解，图1A中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。

如图1B所示，图1B为本申请可以应用于其中的遥操作系统的示例性系统架构图。

本申请实施例还提供一种遥操作系统100，该系统包括：N个姿态传感器110、机器人120和控制器130，其中，N为大于等于2的整数。

N个姿态传感器110和机器人120可以分别与控制器130通过有线或者无线的方式通信连接。

需要指出的是，上述无线连接方式可以包括但不限于3G/4G/5G连接、WiFi连接、蓝牙连接、WiMAX连接、Zigbee连接、UWB( ultra wideband )连接、以及其他现在已知或将来开发的无线连接方式。

N个姿态传感器110，用于分别采集操作者N个部位的N组当前运动反馈数据。

上述姿态传感器110可以为可直接采集或者基于某些处理后得到运动反馈数据的各种现在已有或将来开发的传感器，比如：IMU。为方便理解，本申请实施例主要以姿态传感器110为IMU110为例进行详细说明。

其中，IMU是惯性测量单元，用于测量目标物相关的运动数据，包括：三维加速度和三维角速度。

具体地，姿态传感器110可以直接固定于操作者关节部位（比如：关节），或者通过可穿戴设备固定于操作者关键部位，比如：姿态传感器预先固定于手臂外骨骼，将手臂外骨骼穿戴在操作者的手臂，从而将姿态传感器固定于操作者的手臂。为方便理解，本申请实施例主要以将姿态传感器直接固定于操作者关键部位为例进行说明。

机器人120，用于基于控制器发送的运动指令，模仿操作者完成目标任务。

具体地，机器人可以但不限于是：6轴、7轴机械臂；具有双臂的人形机器人或具有双臂的人形机器人的上半身；机器人的手爪。为方便理解，本申请实施例以机器人120为七轴机械臂120为例进行详细说明，则下面实施例所述的机器人末端可以是指七轴机械臂的末端关节的输出端，比如：机械臂的末端轴输出端的法兰盘的中心。

控制器130，用于执行本申请实施例所述的姿态传感器标定方法等的方法步骤。

本发明实施例提供的姿态传感器标定方法可以应用于计算机终端(PersonalComputer，PC)；工业控制计算机终端(Industrial Personal Computer， IPC)；移动终端；服务器；包括终端和服务器的系统，并通过终端和服务器的交互实现；可编程逻辑控制器(Programmable Logic Controller，PLC)；现场可编程门阵列(Field-Programmable GateArray，FPGA)；数字信号处理器(Digital SignalProcesser，DSP)或微控制单元(Microcontroller unit，MCU)等类似的控制器中执行。控制器根据预先固定的程序，结合外部的IMU110等采集的数据等生成程序指令。有关控制器的具体限定可以参见下面实施例中姿态传感器标定方法的限定。

具体的，可以应用于如图5所示的计算机设备中，该计算机设备可以是终端,也可以是服务器。该计算机设备包括通过系统总线连接的处理器、存储器、通信接口、显示屏和输入装置。其中，该计算机设备的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的通信接口用于与外部的终端进行有线或无线方式的通信，无线方式可通过WIFI、运营商网络、NFC(近场通信)或其它技术实现。该计算机程序被处理器执行时以实现一种用于遥操作的姿态识别方法。该计算机设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该计算机设备的输入装置可以是显示屏上覆盖的触摸层，也可以是计算机设备外壳上设置的安检、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等等。

其中，服务器可以是独立的物理服务器，也可以是多个物理服务器构成的服务器集群或者分布式系统，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN(Content Delivery Network,内容分发网络)，以及大数据和人工智能平台等基础云计算服务的云服务器。终端可以是智能手机、平板电脑、笔记本电脑、台式计算机、智能音响、智能手表等，但并不局限于此。终端以及服务器可以通过有线或者无线通信方式进行直接或间接地连接，本申请在此不做限定。

在一个实施例中，本申请实施例所述的遥操作系统还可以包括从端传感器140。

从端传感器140，用于采集机器人120执行单任务过程中的单任务观测数据。在一个实施例中，还可以用于采集机器人120执行通用任务过程中的通用观测数据。

通过观测数据可以反应机器人的工作状况，比如：机器人周边的环境或者机器人目标任务的完成情况等等。

具体地，从端传感器可以但不限于是：图像传感器、力/力矩传感器（比如：二维或多维压力传感、触觉传感器）、机器人的关节位置编码器感器（比如：可以基于编码器得到关节运动量，进而可以求取机器人的姿态）和/或电流或电压计（比如：可以基于电流大小得到某个关节的运动速度相关的信息），以及其他任何可采集观测数据的传感器。

在一个实施例中，从端传感器140与机器人120存在预设标定关系，这样通过从端传感器采集的观测数据（比如：单任务观测数据）可以与机器人相互映射。

示例性的，图像传感器和机器人之间可以预先通过眼手标定的方式进行标定；机器人的关节位置编码器需要与关节的输出法兰同轴固定；力/力矩从端传感器需要通过弹性梁固定于关节预设位置等等，以实现从端传感器与机器人的标定。

在一个实施例中，上述从端传感器可以直接向操作者展示观测数据。

在另一个实施例中，该遥操作系统还包括：演示器150。

演示器150，用于将观测数据或经可演示化处理后的观测数据向操作者进行演示，通过演示器的演示帮助操作者更好地了解机器人的工作状况。

具体地，演示器可以直接与从端传感器通信连接；除此之外，演示器也可以通过控制器与从端传感器通信连接。

比如：从端传感器采集的观测数据需要通过控制器进行可演示化处理后，向操作者演示（比如：将观测数据转换为虚拟可视化数据后发送给VR演示）；或者，当控制器为服务器时，需要将观测数据发送给服务器，再通过服务器发送给演示器，以实现远程观测数据传输。

具体地，上述演示器可以但不限于是：显示屏、AR/VR类似虚拟显示设备、施力组件、声音播放器。

基于上面实施例所述的系统，本申请实施例所提供的姿态传感器标定方法一般由上面实施例图1A所示的终端设备/服务器，和/或图1B所示的控制器/执行，相应地，用于姿态传感器标定的装置一般设置于图1A所示的终端设备/服务器，和/或图1B所示的控制器中。

如图3所示，图3是本申请的姿态传感器标定方法的一个实施例的流程示意图；上述姿态传感器标定方法，包括以下步骤：

步骤210设定关节坐标系。

在一个实施例中，控制器可以预先构建人体上肢的肩关节和肘关节坐标系并按照预设地址存储在存储器或者服务器，具体如下：

在一个实施例中，人上肢的肩关节S视为球关节有3个自由度，对应3条旋转轴（旋转轴1、旋转轴2、旋转轴3），建立肩关节坐标系S-XYZ，从而得到肩关节坐标系中XYZ三个轴的方向。

示例性的，可以设肩关节坐标系的X轴方向向下，Y轴方向指向对侧肩部，比如：设左肩关节坐标系的X轴方向向下，则Y轴为指向右侧肩关节的方向。

在一个实施例中，对于人上肢的肘关节E有两个自由度，对应2条旋转轴（旋转轴1、旋转轴2），建立肘关节坐标系E-XYZ，Y对应肘关节的旋转轴1，Z对应肘关节的旋转轴2，Y和Z做向量积得到X轴。

在一个实施例中，对于手臂肩关节的基座，还可以构建肩部的基座坐标系S-XYZ。

步骤220获取关节在预设姿态下，姿态传感器在关节坐标系的标定投影矩阵。

在一个实施例中，如图2A-2E所述，图2A是本申请第一预设姿态的一个实施例的示意图；图2B是本申请第二预设姿态的一个实施例的示意图；图2C是本申请第三预设姿态的一个实施例的示意图；图2D是本申请第四预设姿态的一个实施例的示意图；图2E是本申请第五预设姿态的一个实施例的示意图。具体如下：

如图2A所示，第一预设姿态为双臂位于身体两侧下垂状态；

如图2B所示，第二预设姿态为双臂位于身体两侧且向前伸状态；

如图2C所示，第三预设姿态为双臂向身体两侧张开状态;

如图2D所示，第四预设姿态为双臂位于身体两侧并拢且弯曲状态；

如图2E所述，第五预设姿态为双臂位于身体两侧下垂且手腕面向身体前胸转动后的状态。

在一个实施例中，基于上面实施例所述的遥操作系统，姿态传感器可以包括两个IMU，其中，两个IMU可以分别直接固定在手臂的肘关节（简称“上IMU”或”上姿态传感器”）和腕关节(简称“下IMU”或“下姿态传感器”)。

在一个实施例中，基于上面实施例所述的遥操作系统，姿态传感器也可以包括三个IMU，其中，两个IMU可以分别直接固定在手臂的肘关节（简称“上IMU”或”上姿态传感器”）和腕关节(简称“下IMU”或“下姿态传感器”)；一个IMU固定于操作者的胸部或背部（简称“基座IMU”或“基座姿态传感器），以此作为手臂肩关节的基座，通过加入基座IMU,来消除肩关节转动和肘关节转动之外的运动对上下IMU的影响，从而可以提高标定后姿态传感器的应用范围。

在一个实施例中，根据上面实施例所述，上IMU固定在肘部，下IMU固定在手腕处，基座IMU固定在胸部或背部，参见表1，操作者在5个预设标定姿态下，控制器分别获取对应的上IMU输出的四元数读数qUi(i取值范围0-5)；下IMU输出的四元数读数qLi(i取值范围0-5)，以及基座IMU输出的四元数读数qBi(i取值范围0-5)。

具体地，基座姿态传感器BaseIMU安装要保证基座坐标系与肩关节坐标系对齐，即BaseIMU安装使得传感器的XYZ三个轴与人体肩关节坐标系的三个轴处于平行状态。而对于上IMU固定在肘部，下IMU固定在手腕并没有位置要求，只需要大概固定在肘部和手腕部即可，并没有精度要求。

具体地，本申请实施例中，在五个预设姿态下，上IMU、下IMU和基座IMU的读数可以如表1中所示，表1是本申请在五个预设姿态下上IMU、下IMU和基座IMU的读数示意图表。

表1

其中，由于在五个预设姿态下，基座IMU固定于身体胸部或背部，处于静止状态，因此qB1-qB5的读数相同。

需要说明的是，两个静止的读数通过下述公式提取出轴线的单位方向向量和角度，具体如下：

qU1和qU2之间的变换是因肩关节旋转轴1引起的，相应的变换用四元数表示，；

qU1和qU3之间的变换是因肩关节旋转轴2引起的，相应的变换用四元数表示，；

qL1和qL2之间的变换是因肩关节旋转轴1引起的，相应的变换用四元数表示，；

qL1和qL3之间的变换是因肩关节旋转轴2引起的，相应的变换用四元数表示，；

qL1和qL4之间的变换是因肘关节旋转轴1引起的，相应的变换用四元数表示，；

qL1和qL5之间的变换是因肘关节旋转轴2引起的，相应的变换用四元数表示，。

在一个实施例中，对于上姿态传感器标定而言，步骤220可以包括如下方法步骤：

步骤221求取在第一预设姿态和第二预设姿态下，上姿态传感器的第一读数差值。

步骤222求取在第一预设姿态和第三预设姿态下，上姿态传感器的第二读数差值。

步骤223基于第一读数差值和第二读数差值求取上姿态传感器在肩关节坐标系下的上标定投影矩阵。

具体地，可以通过下述公式提取出轴线的单位方向向量和角度：

qU1和qU2之间的变换是因肩关节旋转轴1引起的，相应的变换可以直接用IMU读数四元数表示，；

qU1和qU3之间的变换是因肩关节旋转轴2引起的，相应的变换用四元数表示，；

具体地，以qU1和qU2之间的变换为例，根据四元数的定义式，可以算出肩关节的旋转轴1在上IMU的投影，即旋转轴1单位向量在上IMU读数为零时的坐标系里的投影u, 具体地，u可以是列向量的形式。

同理，基于上述方法得出肩关节的旋转轴2在下IMU的投影矩阵。

通过旋转轴1和旋转轴2的向量乘积得到肩关节的旋转轴3在分别在上IMU和下IMU的投影。

需要说明的是，上述旋转轴1、旋转轴2和旋转轴3可以根据肩关节预设坐标系的不同对应肩关节坐标系S-XYZ任意轴，比如： X-旋转轴3, Y-旋转轴2, Z-旋转轴1。

基于上面实施例所述，可以得到肩关节坐标系S-XYZ在上姿态传感器坐标系的投影矩阵，经过转置后，得到上IMU在肩关节坐标系里的投影矩阵/>。

在一个实施例中，对于下姿态传感器标定而言，步骤220还可以包括如下方法步骤：

步骤321求取在第一预设姿态和第二预设姿态下，下姿态传感器的第三读数差值。

步骤322求取在第一预设姿态和第三预设姿态下，下姿态传感器的第四读数差值。

步骤323基于第三读数差值和第四读数差值，求取下姿态传感器在肩关节坐标系下的第一下标定投影矩阵。

同理，基于上面实施例所述，基于第三读数差值和第四读数差值可以求取求取下姿态传感器在肩关节坐标系下的第一下标定投影矩阵。

在一个实施例中，对于下姿态传感器标定而言，步骤220还可以包括如下方法步骤：

步骤421求取在第一预设姿态和第四预设姿态下，下姿态传感器的第五读数差值。

步骤422求取在第一预设姿态和第五预设姿态下，下姿态传感器的第六读数差值。

步骤423基于第五读数差值和第六读数差值求取下姿态传感器在肘部关节坐标系下的第二下标定投影矩阵。

同理，基于上面实施例所述，基于第五读数差值和第六读数差值求取下姿态传感器在所述肘部关节坐标系下的第二下标定投影矩阵。

在一个实施例中，对于肘关节坐标系标定而言，步骤220还可以包括如下方法步骤：

步骤521基于第二下标定投影矩阵和肩关节坐标系在下姿态传感器坐标系的投影矩阵，求取肩关节坐标系在肘关节坐标系的肩部标定投影矩阵。

具体地，利用投影之间的如下关系式可计算得到，肩关节坐标系在肘关节坐标系的投影。

。

其中，S-肩关节坐标系 E肘关节坐标系 上-上IMU 下-下IMU。

需要说明的是，上述标定数据可以是四元数形式，也可以是旋转矩阵的形式（比如在Hamilton表达中，可以将四元数形式转换为旋转矩阵的形式）等等，后续运算遵循相应的运算规则即可。

以旋转矩阵为例，则基于某个四元数q0，进行旋转矩阵变换后，可以得到q=[q0,q1,q2,q3] q0是实部，q1,q2,q3是虚部的三个分量。

在Hamilton表达中，ij=k,i²+j²+k²=ijk=-1，在这种定义下，单位四元数转化为旋转矩阵的公式如下：

在一个实施例中，对于基座姿态传感器的标定而言，步骤220还可以包括如下方法步骤：

步骤621基于基座传感器坐标系和肩关节坐标系，求取基座传感器坐标系在所述肩关节坐标系的基座投影矩阵。

在一个实施例中，基于固定后的基座IMU,和预设的肩关节坐标系S-XYZ，求取固定后的基座IMU坐标系在肩关节坐标系的投影矩阵。

示例性的：在某个情况下，当固定后的基座IMU的X’Y’Z’三个轴与肩关节坐标系的S-XYZ三个轴正好对应，即 X’-X; Y’-Y；Z’-Z,且方向相同，则为三个轴投影结果为1的3*3矩阵。

步骤622获取基座传感器的基座姿态读数。

在一个实施例中，基于上面实施例所述，qB1-qB5的读数相同，以qB1作为基座姿态读数。

步骤623基于基座姿态读数和基座投影矩阵，求取基座姿态传感器坐标系在肩关节坐标系下的基座标定投影矩阵。

通过,得到IMU初始姿态（即其四元数读数为单位四元数时的姿态）在肩关节坐标系S-XYZ的标定投影矩阵/>。

步骤230对述投影矩阵进行处理，以得到满足正交关系的与投影矩阵最接近的正交矩阵。

需要说明的是，上述处理方法可以根据需要采用各种现在已有或将来开发的方法，只要使得经过处理后可以得到满足正交关系的与所述投影矩阵最接近的正交矩阵，都属于本申请保护的范围。

为方便理解，本申请实施例下面以基于奇异值分解（Singular ValueDecomposition SVD）处理方法为例进行说明，则步骤230可以包括如下方法步骤：

步骤231对投影矩阵进行SVD分解，得到U,V矩阵。

步骤232将U乘以V的转置，以得到满足正交关系的与所述投影矩阵最接近的正交矩阵。

本申请实施例由于实际使用中无法保证1、2轴线对应动作的垂直，因此需要对投影矩阵进行SVD分解得到 [U,S,V]，将U乘以V的转置,计算/>作为最终的标定投影矩阵。

需要说明的是，对得到的上述结果，/>，/>, />和/或/>投影矩阵分别进行SVD分解，以分解后得到的结果代替原结果最为最终的标定结果。

在一个实施例中，基于上面实施例所述，可以根据需要将qU1, qL1, qB1，，，/>,/>和/>中的全部或者部分结果作为姿态传感器的最终标定结果按照预设地址进行保存。其中，qU1, qL1, qB1为在第一预设姿态下,上姿态传感器、下姿态传感器和基座姿态传感器的读数。

控制器后续基于各姿态传感器的读数，以及上述全部或部分标定结果，可以求取得到各个关节的位置信息，进而基于各个关节的位置信息映射得到机器人各个关节的运动量信息。

比如：在一个实施例中，控制器可以基于上述标定结果将基座IMU采集的运动数据读数转到肩关节坐标系下

基座IMU标定的初始数据也转换到肩关节坐标系下

/>

可以得到在肩关节坐标系中的基座运动的旋转矩阵

控制器还可以基于上述标定结果将上IMU采集的运动后数据读数转到肩关节坐标系下。

控制器可以基于标定结果将上IMU采集的运动后的数据读数转到肩关节坐标系下。

上IMU标定的初始数据即运动前也转换到肩关节坐标系下。

可以得到在肩关节坐标系中的基座和肩关节三个轴线的综合运动的旋转矩阵

那么可以得到肩关节三个轴线的净运动的旋转矩阵

对 进行欧拉角分解，因为前面采用肩关节轴线1-Z轴，肩关节轴线2-Y轴，肩关节轴线3-X轴，所以按照ZYX欧拉角分解，分解得到的Z,Y,X角度分别就是肩关节1，2，3轴的旋转角度。

后续，可以设上述肩关节坐标系中的Z轴对应机器人第一关节的轴线1，Y轴对应机器人第二关节的轴线2，X轴对应机器人第三关节的轴线3（除此之外，上述对应关系也可以任意进行其他设定，比如：Z对应轴线2，Y对应轴线3，X对应轴线1），从而基于此可以进一步映射得到机器人第一、第二、第三关节的运动量。

本申请实施例通过设定关节坐标系；求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵；对投影矩阵进行处理，以得到满足正交关系的与所述投影矩阵最接近的正交矩阵，从而得到姿态传感器进行标定，免去了对上姿态传感器和下姿态传感器安装精度要求，提高姿态传感器标定效率。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，该计算机程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，前述的存储介质可为磁碟、光盘、只读存储记忆体（Read-Only Memory，ROM）等非易失性存储介质，或随机存储记忆体（Random Access Memory，RAM）等。

应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。

进一步参考图4，作为对上述图4所示方法的实现，本申请提供了一种姿态传感器标定装置的一个实施例，该装置实施例与图3所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。

如图4所示，本实施例所述的姿态传感器标定装置300包括：数据获取模块310、数据投影模块320以及运动映射模块330。其中；

坐标设定模块310，用于设定关节坐标系。

投影求取模块320，用于获取关节在预设姿态下，姿态传感器在关节坐标系的标定投影矩阵。

结果修正模块340，用于对投影矩阵进行处理，以得到满足正交关系的与投影矩阵最接近的正交矩阵。

在一个实施例中，结果修正模块340可以包括：

矩阵分解子模块，用于对投影矩阵进行奇异值分解，得到U,V矩阵；

矩阵求取子模块，用于将U乘以V的转置得到正交矩阵。

在一个实施例中，关节坐标系包括肩关节坐标系和肘关节坐标系；以及姿态传感器包括：上姿态传感器和下姿态传感器；其中，上姿态传感器设置在肘部；下姿态传感器设置在腕部。

在一个实施例中，关节坐标系还包括肩部的基座坐标系；以及姿态传感器还包括：基座姿态传感器；基座姿态传感器设置在人体胸部或背部；且基座姿态传感器与肩关节坐标系对齐。

在一个实施例中，预设姿态包括：第一预设姿态、第二预设姿态、第三预设姿态、第四预设姿态和第五预设姿态；

第一预设姿态为双臂位于身体两侧下垂状态；

第二预设姿态为双臂位于身体两侧且向前伸状态;

第三预设姿态为双臂向身体两侧张开状态；

第四预设姿态为双臂位于身体两侧并拢且弯曲状态；

第五预设姿态为双臂位于身体两侧下垂且手腕面向身体前胸转动后的状态。

在一个实施例中，投影求取模块320包括：

第一求取子模块，用于求取在第一预设姿态和第二预设姿态下，上姿态传感器的第一读数差值；求取在第一预设姿态和第三预设姿态下，上姿态传感器的第二读数差值；基于第一读数差值和第二读数差值求取上姿态传感器在肩关节坐标系下的上标定投影矩阵；和/或

第一求取子模块，用于求取在第一预设姿态和第二预设姿态下，下姿态传感器的第三读数差值；求取在第一预设姿态和第三预设姿态下，下姿态传感器的第四读数差值；基于第三读数差值和第四读数差值求取下姿态传感器在肩关节坐标系下的第一下标定投影矩阵；和/或

第三求取子模块，用于求取在第一预设姿态和第四预设姿态下，下姿态传感器的第五读数差值；求取在第一预设姿态和第五预设姿态下，下姿态传感器的第六读数差值；基于第五读数差值和第六读数差值求取下姿态传感器在肘部关节坐标系下的第二下标定投影矩阵。

在一个实施例中，当所述标定投影矩阵包括所述第二下标定投影矩阵，投影求取模块320还包括：

第四求取子模块，用于基于第二下标定投影矩阵和肩关节坐标系在下姿态传感器坐标系的投影矩阵；求取肩关节坐标系在肘关节坐标系的肩部标定投影矩阵。

在一个实施例中，投影求取模块320还包括：

第五求取子模块，用于基于基座传感器坐标系和肩关节坐标系，求取基座传感器坐标系在肩关节坐标系的基座投影矩阵；

读数获取子模块，用于获取基座传感器的基座姿态读数；

第六求取子模块，用于基于基座姿态读数和基座投影矩阵，求取基座姿态传感器坐标系在肩关节坐标系下的基座标定投影矩阵。

为解决上述技术问题，本申请实施例还提供计算机设备。具体请参阅图5，图5为本实施例计算机设备基本结构框图。

所述计算机设备6包括通过系统总线相互通信连接存储器61、处理器62、网络接口63。需要指出的是，图中仅示出了具有组件61-63的计算机设备6，但是应理解的是，并不要求实施所有示出的组件，可以替代的实施更多或者更少的组件。其中，本技术领域技术人员可以理解，这里的计算机设备是一种能够按照事先设定或存储的指令，自动进行数值计算和/或信息处理的设备，其硬件包括但不限于微处理器、专用集成电路(ApplicationSpecific Integrated Circuit，ASIC)、可编程门阵列(Field－Programmable GateArray，FPGA)、数字处理器 (Digital Signal Processor，DSP)、嵌入式设备等。

所述计算机设备可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述计算机设备可以与用户通过键盘、鼠标、遥控器、触摸板或声控设备等方式进行人机交互。

所述存储器61至少包括一种类型的可读存储介质，所述可读存储介质包括闪存、硬盘、多媒体卡、卡型存储器（例如，SD或DX存储器等）、随机访问存储器（RAM）、静态随机访问存储器（SRAM）、只读存储器（ROM）、电可擦除可编程只读存储器（EEPROM）、可编程只读存储器（PROM）、磁性存储器、磁盘、光盘等。在一些实施例中，所述存储器61可以是所述计算机设备6的内部存储单元，例如该计算机设备6的硬盘或内存。在另一些实施例中，所述存储器61也可以是所述计算机设备6的外部存储设备，例如该计算机设备6上配备的插接式硬盘，智能存储卡（Smart Media Card, SMC），安全数字（Secure Digital, SD）卡，闪存卡（FlashCard）等。当然，所述存储器61还可以既包括所述计算机设备6的内部存储单元也包括其外部存储设备。本实施例中，所述存储器61通常用于存储安装于所述计算机设备6的操作系统和各类应用软件，例如姿态传感器标定方法的程序代码等。此外，所述存储器61还可以用于暂时地存储已经输出或者将要输出的各类数据。

所述处理器62在一些实施例中可以是中央处理器（Central Processing Unit，CPU）、控制器、微控制器、微处理器、或其他数据处理芯片。该处理器62通常用于控制所述计算机设备6的总体操作。本实施例中，所述处理器62用于运行所述存储器61中存储的程序代码或者处理数据，例如运行姿态传感器标定方法的程序代码。

所述网络接口63可包括无线网络接口或有线网络接口，该网络接口63通常用于在所述计算机设备6与其他电子设备之间建立通信连接。

本申请还提供了另一种实施方式，即提供一种计算机可读存储介质，所述计算机可读存储介质存储有姿态传感器标定程序，所述运动识别方法程序可被至少一个处理器执行，以使所述至少一个处理器执行如上述的姿态传感器标定方法的步骤。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质（如ROM/RAM、磁碟、光盘）中，包括若干指令用以使得一台终端设备（可以是手机，计算机，服务器，空调器，或者网络设备等）执行本申请各个实施例所述的方法。

显然，以上所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例，附图中给出了本申请的较佳实施例，但并不限制本申请的专利范围。本申请可以以许多不同的形式来实现，相反地，提供这些实施例的目的是使对本申请的公开内容的理解更加透彻全面。尽管参照前述实施例对本申请进行了详细的说明，对于本领域的技术人员来而言，其依然可以对前述各具体实施方式所记载的技术方案进行修改，或者对其中部分技术特征进行等效替换。凡是利用本申请说明书及附图内容所做的等效结构，直接或间接运用在其他相关的技术领域，均同理在本申请专利保护范围之内。

Claims (6)

1.一种姿态传感器标定方法，以得到关节坐标系与姿态传感器坐标系之间的转换关系，其中，关节坐标系包括：肩关节坐标系和肘部关节坐标系；姿态传感器包括：上姿态传感器和下姿态传感器；其中，所述上姿态传感器固定在手臂的肘关节；所述下姿态传感器固定在手臂的腕关节；其特征在于，所述方法包括下述步骤：

设定关节坐标系；

求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵；

对所述标定投影矩阵进行奇异值分解处理，得到U,V矩阵；将U乘以V的转置，以得到满足正交关系的与所述标定投影矩阵最接近的正交矩阵；其中，

所述预设姿态包括：第一预设姿态、第二预设姿态、第三预设姿态、第四预设姿态和第五预设姿态；

所述第一预设姿态为双臂位于身体两侧下垂状态；

所述第二预设姿态为双臂位于身体两侧且向前伸状态;

所述第三预设姿态为双臂向身体两侧张开状态；

所述第四预设姿态为双臂位于身体两侧并拢且弯曲状态；

所述第五预设姿态为双臂位于身体两侧下垂且手腕面向身体前胸转动后的状态；

所述求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵包括如下方法步骤：

求取在所述第一预设姿态和所述第二预设姿态下，上姿态传感器的第一读数差值；求取在所述第一预设姿态和所述第三预设姿态下，所述上姿态传感器的第二读数差值；基于所述第一读数差值和所述第二读数差值求取所述上姿态传感器在肩关节坐标系下的上标定投影矩阵；和/或

求取在所述第一预设姿态和所述第二预设姿态下，下姿态传感器的第三读数差值；求取在所述第一预设姿态和所述第三预设姿态下，所述下姿态传感器的第四读数差值；基于所述第三读数差值和所述第四读数差值求取所述下姿态传感器在肩关节坐标系下的第一下标定投影矩阵；和/或

求取在所述第一预设姿态和所述第四预设姿态下，所述下姿态传感器的第五读数差值；求取在所述第一预设姿态和所述第五预设姿态下，所述下姿态传感器的第六读数差值；基于所述第五读数差值和所述第六读数差值求取所述下姿态传感器在肘部关节坐标系下的第二下标定投影矩阵。

2.根据权利要求1所述的姿态传感器标定方法，其特征在于，所述关节坐标系还包括肩部的基座坐标系；以及

所述姿态传感器还包括：基座姿态传感器；所述基座姿态传感器设置在人体胸部或背部；其中，所述基座姿态传感器作为肩部的基座，则所述肩部的基座坐标系指所述基座姿态传感器的基座姿态传感器坐标系；且所述基座姿态传感器坐标系与所述肩关节坐标系对齐；所述对齐指所述基座姿态传感器坐标系的XYZ三个轴与所述肩关节坐标系的XYZ三个轴处于平行状态。

3.根据权利要求2所述的姿态传感器标定方法，其特征在于，当所述标定投影矩阵包括所述第二下标定投影矩阵，所述求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵还包括下述步骤：

基于所述第二下标定投影矩阵和所述肩关节坐标系在下姿态传感器坐标系的投影矩阵；求取所述肩关节坐标系在肘部关节坐标系的肩部标定投影矩阵；和/或

所述求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵还包括下述步骤：

基于所述基座姿态传感器坐标系和所述肩关节坐标系，求取所述基座姿态传感器坐标系在所述肩关节坐标系的基座投影矩阵；

获取所述基座姿态传感器的基座姿态读数；

基于所述基座姿态读数和所述基座投影矩阵，求取所述基座姿态传感器坐标系在所述肩关节坐标系下的基座标定投影矩阵。

4.一种姿态传感器标定装置，以得到关节坐标系与姿态传感器坐标系之间的转换关系，其中，关节坐标系包括：肩关节坐标系和肘部关节坐标系；姿态传感器包括：上姿态传感器和下姿态传感器；其中，所述上姿态传感器固定在手臂的肘关节；所述下姿态传感器固定在手臂的腕关节；其特征在于，所述装置包括：

坐标设定模块，用于设定关节坐标系；

投影求取模块，用于获取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵；

结果修正模块，用于对所述标定投影矩阵进行奇异值分解处理，得到U,V矩阵；将U乘以V的转置，以得到满足正交关系的与所述标定投影矩阵最接近的正交矩阵；其中，

所述预设姿态包括：第一预设姿态、第二预设姿态、第三预设姿态、第四预设姿态和第五预设姿态；

所述第一预设姿态为双臂位于身体两侧下垂状态；

所述第二预设姿态为双臂位于身体两侧且向前伸状态;

所述第三预设姿态为双臂向身体两侧张开状态；

所述第四预设姿态为双臂位于身体两侧并拢且弯曲状态；

所述第五预设姿态为双臂位于身体两侧下垂且手腕面向身体前胸转动后的状态；

所述求取关节在预设姿态下，姿态传感器在所述关节坐标系的标定投影矩阵包括如下方法步骤：

求取在所述第一预设姿态和所述第二预设姿态下，上姿态传感器的第一读数差值；求取在所述第一预设姿态和所述第三预设姿态下，所述上姿态传感器的第二读数差值；基于所述第一读数差值和所述第二读数差值求取所述上姿态传感器在肩关节坐标系下的上标定投影矩阵；和/或

求取在所述第一预设姿态和所述第二预设姿态下，下姿态传感器的第三读数差值；求取在所述第一预设姿态和所述第三预设姿态下，所述下姿态传感器的第四读数差值；基于所述第三读数差值和所述第四读数差值求取所述下姿态传感器在肩关节坐标系下的第一下标定投影矩阵；和/或

求取在所述第一预设姿态和所述第四预设姿态下，所述下姿态传感器的第五读数差值；求取在所述第一预设姿态和所述第五预设姿态下，所述下姿态传感器的第六读数差值；基于所述第五读数差值和所述第六读数差值求取所述下姿态传感器在肘部关节坐标系下的第二下标定投影矩阵。

5.一种计算机设备，其特征在于，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器执行所述计算机程序时实现权利要求1至3中任一项所述的姿态传感器标定方法的步骤。

6.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现权利要求1至3中任一项所述的姿态传感器标定方法的步骤。