公开/公告号CN103576141A
专利类型发明专利
公开/公告日2014-02-12
原文格式PDF
申请/专利权人 财团法人交大思源基金会;
申请/专利号CN201210363853.2
申请日2012-09-26
分类号G01S13/50;G01S13/58;
代理机构北京同立钧成知识产权代理有限公司;
代理人臧建明
地址 中国台湾新竹市大学路1001号
入库时间 2024-02-19 22:27:24
法律状态公告日
法律状态信息
法律状态
2017-09-05
授权
授权
2014-03-12
实质审查的生效 IPC(主分类):G01S13/50 申请日:20120926
实质审查的生效
2014-02-12
公开
公开
技术领域
本发明是有关于一种参数估计方法,且特别是有关于一种移动参数估计 方法、角度估计方法及判断方法。
背景技术
随着人口急速增长以及世界经济的逐渐复苏,个人可支配所得与消费力 逐年上升,使得全球汽车销售量也随之不断攀升。根据统计,在2010年中, 大部分国家的汽车销量都呈现正向成长,其中又以中国大陆的汽车销售量为 最高,拥有1806万辆的销量,随后则是美国和日本,各自有约1200万辆及 500万辆的销售量。
随着汽车销售量不断成长,车用电子产业亦蓬勃发展,其中包含了车用 安全系统、车身系统、驾驶信息系统、悬吊底盘系统、引擎传动系统、保全 系统等六大方面,其中又以车用安全系统的年复合成长率为最高。
随着汽车数量日益增长,发生道路事故的机率也逐年增加,欧洲研究显 示,驾驶员只要在发生碰撞的0.5秒前得到预警,即可以避免至少60%的追 尾撞车事故、30%的迎面撞车事故和50%的路面相关事故,而若有1秒钟的 预警时间,则可避免90%的事故。因此,车用安全系统的重要性可见一斑。
在车用安全系统中,根据功能及种类不同,主要可分为主动式及被动式 两种类型。在2000年以前,市场上几乎是以被动式安全系统为主,例如安全 带、各类安全气囊等,直到2000年以后,随着半导体科技进展及高频电子电 路技术的进步,主动式安全系统才开始快速发展。同时,人们对安全的要求 也不断提升,从原本被动地降低意外伤害,转变为主动地避免意外发生。而 在道路行驶的实际状况中,主动式安全系统能在危险发生前发挥预警功能, 提醒驾驶甚至主动操纵车辆闪避危险,达到保护生命安全的目的,将比被动 系统扮演更重要的角色。
因此,现今主动式安全系统获得极大发展,主导整体车用安全系统的市 场,举例而言,适应性巡航控制(Adaptive Cruise Control,ACC)、智能型停走 系统(Stop & Go)、车道偏离警示系统(Lane Departure Warning System)、环境 辨识(Environment Recognition)、智能型防撞系统(Collision Avoidance)、先进 驾驶辅助系统(Advanced Driver Aid System,ADAS)等皆在其范畴内。而各国 对于主动安全系统主要发展三大方向为:1.基本防撞警示技术-适应性巡航控 制、防止车辆后方追撞、检测车辆前方行人与障碍物。2.进阶防撞警示技术 -避免不当切换车道、十字路口所发生的碰撞事故,以及车辆诊断。3.基本驾 驶人信息设备-路线指引、实时交通、驾驶人信息与行车导航。
目前欧洲的政府研究计划着重在驾驶者监视系统、路况检测系统以及智 能化方向控制等功能开发。美国政府与通用汽车合作,开发并测试后视汽车 防撞系统。日本现正大量进行智能化公路(Smartway)实际验证,采用前视雷 达及车道偏离警示器等子系统,完成巡航、防撞、循轨前进等先进汽车功能, 这些功能预计在公元2015年在日本大量实施。此外,韩国亦已规划在2020 年完成汽车及公路的自动化。
主动安全系统中的适应性巡航控制(ACC),主要往车辆定速驾驶功能的 趋势发展,其应用范围可分为高速域、低速域及全速域的巡航控制。其中, 适应性巡航控制的原理为利用装设于车辆前方的雷达系统,在车辆进行定速 驾驶时,同时检测车辆前方的交通状况,以维持安全距离。当其他车辆进入 车道使得安全距离不足时,车辆便自动减速驾驶,而当车辆前方有足够安全 距离时便自动加速,回复至驾驶者设定的速度。目前为止,例如NISSAN、 BMW、Mercedes Benz、Lexus、和Infiniti等的汽车制造厂商,都已在高级车 款配备适应性巡航控制系统。而Stop & Go系统功能为适应性巡航控制的进 阶版,其改良处在于可视需求将车辆减速至停止,而非在特定速度下关闭系 统功能。此外,Stop & Go系统亦可在前方车辆再度移动时,自动加速至默认 速度且保持车距,且默认速度可随交通状况不同而自动调整,适用于拥塞的 市区道路。
由上述可知在适应性巡航控制及Stop & Go系统中,最前端的防撞雷达 是非常关键的一部分,一旦雷达判断失误而提供不正确的信息,将导致整个 系统对应产生错误的控制方式。
然而,在一般适应性巡航控制的信号处理技术中,常需较大的运算量以 及较长的观察时间才能达到一定的检测精确度,使得适应性巡航控制的实际 应用仍有其不理想之处。
发明内容
有鉴于此,本发明提供一种移动参数估计方法、角度估计方法以及 判断方法。在移动参数估计方法中,通过所提的修正数学式,可使估计 的移动参数更符合目标物的移动情形。在角度估计方法中,通过所推导 的封闭形式解,可使估计角度的操作具有低运算量及高精确度的特性。 在判断方法中,通过简单的操作,可判断所估计的估计角度是否可靠。
本发明提供一种移动参数估计方法,适于电子装置,电子装置包括至少 一天线,所述方法包括下列步骤。首先发送第一调频连续波(Frequency Modulation Continuous Wave,FMCW)信号,并由各所述天线接收目标物反射 第一调频连续波信号而形成的第二调频连续波信号。接着,依据各所述天线 接收的第二调频连续波信号,个别获得第一数学式及一第二数学式。之后, 依据第一数学式以及第二数学式估计关联于目标物的多个移动参数。其中, 第一数学式为
第二数学式为
其中,B为第一调频连续波信号的带宽,R为电子装置与目标物的相对 距离,fc为第一调频连续波信号的载波频率,v为电子装置与目标物的相对速 度,Tramp为关联于所述多个量测值的量测时间,c为光速。
本发明提供一种角度估计方法,适于电子装置,电子装置包括至少一天 线,所述方法包括下列步骤。首先,发送第一调频连续波信号,并由各所述 天线接收目标物反射第一调频连续波信号而形成的第二调频连续波信号。接 着,依据第一调频连续波信号以及第二调频连续波信号获得关联于目标物的 多个移动参数,以及依据所述多个移动参数以及各所述天线的组态参数而获 得对应于各所述天线的多个量测值。之后,将所述多个量测值代入数学式, 以获得电子装置的默认方向与目标物之间的估计角度。其中,数学式为:
其中,为估计角度,N为至少一天线的数量,d为至少一天线的间距, λ为第一调频连续波信号的波长,ri为所述多个量测值中的第i个量测值,rj为所述多个量测值中的第j个量测值,ARG(ri*·rj)为ri及rj进行共轭转置运 算(Hermitian operation)之后的主幅角。
在本发明的一实施例中,上述第二数学式为
在本发明的一实施例中,上述第二数学式为
本发明提供一种判断方法,适于电子装置,电子装置包括至少一天线, 所述方法包括下列步骤。首先,发送第一调频连续波信号,并由各所述天线 接收目标物反射第一调频连续波信号而形成的第二调频连续波信号。接着, 依据第一调频连续波信号以及第二调频连续波信号获得关联于目标物的多个 移动参数,并依据所述多个移动参数以及各所述天线的组态参数而获得对应 于各所述天线的多个量测值。之后,依据所述多个量测值获得电子装置的默 认方向与目标物之间的估计角度,并分别设计第一滤波器及第二滤波器,其 中第一滤波器的零点位于电子装置的视场角的范围之外而,第二滤波器的零 点位于估计角度。然后,分别计算所述多个量测值对应于第一滤波器以及第 二滤波器的第一运算值及第二运算值,并依据第一运算值与第二运算值的比 例值判断估计角度的可靠度。
在本发明的一实施例中,在将所述多个量测值代入数学式,以获得电子 装置的默认方向与目标物之间的估计角度的步骤之后,还包括将估计角度代 入第三数学式,以获得在电子装置的视场角的范围中未出现同位角效应的至 少一角度。其中,第三数学式为:
其中,
θFOV为视场角。
在本发明的一实施例中,上述第一调频连续波信号包括第一信号及第二 信号,第二调频连续波信号包括第三信号及第四信号,且其中依据第一调频 连续波信号以及第二调频连续波信号获得第一数学式及第二数学式的步骤包 括依据第一信号及第三信号的相对关系获得第一数学式。之后,依据第二信 号及第四信号的相对关系获得第二数学式。
在本发明的一实施例中,上述多个移动参数包括相对距离以及相对速度。
在本发明的一实施例中,上述带宽可依据关联于相对距离的量测距离分 辨率决定。
在本发明的一实施例中,上述量测时间可依据关联于相对速度的量测速 度分辨率决定。
在本发明的一实施例中,上述第一运算值为其中w1为第一滤波 器的系数,r为所述多个量测值组成的向量,(·)H为共轭转置运算符,||·||2为 模数2(2-norm)运算符。
在本发明的一实施例中,上述第二运算值为其中w2为第二滤波 器的系数,r为所述多个量测值组成的向量,(·)H为共轭转置运算符,||·||2为 模数2运算符。
基于上述,本发明实施例提供一种移动参数估计方法、角度估计方法及 判断方法。在所述移动参数估计方法中,通过第一数学式及第二数学式,可 求得较符合于目标物的实际移动情形的移动参数。在所述角度估计方法中, 可依据所推导的封闭形式解来找出电子装置与目标物之间的估计角度。此外, 由于所述封闭形式解具有低运算量及高精确度的特性,使得本发明实施例的 角度估计方法更适合应用在实际情形中。另一方面,在由角度估计方法得到 估计角度之后,可通过所述判断方法来判断此估计角度是否可靠。
为让本发明的上述特征和优点能更明显易懂,下文特举实施例,并配合 所附图式作详细说明如下。
附图说明
图1是本发明的一实施例示出的利用电子装置对目标物进行移动参数估 计的示意图;
图2是本发明的一实施例示出的移动参数估计方法的流程图;
图3是本发明的一实施例示出的第一调频连续波信号及第二调频连续波 信号之间相对关系的示意图;
图4是本发明的一实施例示出的角度估计方法的流程图;
图5是本发明的一实施例示出的判断方法的流程图;
图6是本发明一实施例示出的将电子装置用于估计车辆间相对移动参数 及角度的示意图。
附图标记说明:
100:电子装置;
110_1~110_N:信号处理单元;
120_1~120_N:天线;
130:角度估计单元;
140:目标物;
610~630:车辆;
r1~rN:量测值;
DI1、DI2、DI1’:预设方向;
θ:真实角度;
S210~S240、S410~S450、S510~S590:步骤;
B:带宽;
d:间距;
fc:载波频率;
fb、fb’:拍频;
fd:多普勒偏移频率;
Tramp:量测时间;
T1、T2、R1、R2:信号;
τ:时间差;
R12、R13:相对距离;
θ12、θ13:角度。
具体实施方式
现将详细参考本发明的实施例,在附图中说明所述实施例的实例。另外, 凡可能之处,在图式及实施方式中使用相同标号的组件/构件代表相同或类似 部分。
图1是本发明的一实施例示出的利用电子装置对目标物进行移动参数估 计的示意图。在本实施例中,电子装置100包括信号处理单元110_1~110_N(N 为正整数)、天线120_1~120_N以及角度估计单元130。其中,天线 120_1~120_N通过信号处理单元110_1~110_N连接至角度估计单元130。信 号处理单元110_1~110_N可以是通信信号处理芯片、处理器或计算机系统等 硬件组件,或是具备通信信号处理功能的软件组件(例如能对通信信号解调变 的应用程序),此外,信号处理单元110_1~110_N也可以是硬件及软件组件的 组合。天线120_1~120_N可用数组方式配置在电子装置100上,且天线 120_1~120_N两两之间的间距例如是间距d。
电子装置100可利用例如天线数组及无线通信信号处理技术来检测目标 物140的移动参数(例如目标物140与电子装置100之间的相对速度以及相对 距离等)。举例而言,电子装置100可通过传送天线(未示出)发送量测信号, 而当此量测信号接触例如目标物140的物体并产生反射之后,电子装置100 可利用天线120_1~120_N接收反射后的量测信号,并由信号处理单元 110_1~110_N其个别对应的天线接收的信号特性来获得关联于目标物140的 移动参数。
图2是本发明的一实施例示出的移动参数估计方法的流程图。图3是本 发明的一实施例示出的第一调频连续波(Frequency Modulation Continuous Wave,FMCW)信号及第二调频连续波信号之间相对关系的示意图。请同时 参照图1、图2及图3,即搭配图1的组件来说明将电子装置100用于估计例 如目标物140的移动参数的详细步骤。在步骤S210中,电子装置100利用例 如传送天线来发送第一FMCW信号(亦即用于量测电子装置100周遭是否有 其他物体的量测信号)。
在一实施例中,第一FMCW信号在调变时的频率变化可依据例如图3示 出的第一信号T1来进行。依据图3中的各个参数,第一信号T1可表示为:
其中,带宽B为第一FMCW信号的带宽,载波频率fc为第一FMCW 信号的载波频率,量测时间Tramp为第一FMCW信号由载波频率fc上升 至(fc+B)所需的时间。
在其他实施例中,带宽B可依据所要检测的量测距离(例如电子装置100 与目标物140的相对距离)分辨率来决定。举例而言,B=c/(2·ΔR),其中c 为光速,ΔR为量测距离分辨率。此外,量测时间Tramp可依据所要检测的量 测速度(例如电子装置100与目标物140的相对速度)分辨率来决定。举例而言, Tramp=c/(2fcΔv),其中Δv为量测速度分辨率。换言之,当所要求的相对 距离测量精准度越高时,带宽B将随之而变大;而当所要求的相对速度测量 精准度越高时,量测时间Tramp也将随之而变长。
接着,在步骤S220中,当第一FMCW信号接触例如目标物140的物体 并产生反射后,电子装置100可由天线120_1~120_N接收第二FMCW信号(亦 即反射后的第一FMCW信号)。由于第二FMCW信号可能以平面波的方式传 递,因此对于天线120_1~120_N而言,其个别接收的第二FMCW信号本质 上可能有所差别(例如相位或是功率的差别)。
以天线120_1为例,其接收的第二FMCW信号频率变化例如是图3中的 第三信号R1,其可依据图3中的各个参数而表示为:
其中,时间差τ为发送第一FMCW信号的时间点与接收第二FMCW信 号的时间点之间的时间差,拍频(beat frequency)fb为第一信号T1与第三信号 R1之间在同一时间点时的频率差(亦即fT1(t)-fR1(t))。此外,当电子装置100 与目标物140之间存在相对运动时,第一信号T1与第三信号R1之间可能具 有因多普勒效应(Doppler effect)而产生的多普勒偏移频率fd。
因此,在步骤S230中,拍频fb可依据数学式(1)及(2)而表示为:
其中,相对距离R为电子装置100与目标物140之间的相对距离,相对 速度v为电子装置100与目标物140之间的相对速度。
依据上述教示,本领域技术人员可依据第二信号T2及第四信号R2之间 的相对关系而对应将拍频fb’表示为:
在一实施例中,拍频fb及fb’可由信号处理单元1101将其对应的第一 FMCW信号及第二FMCW信号进行混频,再将此混频后信号经由例如低通 滤波器(未示出)和快速傅利叶转换单元(未示出)而分别求得,但本发明的可实 施方式不限于此。
因此,当拍频fb及fb’为已知时,信号处理单元110_1可依据数学式(3)及 (4)而估计电子装置100与目标物140之间的相对距离R及相对速度v。
然而,实际而言,当量测时间Tramp较长时,电子装置100与目标物140 之间的相对距离R可能产生变化。因此,为了使相对距离R及相对速度v的 估计能更符合电子装置100与目标物140之间的实际移动情形,在其他实施 例中,数学式(4)可修改为:
接着,在步骤S240中,信号处理单元110_1即可依据数学式(3)及(5)求 得电子装置100与目标物140之间相对距离R及相对速度v(亦即关联于目标 物140的移动参数)。
对于天线120_2~120_N而言,其个别接收的第二FMCW信号将与天线 120_1接收的第二FMCW信号有所差别(例如相位或功率的差别),但皆可依 据上述教示而各自求得关联于目标物140的移动参数。
在其他实施例中,当多个目标物皆反射第一FMCW信号,而造成天线 120_1~120_N个别皆收到多个第二FMCW信号时,信号处理单元 120_1~120_N仍可通过上述教示的方法而分别求得这些目标物对应的移动参 数。以信号处理单元110_1及天线120_1为例,当天线120_1接收对应于上 述多个目标物反射的多个第二FMCW信号时,信号处理单元可将第一FMCW 信号及其所接收的多个第二FMCW进行混频,再将此混频信号经由例如低通 滤波器及快速傅利叶转换单元的操作而找出各个目标物对应的拍频,进而进 行后续的移动参数估计操作。
图4是本发明的一实施例示出的角度估计方法的流程图。在本实施例中, 步骤S410~S430可参照图2实施例的内容,在此不再赘述。值得注意的是, 在估计关联于目标物140的移动参数(亦即电子装置100与目标物140之间 相对距离R及相对速度v)时,可利用数学式(3)及(5)来计算,也可利用数学 式(3)及(4)来计算。然而,本领域技术人员可知,利用数学式(3)及(5)来计算目 标物140移动参数的方式由于更能符合实际应用上的情况,因而能导致较高 的估计精准度。
请同时参照图1及图4,在步骤S430后,关联于目标物140的移动参数 (亦即电子装置100与目标物140之间的相对距离R及相对速度v)应可对应求 出。接着,在步骤S440中,信号处理单元110_1~110_N即可将天线 120_1~120_N个别收到的第二FMCW信号搭配个别的天线组态参数而获得对 应的量测值r1~rN,其中,量测值r1~rN可用向量形式表示为:
其中,α为天线120_1~120_N对应于目标物140接收的复振幅(complex amplitude),a(φ)为天线120_1~120_N的指向向量(steering vector),g(φ)为天 线120_1~120_N对应于角度(φ)的增益响应(gain response),s为将天线 120_1~120_N接收的第二FMCW信号进行解调变(demodulation)后的信号(其 为上述相对距离R及相对速度v的函数),n为复数高斯白噪声(complex white Gaussian noise)向量。
接着,在电子装置100的视场角(Field of View,FOV)中,电子装置100 的默认方向与目标物140之间(例如预设方向DI1与DI2之间)的真实角度θ可 依据最大似然估计器(maximum likelihood estimator)来估计。举例而言,真实 角度θ的估计角度可表示为:
其中,天线数量N为天线120_1~120_N的数量,间距d为天线 120_1~120_N两两之间的间距,λ为第一FMCW信号的波长,(·)H为共轭转 置运算符(Hermitian operator)。在一番运算后,数学式(7)可化简为一封闭形式 (closed-form)解,亦即:
其中,ARG(ri*·rj)为ri及rj进行共轭转置运算(Hermitian operation)之后 的主幅角。
在步骤S450中,角度估计单元130即可将量测值r1~rN代入数学式(8), 以获得电子装置100的默认方向DI1与目标物140之间的估计角度
在其他实施例中,由于ARG(ri*·rj)仅取主幅角,而当其出现同位角的情 形时,将导致错误的估计角度因此,为了使估计角度避免同位角效应导 致的错误估计,电子装置100的视场角θFOV需满足以下不等式:
一般而言,越大的视场角θFOV代表电子装置100具有较大的检测范围, 而为了使天线120_1~120_N具有足够的天线增益(antenna gain)以及较低的旁 瓣(sidelobe),需要较大的间距d方能达成。然而,由于视场角θFOV与间距d 之间存在权衡取舍(tradeoff)的问题,使得数学式(9)的条件不容易满足。举例 而言,当天线120_1~120_N被以较大的间距d配置时(亦即天线120_1~120_N 所占空间较大),将使得视场角θFOV对应的变小,因而导致电子装置100的检 测范围变小。因此,当数学式(9)不成立时,估计角度的估计可能因而产生 错误。
因此,为了找到出现同位角效应的ARG(ri*·rj),可将估计角度代入下 式:
其中,
为高斯整数运算符(Gaussian integer operator)。换言之,在决定任 意的视场角θFOV以及间距d之后,即可由数学式(11)求出参数p的数值范围。 之后,再将参数p及估计角度代回数学式(10),即可在视场角θFOV的范围中 找出没有发生同位角效应的角度θp。接着,数学式(7)即可修改为:
因此,在通过数学式(10)以及(11)找出没有发生同位角效应的角度θp之 后,即可依据数学式(11)求出正确的估计角度
以N=3为例,数学式(8)可化为
而由于ARG(ri*·rj)的运算可能出现同位角的问题,此时可通过数学式(10) 来在视场角θFOV的范围中找到没有出现同位角效应的角度θp,亦即
其中,
之后,再将由数学式(14)求得的角度θp代回数学式(12),即可对应求得估 计角度
图5是本发明的一实施例示出的判断方法的流程图。请同时参照图1及 图5,在本实施例中,步骤S510~S550可参照图2及图4实施例的内容,在 此不再赘述。此外,电子装置100可还包括一判断单元(未示出),用于判断估 计角度是否可靠。
举例而言,假若电子装置100在估计例如目标物140的移动参数时,电 子装置100的视场角θFOV内同时有另一目标物(未示出)以相对距离R及相对 速度v的方式移动,此将导致估计角度出现不可靠的情形。此时,在经由 步骤S550及S550获得例如量测值r1~rN以及估计角度之后,在步骤S560 中,可在所述判断单元中设计第一滤波器及第二滤波器,并依据其个别与量 测值r1~rN之间的关系来判断估计角度是否可靠。举例而言,第一滤波器的 零点可设计为位于视场角θFOV之外,而第二滤波器的零点可设计为约略位于 估计角度处。
在步骤S570中,判断单元可依据例如第一滤波器系数w1以及量测值 r1~rN来计算一第一运算值P1。而在步骤S580中,判断单元可依据例如第二 滤波器系数w2以及量测值r1~rN来计算一第二运算值P2。
在步骤S590中,判断单元可计算第一运算值P1与第二运算值P2之间的 比例值P,并据以判断估计角度的可靠度。
在一实施例中,比例值P例如可依据
求得,其中,||·||2为模数2(2-norm)运算符。此时,假若估计角度为可 靠时,第二运算值P2的值将会因第二滤波器已将量测值r1~rN的功率大部分 滤除而产生较小的值。因此,当比例值P大于一预设门限值时,判断单元即 可得知此时量测值r1~rN是由多个目标物(其与电子装置100之间的相对距离R 及相对速度v皆相同)所反射的信号组成,进而判断估计角度为不可靠。在 其他实施例中,所述预设门限值可在预定的假警报率(false alarm rate)条件下, 依据例如Bayesian定理推得,但本发明的可实施方式不限于此。
图6是本发明的一实施例示出的将电子装置用于估计车辆间相对移动参 数及角度的示意图。在本实施例中,电子装置100例如可安装于车辆610的 前端,但本发明的可实施方式不限于此。电子装置100可通过例如图2实施 例中的方法来同时测量例如车辆610与620之间的相对距离R12以及车辆610 与630之间的相对距离R13。此外,电子装置100也可通过例如图4实施例中 的方法来同时估计例如预设方向DI1’与车辆620和630之间的角度θ12和θ13。
综上所述,本发明实施例提供一种移动参数估计方法、角度估计方法及 判断方法。在所述移动参数估计方法中,通过第一数学式及第二数学式,可 求得较符合于目标物的实际移动情形的移动参数。在所述角度估计方法中, 可依据所推导的封闭形式解来找出电子装置与目标物之间的估计角度。此外, 由于所述封闭形式解具有低运算量及高精确度的特性,使得本发明实施例之 角度估计方法更适合应用在实际情形中。另一方面,在由角度估计方法得到 估计角度之后,可通过所述判断方法来判断此估计角度是否可靠。
最后应说明的是:以上各实施例仅用以说明本发明的技术方案,而非对 其限制;尽管参照前述各实施例对本发明进行了详细的说明,本领域的普通 技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改, 或者对其中部分或者全部技术特征进行等同替换;而这些修改或者替换,并 不使相应技术方案的本质脱离本发明各实施例技术方案的范围。
机译: 运动参数估计方法,角度估计方法和确定方法
机译: 运动参数估计方法,角度估计方法和确定方法
机译: 移动位置/姿势角度估计装置及移动位置/姿势角度估计方法