CN104147770A - 基于惯性传感器可穿戴式偏瘫康复设备及捷联姿态算法 - Google Patents

Abstract

一种基于惯性传感器可穿戴式偏瘫康复设备及捷联姿态算法，主要由上位机和惯性测量系统两部分组成，布置于患者不同部位的惯性测量单元通过惯性传感器采集患者运动时的姿态角度信息，通过无线传输模块将姿态角度信息传输到上位机中，上位机根据不同位置的姿态角度信息对患者的恢复训练过程进行实时的跟踪。本发明另提出了基于平面约束的捷联算法，根据动作运动过程中上一时刻前臂和后臂的运动姿态，构建约束平面，对加速度信息进行约束，从而实现系统的自约束测量，增强角度测量的稳定性和准确性。本发明能够根据偏瘫患者的病情，提供准确的恢复运动姿势及运动量，记录病人的每组动作完成程度和完成次数，还具有对偏瘫患者运动功能障碍及治疗效果进行简易评定的功能。

Description

基于惯性传感器可穿戴式偏瘫康复设备及捷联姿态算法

技术领域

本发明涉及一种康复设备，尤其是一种利用惯性传感器检测信息并用于偏瘫患者的康复设备。 

背景技术

偏瘫是一种常见的脑卒中后遗症，偏瘫患者一侧肢体运动功能丧失，轻则行动不便，重则生活不能自理。众所周知，有效的康复训练不仅能够维持关节活动度、防止关节挛缩，而且能够明显提高患者运动功能，最终恢复到正常人的运动能力。 

目前，传统的康复训练主要是以治疗医师对患者进行一对一的训练为主，这种方式耗费医务工作者大量的时间，效率低下，而且不能精确的控制和记录训练的参数；随着科技的发展，偏瘫恢复机器人的应用得到了广泛的推广，这种方法提高了效率，节约了人力，并能行之有效的完成偏瘫患者的训练项目。但以上两种方法仅仅关注医师或机器人的协助训练过程，却忽略了在协助训练之余偏瘫患者的自主训练环节。偏瘫患者在患有肢体运动障碍的同时，常常伴有轻微的感觉、语言、认知等方面的障碍。所以偏瘫患者在自主恢复训练过程中，由于缺乏准确的运动姿势指导及运动评价，往往难以达到预期的动作标准，从而影响恢复的效果和进度。 

发明内容

本发明目的在于提供一种融合传感器信号及人体生物力学约束、提供准确 恢复运动姿势及运动量、记录每组动作完成程度和完成次数、并且可以穿戴使用的基于惯性传感器可穿戴式偏瘫康复设备及捷联姿态算法。 

为实现上述目的，采用了以下技术方案：本发明所述康复设备由上位机系统和惯性测量系统构成； 

所述上位机系统由控制器主机、显示器、打印机、微软WPF图形系统和无线通讯模块组成；上位机系统采用微软的WPF图形系统开发设计上位机用户界面，控制器主机对显示器和打印机进行控制；上位机系统通过无线通讯模块与惯性测量系统进行对接，完成对偏瘫患者康复训练过程的实时监控，并对病人的每组动作完整程度和完成次数进行记录；上位机系统根据病人康复动作的完成度，对偏瘫患者运动功能障碍及治疗效果进行简易评定；根据偏瘫患者的病情，提供准确的恢复运动姿势演示及恢复作业运动量。 

所述惯性测量系统由无线通信模块、一个中心惯性测量单元和多个普通惯性测量单元组成；其中，惯性测量单元以MSP430系列单片机作为核心控制芯片，每个惯性测量单元由一个9自由度的测量传感器和一个无线通信模块组成，所述9自由度的测量传感器分别为三轴加速度计、三轴陀螺仪和三周磁力计，分别用以测量物体的运动加速度、角速度和物体周围磁场强度；物体运动加速度、角速度和磁场强度的测量信息经卡尔曼滤波后转化成姿态角度信息，所述卡尔曼滤波过程是在MSP430系列单片机中以硬件描述语言的形式实现的。 

所述无线通信模块采用NRF24L01无线通信模块，用于实现普通惯性测量单元与中心惯性测量单元之间、中心惯性测量单元与上位机系统之间的通信。 

普通惯性测量单元是以分布方式安装在患者各患病上下肢的位置上(上臂、下臂、大腿、小腿等)，采集患者各患病肢体的位置和姿态信息，将数据通过无线通信模块传输给中心惯性测量单元；中心惯性测量单元安装在患者的躯干部位，采集躯干部位的位置和姿态信息；普通惯性测量单元与中心惯性测量单元之间通过无线通信模块进行数据传输，中心惯性测量单元融合所有普通惯性测量单元的数据，统一坐标数据；中心惯性测量单元与上位机系统之间也通过无线通信模块进行数据对接，将姿态信息统一传入上位机系统进行处理。 

本发明另提供了一种应用上述康复设备的基于平面约束的捷联姿态算法，所述算法是根据患者上下肢动作在运动过程中上一时刻的运动姿态，构建约束平面，对加速度信息进行约束；具体算法步骤如下， 

a、根据三轴陀螺仪更新的角速度信息，更新惯性测量单元的姿态，然后估计t时刻欧拉角度信息，将估计的欧拉角度信息作为卡尔曼滤波器的状态向量； 

b、由t-1时刻的前臂和后臂的惯性测量单元姿态角度信息构建约束平面，为了防止传感器噪声引起的漂移，通过平面约束加速度信号，然后根据约束后的加速度信息以及三轴磁力计输出的磁场强度信息，解算姿态角度信息，作为卡尔曼滤波器的观测向量； 

c、设定一个参考面用以判断约束平面存在的有效性，即当约束平面与规定的参考面的夹角γ小于规定阈值m的时候，约束平面有效，可对加速度信号进行约束，则经过约束后的加速度信息为： 

${\stackrel{\→}{A}}^{\′}=\left\{\begin{array}{cc}\stackrel{\→}{A},& \mathrm{\γ}>m\\ \stackrel{\→}{A}-(\stackrel{\→}{A}\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|})\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|},& \mathrm{\γ}\≤m\end{array}\right.$

式中，A′和A分别为加速度在平面约束修正前后的值；γ为约束平面与规定参考面的夹角；m为规定阈值；X_f、X_p分别为地理坐标系中前臂、后臂的方向向量；为状态转移矩阵； 

$\mathrm{\γ}=\arccos \left(\frac{{\stackrel{\→}{n}}_{x-y}\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|}}{\left|{\stackrel{\→}{n}}_{x-y}\right|\·\left|\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|}\right|}\right)$

式中，X_f、X_p分别为地理坐标系中前臂、后臂的方向向量，为状态转移矩阵，为参考平面法向量。 

本发明设备的工作过程大致如下： 

使用时，根据病人的不同组织结构及骨骼特点，将多个惯性测量单元分别布置与人体的不同部位(例如：上臂、下臂、躯干、大腿、小腿等)，通过惯性测量单元对人体不同部位的磁场强度信息、加速度信息、角速度信息等进行采集；所采集到的信息经过数据处理后通过无线传输模块传给上位机系统。上位机根据相关的角度、位置信息对偏瘫患者的动作进行实时的监测、数据记录以及评定等。同时，上位机还可根据偏瘫患者的不同病情，提供不同的训练姿势及训练量，为病人提供最合理的康复运动训练内容。 

与现有技术相比，本发明具有如下优点：结构合理、安装简便、信号传输准确，利用惯性传感器进行动作信息采集，经上位机处理后得出信息评定；能 根据偏瘫患者的病情提供准确的恢复运动姿势及运动量，记录病人的动作完成度和完成次数，提高患者的康复训练效率，并能对患者的运动功能障碍和治疗效果进行简易评定，提高康复治疗效果。 

附图说明

图1是本发明的总体结构示意图。 

图2是本发明在WPF图形系统中制作的上位机控制系统功能图。 

图3是本发明中惯性测量单元的系统结构图。 

图4是本发明所建立的手臂屈伸动作模型(a)和摆臂动作模型(b)。 

图5是本发明所提出的基于平面约束的捷联算法的原理图。 

图6是本发明实际运行的效果展示组图。 

图7是本发明手臂屈伸动作(a)和抬臂动作(b)的姿态评价角度仿真图。 

具体实施方式

下面结合附图对本发明做进一步说明： 

如图1所示，本发明所述康复设备由上位机系统和惯性测量系统构成； 

所述上位机系统由控制器主机、显示器、打印机、微软WPF图形系统和无线通讯模块组成；上位机系统采用微软的WPF图形系统开发设计上位机用户界面，控制器主机对显示器和打印机进行控制；所述的上位机根据病人康复动作的完成度，对偏瘫患者运动功能障碍及治疗效果进行简易评定；所述的上位机根据偏瘫患者的病情，提供准确的恢复运动姿势演示及恢复作业运动量。 

所述惯性测量系统由无线通信模块、一个中心惯性测量单元和多个普通惯 性测量单元组成；其中，惯性测量单元以MSP430系列单片机作为核心控制芯片，每个惯性测量单元由一个9自由度的测量传感器和一个无线通信模块组成，所述9自由度的测量传感器分别为三轴加速度计、三轴陀螺仪和三周磁力计，分别用以测量物体的运动加速度、角速度和物体周围磁场强度；物体运动加速度、角速度和磁场强度的测量信息经卡尔曼滤波后转化成姿态角度信息，所述卡尔曼滤波过程是在MSP430系列单片机中以硬件描述语言的形式实现的。 

所述无线通信模块采用NRF24L01无线通信模块，用于实现普通惯性测量单元与中心惯性测量单元之间、中心惯性测量单元与上位机系统之间的通信。 

普通惯性测量单元是以分布方式安装在患者各患病上下肢的位置上(上臂、下臂、大腿、小腿等)，采集患者各患病肢体的位置和姿态信息，将数据通过无线通信模块传输给中心惯性测量单元；中心惯性测量单元安装在患者的躯干部位，采集躯干部位的位置和姿态信息；普通惯性测量单元与中心惯性测量单元之间通过无线通信模块进行数据传输，中心惯性测量单元融合所有普通惯性测量单元的数据，统一坐标数据；中心惯性测量单元与上位机系统之间也通过无线通信模块进行数据对接，将姿态信息统一传入上位机系统进行处理。 

如图1所示的本发明总体结构示意图，上位机系统采用微软的WPF图形系统开发设计上位机用户界面，通过工控机的显示屏显示，界面简洁美观，并与打印机连接实现报表、画面的打印。上位机通过NRF24L01无线通讯模块与惯性测量系统进行对接，从而进行相互通信，实现对患者运动过程中的运动姿态的实时捕捉。下位机由多个惯性测量单元组成，每个惯性测量单元均由三轴加 速度计、三轴磁力计、三轴陀螺仪和无线通信模块组成，其中普通惯性测量单元布置在人体的四肢部位，只负责采集相应身体部位的姿态信息；中心惯性测量单元布置在人体的胸部位置，在采集人体胸部位置姿态信息的同时，通过无线模块与普通惯性测量单元进行通信，并与普通惯性测量单元的姿态角度信息进行融合，统一发送至上位机。 

如图2所示的在WPF图形系统中制作的上位机控制系统功能图，上位机系统通过工控机与惯性测量系统终端连接，通过采集到的人体不同部位的姿态信息，对患者进行恢复训练时所做的动作进行实时跟踪。整个上位机界面中包括用户登录、恢复训练姿势演示、患者恢复训练跟踪、偏瘫运动功能评定以及患者档案等多个功能。其中用户登录部分，用户可根据自己的需求注册用户以及使用权限，不同的用户权限可使用不同的系统功能；偏瘫恢复训练姿势演示部分可根据患者的病情程度，以视频演示的形式向患者演示标准的训练姿势；患者恢复训练实时跟踪部分即通过与惯性测量系统的对接，可对患者进行恢复训练时所做的动作进行实时跟踪；用户档案部分可对用户的病例信息进行存储，方便用户查询。整个上位系统界面简洁、交互性强、操作简便，适用于不同年龄段的偏瘫患者。 

图3是本发明中惯性测量单元的系统结构图，惯性测量单元以MSP430系列单片机作为核心控制芯片，主要包含惯性传感器(三轴加速度计、三轴陀螺仪和三轴磁力计)和无线通信模块两个部分。其中惯性传感器用来采集患者运动的加速度信息、磁场强度信息和角速度信息，采集到的信息通过IIC总线传给 MSP430并进行数据处理；无线通信模块选用NRF24L01射频芯片，该模块工作于2.4GHz公共ISM频段，通过SPI总线与MSP430通讯。 

图4是本发明所建立的手臂屈伸动作模型(a)和摆臂动作模型(b)。手臂屈伸动作和摆臂动作均为偏瘫患者上肢运动恢复训练过程中的两组常见的运动恢复动作，为了评价两组动作的运动完整度，我们分别定义了3个相关角度信息，动作模型及角度标注如附图4所示，其中θ_elbow为肘关节处夹角；θ_f为前臂(forearm)与参考轴之间的夹角，用于观测前臂姿态完成度；θ_p为后臂(postbrachium)与参考轴之间的夹角，用于观测后臂姿态完成度。选取中心惯性测量单元所处的坐标为中心坐标系，将手臂屈伸动作整个动作过程约束至中心坐标系Y-Z平面，以中心坐标系的Z轴作为参考轴；摆臂动作约束在中心坐标系的X-Y平面，以中心坐标系的X轴作为参考轴。 

则手臂屈伸动作时， 

${\mathrm{\θ}}_{\mathrm{elbow}}=\arccos \left(\frac{X_{c,f}\·X_{c,p}}{\left|X_{c,f}\right|\·\left|X_{c,p}\right|}\right),{\mathrm{\θ}}_{f-z}=\arctan \left(\right|\frac{X_{f,y}}{X_{f,z}}\left|\right),{\mathrm{\θ}}_{p-z}=\arctan \left(\right|\frac{X_{p,y}}{X_{p,z}}\left|\right);$

抬臂动作时， 

${\mathrm{\θ}}_{\mathrm{elbow}}=\arccos \left(\frac{X_{c,f}\·X_{c,p}}{\left|X_{c,f}\right|\·\left|X_{c,p}\right|}\right),{\mathrm{\θ}}_{f-x}=\arctan \left(\right|\frac{X_{f,y}}{X_{f,x}}\left|\right),{\mathrm{\θ}}_{p-x}=\arctan \left(\right|\frac{X_{p,y}}{X_{p,x}}\left|\right).$

其中X_c,f、X_c,p分别为在中心坐标系中手臂(前臂、后臂)的方向向量。 

图5是本发明所提出的基于平面约束的捷联算法的原理图。针对手臂和腿部的偏瘫恢复动作中约束在平面内进行的偏瘫恢复动作，本发明提出一种基于面约束的捷联姿态算法，实现系统的自约束测量，增强角度测量的稳定性和准确性。 

具体算法步骤如下， 

a、根据三轴陀螺仪更新的角速度信息，更新惯性测量单元的姿态，然后估计t时刻欧拉角度信息，将估计的欧拉角度信息作为卡尔曼滤波器的状态向量； 

b、由t-1时刻的前臂和后臂的惯性测量单元姿态角度信息构建约束平面，为了防止传感器噪声引起的漂移，通过平面约束加速度信号，然后根据约束后的加速度信息以及三轴磁力计输出的磁场强度信息，解算姿态角度信息，作为卡尔曼滤波器的观测向量； 

c、设定一个参考面用以判断约束平面存在的有效性，即当约束平面与规定的参考面的夹角γ小于规定阈值m的时候，约束平面有效，可对加速度信号进行约束，则经过约束后的加速度信息为： 

${\stackrel{\→}{A}}^{\′}=\left\{\begin{array}{cc}\stackrel{\→}{A},& \mathrm{\γ}>m\\ \stackrel{\→}{A}-(\stackrel{\→}{A}\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|})\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|},& \mathrm{\γ}\≤m\end{array}\right.$

式中，A′和A分别为加速度在平面约束修正前后的值，γ为约束平面与规定参考面的夹角；m为规定阈值；X_f、X_p分别为地理坐标系中前臂、后臂的方向向量；为状态转移矩阵； 

$\mathrm{\γ}=\arccos \left(\frac{{\stackrel{\→}{n}}_{x-y}\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|}}{\left|{\stackrel{\→}{n}}_{x-y}\right|\·\left|\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|}\right|}\right)$

式中，X_f、X_p分别为地理坐标系中前臂、后臂的方向向量，为状态转移矩阵，为参考平面法向量。 

图6是本发明实际运行的效果展示组图。将惯性测量单元分别安装于人体 手臂部位和胸部部位。惯性测量单元用于采集人体手臂部位和胸部部位的动作姿势信息，上位机再通过无线通信模块接收惯性测量单元的采集信号，将人体的动作实时的展示在上位机界面中。由图6中可以看出界面中的人物与使用者做相同的姿态动作，可以直观的展示使用者的动作是否完整和规范。 

图7是本发明手臂屈伸动作(a)和抬臂动作(b)姿态评价角度仿真图。如图所示，试验者保持身体的不动，右臂以较慢的速度做手臂屈伸运动和抬臂运动各十次，以25Hz的速率采集信号。保证每次手臂屈伸动作和抬臂动作相对完整。其中θ_elbow为肘关节处夹角；θ_f为前臂(forearm)与参考轴之间的夹角，用于观测前臂姿态完成度；θ_p为后臂(postbrachium)与参考轴之间的夹角，用于观测后臂姿态完成度。 

图7(a)为手臂屈伸动作的姿态评价角度仿真图。对于手臂屈伸动作，我们取手臂伸展时和手臂完全曲臂时的三个角度，分别计算出其平均值和标准差。取一个标准差来规范手臂屈伸动作的完整度，并对手臂屈伸动作的完整度评价作出如下定义：在单次手臂屈伸动作过程中， 

1)手臂伸展时，评价角度θ_f-low∈[0，19.43]、θ_p-low∈[27.19，56.40]、θ_elbow∈[113.22，135.45]； 

2)当手臂完全曲臂时，评价角度θ_f-peak∈[53.52，75.29]、θ_p-peak∈[41.72，67.49]、θ_elbow∈[47.64，65.63]，则此次手臂屈伸动作为完整的。 

图7(b)为抬臂动作的姿态评价角度仿真图。对于手臂屈伸动作，我们取水平伸展手臂时和手臂向内侧收敛时的三个角度，分别计算出其平均值和标准差。我们取一个标准差来规范抬臂动作的完整度，并对抬臂动作的完整度评价作出如下定义：在单次抬臂动作过程中， 

1)水平伸展手臂时，评价角度θ_f∈[81.155，90]、θ_p∈[80.885，90]、θ_elbow∈[157.01，179.64]； 

2)当手臂向身体内侧收敛时，评价角度θ_f∈[38.281，46.477]、θ_p∈[23.54，35.03]、θ_elbow∈[151.38，167.36]，则此次抬臂动作为完整的。 

