申请/专利权人：深圳清华大学研究院

申请日：2018-08-14

公开（公告）日：2020-11-10

公开（公告）号：CN109186596B

主分类号：G01C21/18(20060101)

分类号：G01C21/18(20060101)

优先权：

专利状态码：有效-授权

法律状态：2020.11.10#授权;2019.02.12#实质审查的生效;2019.01.11#公开

摘要：本发明提供一种IMU测量数据生成方法及装置、计算机装置及可读存储介质，包括获取位姿轨迹数据，其中所述位姿轨迹数据包含若干位姿轨迹信息；对两相邻位姿轨迹信息进行插值处理，以得到一个或多个位于所述两相邻位姿轨迹信息之间的插值处理后的位姿轨迹信息；根据所述位姿轨迹数据中位置信息计算运动模型的加速度信息；并根据所述位姿轨迹数据中旋转信息所述计算运动模型的角速度信息；对所述运动模型执行位姿变换操作。本发明可根据相机的位姿轨迹信息来生成IMU的测量数据，避免了在机器人上安装IMU后涉及的硬件同步问题，以及相机与IMU相对安装位置的问题。

主权项：1.一种IMU测量数据生成方法，其特征在于，所述方法包括：获取位姿轨迹数据，其中所述位姿轨迹数据包含若干位姿轨迹信息，每一位姿轨迹信息包含位置信息及旋转信息；对两相邻位姿轨迹信息进行插值处理，以得到一个或多个位于所述两相邻位姿轨迹信息之间的插值处理后的位姿轨迹信息，并更新所述位姿轨迹数据；根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息；并根据更新后的所述位姿轨迹数据中旋转信息计算所述运动模型的角速度信息；对所述运动模型执行位姿变换操作，以将所述运动模型变换至惯性测量单元坐标系下的运动模型；所述对两相邻位姿轨迹信息进行插值处理包括：利用线性插值算法或多项式函数算法对所述两相邻位姿轨迹信息中的位置信息进行插值操作，利用球面插值对所述两相邻位姿轨迹信息中的旋转信息进行插值操作；所述加速度信息根据下式计算：a[k]＝v[k+1]-v[k]△t；v[k]＝p[k+1]-p[k]△t；所述角速度信息根据下式计算： 其中a[k]表示所述加速度信息，ω表示所述角速度信息，p[k+1]、p[k]表示两相邻位姿轨迹信息中的位置信息，q[k+1]、q[k]表示两相邻位姿轨迹信息中的旋转信息，△t表示两相邻位姿轨迹信息中的时间戳之差，k为自然数；所述对所述运动模型执行位姿变换操作包括：将所述运动模型与齐次变换矩阵相乘，得到所述惯性测量单元坐标系下的运动模型。

全文数据：IMU测量数据生成方法、系统、计算机装置及可读存储介质技术领域本发明涉及机器人领域，尤其涉及一种基于相机位姿的IMU测量数据生成方法、系统、计算机装置及可读存储介质。背景技术本部分旨在为权利要求书及具体实施方式中陈述的本发明的实施方式提供背景或上下文。此处的描述不因为包括在本部分中就承认是现有技术。随着移动机器人在人类生活中不断的使用与发展，清扫机器人、家庭陪伴机器人、送餐机器人等陆续进入公众的视线。当今机器人发展的热点和重点是，机器人要能够通过各种传感器获取环境信息，利用人工智能识别、理解、推理并进行判断和决策来完成一定的任务。SLAMSimultaneousLocalizationAndMapping，同时建图与定位问题可以描述为：将一个机器人放入未知环境中的未知位置，是否有办法让机器人一边逐步描绘出此环境的地图，一边根据已经生成的地图完成对自身位置与姿态的估计。SLAM最早由Smith、Self和Cheeseman于1988年提出，由于其重要的理论与应用价值，被很多学者认为是实现真正全自主移动机器人的关键。而视觉SLAM是一种以视觉传感器为机器人感知设备的SLAM方法。视觉传感器可以分为单目相机、双目相机、RGB-D相机等，其中RGB-D相机是一种能够同时获得彩色图像信息RGB与图像的深度信息Depth的视觉传感器。视觉里程计是SLAM技术的重要组成部分，这一部分主要负责收集传感器的信息，初步的估计出相机的运动轨迹与运动状态即固连的机器人的运动轨迹与运动状态。再初步的估计出以上的信息之后，里程计部分会将此结果传递给SLAM的其它部分，以完成更高精度的估计。IMUInertialMeasurementUnit，惯性测量单元是测量物体三轴的角速率以及线性加速度的装置。一般的，一个IMU包含了三个单轴的加速度计accelerometer和三个单轴的陀螺仪gyroscope。加速度计能够检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于载体坐标系的角速度信号，与加速度计和陀螺仪一起测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。因而，IMU在机器人的导航与控制之中有着很重要的应用价值。在衡量视觉惯性里程计以及视觉惯性SLAM算法效果的领域，以及测试算法可用性的时候，可在机器人上再安装IMU，但是如果想要同时安装上IMU，需要考虑IMU测量数据与相机测量数据硬件同步即测量时间同步的问题，以及相机与IMU相对安装位置的问题，这就涉及到了相机与IMU数据的标定问题，在目前尚没有完美的解决办法。并且对于大多数在视觉SLAM领域的开源数据集来说，没有含有IMU的测量数据。发明内容鉴于上述，本发明提供一种IMU测量数据生成方法、系统、计算机装置及可读存储介质，可基于相位位姿信息来生成IMU数据，有利于解决在机器人安装IMU后涉及的硬件同步问题，以及相机与IMU相对安装位置的问题。一种IMU测量数据生成方法，包括：获取位姿轨迹数据，其中所述位姿轨迹数据包含若干位姿轨迹信息，每一位姿轨迹信息包含位置信息及旋转信息；对两相邻位姿轨迹信息进行插值处理，以得到一个或多个位于所述两相邻位姿轨迹信息之间的插值处理后的位姿轨迹信息，并更新所述位姿轨迹数据；根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息；并根据更新后的所述位姿轨迹数据中旋转信息计算所述运动模型的角速度信息；对所述运动模型执行位姿变换操作，以将所述运动模型变换至惯性测量单元坐标系下的运动模型。进一步地，在所述IMU测量数据生成方法中，所述对两相邻位姿轨迹信息进行插值处理包括：根据惯性测量单元的频率对所述两相邻位姿轨迹信息进行插值处理。进一步地，在所述IMU测量数据生成方法中，所述对两相邻位姿轨迹信息进行插值处理包括：利用线性插值算法对所述两相邻位姿轨迹信息中的位置信息进行插值操作；利用球面插值对所述两相邻位姿轨迹信息中的旋转信息进行插值操作。进一步地，在所述IMU测量数据生成方法中，所述对两相邻位姿轨迹信息进行插值处理包括：利用多项式函数算法对所述两相邻位姿轨迹信息中的位置信息进行插值操作；利用球面插值对所述两相邻位姿轨迹信息中的旋转信息进行插值操作。进一步地，在所述IMU测量数据生成方法中，所述利用球面插值对所述两相邻位姿轨迹信息中的旋转信息进行插值操作包括：判断第一位姿轨迹信息中的旋转信息与第二位姿轨迹信息中的旋转信息点积是否为负，其中所述两相邻位姿轨迹信息包括所述第一位姿轨迹信息及所述第二位姿轨迹信息；当所述点积为负时，在所述第一位姿轨迹信息中的旋转信息与第二位姿轨迹信息中的旋转信息的共轭之间进行插值操作；当所述点积为正时，在所述第一位姿轨迹信息中的旋转信息与第二位姿轨迹信息中的旋转信息之间进行插值操作。进一步地，在所述IMU测量数据生成方法中，所述对所述运动模型执行位姿变换操作包括：将所述运动模型与齐次变换矩阵相乘，以得到所述惯性测量单元坐标系下的运动模型。进一步地，所述IMU测量数据生成方法还包括：对变换后的所述运动模型进行噪声处理，以生成对应所述惯性测量单元的测量数据。一种IMU测量数据生成系统，包括：获取单元，用于获取位姿轨迹数据，其中所述位姿轨迹数据包含若干位姿轨迹信息，每一位姿轨迹信息包含位置信息及旋转信息；插值单元，用于对两相邻位姿轨迹信息进行插值处理，以得到一个或多个位于所述两相邻位姿轨迹信息之间的插值处理后的位姿轨迹信息，并更新所述位姿轨迹数据；计算单元，用于根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息；并根据更新后的所述位姿轨迹数据中旋转信息所述计算运动模型的角速度信息；变换单元，用于对所述运动模型执行位姿变换操作，以将所述运动模型变换至惯性测量单元坐标系下的运动模型。一种计算机装置，所述计算机装置包括处理器，所述处理器用于执行存储器中存储的计算机程序时实现所述的IMU测量数据生成方法的步骤。一种可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现所述的IMU测量数据生成方法的步骤。上述IMU测量数据生成方法、系统、计算机装置及可读存储介质可根据相机的位姿轨迹信息来生成IMU的测量数据，避免了在机器人上安装IMU后涉及的硬件同步问题，以及相机与IMU相对安装位置的问题。附图说明为了更清楚地说明本发明实施方式的技术方案，下面将对实施方式描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。图1是本发明IMU测量数据生成方法的较佳实施方式的流程图；图2是本发明IMU测量数据生成系统应用于计算机装置的较佳实施方式的示意图；图3是本发明IMU测量数据生成系统较佳实施方式的方框图。主要元件符号说明计算机装置1IMU测量数据生成系统417处理器401显示屏403存储器405输入输出接口407网络接口409获取单元300插值单元302计算单元304变换单元306处理单元308具体实施方式为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行详细描述。需要说明的是，在不冲突的情况下，本申请的实施方式及实施方式中的特征可以相互组合。在下面的描述中阐述了很多具体细节以便于充分理解本发明，所描述的实施方式仅仅是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施方式的目的，不是旨在于限制本发明。图1是本发明第一实施方式的IMU测量数据生成方法的流程图，应说明的是，本实施方式的所述抓取方法并不限于图1所示的流程图中的步骤及顺序。根据不同的需求，所示流程图中的步骤可以增加、移除、或者改变顺序。步骤100，获取位姿轨迹数据，其中所述位姿轨迹数据包含若干位姿轨迹信息，每一位姿轨迹信息包含位置信息及旋转信息。可以理解地，机器人上可设置相机，以获得机器人的运动轨迹与运动状态等位姿数据。所述相机可具有六自由度的轨迹其数据集可来自的Groundtruth。所述位姿数据可存储于位姿轨迹文件内或是存储于位姿轨迹数据库内，因此，获取所述位姿轨迹数据时可从对应的位姿轨迹文件或数据库内获得。较佳地，所述位姿轨迹数据可包含基于位姿轨迹信息，每一位姿轨迹信息可包含位置时间戳、位置信息及旋转信息等信息。具体地，所述时间戳的数据类型可为双精度double类型；所述位置信息可对应三自由度的位置，可具有三个双精度类型：所述旋转信息可对应三自由度的旋转信息，可具有四个双精度类型，其中所述旋转信息可以是四元数，其表示表示形式可为：w+xi+yj+zk。例如，对于一位姿轨迹信息，其时间戳可为：1305031102.211214，所述位置信息中第一位置、第二位置及第三位置可分别为-0.001420095、0.007655886、0.010920254；所述旋转信息中四元数x、四元数y、四元数z及四元数w可分别为：-0.001964978、-0.003080067、-0.001635573、0.999992013。步骤102，对两相邻位姿轨迹信息进行插值处理，以得到一个或多个位于所述两相邻位姿轨迹信息之间的插值处理后的位姿轨迹信息，并更新所述位姿轨迹数据。在一实施方式中，可根据惯性测量单元的频率对所述两相邻位姿轨迹信息进行插值处理。IMU数据测量的频率一般比相机测量频率要高，有的可达5倍以上。例如，相机的测量频率为30Hz左右，IMU测量的频率可在200Hz左右。在其他实施方式中，可按照其他方式对所述两相邻位姿轨迹信息进行插值处理通过在所述两相邻位姿轨迹信息中插入固定数量的位姿轨迹信息。在一实施方式中，可通过对相机的时间戳的计算得到相机测量的频率。例如，所述位姿轨迹数据中可包括第一位姿轨迹信息、第二位姿轨迹信息、第三位姿轨迹信息及第四位姿轨迹信息。所述相机的测量频率可根据单位时间内如1秒内所述位姿轨迹数据中所述位姿轨迹信息的记录的数量，因而，当两相邻位姿轨迹数据所对应的时间间隔不同两相邻位姿轨迹数据的时间戳之差可表示为时间间隔时，在两相邻位姿轨迹数据中插入的位姿轨迹信息的数量可能会不同。在其他实施方式中，所相机的测量频率亦可通过其他方式获取，如获取所述相机的相关参数等。较佳地，当所述相机的测量频率确认后，可根据所述惯性测量单元的频率来对两相邻位姿轨迹信息进行插值处理。例如，可在所述第一位姿轨迹信息与第二位姿轨迹信息间插入N个位姿轨迹信息，可在所述第二位姿轨迹信息与第三位姿轨迹信息间插入M个位姿轨迹信息，可在所述第三位姿轨迹信息与第四位姿轨迹信息间插入P个位姿轨迹信息。可以理解地，所述惯性测量单元的频率可存储于配制文件内，如此，可通过所述配制文件来获取所述惯性测量单元的频率。可以理解地，在所述两相邻位姿轨迹信息中进行插值处理可包括对位置信息的插值处理及对旋转信息的插值处理。较佳地，在对位置信息的插值处理中，若所述两相邻位姿轨迹信息之间时间间隔在预设范围内如0.03s内，表示机器人或载体的运动变化不剧烈，可以使用插值算法推算出两相邻位姿轨迹信息之间的多个虚拟相机的位姿轨迹信息。对于相机的三自由度位姿信息，使用线性插值算法来完成插值操作，如下所示：y＝ty0+1-ty1其中，t设置在区间[0，1]之间，y0表示所述两相邻位姿轨迹信息中对应第一时间戳的位姿轨迹信息，y1表示所述两相邻位姿轨迹信息中对应第二时间戳的位姿轨迹信息，其中所述第一时间戳可早于第二时间戳，如此，可保证插值结果在y0与y1之间。通过设置多个t即可以获得多个插值结果。例如，在所述第一位姿轨迹信息与第二位姿轨迹信息间插入N个位姿轨迹信息，所述t可为N；在所述第二位姿轨迹信息与第三位姿轨迹信息间插入M个位姿轨迹信息，所述t可为M。对于位姿轨迹信息中时间戳亦可基于上述公式来进行确定，如将上述公式中y0及y1替换成对应所述第一位姿轨迹信息中的时间戳及第二位姿轨迹信息中的时间戳。可以理解地，除了线性插值以外，在相机的运动变化比较剧烈时，可利用多项式函数进行插值运算，这种插值运算可以模拟多项式的函数变化。较佳地，在对旋转信息的插值处理中，可使用球面插值算法。由于旋转信息的四元数是分布在流形空间如空间球面的，它对加减法运算是不封闭的，所以使用简单的线性插值的结果是不均匀的，也不能保证其仍然是单位四元数。因此，可通过球面插值方法来对旋转信息进行插值处理。例如，对于第一位姿轨迹信息中的旋转信息Q1与相邻的第二位姿轨迹信息中的旋转信息Q2而言，可先计算Q1与Q2的点积，如果点积为负，说明Q1与Q2之间的夹角为大于180度的角，则将Q2取共轭为Q2*，并在Q1与Q2*之间的插值是最优的插值；若点积为正，则在Q1与Q2之间进行插值。对于接着计算：K0＝sin1.0-t*atansinacosasina；K1＝sint*atansinacosasina；其中cosa为Q1与Q2的点积的绝对值，sina＝sqrt1-cosa*cosa。输出结果：K0*Q1+K1*Q2或是K0*Q1+K1*Q2*，进而可这样的插值结果保证了四元数的均匀分布。可以理解地，在所述两相邻位姿轨迹信息中完成插值操作后，所述两相邻位姿轨迹信息之间可包含一个或一个插值处理后的位姿轨迹信息，此时，可对所述位姿轨迹数据进行更新。例如，在所述第一位姿轨迹信息与第二位姿轨迹信息间插入3个位姿轨迹信息第一插值位姿轨迹信息、第二插值位姿轨迹信息及第三插值位姿轨迹信息时，更新后的所述位姿轨迹数据可包括所述第一位姿轨迹信息、第一插值位姿轨迹信息、第二插值位姿轨迹信息、第三插值位姿轨迹信息及第二位姿轨迹信息。步骤104，根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息；并根据更新后的所述位姿轨迹数据中旋转信息计算所述运动模型的角速度信息。可以理解地，根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息a[k]可表示为：V[k]＝p[k+1]一p[k]Δt；a[k]＝v[k+1]-v[k]Δt；其中，p[k+1]、p[k]表示两相邻位姿轨迹信息中的位置信息，Δt表示两相邻位姿轨迹信息中的时间戳之差，k为自然数。例如，对于更新后位姿轨迹数据：所述第一位姿轨迹信息、第一插值位姿轨迹信息、第二插值位姿轨迹信息、第三插值位姿轨迹信息及第二位姿轨迹信息，当k为1时，p[k+1]、p[k]表示第一位姿轨迹信息及第一插值位姿轨迹信息中的位置信息，Δt表示表示第一位姿轨迹信息及第一插值位姿轨迹信息中的时间戳之差。可以理解地，根据更新后的所述位姿轨迹数据中旋转信息所述计算运动模型的角速度信息ω可表示为：其中，q[k+1]、q[k]表示两相邻位姿轨迹信息中的旋转信息，Δt表示两相邻位姿轨迹信息中的时间戳之差，k为自然数。例如，对于更新后位姿轨迹数据：所述第一位姿轨迹信息、第一插值位姿轨迹信息、第二插值位姿轨迹信息、第三插值位姿轨迹信息及第二位姿轨迹信息，当k为1时，q[k+1]、q[k]表示第一位姿轨迹信息及第一插值位姿轨迹信息中的旋转信息，Δt表示表示第一位姿轨迹信息及第一插值位姿轨迹信息中的时间戳之差。步骤106，对所述运动模型执行位姿变换操作，以将所述运动模型变换至惯性测量单元坐标系下的运动模型。可以理解地，在整个系统中，一共会使用三个坐标系：相机坐标系FC、IMU坐标系FI以及世界坐标系FW。世界坐标系为实际的东北天坐标系，在这个坐标系中，地球重力向量为0，0，-9.78深圳。而相机坐标系为相机自身运动过程中原点固连在相机上的坐标系，IMU坐标系为IMU运动过程中固连在IMU上的坐标系。因为，在一般情况下，IMU与相机均固连在载体之上，二者之间存在着一个固定的齐次变换矩阵TCS，并且相机坐标系与世界坐标系之间也存在着一个齐次变换矩阵TCW。因此，根据更新后的所述位姿轨迹数据计算得到的运动模型是基于相机坐标，所述运动模型可应用于IMU时，需要将所述运动模型通过位姿变换操作，如将所述运动模型与齐次变换矩阵TCS相乘即可变换为惯性测量单元坐标系下的运动模型。步骤108，对变换后的所述运动模型进行噪声处理，以生成对应所述惯性测量单元的测量数据。IMU是一种微型的电子测量元件，其测得的数据包含着随机游走误差与读数噪声，分别建模为维纳噪声bias与高斯白噪声noise。为了使得仿真的IMU数据更加真实，在每一帧生成的IMU数据中使用算法加入了这两种噪声。算法如下：算法：增加噪声输入：运动模型的加速度与角速度信息，高斯噪声的方法，维纳噪声的方差。开始：生成一个随机数发生器，定义一个标准的高斯分布模型，利用标准高斯分布生成三个随机数，利用高斯噪声方差乘上3x3的单位矩阵得到三轴加速度的协方差矩阵，高斯噪声等于协方差矩阵乘上生成的3个随机数构成的向量再除以时间间隔的算法平方根得到。维纳噪声利用一个新的标准高斯分布生成3个随机数，再用维纳噪声的方差乘上时间间隔的算术平方根乘上三个随机数构成的向量，乘积结果作为维纳噪声随机游走噪声的更新值，加在之前的维纳噪声之上，形成新的维纳噪声。而角速度的噪声生成方式与加速度的噪声生成方式一样，将两个加速度的方差换成角速度的方差即可。具体的公式如下所示：高斯噪声：维纳噪声：至此，即可输出IMU的仿真数据，可得到所述IMU的测量数据。上述IMU测量数据生成方法可根据相机的位姿轨迹信息来生成IMU的测量数据，避免了在机器人上安装IMU后涉及的硬件同步问题，以及相机与IMU相对安装位置的问题。另外，可利用公开数据集提供的Groundtruth生成仿真IMU数据，便于测试加上惯性融合之后的视觉惯性算法与之前没有加入惯性传感器数据的视觉里程计算法的性能变化。请参阅图2本发明一实施方式的计算机装置的示例性的结构示意图。本实施例提供的计算机装置1包括存储器405、输入输出接口407、显示屏403、网络接口409及通过总线411与所述存储器405、输入输出接口407、网络接口409与显示屏403进行数据交换的处理器401。其中，所述输入输出接口407可连接于鼠标及或键盘图未示。本发明所称的模块是完成一特定功能的程序段，比程序更适合于描述软件在处理器中的执行过程。所称处理器401可以是中央处理单元CentralProcessingUnit，CPU，还可以是其他通用处理器、数字信号处理器DigitalSignalProcessor，DSP、专用集成电路ApplicationSpecificIntegratedCircuit，ASIC、现成可编程门阵列Field-ProgrammableGateArray，FPGA或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器401是所述计算机装置1的控制中心，利用各种接口和线路连接整个计算机装置1的各个部分。所述存储器405可用于存储所述计算机程序和或模块，所述处理器401通过运行或执行存储在所述存储器405内的计算机程序和或模块，以及调用存储在存储器405内的数据，实现所述计算机装置1的各种功能。所述存储器405可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序比如图形界面显示功能等等；存储数据区可存储根据计算机装置的使用所创建的数据比如音频数据、视频数据等等。此外，存储器405可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡SmartMediaCard，SMC，安全数字SecureDigital，SD卡，闪存卡FlashCard、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。本实施例中，所述显示屏403可为具有触摸功能的显示屏，进而为用户的操作提供便利。所述存储器405可存储有若干程序代码，以被所述处理器401执行，进而实现所述IMU测量数据生成系统417的功能。示例性的，所述计算机程序可以被分割成一个或多个模块单元，所述一个或者多个模块被存储在所述存储器405中，并由所述处理器401执行，以完成本发明。所述一个或多个模块可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述计算机装置1中的执行过程。例如，所述计算机程序可以被分割成如图3所示的IMU测量数据生成系统417，包括获取单元300、插值单元302、计算单元304、变换单元306及处理单元308，各模块单元具体功能如下：所述获取单元300用于获取位姿轨迹数据，其中所述位姿轨迹数据包含若干位姿轨迹信息，每一位姿轨迹信息包含位置信息及旋转信息。可以理解地，机器人上可设置相机，以获得机器人的运动轨迹与运动状态等位姿数据。所述相机可具有六自由度的轨迹其数据集可来自的Groundtruth。所述位姿数据可存储于位姿轨迹文件内或是存储于位姿轨迹数据库内，因此，所述获取单元300可获取所述位姿轨迹数据时可从对应的位姿轨迹文件或数据库内获得。较佳地，所述位姿轨迹数据可包含基于位姿轨迹信息，每一位姿轨迹信息可包含位置时间戳、位置信息及旋转信息等信息。具体地，所述时间戳的数据类型可为双精度double类型；所述位置信息可对应三自由度的位置，可具有三个双精度类型；所述旋转信息可对应三自由度的旋转信息，可具有四个双精度类型，其中所述旋转信息可以是四元数，其表示表示形式可为：w+xi+yj+zk。例如，对于一位姿轨迹信息，其时间戳可为：1305031102.211214，所述位置信息中第一位置、第二位置及第三位置可分别为-0.001420095、0.007655886、0.010920254；所述旋转信息中四元数x、四元数y、四元数z及四元数w可分别为：-0.001964978、-0.003080067、-0.001635573、0.999992013。所述插值单元302用于对两相邻位姿轨迹信息进行插值处理，以得到一个或多个位于所述两相邻位姿轨迹信息之间的插值处理后的位姿轨迹信息，并更新所述位姿轨迹数据。在一实施方式中，所述插值单元302可根据惯性测量单元的频率对所述两相邻位姿轨迹信息进行插值处理。IMU数据测量的频率一般比相机测量频率要高，有的可达5倍以上。例如，相机的测量频率为30Hz左右，IMU测量的频率可在200Hz左右。在其他实施方式中，可按照其他方式对所述两相邻位姿轨迹信息进行插值处理通过在所述两相邻位姿轨迹信息中插入固定数量的位姿轨迹信息。在一实施方式中，所述插值单元302可通过对相机的时间戳的计算得到相机测量的频率。例如，所述位姿轨迹数据中可包括第一位姿轨迹信息、第二位姿轨迹信息、第三位姿轨迹信息及第四位姿轨迹信息。所述相机的测量频率可根据单位时间内如1秒内所述位姿轨迹数据中所述位姿轨迹信息的记录的数量，因而，当两相邻位姿轨迹数据所对应的时间间隔不同两相邻位姿轨迹数据的时间戳之差可表示为时间间隔时，在两相邻位姿轨迹数据中插入的位姿轨迹信息的数量可能会不同。在其他实施方式中，所相机的测量频率亦可通过其他方式获取，如获取所述相机的相关参数等。较佳地，当所述相机的测量频率确认后，所述插值单元302可根据所述惯性测量单元的频率来对两相邻位姿轨迹信息进行插值处理。例如，所述插值单元302可在所述第一位姿轨迹信息与第二位姿轨迹信息间插入N个位姿轨迹信息，所述插值单元302可在所述第二位姿轨迹信息与第三位姿轨迹信息间插入M个位姿轨迹信息，所述插值单元302可在所述第三位姿轨迹信息与第四位姿轨迹信息间插入P个位姿轨迹信息。可以理解地，所述惯性测量单元的频率可存储于配制文件内，如此，可通过所述配制文件来获取所述惯性测量单元的频率。可以理解地，在所述两相邻位姿轨迹信息中进行插值处理可包括对位置信息的插值处理及对旋转信息的插值处理。较佳地，在对位置信息的插值处理中，若所述两相邻位姿轨迹信息之间时间间隔在预设范围内如0.03s内，表示机器人或载体的运动变化不剧烈，所述插值单元302可以使用插值算法推算出两相邻位姿轨迹信息之间的多个虚拟相机的位姿轨迹信息。对于相机的三自由度位姿信息，使用线性插值算法来完成插值操作，如下所示：y＝ty0+1-ty1其中，t设置在区间[0，1]之间，y0表示所述两相邻位姿轨迹信息中对应第一时间戳的位姿轨迹信息，y1表示所述两相邻位姿轨迹信息中对应第二时间戳的位姿轨迹信息，其中所述第一时间戳可早于第二时间戳，如此，可保证插值结果在y0与y1之间。通过设置多个t即可以获得多个插值结果。例如，在所述第一位姿轨迹信息与第二位姿轨迹信息间插入N个位姿轨迹信息，所述t可为N；在所述第二位姿轨迹信息与第三位姿轨迹信息间插入M个位姿轨迹信息，所述t可为M。对于位姿轨迹信息中时间戳亦可基于上述公式来进行确定，如将上述公式中y0及y1替换成对应所述第一位姿轨迹信息中的时间戳及第二位姿轨迹信息中的时间戳。可以理解地，除了线性插值以外，在相机的运动变化比较剧烈时，所述插值单元302可利用多项式函数进行插值运算，这种插值运算可以模拟多项式的函数变化。较佳地，在对旋转信息的插值处理中，所述插值单元302可使用球面插值算法。由于旋转信息的四元数是分布在流形空间如空间球面的，它对加减法运算是不封闭的，所以使用简单的线性插值的结果是不均匀的，也不能保证其仍然是单位四元数。因此，所述插值单元302可通过球面插值方法来对旋转信息进行插值处理。例如，对于第一位姿轨迹信息中的旋转信息Q1与相邻的第二位姿轨迹信息中的旋转信息Q2而言，可先计算Q1与Q2的点积，如果点积为负，说明Q1与Q2之间的夹角为大于180度的角，则将Q2取共轭为Q2*，并在Q1与Q2*之间的插值是最优的插值；若点积为正，则在Q1与Q2之间进行插值。对于接着计算：K0＝sin1.0-t*atansinacosasina；K1＝sint*atansinacosasina；其中cosa为Q1与Q2的点积的绝对值，sina＝sqrt1-cosa*cosa。输出结果：K0*Q1+K1*Q2或是K0*Q1+K1*Q2*，进而可这样的插值结果保证了四元数的均匀分布。可以理解地，在所述两相邻位姿轨迹信息中完成插值操作后，所述两相邻位姿轨迹信息之间可包含一个或一个插值处理后的位姿轨迹信息，此时，可对所述位姿轨迹数据进行更新。例如，在所述第一位姿轨迹信息与第二位姿轨迹信息间插入3个位姿轨迹信息第一插值位姿轨迹信息、第二插值位姿轨迹信息及第三插值位姿轨迹信息时，更新后的所述位姿轨迹数据可包括所述第一位姿轨迹信息、第一插值位姿轨迹信息、第二插值位姿轨迹信息、第三插值位姿轨迹信息及第二位姿轨迹信息。所述计算单元304可根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息；并根据更新后的所述位姿轨迹数据中旋转信息所述计算运动模型的角速度信息。可以理解地，所述计算单元304可根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息a[k]可表示为：v[k]＝p[k+1]-p[k]Δt；a[k]＝v[k+1]-v[k]Δt；其中，p[k+1]、p[k]表示两相邻位姿轨迹信息中的位置信息，Δt表示两相邻位姿轨迹信息中的时间戳之差，k为自然数。例如，对于更新后位姿轨迹数据：所述第一位姿轨迹信息、第一插值位姿轨迹信息、第二插值位姿轨迹信息、第三插值位姿轨迹信息及第二位姿轨迹信息，当k为1时，p[k+1]、p[k]表示第一位姿轨迹信息及第一插值位姿轨迹信息中的位置信息，Δt表示表示第一位姿轨迹信息及第一插值位姿轨迹信息中的时间戳之差。可以理解地，所述计算单元304可根据更新后的所述位姿轨迹数据中旋转信息所述计算运动模型的角速度信息ω可表示为：其中，q[k+1]、q[k]表示两相邻位姿轨迹信息中的旋转信息，Δt表示两相邻位姿轨迹信息中的时间戳之差，k为自然数。例如，对于更新后位姿轨迹数据：所述第一位姿轨迹信息、第一插值位姿轨迹信息、第二插值位姿轨迹信息、第三插值位姿轨迹信息及第二位姿轨迹信息，当k为1时，q[k+1]、q[k]表示第一位姿轨迹信息及第一插值位姿轨迹信息中的旋转信息，Δt表示表示第一位姿轨迹信息及第一插值位姿轨迹信息中的时间戳之差。所述变换单元306可对所述运动模型执行位姿变换操作，以将所述运动模型变换至惯性测量单元坐标系下的运动模型。可以理解地，在整个系统中，一共会使用三个坐标系：相机坐标系FC、IMU坐标系FI以及世界坐标系FW。世界坐标系为实际的东北天坐标系，在这个坐标系中，地球重力向量为0，0，-9.78深圳。而相机坐标系为相机自身运动过程中原点固连在相机上的坐标系，IMU坐标系为IMU运动过程中固连在IMU上的坐标系。因为，在一般情况下，IMU与相机均固连在载体之上，二者之间存在着一个固定的齐次变换矩阵TCS，并且相机坐标系与世界坐标系之间也存在着一个齐次变换矩阵TCW。因此，根据更新后的所述位姿轨迹数据计算得到的运动模型是基于相机坐标，所述运动模型可应用于IMU时，需要将所述运动模型通过位姿变换操作，如所述变换单元306可将所述运动模型与齐次变换矩阵TCS相乘即可变换为惯性测量单元坐标系下的运动模型。所述处理单元308可对变换后的所述运动模型进行噪声处理，以生成对应所述惯性测量单元的测量数据，进而达到提高测量数据精确度的目的。在其他实施方式中，所述处理单元308亦可省略。IMU是一种微型的电子测量元件，其测得的数据包含着随机游走误差与读数噪声，分别建模为维纳噪声bias与高斯白噪声noise。为了使得仿真的IMU数据更加真实，在每一帧生成的IMU数据中使用算法加入了这两种噪声。算法如下：算法：增加噪声输入：运动模型的加速度与角速度信息，高斯噪声的方法，维纳噪声的方差。开始：生成一个随机数发生器，定义一个标准的高斯分布模型，利用标准高斯分布生成三个随机数，利用高斯噪声方差乘上3x3的单位矩阵得到三轴加速度的协方差矩阵，高斯噪声等于协方差矩阵乘上生成的3个随机数构成的向量再除以时间间隔的算法平方根得到。维纳噪声利用一个新的标准高斯分布生成3个随机数，再用维纳噪声的方差乘上时间间隔的算术平方根乘上三个随机数构成的向量，乘积结果作为维纳噪声随机游走噪声的更新值，加在之前的维纳噪声之上，形成新的维纳噪声。而角速度的噪声生成方式与加速度的噪声生成方式一样，将两个加速度的方差换成角速度的方差即可。具体的公式如下所示：高斯噪声：维纳噪声：至此，即可输出IMU的仿真数据，可得到所述IMU的测量数据。上述IMU测量数据生成系统可根据相机的位姿轨迹信息来生成IMU的测量数据，避免了在机器人上安装IMU后涉及的硬件同步问题，以及相机与IMU相对安装位置的问题。另外，可利用公开数据集提供的Groundtruth生成仿真IMU数据，便于测试加上惯性融合之后的视觉惯性算法与之前没有加入惯性传感器数据的视觉里程计算法的性能变化。本发明所述计算机装置1集成的模块如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述各实施方式的音量控制方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各实施方式的音量控制方法中的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器ROM，Read-OnlyMemory、随机存取存储器RAM，RandomAccessMemory、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。系统、装置或移动终端装置权利要求中陈述的多个单元、模块或装置也可以由同一个单元、模块或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。以上实施方式仅用以说明本发明的技术方案而非限制，尽管参照以上较佳实施方式对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或等同替换都不应脱离本发明技术方案的精神和范围。

权利要求：1.一种IMU测量数据生成方法，其特征在于，所述方法包括：获取位姿轨迹数据，其中所述位姿轨迹数据包含若干位姿轨迹信息，每一位姿轨迹信息包含位置信息及旋转信息；对两相邻位姿轨迹信息进行插值处理，以得到一个或多个位于所述两相邻位姿轨迹信息之间的插值处理后的位姿轨迹信息，并更新所述位姿轨迹数据；根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息；并根据更新后的所述位姿轨迹数据中旋转信息计算所述运动模型的角速度信息；对所述运动模型执行位姿变换操作，以将所述运动模型变换至惯性测量单元坐标系下的运动模型。2.如权利要求1所述的IMU测量数据生成方法，其特征在于，所述对两相邻位姿轨迹信息进行插值处理包括：根据惯性测量单元的频率对所述两相邻位姿轨迹信息进行插值处理。3.如权利要求1所述的IMU测量数据生成方法，其特征在于，所述对两相邻位姿轨迹信息进行插值处理包括：利用线性插值算法对所述两相邻位姿轨迹信息中的位置信息进行插值操作；利用球面插值对所述两相邻位姿轨迹信息中的旋转信息进行插值操作。4.如权利要求1所述的IMU测量数据生成方法，其特征在于，所述对两相邻位姿轨迹信息进行插值处理包括：利用多项式函数算法对所述两相邻位姿轨迹信息中的位置信息进行插值操作；利用球面插值对所述两相邻位姿轨迹信息中的旋转信息进行插值操作。5.如权利要求3或4所述的IMU测量数据生成方法，其特征在于，所述利用球面插值对所述两相邻位姿轨迹信息中的旋转信息进行插值操作包括：判断第一位姿轨迹信息中的旋转信息与第二位姿轨迹信息中的旋转信息点积是否为负，其中所述两相邻位姿轨迹信息包括所述第一位姿轨迹信息及所述第二位姿轨迹信息；当所述点积为负时，在所述第一位姿轨迹信息中的旋转信息与第二位姿轨迹信息中的旋转信息的共轭之间进行插值操作；当所述点积为正时，在所述第一位姿轨迹信息中的旋转信息与第二位姿轨迹信息中的旋转信息之间进行插值操作。6.如权利要求1所述的IMU测量数据生成方法，其特征在于，所述对所述运动模型执行位姿变换操作包括：将所述运动模型与齐次变换矩阵相乘，以得到所述惯性测量单元坐标系下的运动模型。7.如权利要求1-6中任意一项所述的IMU测量数据生成方法，其特征在于，所述方法还包括：对变换后的所述运动模型进行噪声处理，以生成对应所述惯性测量单元的测量数据。8.一种IMU测量数据生成系统，其特征在于，所述系统包括：获取单元，用于获取位姿轨迹数据，其中所述位姿轨迹数据包含若干位姿轨迹信息，每一位姿轨迹信息包含位置信息及旋转信息；插值单元，用于对两相邻位姿轨迹信息进行插值处理，以得到一个或多个位于所述两相邻位姿轨迹信息之间的插值处理后的位姿轨迹信息，并更新所述位姿轨迹数据；计算单元，用于根据更新后的所述位姿轨迹数据中位置信息计算运动模型的加速度信息；并根据更新后的所述位姿轨迹数据中旋转信息所述计算运动模型的角速度信息；变换单元，用于对所述运动模型执行位姿变换操作，以将所述运动模型变换至惯性测量单元坐标系下的运动模型。9.一种计算机装置，其特征在于，所述计算机装置包括处理器，所述处理器用于执行存储器中存储的计算机程序时实现如权利要求1-7中任意一项所述的IMU测量数据生成方法的步骤。10.一种可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-7中任意一项所述的IMU测量数据生成方法的步骤。

百度查询： 深圳清华大学研究院 IMU测量数据生成方法、系统、计算机装置及可读存储介质

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD