CN109521802A - 用于运动轨迹跟踪的方法、装置和设备 - Google Patents
用于运动轨迹跟踪的方法、装置和设备 Download PDFInfo
- Publication number
- CN109521802A CN109521802A CN201710844684.7A CN201710844684A CN109521802A CN 109521802 A CN109521802 A CN 109521802A CN 201710844684 A CN201710844684 A CN 201710844684A CN 109521802 A CN109521802 A CN 109521802A
- Authority
- CN
- China
- Prior art keywords
- axis
- sampling time
- time point
- locating
- rotation angle
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 230000003287 optical effect Effects 0.000 claims abstract description 66
- 238000005259 measurement Methods 0.000 claims abstract description 50
- 238000005070 sampling Methods 0.000 claims description 148
- 238000010586 diagram Methods 0.000 description 7
- 230000005484 gravity Effects 0.000 description 5
- 238000005516 engineering process Methods 0.000 description 4
- 238000012986 modification Methods 0.000 description 4
- 230000004048 modification Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 1
- 230000005684 electric field Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D3/00—Control of position or direction
- G05D3/12—Control of position or direction using feedback
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明涉及用于运动轨迹跟踪的设备、方法和装置,所述设备包括两个光学导航芯片、一个惯性测量单元和控制器,其中该控制器利用该两个光学导航芯片和该惯性测量单元的测量数据来确定对象的姿态角度数据和实际位置数据。该设备、方法和装置能够以低成本、低功耗和低运算需求的方式来提供运动轨迹跟踪。
Description
技术领域
本发明涉及运动轨迹跟踪领域,尤其涉及用于运动轨迹跟踪的方法、装置和设备、控制器和机器可读介质。
背景技术
运动跟踪技术是一种用于跟踪诸如人的手掌、手臂或人本身等的对象的运动轨迹的技术。运动跟踪技术通常分成外-内解决方案和内-外解决方案。
外-内解决方案通常使用各种场技术(例如,光场、电场、磁场、声场和超声场等)来实现,并且需要部署场发射器或场接收器以与用于跟踪目的的运动设备协作。外-内解决方案能够提供精确的运动轨迹跟踪,但其成本较高、功耗大和数据处理的运算量大。
内-外解决方案通常使用高分辨率的照相机和惯性测量单元(IMU:InertialMeasurement Unit)来实现。高分辨率的照相机所拍摄的图像的处理会产生很大的数据处理运算量,因此,内-外解决方案通常使用复杂的处理器来处理高分辨率的照相机所拍摄的图像。而且,高分辨率的照相机所拍摄的图像的处理所产生的大数据处理运算量,导致内-外解决方案的功耗较高。例如:其需要强大的图像/视频运算支持,难以在穿戴系统中用小容量电池支撑数小时使用。此外,高分辨率的照相机和复杂的处理器通常比较昂贵,导致内-外解决方案的成本也比较高。
发明内容
本发明的实施例提供用于运动轨迹跟踪的方法、装置和设备、控制器和机器可读介质,其能够以低成本、低功耗和低运算需求的方式来提供运动轨迹跟踪。
按照本发明的实施例的一种用于运动轨迹跟踪的设备,包括:惯性测量单元,用于测量在多个轨迹采样时间点的任一轨迹采样时间点处对象绕着第一轴的第一旋转角度、绕着第二轴的第二旋转角度和绕着第三轴的第三旋转角度,其中,所述第一轴、所述第二轴和所述第三轴相互垂直;第一光学导航芯片,用于测量在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一位置和在所述第三轴上所处的第二位置;第二光学导航芯片,用于测量在所述任一轨迹采样时间点处所述对象在所述第二轴所处上的第三位置和在所述第三轴所处上的第四位置;以及,控制器,其与所述惯性测量单元、所述第一光学导航芯片和所述第二光学导航芯片连接,并且用于:至少基于所述第一旋转角度、所述第二旋转角度、所述第三旋转角度、所述第一位置、所述第二位置、所述第三位置和所述第四位置,估算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一观测位置、在所述第二轴上所处的第二观测位置和在所述第三轴上所处的第三观测位置;至少基于所述第一观测位置、所述第二观测位置和所述第三观测位置,计算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一实际位置、在所述第二轴上所处的第二实际位置和在所述第三轴上所处的第三实际位置;以及,输出在所述任一轨迹采样时间点处所述对象的姿态角度数据和实际位置数据,其中,所述姿态角度数据包括所述第一旋转角度、所述第二旋转角度和所述第三旋转角度,以及,所述实际位置数据包括所述第一实际位置、所述第二实际位置和所述第三实际位置。
按照本发明的实施例的一种用于运动轨迹跟踪的方法,包括:至少基于在多个轨迹采样时间点的任一轨迹采样时间点处对象的第一旋转角度、第二旋转角度、第三旋转角度、第一位置、第二位置、第三位置和第四位置,估算在所述任一轨迹采样时间点处所述对象在第一轴上所处的第一观测位置、在第二轴上所处的第二观测位置和在第三轴上所处的第三观测位置,其中,所述第一轴、所述第二轴和所述第三轴相互垂直;至少基于所述第一观测位置、所述第二观测位置和所述第三观测位置,计算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一实际位置、在所述第二轴上所处的第二实际位置和在所述第三轴上所处的第三实际位置;以及,输出在所述任一轨迹采样时间点处所述对象的姿态角度数据和实际位置数据,其中,所述姿态角度数据包括所述第一旋转角度、所述第二旋转角度和所述第三旋转角度,以及,所述实际位置数据包括所述第一实际位置、所述第二实际位置和所述第三实际位置,其中,所述第一旋转角度、所述第二旋转角度和所述第三旋转角度分别是惯性测量单元测量的在所述任一轨迹采样时间点处所述对象绕着所述第一轴、所述第二轴和所述第三轴的旋转角度,所述第一位置和所述第二位置分别是第一光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的位置和在所述第三轴上所处的位置,以及,所述第三位置和第四位置分别是第二光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第二轴所处上的位置和在所述第三轴所处上的位置。
按照本发明的实施例的一种用于运动轨迹跟踪的装置,包括:估算模块,用于至少基于在多个轨迹采样时间点的任一轨迹采样时间点处对象的第一旋转角度、第二旋转角度、第三旋转角度、第一位置、第二位置、第三位置和第四位置,估算在所述任一轨迹采样时间点处所述对象在第一轴上所处的第一观测位置、在第二轴上所处的第二观测位置和在第三轴上所处的第三观测位置,其中,所述第一轴、所述第二轴和所述第三轴相互垂直;计算模块,用于至少基于所述第一观测位置、所述第二观测位置和所述第三观测位置,计算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一实际位置、在所述第二轴上所处的第二实际位置和在所述第三轴上所处的第三实际位置;以及,输出模块,用于输出在所述任一轨迹采样时间点处所述对象的姿态角度数据和实际位置数据,其中,所述姿态角度数据包括所述第一旋转角度、所述第二旋转角度和所述第三旋转角度,以及,所述实际位置数据包括所述第一实际位置、所述第二实际位置和所述第三实际位置,其中,所述第一旋转角度、所述第二旋转角度和所述第三旋转角度分别是惯性测量单元测量的在所述任一轨迹采样时间点处所述对象绕着所述第一轴、所述第二轴和所述第三轴的旋转角度,所述第一位置和所述第二位置分别是第一光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的位置和在所述第三轴上所处的位置,以及,所述第三位置和第四位置分别是第二光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第二轴所处上的位置和在所述第三轴所处上的位置。
按照本发明的实施例的一种控制器,包括:处理器;以及,存储器,其存储有可执行指令,所述可执行指令当被执行时使得所述处理器执行前述的方法。
按照本发明的实施例的一种机器可读存储介质,其存储有可执行指令,所述可执行指令当被执行时使得机器执行前述的方法。
从以上可以看出,本发明的实施例的方案使用光学导航芯片和惯性测量单元来跟踪对象的运动轨迹,由于光学导航芯片和惯性测量单元比较便宜、所产生的数据处理量较小和功耗较低,因此,与现有技术相比,本发明的实施例的方案能够以低成本、低功耗和低运算需求的方式来提供运动轨迹跟踪。
附图说明
本发明的特征、特点、优点和益处通过以下结合附图的详细描述将变得显而易见。
图1A示出了按照本发明的一个实施例的用于运动轨迹跟踪的设备的架构示意图。
图1B示出了按照本发明的一个实施例的光学导航芯片的示意图。
图2示出了按照本发明的一个实施例的用于运动轨迹跟踪的方法的总体流程图。
图3示出了按照本发明的一个实施例的用于运动轨迹跟踪的方法的流程图。
图4示出了按照本发明的一个实施例的用于运动轨迹跟踪的装置的示意图。
图5示出了按照本发明的一个实施例的控制器的示意图。
具体实施方式
下面,结合附图详细描述本发明的各个实施例。
图1A示出了按照本发明的一个实施例的用于运动轨迹跟踪的设备的架构示意图。图1A所示的用于运动轨迹跟踪的设备10用来提供在各个轨迹采样时间点处对象O1的姿态角度数据和实际位置数据。如图1A所示,设备10可以包括两个光学导航芯片20和30、一个惯性测量单元40和控制器50。
光学导航芯片(Optical Navigation Chip)20和30可以测量对象在相互垂直的两个方向D1、D2上所处的位置,如图1B所示。在本实施例中,光学导航芯片20和30被放置在不同的位置,以使得光学导航芯片20以采样率SR来测量在多个轨迹采样时间点T中的每一个轨迹采样时间点T(i)处对象O1在X轴上所处的位置Max(i)和在Z轴上所处的位置May(i),以及,使得光学导航芯片30能够以采速率SR来测量在每一个轨迹采样时间点T(i)处对象O1在Y轴上所处的位置Mbx(i)和在Z轴上所处的位置Mby(i)。在本实施例中,假设Z轴是垂直于地面的坐标轴。如果采速率SR足够高,则对象O1在两个相邻的轨迹采样时间点之间的运动可以被认为是线性增加的。光学导航芯片20和30是现有技术中常见的包含传感器的组件。光学导航芯片20和30采用低分辨率摄像头来采集图像数据,低分辨率的图像数据的处理所产生的数据处理运算量较小,不需要消耗很多功率,而且由于低分辨率的图像数据的处理所产生的数据处理运算量较小,光学导航芯片20和30不需要使用复杂的处理器来处理,并且低分辨率摄像头本身也很便宜,因此,光学导航芯片20和30具有成本低、功耗小和运算需求低的优点。
惯性测量单元40用于以采样率SR来测量在每一个轨迹采样时间点T(i)处对象O1绕着X轴的旋转角度A-roll(i)、绕着Y轴的旋转角度A-yaw(i)和绕着Z轴的旋转角度A-pitch(i)。惯性测量单元40也是常见的比较廉价的测量器件。惯性测量单元40和光学导航芯片20、30具有相同的采样率,都是采样率SR。惯性测量单元40本身很便宜,数据处理的运算量也很小,而且功耗也不高。
控制器50与光学导航芯片20和30以及惯性测量单元40连接,以利用来自光学导航芯片20、30和惯性测量单元40的测量数据,确定在每一个轨迹采样时间点T(i)处对象O1的姿态角度数据和实际位置数据,这将在下面结合图2详细描述。
图2示出了按照本发明的一个实施例的用于运动轨迹跟踪的方法的总体流程图。下面结合图1A所示的设备10来详细描述图2所示的方法200。
如图2所示,在方框202,当时间到达每一个轨迹采样时间点T(i)时,光学导航芯片20测量在轨迹采样时间点T(i)处对象O1在X轴上所处的位置Max(i)和在Z轴上所处的位置May(i)并输出给控制器50。
在方框204,当时间到达每一个轨迹采样时间点T(i)时,光学导航芯片30测量在轨迹采样时间点T(i)处对象O1在Y轴上所处的位置Mbx(i)和在Z轴上所处的位置Mby(i)并输出给控制器50。在本实施例中,假设Z轴是垂直于地面的坐标轴。
在方框206,当时间到达每一个轨迹采样时间点T(i)时,惯性测量单元40测量在轨迹采样时间点T(i)处对象O1绕着X轴的旋转角度A-roll(i)、绕着Y轴的旋转角度A-yaw(i)和绕着Z轴的旋转角度A-pitch(i)并输出给控制器50。
在方框208,在接收到来自光学导航芯片20的Max(i)和May(i)、来自光学导航芯片30的Mbx(i)和Mby(i),以及,来自惯性测量单元40的A-roll(i)、A-yaw(i)和A-pitch(i)之后,控制器50计算在轨迹采样时间点T(i)的前一轨迹采样时间点T(i-1)处光学导航芯片20距离地面的高度Ha(i-1)与在轨迹采样时间点T(i-1)的前一轨迹采样时间点T(i-2)处光学导航芯片20距离地面的高度Ha(i-2)之间的高度差,作为在轨迹采样时间点T(i)和轨迹采样时间点T(i-1)之间光学导航芯片20的高度变化Va,以及,计算在轨迹采样时间点T(i-1)处光学导航芯片30距离地面的高度Hb(i-1)与在轨迹采样时间点T(i-2)处光学导航芯片20距离地面的高度Hb(i-2)之间的高度差,作为在轨迹采样时间点T(i)和轨迹采样时间点T(i-1)之间光学导航芯片30的高度变化Vb。
在方框210,控制器50计算在轨迹采样时间点T(i-1)处光学导航芯片20距离地面的高度Ha(i-1)与高度变化Va之和,作为在轨迹采样时间点T(i)处光学导航芯片20距离地面的高度Ha(i),以及,计算在轨迹采样时间点T(i-1)处光学导航芯片30距离地面的高度Hb(i-1)与高度变化Vb之和,作为在轨迹采样时间点T(i)处光学导航芯片30距离地面的高度Hb(i)。
在方框212,控制器50利用下面的等式来计算在轨迹采样时间点T(i)处对象O1在X轴上所处的观测位置LMx(i)、在Y轴上所处的位置观测LMy(i)和在Z轴上所处的观测位置LMz(i):
Max(i)=A-roll(i)*Ha(i)+LMx(i)*Ha(i),
Mbx(i)=A-pitch(i)*Hb(i)+LMy(i)*Ha(i),
May(i)=-LMz(i)*Ha(i)-A-yaw(i)*Ha(i),
Mby(i)=-A-yaw(i)*Hb(i)+LMy(i)*Hb(i)。
在方框214,控制器50计算在轨迹采样时间点T(i-1)处对象O1在X轴上所处的实际位置ALMx(i-1)与在轨迹采样时间点T(i-2)处对象O1在X轴上所处的实际位置ALMx(i-2)之间的差值,作为在轨迹采样时间点T(i)和轨迹采样时间点T(i-1)之间对象O1在X轴上的预测位置变化量X1,计算在轨迹采样时间点T(i-1)处对象O1在Y轴上所处的实际位置ALMy(i-1)与在轨迹采样时间点T(i-2)处对象O1在Y轴上所处的实际位置ALMy(i-2)之间的差值,作为在轨迹采样时间点T(i)和轨迹采样时间点T(i-1)之间对象O1在Y轴上的预测位置变化量Y1,以及,计算在轨迹采样时间点T(i-1)处对象O1在Z轴上所处的实际位置ALMz(i-1)与在轨迹采样时间点T(i-2)处对象O1在Z轴上所处的实际位置ALMz(i-2)之间的差值,作为在轨迹采样时间点T(i)和轨迹采样时间点T(i-1)之间对象O1在Z轴上的预测位置变化量Z1。
在方框216,控制器50计算在轨迹采样时间点T(i-1)处对象O1在X轴上所处的实际位置ALMx(i-1)与预测位置变化量X1之和,作为在轨迹采样时间点T(i)处对象O1在X轴上所处的预测位置PLMx(i),计算在轨迹采样时间点T(i-1)处对象O1在Y轴上所处的实际位置ALMy(i-1)与预测位置变化量Y1之和,作为在轨迹采样时间点T(i)处对象O1在Y轴上所处的预测位置PLMy(i),以及,计算在轨迹采样时间点T(i-1)处对象O1在Z轴上所处的实际位置ALMz(i-1)与预测位置变化量Z1之和,作为在轨迹采样时间点T(i)处对象O1在Z轴上所处的预测位置PLMz(i)。
在方框218,控制器50根据观测位置LMx(i)、LMy(i)和LMz(i)以及预测位置PLMx(i)、PLMy(i)和PLMz(i),利用诸如卡尔曼滤波器算法或最小二乘法等的算法,确定在轨迹采样时间点T(i)处对象O1在X轴上所处的实际位置ALMx(i)、在Y轴上所处的实际位置ALMy(i)和在Z轴上所处的实际位置ALMz(i)。
在方框220,控制器50计算光学导航芯片20相对于设备10的重心的高度Ca和在轨迹采样时间点T(i)处设备10的重心相对于地面的高度DC之和,作为在轨迹采样时间点T(i)处光学导航芯片20距离地面的高度Ha(i),以及,计算光学导航芯片30相对于设备10的重心的高度Cb和在轨迹采样时间点T(i)处设备10的重心相对于地面的高度DC之和,作为在轨迹采样时间点T(i)处光学导航芯片30距离地面的高度Hb(i)。这里,高度Ca和高度Cb是当光学导航芯片20和30被布置在设备10中时就已经是确定的并且是已知的。在本实施例中,在轨迹采样时间点T(i)处设备10的重心相对于地面的高度DC等于在轨迹采样时间点T(i)处对象O1在Z轴上所处的实际位置ALMz(i)。
在方框222,控制器50输出A-roll(i)、A-yaw(i)和A-pitch(i)作为在轨迹采样时间点T(i)处对象O1的姿态角度数据,以及,输出ALMx(i)、ALMy(i)和ALMz(i)作为在轨迹采样时间点T(i)处对象O1的实际位置数据。
从上面的描述可以看出,本实施例的方案利用光学导航芯片和惯性测量单元来跟踪对象的运动轨迹,由于光学导航芯片和惯性测量单元比较便宜、所产生的数据处理量较小和功耗较低,因此,与现有技术相比,本实施例的方案能够以低成本、低功耗和低运算需求的方式来提供运动轨迹跟踪。
其它变型
本领域技术人员应当理解,虽然在上面的实施例中,控制器50通过等式Max(i)=A-roll(i)*Ha(i)+LMx(i)*Ha(i)、Mbx(i)=A-pitch(i)*Hb(i)+LMy(i)*Ha(i)、May(i)=-LMz(i)*Ha(i)-A-yaw(i)*Ha(i)和Mby(i)=-A-yaw(i)*Hb(i)+LMy(i)*Hb(i)来计算观测位置LMx(i)、LMy(i)和LMz(i),然而,本发明并不局限于此。在本发明的其它一些实施例中,控制器50也可以通过其它合适的方式来计算观测位置LMx(i)、LMy(i)和LMz(i)。
本领域技术人员应当理解,虽然在上面的实施例中,控制器50根据观测位置LMx(i)、LMy(i)和LMz(i)以及预测位置PLMx(i)、PLMy(i)和PLMz(i)来确定在轨迹采样时间点T(i)处对象O1的实际位置ALMx(i)、ALMy(i)和ALMz(i),然而,本发明并不局限于此。在本发明的其它一些实施例中,控制器50也可以不计算预测位置PLMx(i)、PLMy(i)和PLMz(i),而是直接将观测位置LMx(i)、LMy(i)和LMz(i)用作实际位置ALMx(i)、ALMy(i)和ALMz(i)。
本领域技术人员应当理解,虽然在上面的实施例中,光学导航芯片20和30都测量对象在Z轴上所处的位置,然而,本发明并不局限于此。在本发明的其它一些实施例中,光学导航芯片20和30也可以都测量对象在X或Y轴上所处的位置。
图3示出了按照本发明的用于运动轨迹跟踪的方法的流程图。图3所示的方法300例如可以由控制器50或其它合适的设备来执行。
如图3所示,方法300可以包括,在方框302,至少基于在多个轨迹采样时间点的任一轨迹采样时间点处对象的第一旋转角度、第二旋转角度、第三旋转角度、第一位置、第二位置、第三位置和第四位置,估算在所述任一轨迹采样时间点处所述对象在第一轴上所处的第一观测位置、在第二轴上所处的第二观测位置和在第三轴上所处的第三观测位置,其中,所述第一轴、所述第二轴和所述第三轴相互垂直,其中,所述第一旋转角度、所述第二旋转角度和所述第三旋转角度分别是惯性测量单元测量的在所述任一轨迹采样时间点处所述对象绕着所述第一轴、所述第二轴和所述第三轴的旋转角度,所述第一位置和所述第二位置分别是第一光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的位置和在所述第三轴上所处的位置,以及,所述第三位置和第四位置分别是第二光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第二轴所处上的位置和在所述第三轴所处上的位置。
方法300还可以包括,在方框304,至少基于所述第一观测位置、所述第二观测位置和所述第三观测位置,计算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一实际位置、在所述第二轴上所处的第二实际位置和在所述第三轴上所处的第三实际位置。
方法300还可以包括,在方框306,输出在所述任一轨迹采样时间点处所述对象的姿态角度数据和实际位置数据,其中,所述姿态角度数据包括所述第一旋转角度、所述第二旋转角度和所述第三旋转角度,以及,所述实际位置数据包括所述第一实际位置、所述第二实际位置和所述第三实际位置,
在一个方面,方框302可以包括:估计在所述任一轨迹采样时间点处所述第一光学导航芯片距离地面的第一高度和所述第二光学导航芯片距离地面的第二高度;以及,利用以下等式来计算所述第一观测位置、所述第二观测位置和所述第三观测位置:
Max=A-roll*Ha+LMx*Ha,
Mbx=A-pitch*Hb+LMy*Ha,
May=-LMz*Ha-A-yaw*Ha,
Mby=-A-yaw*Hb+LMy*Hb,
其中,Max表示所述第一位置,May表示所述第二位置,Mbx表示所述第三位置,Mby表示所述第四位置,A-roll表示所述第一旋转角度,A-yaw表示所述第二旋转角度,A-pitch表示所述第三旋转角度,Ha表示所述第一高度,Hb表示所述第二高度,LMx表示所述第一观测位置,LMy表示所述第二观测位置,以及,LMz表示所述第三观测位置。
在又一个方面,方框304可以包括:基于在所述任一轨迹采样时间点的前一个轨迹采样时间点处所述对象的所述实际位置数据,以及,在所述前一个轨迹采样时间点和所述前一个轨迹采样时间点的前一个轨迹采样时间点之间所述对象分别沿着所述第一轴、所述第二轴和所述第三轴的位置变化量,预测在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一预测位置、在所述第二轴上所处的第二预测位置和在所述第三轴上所处的第三预测位置;以及,基于所述第一观测位置、所述第二观测位置、所述第三观测位置、所述第一预测位置、所述第二预测位置和所述第三预测位置,确定所述第一实际位置、所述第二实际位置和所述第三实际位置。
图4示出了按照本发明的用于运动轨迹跟踪的装置的示意图。图4所示的装置400可以利用软件、硬件或软硬件结合的方式来实现,并且例如可以安装在控制器50或其它合适的设备中。
如图4所示,装置400可以包括估算模块402、计算模块404和输出模块406。估算模块402用于至少基于在多个轨迹采样时间点的任一轨迹采样时间点处对象的第一旋转角度、第二旋转角度、第三旋转角度、第一位置、第二位置、第三位置和第四位置,估算在所述任一轨迹采样时间点处所述对象在第一轴上所处的第一观测位置、在第二轴上所处的第二观测位置和在第三轴上所处的第三观测位置,其中,所述第一轴、所述第二轴和所述第三轴相互垂直。计算模块404用于至少基于所述第一观测位置、所述第二观测位置和所述第三观测位置,计算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一实际位置、在所述第二轴上所处的第二实际位置和在所述第三轴上所处的第三实际位置。输出模块406用于输出在所述任一轨迹采样时间点处所述对象的姿态角度数据和实际位置数据,其中,所述姿态角度数据包括所述第一旋转角度、所述第二旋转角度和所述第三旋转角度,以及,所述实际位置数据包括所述第一实际位置、所述第二实际位置和所述第三实际位置。其中,所述第一旋转角度、所述第二旋转角度和所述第三旋转角度分别是惯性测量单元测量的在所述任一轨迹采样时间点处所述对象绕着所述第一轴、所述第二轴和所述第三轴的旋转角度,所述第一位置和所述第二位置分别是第一光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的位置和在所述第三轴上所处的位置,以及,所述第三位置和第四位置分别是第二光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第二轴所处上的位置和在所述第三轴所处上的位置。
在一个方面,估算模块402可以包括:第一估计模块,用于估计在所述任一轨迹采样时间点处所述第一光学导航芯片距离地面的第一高度和所述第二光学导航芯片距离地面的第二高度;以及,第二估计模块,用于利用以下等式来计算所述第一观测位置、所述第二观测位置和所述第三观测位置:
Max=A-roll*Ha+LMx*Ha,
Mbx=A-pitch*Hb+LMy*Ha,
May=-LMz*Ha-A-yaw*Ha,
Mby=-A-yaw*Hb+LMy*Hb,
其中,Max表示所述第一位置,May表示所述第二位置,Mbx表示所述第三位置,Mby表示所述第四位置,A-roll表示所述第一旋转角度,A-yaw表示所述第二旋转角度,A-pitch表示所述第三旋转角度,Ha表示所述第一高度,Hb表示所述第二高度,LMx表示所述第一观测位置,LMy表示所述第二观测位置,以及,LMz表示所述第三观测位置。
在又一个方面,计算模块404可以包括:预测模块,用于基于在所述任一轨迹采样时间点的前一个轨迹采样时间点处所述对象的所述实际位置数据,以及,在所述前一个轨迹采样时间点和所述前一个轨迹采样时间点的前一个轨迹采样时间点之间所述对象分别沿着所述第一轴、所述第二轴和所述第三轴的位置变化量,预测在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一预测位置、在所述第二轴上所处的第二预测位置和在所述第三轴上所处的第三预测位置;以及,确定模块,用于基于所述第一观测位置、所述第二观测位置、所述第三观测位置、所述第一预测位置、所述第二预测位置和所述第三预测位置,确定所述第一实际位置、所述第二实际位置和所述第三实际位置。
图5示出了按照本发明的一个实施例的控制器的示意图。如图5所示,控制器500可以包括处理器502和存储器504。其中,存储器504存储有可执行指令,所述可执行指令当被执行时使得处理器502执行图2所示的方法200或图3所示的方法300。
本发明的实施例还提供一种机器可读存储介质,其存储有可执行指令,所述可执行指令当被执行时使得机器执行图2所示的方法200或图3所示的方法300。
本领域技术人员将理解,本发明所公开的各个实施例可以在不偏离发明实质的情况下做出各种变形、修改和/或调整,这些变形、修改和/或调整都落在本发明的保护范围之内。因此,本发明的保护范围由所附的权利要求书来定义。
Claims (11)
1.一种用于运动轨迹跟踪的设备,包括:
惯性测量单元,用于测量在多个轨迹采样时间点的任一轨迹采样时间点处对象围绕第一轴的第一旋转角度、绕着第二轴的第二旋转角度和绕着第三轴的第三旋转角度,其中,所述第一轴、所述第二轴和所述第三轴相互垂直;
第一光学导航芯片,用于测量在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一位置和在所述第三轴上所处的第二位置;
第二光学导航芯片,用于测量在所述任一轨迹采样时间点处所述对象在所述第二轴所处上的第三位置和在所述第三轴所处上的第四位置;以及
控制器,其与所述惯性测量单元、所述第一光学导航芯片和所述第二光学导航芯片连接,并且用于:
至少基于所述第一旋转角度、所述第二旋转角度、所述第三旋转角度、所述第一位置、所述第二位置、所述第三位置和所述第四位置,估算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一观测位置、在所述第二轴上所处的第二观测位置和在所述第三轴上所处的第三观测位置;
至少基于所述第一观测位置、所述第二观测位置和所述第三观测位置,计算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一实际位置、在所述第二轴上所处的第二实际位置和在所述第三轴上所处的第三实际位置;以及,
输出在所述任一轨迹采样时间点处所述对象的姿态角度数据和实际位置数据,其中,所述姿态角度数据包括所述第一旋转角度、所述第二旋转角度和所述第三旋转角度,以及,所述实际位置数据包括所述第一实际位置、所述第二实际位置和所述第三实际位置。
2.如权利要求1所述的设备,其中
所述估算所述第一观测位置、所述第二观测位置和所述第三观测位置包括:
估计在所述任一轨迹采样时间点处所述第一光学导航芯片距离地面的第一高度和所述第二光学导航芯片距离地面的第二高度;以及
利用以下等式来计算所述第一观测位置、所述第二观测位置和所述第三观测位置:
Max=A-roll*Ha+LMx*Ha,
Mbx=A-pitch*Hb+LMy*Ha,
May=-LMz*Ha-A-yaw*Ha,
Mby=-A-yaw*Hb+LMy*Hb,
其中,Max表示所述第一位置,May表示所述第二位置,Mbx表示所述第三位置,Mby表示所述第四位置,A-roll表示所述第一旋转角度,A-yaw表示所述第二旋转角度,A-pitch表示所述第三旋转角度,Ha表示所述第一高度,Hb表示所述第二高度,LMx表示所述第一观测位置,LMy表示所述第二观测位置,以及,LMz表示所述第三观测位置。
3.如权利要求1所述的设备,其中
所述计算所述第一实际位置、所述第二实际位置和所述第三实际位置包括:
基于在所述任一轨迹采样时间点的前一个轨迹采样时间点处所述对象的所述实际位置数据,以及,在所述前一个轨迹采样时间点和所述前一个轨迹采样时间点的前一个轨迹采样时间点之间所述对象分别沿着所述第一轴、所述第二轴和所述第三轴的位置变化量,预测在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一预测位置、在所述第二轴上所处的第二预测位置和在所述第三轴上所处的第三预测位置;以及
基于所述第一观测位置、所述第二观测位置、所述第三观测位置、所述第一预测位置、所述第二预测位置和所述第三预测位置,确定所述第一实际位置、所述第二实际位置和所述第三实际位置。
4.一种用于运动轨迹跟踪的方法,包括:
至少基于在多个轨迹采样时间点的任一轨迹采样时间点处对象的第一旋转角度、第二旋转角度、第三旋转角度、第一位置、第二位置、第三位置和第四位置,估算在所述任一轨迹采样时间点处所述对象在第一轴上所处的第一观测位置、在第二轴上所处的第二观测位置和在第三轴上所处的第三观测位置,其中,所述第一轴、所述第二轴和所述第三轴相互垂直;
至少基于所述第一观测位置、所述第二观测位置和所述第三观测位置,计算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一实际位置、在所述第二轴上所处的第二实际位置和在所述第三轴上所处的第三实际位置;以及,
输出在所述任一轨迹采样时间点处所述对象的姿态角度数据和实际位置数据,其中,所述姿态角度数据包括所述第一旋转角度、所述第二旋转角度和所述第三旋转角度,以及,所述实际位置数据包括所述第一实际位置、所述第二实际位置和所述第三实际位置,
其中,所述第一旋转角度、所述第二旋转角度和所述第三旋转角度分别是惯性测量单元测量的在所述任一轨迹采样时间点处所述对象绕着所述第一轴、所述第二轴和所述第三轴的旋转角度,
所述第一位置和所述第二位置分别是第一光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的位置和在所述第三轴上所处的位置,以及,
所述第三位置和第四位置分别是第二光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第二轴所处上的位置和在所述第三轴所处上的位置。
5.如权利要求4所述的方法,其中
所述估算所述第一观测位置、所述第二观测位置和所述第三观测位置包括:
估计在所述任一轨迹采样时间点处所述第一光学导航芯片距离地面的第一高度和所述第二光学导航芯片距离地面的第二高度;以及
利用以下等式来计算所述第一观测位置、所述第二观测位置和所述第三观测位置:
Max=A-roll*Ha+LMx*Ha,
Mbx=A-pitch*Hb+LMy*Ha,
May=-LMz*Ha-A-yaw*Ha,
Mby=-A-yaw*Hb+LMy*Hb,
其中,Max表示所述第一位置,May表示所述第二位置,Mbx表示所述第三位置,Mby表示所述第四位置,A-roll表示所述第一旋转角度,A-yaw表示所述第二旋转角度,A-pitch表示所述第三旋转角度,Ha表示所述第一高度,Hb表示所述第二高度,LMx表示所述第一观测位置,LMy表示所述第二观测位置,以及,LMz表示所述第三观测位置。
6.如权利要求4所述的方法,其中
所述计算所述第一实际位置、所述第二实际位置和所述第三实际位置包括:
基于在所述任一轨迹采样时间点的前一个轨迹采样时间点处所述对象的所述实际位置数据,以及,在所述前一个轨迹采样时间点和所述前一个轨迹采样时间点的前一个轨迹采样时间点之间所述对象分别沿着所述第一轴、所述第二轴和所述第三轴的位置变化量,预测在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一预测位置、在所述第二轴上所处的第二预测位置和在所述第三轴上所处的第三预测位置;以及
基于所述第一观测位置、所述第二观测位置、所述第三观测位置、所述第一预测位置、所述第二预测位置和所述第三预测位置,确定所述第一实际位置、所述第二实际位置和所述第三实际位置。
7.一种用于运动轨迹跟踪的装置,包括:
估算模块,用于至少基于在多个轨迹采样时间点的任一轨迹采样时间点处对象的第一旋转角度、第二旋转角度、第三旋转角度、第一位置、第二位置、第三位置和第四位置,估算在所述任一轨迹采样时间点处所述对象在第一轴上所处的第一观测位置、在第二轴上所处的第二观测位置和在第三轴上所处的第三观测位置,其中,所述第一轴、所述第二轴和所述第三轴相互垂直;
计算模块,用于至少基于所述第一观测位置、所述第二观测位置和所述第三观测位置,计算在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一实际位置、在所述第二轴上所处的第二实际位置和在所述第三轴上所处的第三实际位置;以及,
输出模块,用于输出在所述任一轨迹采样时间点处所述对象的姿态角度数据和实际位置数据,其中,所述姿态角度数据包括所述第一旋转角度、所述第二旋转角度和所述第三旋转角度,以及,所述实际位置数据包括所述第一实际位置、所述第二实际位置和所述第三实际位置,
其中,所述第一旋转角度、所述第二旋转角度和所述第三旋转角度分别是惯性测量单元测量的在所述任一轨迹采样时间点处所述对象绕着所述第一轴、所述第二轴和所述第三轴的旋转角度,
所述第一位置和所述第二位置分别是第一光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的位置和在所述第三轴上所处的位置,以及,
所述第三位置和第四位置分别是第二光学导航芯片测量的在所述任一轨迹采样时间点处所述对象在所述第二轴所处上的位置和在所述第三轴所处上的位置。
8.如权利要求7所述的装置,其中,所述估算模块包括:
第一估计模块,用于估计在所述任一轨迹采样时间点处所述第一光学导航芯片距离地面的第一高度和所述第二光学导航芯片距离地面的第二高度;以及
第二估计模块,用于利用以下等式来计算所述第一观测位置、所述第二观测位置和所述第三观测位置:
Max=A-roll*Ha+LMx*Ha,
Mbx=A-pitch*Hb+LMy*Ha,
May=-LMz*Ha-A-yaw*Ha,
Mby=-A-yaw*Hb+LMy*Hb,
其中,Max表示所述第一位置,May表示所述第二位置,Mbx表示所述第三位置,Mby表示所述第四位置,A-roll表示所述第一旋转角度,A-yaw表示所述第二旋转角度,A-pitch表示所述第三旋转角度,Ha表示所述第一高度,Hb表示所述第二高度,LMx表示所述第一观测位置,LMy表示所述第二观测位置,以及,LMz表示所述第三观测位置。
9.如权利要求7所述的装置,其中,所述计算模块包括:
预测模块,用于基于在所述任一轨迹采样时间点的前一个轨迹采样时间点处所述对象的所述实际位置数据,以及,在所述前一个轨迹采样时间点和所述前一个轨迹采样时间点的前一个轨迹采样时间点之间所述对象分别沿着所述第一轴、所述第二轴和所述第三轴的位置变化量,预测在所述任一轨迹采样时间点处所述对象在所述第一轴上所处的第一预测位置、在所述第二轴上所处的第二预测位置和在所述第三轴上所处的第三预测位置;以及
确定模块,用于基于所述第一观测位置、所述第二观测位置、所述第三观测位置、所述第一预测位置、所述第二预测位置和所述第三预测位置,确定所述第一实际位置、所述第二实际位置和所述第三实际位置。
10.一种控制器,包括:
处理器;以及
存储器,其存储有可执行指令,所述可执行指令当被执行时使得所述处理器执行权利要求4-6中的任意一个所述的方法。
11.一种机器可读存储介质,其存储有可执行指令,所述可执行指令当被执行时使得机器执行权利要求4-6中的任意一个所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710844684.7A CN109521802A (zh) | 2017-09-19 | 2017-09-19 | 用于运动轨迹跟踪的方法、装置和设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710844684.7A CN109521802A (zh) | 2017-09-19 | 2017-09-19 | 用于运动轨迹跟踪的方法、装置和设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109521802A true CN109521802A (zh) | 2019-03-26 |
Family
ID=65767937
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710844684.7A Pending CN109521802A (zh) | 2017-09-19 | 2017-09-19 | 用于运动轨迹跟踪的方法、装置和设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109521802A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111105644A (zh) * | 2019-11-22 | 2020-05-05 | 京东数字科技控股有限公司 | 一种车辆盲区监测、行驶控制方法、装置及车路协同系统 |
CN111866721A (zh) * | 2019-04-30 | 2020-10-30 | 博世汽车部件(苏州)有限公司 | 用于获取行驶轨迹的方法、装置和行驶设备 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1563889A (zh) * | 2004-03-26 | 2005-01-12 | 清华大学 | 激光跟踪惯性组合测量系统及其测量方法 |
RU2325671C1 (ru) * | 2006-12-11 | 2008-05-27 | Государственное унитарное предприятие "Конструкторское бюро приборостроения" | Следящая локационно-оптическая система сопровождения подвижных объектов |
CN104506827A (zh) * | 2015-01-13 | 2015-04-08 | 苏州创捷传媒展览股份有限公司 | 具有人体跟踪功能的互动视觉拍摄装置 |
CN104764452A (zh) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | 一种基于惯性和光学跟踪系统的混合位姿跟踪方法 |
CN106468547A (zh) * | 2015-08-17 | 2017-03-01 | 波音公司 | 利用多个光学传感器的用于自导飞行器的不依赖全球定位系统(“gps”)的导航系统 |
-
2017
- 2017-09-19 CN CN201710844684.7A patent/CN109521802A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1563889A (zh) * | 2004-03-26 | 2005-01-12 | 清华大学 | 激光跟踪惯性组合测量系统及其测量方法 |
RU2325671C1 (ru) * | 2006-12-11 | 2008-05-27 | Государственное унитарное предприятие "Конструкторское бюро приборостроения" | Следящая локационно-оптическая система сопровождения подвижных объектов |
CN104506827A (zh) * | 2015-01-13 | 2015-04-08 | 苏州创捷传媒展览股份有限公司 | 具有人体跟踪功能的互动视觉拍摄装置 |
CN104764452A (zh) * | 2015-04-23 | 2015-07-08 | 北京理工大学 | 一种基于惯性和光学跟踪系统的混合位姿跟踪方法 |
CN106468547A (zh) * | 2015-08-17 | 2017-03-01 | 波音公司 | 利用多个光学传感器的用于自导飞行器的不依赖全球定位系统(“gps”)的导航系统 |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111866721A (zh) * | 2019-04-30 | 2020-10-30 | 博世汽车部件(苏州)有限公司 | 用于获取行驶轨迹的方法、装置和行驶设备 |
CN111105644A (zh) * | 2019-11-22 | 2020-05-05 | 京东数字科技控股有限公司 | 一种车辆盲区监测、行驶控制方法、装置及车路协同系统 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11125581B2 (en) | Method and system for calibrating components of an inertial measurement unit (IMU) using scene-captured data | |
Zhang et al. | Quaternion-based Kalman filter with vector selection for accurate orientation tracking | |
Lobo et al. | Vision and inertial sensor cooperation using gravity as a vertical reference | |
GB2498177A (en) | Apparatus for determining a floor plan of a building | |
JP2016525842A (ja) | カメラ動き予測及び修正のための方法 | |
CN110268445A (zh) | 利用陀螺仪的相机自动校准 | |
Liu et al. | An external parameter calibration method for multiple cameras based on laser rangefinder | |
Kumagai et al. | Development of a three-dimensional ball rotation sensing system using optical mouse sensors | |
US20190376860A1 (en) | Visuo-haptic sensor | |
CN108827341A (zh) | 用于确定图像采集装置的惯性测量单元中的偏差的方法 | |
CN105607760A (zh) | 一种基于微惯性传感器的轨迹还原方法及系统 | |
CN108180909A (zh) | 相对位置确定方法、装置及电子设备 | |
CN109445599A (zh) | 交互笔检测方法及3d交互系统 | |
CN109521802A (zh) | 用于运动轨迹跟踪的方法、装置和设备 | |
CN108932732A (zh) | 一种获取监测对象数据信息的方法及装置 | |
CN109983299A (zh) | 工业机器人的测量系统和方法 | |
JP6485195B2 (ja) | 傾斜度測定方法及び装置並びに電子機器及びプログラム | |
JP6052090B2 (ja) | 磁気測定システム | |
EP3392748B1 (en) | System and method for position tracking in a virtual reality system | |
Pajor et al. | Stereovision system for motion tracking and position error compensation of loading crane | |
Haase et al. | AR binocular: Augmented reality system for nautical navigation | |
He et al. | Rotational coordinate transformation for visual-inertial sensor fusion | |
Saponaro et al. | Towards auto-calibration of smart phones using orientation sensors | |
Hui et al. | Visual SLAM based on EKF filtering algorithm from omnidirectional camera | |
JP6477214B2 (ja) | 傾斜度測定方法及び装置並びに電子機器及びプログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20190326 |
|
WD01 | Invention patent application deemed withdrawn after publication |