以上所述的实施例仅仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。 

Claims (3)

1.一种基于惯性传感器可穿戴式偏瘫康复设备，其特征在于：所述康复设备由上位机系统和惯性测量系统构成；

所述上位机系统由控制器主机、显示器、打印机、微软WPF图形系统和无线通讯模块组成；上位机系统采用微软的WPF图形系统开发设计上位机用户界面，控制器主机对显示器和打印机进行控制；

所述惯性测量系统由无线通信模块和惯性测量单元组成；惯性测量单元以MSP430系列单片机作为核心控制芯片，每个惯性测量单元由一个9自由度的测量传感器和一个无线通信模块组成，所述9自由度的测量传感器分别为三轴加速度计、三轴陀螺仪和三周磁力计，分别用以测量物体的运动加速度、角速度和物体周围磁场强度；物体运动加速度、角速度和磁场强度的测量信息经卡尔曼滤波后转化成姿态角度信息，所述卡尔曼滤波过程是在MSP430系列单片机中以硬件描述语言的形式实现的；

所述惯性测量单元分为中心惯性测量单元和至少两个普通惯性测量单元；普通惯性测量单元是以分布方式安装在患者各患病上下肢的位置上，采集患者各患病肢体的位置和姿态信息；中心惯性测量单元安装在患者的躯干部位，采集躯干部位的位置和姿态信息；普通惯性测量单元与中心惯性测量单元之间通过无线通信模块进行数据传输，中心惯性测量单元融合所有普通惯性测量单元的数据，统一坐标数据；中心惯性测量单元与上位机系统之间也通过无线通信模块进行数据对接。

2.根据权利要求1所述的基于惯性传感器可穿戴式偏瘫康复设备，其特征在于：所述无线通信模块采用NRF24L01无线通信模块。

3.一种应用于权利要求1的基于平面约束的捷联姿态算法，其特征在于：所述算法是根据患者上下肢动作在运动过程中上一时刻的运动姿态，构建约束平面，对加速度信息进行约束；具体算法步骤如下，

a、根据三轴陀螺仪更新的角速度信息，更新惯性测量单元的姿态，然后估计t时刻欧拉角度信息，将估计的欧拉角度信息作为卡尔曼滤波器的状态向量；

b、由t-1时刻的前臂和后臂的惯性测量单元姿态角度信息构建约束平面，为了防止传感器噪声引起的漂移，通过平面约束加速度信号，然后根据约束后的加速度信息以及三轴磁力计输出的磁场强度信息，解算姿态角度信息，作为卡尔曼滤波器的观测向量；

c、设定一个参考面用以判断约束平面存在的有效性，即当约束平面与规定的参考面的夹角γ小于规定阈值m的时候，约束平面有效，可对加速度信号进行约束，则经过约束后的加速度信息为：

${\stackrel{\→}{A}}^{\′}=\left\{\begin{array}{cc}\stackrel{\→}{A},& \mathrm{\γ}>m\\ \stackrel{\→}{A}-(\stackrel{\→}{A}\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|})\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|},& \mathrm{\γ}\≤m\end{array}\right.$

式中，A′和A分别为加速度在平面约束修正前后的值；γ为约束平面与规定参考面的夹角；m为规定阈值；X_f、X_p分别为地理坐标系中前臂、后臂的方向向量；为状态转移矩阵；

$\mathrm{\γ}=\arccos \left(\frac{{\stackrel{\→}{n}}_{x-y}\·\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|}}{\left|{\stackrel{\→}{n}}_{x-y}\right|\·\left|\frac{R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)}{|R_n^b\·({\stackrel{\→}{X}}_f\×{\stackrel{\→}{X}}_p)|}\right|}\right)$

式中，X_f、X_p分别为地理坐标系中前臂、后臂的方向向量，为状态转移矩阵，为参考平面法向量。