CN109945889B - 一种基于双姿态传感器的关节角度测量方法 - Google Patents
一种基于双姿态传感器的关节角度测量方法 Download PDFInfo
- Publication number
- CN109945889B CN109945889B CN201910309605.1A CN201910309605A CN109945889B CN 109945889 B CN109945889 B CN 109945889B CN 201910309605 A CN201910309605 A CN 201910309605A CN 109945889 B CN109945889 B CN 109945889B
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- angle
- body section
- joint
- attitude sensor
- 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.)
- Active
Links
Classifications
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Landscapes
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
本发明公开了一种基于双姿态传感器的关节角度测量方法,在体段一上固定安装第一姿态传感器,在体段二上固定安装第二姿态传感器,测量时,所述第一姿态传感器返回第一欧拉角,所述第二姿态传感器返回第二欧拉角,然后根据所述第一欧拉角和所述第二欧拉角解算关节角度。本发明能够直接输出某一位置的关节绝对角度,不需要标定,测量精度高,而且姿态传感器的安装不受限制,可以在体段的任意周向、轴向位置固定,安装简单,便于实施,体积重量可以忽略不计,而且本发明实现还可以应用于机器人关节角度测量中,具有非常好的应用前景。
Description
技术领域
本发明涉及角度测量领域,具体地说涉及一种基于双姿态传感器的关节角度测量方法。
背景技术
人体关节角度检测在医疗康复领域非常有应用价值。对关节极限角度进行检测,可以为医生的临床诊断,病人的康复效果评估提供重要参数;获取关节运动信息,还可以为老年人、康复期患者的跌倒风险评估、步态校正等提供重要依据。
人体关节角度通常使用穿戴式角度传感器测量装置直接测量,包括霍尔磁性角度传感器、光学编码盘等,在实际运用中,患者常常由于肢体障碍或者碍于受伤部位,穿戴很难实现,且价格通常比较昂贵。现有技术中也有一些测量方法可以实现对关节相对运动角度的测量,如申请号为201710170099.3的中国专利申请公开的一种基于姿态传感器的机器人关节角度测量方法,但是,该方法输出的是两个姿态的关节相对运动角度,且因为需要在某个位置标定初始态,精度很难达到要求。
发明内容
本发明所要解决的技术问题是提供一种测量精度高的、便于安装实施的关节角度测量方法。
为了解决上述技术问题,本发明采用如下技术方案:一种基于双姿态传感器的关节角度测量方法,在体段一上固定安装第一姿态传感器,在体段二上固定安装第二姿态传感器,测量时,所述第一姿态传感器返回第一欧拉角,所述第二姿态传感器返回第二欧拉角,然后根据所述第一欧拉角和所述第二欧拉角解算关节角度。
进一步地,包括以下步骤:
步骤一:定义基准坐标系S;定义连杆坐标系U;定义连杆坐标系V;定义体段一长度l1,体段二长度l2;
所述基准坐标系S为东北天坐标系,所述基准坐标系S以体段一自由端端点A为原点,以向东向北向上为坐标轴XYZ的正方向;所述连杆坐标系U以体段一自由端端点A为原点,所述连杆坐标系U的坐标轴指向与所述第一姿态传感器方位相同;所述连杆坐标系V以关节回转中心B为原点,所述连杆坐标系V的坐标轴指向与所述第二姿态传感器方位相同;
步骤二:根据所述第一姿态传感器返回的第一欧拉角计算所述连杆坐标系U与所述基准坐标系S的第一旋转变换矩阵根据所述第二姿态传感器返回的第二欧拉角计算所述连杆坐标系V与所述基准坐标系S的第二旋转变换矩阵/>
步骤三:根据所述第一旋转变换矩阵体段一长度l1,计算关节回转中心B在基准坐标系S下的坐标/>根据所述第二旋转变换矩阵/>体段二长度l2,计算体段二自由端端点C在基准坐标系S下的坐标/>
步骤四:根据体段一自由端端点A、关节回转中心B、体段二自由端端点C三点的空间坐标求取关节回转中心B点与体段一自由端端点A点构成的向量关节回转中心B点与体段二自由端端点C点构成的向量/>最后根据/>计算关节角度。
进一步地,旋转变换矩阵的通式如式1所示:
所述第一欧拉角包括第一滚转角α1、第一俯仰角β1、第一偏航角γ1;所述第二欧拉角包括第二滚转角α2、第二俯仰角β2、第二偏航角γ2,将α1、β1、γ1和α2、β2、γ2分别代入上式计算出
进一步地:
关节回转中心B点在连杆坐标系U下的坐标得到关节回转中心B点在基准坐标系S下的坐标/>
体段二自由端端点C点在连杆坐标系V下的坐标得到体段二自由端端点C点在基准坐标系S下的坐标/>
进一步地:
关节角度的计算公式如式2所示:
进一步地,所述第一姿态传感器和所述第二姿态传感器分别使用柔性绑带绑定在体段一和体段二上,所述第一姿态传感器和所述第二姿态传感器的x轴或y轴或z轴沿着体段一和体段二的长度方向安装。
进一步地,姿态传感器为双轴加速度传感器和单轴陀螺仪的组合,或由陀螺仪与加速度传感器组成的六轴惯性传感器,或九轴姿态传感器。
本发明的有益效果体现在:
本发明能够直接输出某一位置的关节绝对角度,不需要标定,测量精度高,而且姿态传感器的安装不受限制,可以在体段的任意周向、轴向位置固定,安装简单,便于实施,体积重量可以忽略不计,而且本发明实现还可以应用于机器人关节角度测量中,具有非常好的应用前景。
附图说明
图1是本发明一实施例中的关节角度测量示意图。
图2是本发明一实施例中的z-y-x旋转顺序欧拉角示意图。
具体实施方式
下面将参考附图来详细说明本发明。需要说明的是,在不冲突的情况下,本申请中的实施例及实施例中的特征可以相互组合。
为解决现有技术的缺陷,本发明提供一种基于双姿态传感器的关节角度测量方法,是通过在体段一上固定安装第一姿态传感器,在体段二上固定安装第二姿态传感器,测量时,所述第一姿态传感器返回第一欧拉角,所述第二姿态传感器返回第二欧拉角,然后根据所述第一欧拉角和所述第二欧拉角解算关节角度。
优选地,所述第一姿态传感器和所述第二姿态传感器分别使用柔性绑带绑定在体段一和体段二上,所述第一姿态传感器和所述第二姿态传感器的x轴或y轴或z轴沿着体段一和体段二的长度方向安装。
本发明使用的姿态传感器可以为双轴加速度传感器和单轴陀螺仪组合,或陀螺仪与加速度传感器组成的六轴惯性传感器,也不限于九轴姿态传感器,只要能够准确测量包括滚转角、俯仰角、偏航角的欧拉角即可。
参见图1和图2,下面提供一种根据第一欧拉角和第二欧拉角解算关节角度的方法,具体包括以下步骤:
步骤一:定义基准坐标系S;定义连杆坐标系U;定义连杆坐标系V;定义体段一长度l1,体段二长度l2;
所述基准坐标系S为东北天坐标系,所述基准坐标系S以体段一自由端端点A为原点,以向东向北向上为坐标轴XYZ的正方向;所述连杆坐标系U以体段一自由端端点A为原点,所述连杆坐标系U的坐标轴指向与所述第一姿态传感器方位相同(即连杆坐标系的x、y、z轴指向与姿态传感器的x、y、z轴指向相同,下同);所述连杆坐标系V以关节回转中心B为原点,所述连杆坐标系V的坐标轴指向与所述第二姿态传感器方位相同;
本发明中,所述体段长度l1和l2仅仅为了便于对体段的方向向量进行描述和理解,实际并不需要对其长度进行测量,即使取体段一或体段二的二分之一或者三份之一作为A点和C点均可;
步骤二:根据所述第一姿态传感器返回的第一欧拉角计算所述连杆坐标系U与所述基准坐标系S的第一旋转变换矩阵根据所述第二姿态传感器返回的第二欧拉角计算所述连杆坐标系V与所述基准坐标系S的第二旋转变换矩阵/>
步骤三:根据所述第一旋转变换矩阵体段一长度l1,计算关节回转中心B在基准坐标系S下的坐标/>根据所述第二旋转变换矩阵/>体段二长度l2,计算体段二自由端端点C在基准坐标系S下的坐标/>
步骤四:根据体段一自由端端点A、关节回转中心B、体段二自由端端点C三点的空间坐标求取关节回转中心B点与体段一自由端端点A点构成的向量关节回转中心B点与体段二自由端端点C点构成的向量/>最后根据/>计算关节角度。
在一实施例中,所述第一姿态传感器和所述第二姿态传感器在用欧拉角表示姿态时的坐标系旋转顺序定义为z-y-x,若存在某坐标系N与参考系M初始方位相同,N绕zN轴转γ,然后绕yN轴转β角,最后绕xN转α角,如图2所示,N的三维坐标轴依次发生变换:xyz→x’y’z→x”y’z’→x”y”z”,则N相对于M的旋转变换矩阵的通式如式1所示:
α、β、γ分别为欧拉角的滚转角、俯仰角、偏航角,所述第一欧拉角包括第一滚转角α1、第一俯仰角β1、第一偏航角γ1;所述第二欧拉角包括第二滚转角α2、第二俯仰角β2、第二偏航角γ2,将α1、β1、γ1和α2、β2、γ2分别代入上式计算出
在一实施例中,关节回转中心B点在连杆坐标系U下的坐标得到关节回转中心B点在基准坐标系S下的坐标/>
体段二自由端端点C点在连杆坐标系V下的坐标得到体段二自由端端点C点在基准坐标系S下的坐标/>
在一实施例中,
关节角度的计算公式如式2所示:
应当理解本文所述的例子和实施方式仅为了说明,并不用于限制本发明,本领域技术人员可根据它做出各种修改或变化,凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (3)
1.一种基于双姿态传感器的关节角度测量方法,关节包括相互铰接的体段一和体段二,其特征在于:在体段一上固定安装第一姿态传感器,在体段二上固定安装第二姿态传感器,测量时,所述第一姿态传感器返回第一欧拉角,所述第二姿态传感器返回第二欧拉角,然后根据所述第一欧拉角和所述第二欧拉角解算关节角度;包括以下步骤:
步骤一:定义基准坐标系S;定义连杆坐标系U;定义连杆坐标系V;定义体段一长度l1,体段二长度l2;
所述基准坐标系S为东北天坐标系,所述基准坐标系S以体段一自由端端点A为原点,以向东向北向上为坐标轴XYZ的正方向;所述连杆坐标系U以体段一自由端端点A为原点,所述连杆坐标系U的坐标轴指向与所述第一姿态传感器方位相同;所述连杆坐标系V以关节回转中心B为原点,所述连杆坐标系V的坐标轴指向与所述第二姿态传感器方位相同;
步骤二:根据所述第一姿态传感器返回的第一欧拉角计算所述连杆坐标系U与所述基准坐标系S的第一旋转变换矩阵U SR;根据所述第二姿态传感器返回的第二欧拉角计算所述连杆坐标系V与所述基准坐标系S的第二旋转变换矩阵S VR;
旋转变换矩阵的通式如式1所示:
所述第一欧拉角包括第一滚转角α1、第一俯仰角β1、第一偏航角γ1;所述第二欧拉角包括第二滚转角α2、第二俯仰角β2、第二偏航角γ2,将α1、β1、γ1和α2、β2、γ2分别代入上式计算出
步骤三:根据所述第一旋转变换矩阵体段一长度l1,计算关节回转中心B在基准坐标系S下的坐标/>根据所述第二旋转变换矩阵/>体段二长度l2,计算体段二自由端端点C在基准坐标系S下的坐标/>
关节回转中心B点在连杆坐标系U下的坐标得到关节回转中心B点在基准坐标系S下的坐标/>
体段二自由端端点C点在连杆坐标系V下的坐标得到体段二自由端端点C点在基准坐标系S下的坐标/>
步骤四:根据体段一自由端端点A、关节回转中心B、体段二自由端端点C三点的空间坐标求取关节回转中心B点与体段一自由端端点A点构成的向量关节回转中心B点与体段二自由端端点C点构成的向量/>最后根据/>计算关节角度;
关节角度的计算公式如式2所示:
2.如权利要求1所述的基于双姿态传感器的关节角度测量方法,其特征在于:所述第一姿态传感器和所述第二姿态传感器分别使用柔性绑带绑定在体段一和体段二上,所述第一姿态传感器和所述第二姿态传感器的x轴或y轴或z轴沿着体段一和体段二的长度方向安装。
3.如权利要求1或2所述的基于双姿态传感器的关节角度测量方法,其特征在于:姿态传感器为双轴加速度传感器和单轴陀螺仪的组合,或由陀螺仪与加速度传感器组成的六轴惯性传感器,或九轴姿态传感器。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910309605.1A CN109945889B (zh) | 2019-04-17 | 2019-04-17 | 一种基于双姿态传感器的关节角度测量方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910309605.1A CN109945889B (zh) | 2019-04-17 | 2019-04-17 | 一种基于双姿态传感器的关节角度测量方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109945889A CN109945889A (zh) | 2019-06-28 |
CN109945889B true CN109945889B (zh) | 2023-07-25 |
Family
ID=67014306
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910309605.1A Active CN109945889B (zh) | 2019-04-17 | 2019-04-17 | 一种基于双姿态传感器的关节角度测量方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109945889B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111421551A (zh) * | 2020-05-08 | 2020-07-17 | 哈尔滨理工大学 | 一种机器人关节信息获取方法 |
CN114041782A (zh) * | 2021-07-26 | 2022-02-15 | 南宁师范大学 | 一种多通道人体下肢运动信息采集***及方法 |
CN113876317A (zh) * | 2021-11-09 | 2022-01-04 | 南宁师范大学 | 一种根据人体穴位定位的多通道人体下肢运动信息采集***及方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103884351A (zh) * | 2014-01-24 | 2014-06-25 | 中国北方车辆研究所 | 足式机器人运动参数测量方法及其应用 |
CN106996765A (zh) * | 2017-03-21 | 2017-08-01 | 上海岭先机器人科技股份有限公司 | 一种基于姿态传感器的机器人关节角度测量方法 |
CN108209931A (zh) * | 2017-11-06 | 2018-06-29 | 苏州脉吉医疗技术有限公司 | 关节夹角的测量***及方法 |
CN108318035A (zh) * | 2018-01-08 | 2018-07-24 | 山东大学 | 一种基于欧拉角的姿态测量算法中避免万向节死锁的方法 |
CN108458714A (zh) * | 2018-01-11 | 2018-08-28 | 山东大学 | 一种姿态检测***中不含重力加速度的欧拉角求解方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
FR2933185B1 (fr) * | 2008-06-27 | 2017-07-21 | Movea Sa | Systeme et procede de determination d'informations representatives du mouvement d'une chaine articulee |
TWI549655B (zh) * | 2012-05-18 | 2016-09-21 | 國立成功大學 | 關節活動度量測裝置及其量測方法 |
-
2019
- 2019-04-17 CN CN201910309605.1A patent/CN109945889B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103884351A (zh) * | 2014-01-24 | 2014-06-25 | 中国北方车辆研究所 | 足式机器人运动参数测量方法及其应用 |
CN106996765A (zh) * | 2017-03-21 | 2017-08-01 | 上海岭先机器人科技股份有限公司 | 一种基于姿态传感器的机器人关节角度测量方法 |
CN108209931A (zh) * | 2017-11-06 | 2018-06-29 | 苏州脉吉医疗技术有限公司 | 关节夹角的测量***及方法 |
CN108318035A (zh) * | 2018-01-08 | 2018-07-24 | 山东大学 | 一种基于欧拉角的姿态测量算法中避免万向节死锁的方法 |
CN108458714A (zh) * | 2018-01-11 | 2018-08-28 | 山东大学 | 一种姿态检测***中不含重力加速度的欧拉角求解方法 |
Non-Patent Citations (3)
Title |
---|
一种人体头部运动姿态的测量方法;贾培刚等;《西安工业大学学报》;20111025(第05期);全文 * |
可穿戴传感网络中姿态测量技术;张泽浩等;《仪器仪表用户》;20170108(第01期);全文 * |
基于惯性传感器的人体姿态角度测量方法研究进展;赵晓皓等;《医疗卫生装备》;20171015(第10期);全文 * |
Also Published As
Publication number | Publication date |
---|---|
CN109945889A (zh) | 2019-06-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109945889B (zh) | 一种基于双姿态传感器的关节角度测量方法 | |
Mayagoitia et al. | Accelerometer and rate gyroscope measurement of kinematics: an inexpensive alternative to optical motion analysis systems | |
Lee et al. | A real-time gyroscopic system for three-dimensional measurement of lumbar spine motion | |
JP2013500812A (ja) | 運動学的連結の慣性計測 | |
JP4897123B1 (ja) | 内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法 | |
JP4897122B2 (ja) | 内視鏡形状検出装置及び内視鏡の挿入部の形状検出方法 | |
US20130158946A1 (en) | Modelling of hand and arm position and orientation | |
Lee et al. | A novel flexible wearable sensor for estimating joint-angles | |
Kyrarini et al. | Comparison of vision-based and sensor-based systems for joint angle gait analysis | |
Ying et al. | Use of dual Euler angles to quantify the three-dimensional joint motion and its application to the ankle joint complex | |
Jang et al. | Towards finger motion capture system using FBG sensors | |
US5482056A (en) | Determination of thumb position using measurements of abduction and rotation | |
CN109730684B (zh) | 一种基于光纤光栅的肢体关节角度测量装置及方法 | |
Watanabe et al. | Comparison of angle measurements between integral-based and quaternion-based methods using inertial sensors for gait evaluation | |
JP3394979B2 (ja) | 関節角の計測方法及びその装置 | |
Zhang et al. | Snake robot shape sensing using micro-inertial sensors | |
Favre et al. | 3D joint rotation measurement using MEMs inertial sensors: Application to the knee joint | |
TW202024571A (zh) | 多重慣性感測器整合於多剛體連桿角度量測系統之校正方法 | |
JP2016206081A (ja) | 動作推測装置および動作推測方法 | |
Mangiapelo | Implementing an electrogoniometer using freescale’s low g accelerometers | |
Barraza-Madrigal et al. | Instantaneous position and orientation of the body segments as an arbitrary object in 3D space by merging gyroscope and accelerometer information | |
CN111772640B (zh) | 肢体运动训练指导方法、装置及存储介质 | |
CN112057083B (zh) | 可穿戴人体上肢位姿采集设备及采集方法 | |
Lee et al. | Non visual sensor based shape perception method for gait control of flexible colonoscopy robot | |
Taylor et al. | Forward kinematics using imu on-body sensor network for mobile analysis of human kinematics |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |