CN116749168A - 一种基于体势手势示教的康复轨迹获取方法 - Google Patents

Abstract

本发明提出了一种基于体势手势示教的康复轨迹获取方法，针对传统康复训练费时费力、康复训练轨迹复杂等问题，主要使用视觉传感器采集人体体势和手势数据，对人体手掌和手臂运动进行较为精确的测量，通过将人体坐标系姿态映射到机器人坐标系，达到用手势和手臂运动控制机器人运动，获得相应的机器人运动轨迹的目的。本发明主要包括以下几个步骤：(1)人体三维骨骼关键点提取；(2)人体手臂与机器人动作映射；(3)运动轨迹数据处理，得到康复机械臂的期望运动轨迹。本发明的优点在于提高了多自由度机器人与人的关节匹配精确度以及轨迹的平滑性和稳定性，简化康复轨迹获取过程以及优化所提取到的康复轨迹，减少了医师的工作量。

Description

一种基于体势手势示教的康复轨迹获取方法

技术领域

本发明属于人机交互领域，特别涉及一种基于体势手势示教的康复轨迹获取方法。

背景技术

上肢康复机器人是辅助上肢运动功能障碍患者进行康复训练的一种医疗设备，近年来由于神经系统疾病等问题促使上肢康复需求量增多而备受关注。使用上肢康复机器人为患者提供高效、智能的康复训练对于弥补传统康复的不足以及提升康复效率具有重要的意义。

对于上肢运动能力受损较严重的患者而言，主要有三个特点：一是患者的主动运动功能较弱，难以凭靠自己的努力在康复运动过程中占据主导地位；二是患者在该阶段不需要过于精细而复杂的康复训练运动，而是需要一些简单而实用的动作训练；三是动作轨迹需根据特定的应用场景以及专业医师的康复建议来预先设定，需要根据不同的康复动作设定不同的康复训练轨迹。传统机械臂示教方式上手难度高，效率低，一个简单的动作往往都需要复杂的示教，而基于人体体势的机器人示教是目前人机交互研究领域的重点研究之一，技术在不断发展，其本身也具有丰富的语义和自然高效等特点，非常适合作为机器人对康复动作的学习方式。

但目前体势技术在上肢康复机器人方面的应用还比较欠缺，且由于体势示教存在精度尚缺、应用范围有限、不够自然等问题，应用在上肢康复机器人之前需要解决好以上问题。

现有技术(例如《基于医师示教动作的末端式康复机器人训练方法》)中，采用关节空间的人机运动映射方法，对相关设备要求较高，康复机器人关节结构需要设计成与人体手臂相似，且轨迹精度方面有待提高，将人体手臂姿态在机器人上的表达也不够自然。

发明内容

本发明提出了一种基于体势手势示教的康复轨迹获取方法，针对传统康复训练费时费力、康复训练轨迹复杂等问题。采集医师人体体势和手势数据，对医师手掌和手臂运动进行较为精确的测量，通过将人体坐标系姿态映射到机器人坐标系，达到用手势和手臂运动控制机器人运动，获得相应的机器人运动轨迹的目的。

为了实现本发明目的，本发明提供的一种基于体势手势示教的康复轨迹获取方法，包括：

采集医师人体图像，基于人体图像提取医师三维骨骼关键点，得到相机坐标系下的人体骨骼关键点的三维空间坐标；

进行人体坐标系重建以使人体在人体坐标系的姿态和相机坐标系姿态一致，对人体手臂运动和手掌姿态进行归一化处理，基于归一化处理结果得到机器人末端位姿矩阵T_e，将机器人末端位姿矩阵T_e传递给ROS平台的机械臂仿真模型，获得仿真运动轨迹；

记录关节角度数据，基于关节角度数据对仿真运动轨迹进行平滑，平滑后的运动轨迹即为康复机械臂的期望运动轨迹。

进一步地，得到得到相机坐标系下的人体骨骼关键点的三维空间坐标的步骤包括：

采用人体骨骼关键点识别算法提取医师人体骨骼关键点像素坐标；

利用双目视差的原理恢复每个人体骨骼关键点的深度y:

根据人体骨骼关键点的坐标结合深度图得到相机坐标系下的人体骨骼关键点的三维空间坐标。

进一步地，在人体骨骼关键点识别过程中，若肩部被手掌遮挡，则利用另一肩部关键点坐标对被遮肩部位置坐标进行恢复，经过恢复的肩部位置坐标则可以对人体错误姿态进行矫正。

进一步地，所述进行人体坐标系重建的步骤包括：

首先确定人体坐标系的X轴，其是人体左右两肩关键点所在的空间直线，计算左、右肩关键点的方向向量v：

以相机坐标系下的任意坐标点到左肩关键点的方向向量表示任意向量v_n：

v_n＝p_n-p₂ (3)

计算人体坐标系在相机坐标系中的偏航角θ_z：

根据偏航角计算由相机坐标系下的任意向量v_n变换到XOZ平面的任意向量v_n′：

v_n′＝R(θ_z)×v_n (5)

计算人体坐标系在相机坐标系中的俯仰角θ_y：

根据俯仰角计算由XOZ平面的任意向量v_n′变换到人体坐标系下的任意向量v_n″：

v_n″＝R(θ_y)×v_n′ (7)

令p_n′表示第n个人体骨骼关键点在人体坐标系中的坐标向量：

p_n′＝v_n” (8)

通过坐标变换，人体在人体坐标系的姿态和相机坐标系姿态一致。

进一步地，对人体手臂运动进行归一化处理的步骤包括：

分别通过三维坐标信息求出大臂长度l₁、小臂长度l₂、腕肩距离l₃及大臂、小臂之间的夹角α：

计算归一化后的手腕关键点在人体坐标系中的坐标向量p₆"：

进一步地，得到归一化后的手腕坐标值的步骤包括：将手臂上腕部相对于肩部的坐标进行滤波以减少局部抖动，再归一化，得到归一化后的坐标。滤波方法采用滑动窗口大小为w的滑动均值滤波，获取滤波后的手腕坐标有效值/>

对P₆′进行归一化，得到归一化后的手腕坐标值

进一步地，对手掌姿态进行归一化处理，包括：

在手掌上建立局部空间坐标系：

利用坐标轴两两相互垂直XOZ平面的法向量h：

将向量h，e，g归一化，得到手部坐标系相对于人体坐标系的姿态变换矩阵R_p：

进一步地，得到所述机器人末端位姿矩阵T_e的步骤包括：

联合归一化的手腕坐标位置信息p₆"、大臂与小臂之间的夹角β、六自由度机器人第二连杆长度a₂和第三连杆长度a₃计算最终的机器人末端位置P_e:

P_e＝L_e·p₆" (22)

其中L_e是计算得到的机器人坐标系原点到机器人末端的距离。

联合机器人末端位置P_e和手掌姿态信息R_p，求得机器人末端位姿矩阵T_e：

通过计算机器人逆运动学得到的代价最小逆解获得目标关节角度，代价函数如下：

所述对仿真运动轨迹进行平滑包括对关节角度数据进行插补和滤波优化处理。

对关节角度数据进行插补处理，采用五次B样条轨迹规划方法对关节点进行轨迹插值：

采用滑动窗口大小为2n+1的滑动平均法对关节角度数据滤波处理：

最后通过网络套接字将该轨迹发送到上肢康复机器人上作为康复机械臂的期望运动轨迹。

与现有技术相比，本发明至少能够实现以下有益效果：

1.本发明方法检测人体骨骼关键点信息以得到人体骨骼关键点的像素坐标，并根据抖动分析对所得像素坐标进行合理的滤波处理，保持一定精度的同时，也保证了坐标数据的可用性；

2.本发明方法对人体姿态进行三维重建，并对人体手臂运动进行归一化处理，使机器人的运动空间自动与人的运动空间匹配；人机运动映射方法采用笛卡儿空间映射，简化了将人体手臂运动映射到机器人上的过程，并使手臂姿态在机器人上的表达更为自然。

3.本发明方法采用基于五次B样条轨迹规划的方法，保证机械臂在实际运行系统中的连续性以及安全性，同时保证康复运动轨迹规划具有一定的局部支撑性；同时采用平滑滤波数据处理方式进行滤波处理，使轨迹曲线更加平滑、自然。

4、传统的机器人康复训练，都是根据特定的应用场景来开发的，不仅要根据不同的康复动作设定不同的康复训练轨迹，而且对于机械臂设备都有一定的要求，本方法采用基于体势的机器人示教方法对机械臂进行示教，直接通过医师的演示动作获取三维人体体势运动轨迹，来对机器人虚拟仿真模型进行在线示教，再将其发送到机器人控制器，将该轨迹作为康复机械臂的期望运动轨迹。不需要再预先人为编程设定轨迹，只需要医师对康复动作进行演示，并使康复轨迹更符合患者的康复需求解决了传统康复训练费时费力、康复训练轨迹复杂等问题。

附图说明

图1是本发明实施例提供的一种基于体势手势示教的康复轨迹获取方法示意图。

图2是本发明实施例中医师三维骨骼关键点提取示意图。

图3是本发明实施例中人体手臂与机器人动作映射示意图。

图4是本发明实施例中运动轨迹数据处理示意图。

图5是本发明实施例中人体骨骼关键点分布示意图。

图6是本发明实施例中双目相机中成像示意图。

图7是本发明实施例中人体坐标系重建示意图。

图8是本发明实施例中手掌局部坐标系示意图。

具体实施方式

下面结合实施例对本发明作进一步详细的描述，但本发明的实施方式不限于此实施例。

本发明主要包括如下步骤：

S1、提取医师三维骨骼关键点。

请参阅图2，步骤S1包括以下子步骤：

步骤S1.1：首先将双目摄像头和计算机连接，同时采集医师人体图像，再将同一时刻的医师人体图像采用人体骨骼关键点识别算法提取医师人体骨骼关键点像素坐标。

在本发明的其中一些实施例中，人体骨骼关键点识别算法采用Alphapose，其中人体骨骼关键点的分布如图5所示。

步骤S1.2：如图6所示，利用双目视差的原理恢复每个人体骨骼关键点的深度y，得到深度图：

式中，f是相机焦距，x_l是人体骨骼关键点在左相机感光区域的横坐标，x_r是人体骨骼关键点在右相机感光区域的横坐标，b是左右相机光心之间的距离，d是人体骨骼关键点在左右相机中的距离，即视差。

步骤S1.3：根据人体骨骼关键点像素坐标结合深度图得到相机坐标系下的人体骨骼关键点的三维空间坐标。

其中，人体骨骼关键点识别过程中，当肩部被手掌遮挡时，就判断发生了人体遮挡，严重的人体遮挡将导致肩部的空间坐标发生偏移，进一步会导致人体体势姿态发生严重变形。若发生人体遮挡，则利用另一肩部关键点坐标对被遮肩部位置坐标进行恢复，经过恢复的肩部位置坐标则可以对人体错误姿态进行矫正。

步骤S2、人体手臂与机器人动作映射

请参阅图3，步骤S2包括以下子步骤：

步骤2.1：对步骤1得到的人体骨骼关键点的三维空间坐标进行坐标系重建，如图7所示：

首先确定人体坐标系的X轴，其是人体左右两肩关键点所在的空间直线，计算左、右肩关键点的方向向量v：

式中，p₂是相机坐标系下左肩关键点的三维空间坐标，p₃是相机坐标系下右肩关键点的三维空间坐标，x_v、y_v、z_v是左、右肩关键点的方向向量v的x、y、z坐标。

以相机坐标系下的任意坐标点到左肩关键点的方向向量表示任意向量v_n：

v_n＝p_n-p₂ (3)

p_n是相机坐标系下编号为n的人体骨骼关键点的三维空间坐标。

计算人体坐标系在相机坐标系中的偏航角θ_z：

根据偏航角计算由相机坐标系下的任意向量v_n变换到XOZ平面的任意向量v_n′：

v_n′＝R(θ_z)×v_n (5)

R(θ_z)是偏航角的旋转变换矩阵。

计算人体坐标系在相机坐标系中的俯仰角θ_y：

x_v′、y_v′是XOZ平面的左、右肩关键点的方向向量v_n′的x、y坐标。

根据俯仰角θ_y计算由XOZ平面的任意向量v_n′变换到人体坐标系下的任意向量v_n″：

v_n″＝R(θ_y)×v_n′ (7)

R(θ_y)是俯仰角的旋转变换矩阵；

令p_n′表示第n个人体骨骼关键点在人体坐标系中的坐标向量：

p_n′＝v_n” (8)

通过坐标变换，人体在人体坐标系的姿态和相机坐标系姿态一致。

步骤2.2：将手臂上腕部相对于肩部的坐标进行滤波以减少局部抖动，再归一化，得到归一化后的坐标。

滤波方法采用滑动窗口大小w的滑动均值滤波，获取滤波后的手腕坐标有效值

对P₆′进行归一化，得到归一化后的手腕坐标值

其中，滑动窗口大小w的取值为实际使用时效果较好的窗口大小值，在本发明的其中一些实施例中，w取值为2。

步骤2.3：利用手臂组成的空间三角形来表征手臂的运动。其中，手臂三角形由固定长度的大臂、小臂和长度变化的手腕—肩部之间的连线组成，可以分别通过三维坐标信息求出大臂长度l₁、小臂长度l₂、腕肩距离l₃及大臂、小臂之间的夹角α：

计算归一化后的手腕关键点在人体坐标系中的坐标向量p₆"：

x₂、y₂、z₂、x₄、y₄、z₄、x₆、y₆、z₆分别是左肩、左手肘、左手腕关键点的x、y、z坐标。

步骤2.4：在手掌上建立局部空间坐标系，滤波后得到手掌上局部坐标系相对于肩部为原点的坐标系的姿态角。如图8所示，以左手腕关键点(图5中的点“6”)到左中指关键点(图5中的点“10”)的方向向量e作为局部坐标系的Z轴，方向向量e与左手腕关键点指向左拇指关键点(图5中的点“8”)在手掌上建立的f组成的平面作为局部坐标系的XOY平面，过左手腕关键点(图5中的点“6”)的g，g⊥e且g⊥f，以g作为X轴：

p₆、p₈、p₁₀是左手腕、左拇指、左中指关键点的三维空间坐标，x_e、y_e、z_e、x_f、y_f、z_f是方向向量e、f的x、y、z坐标，x₆、y₆、z₆、x₈、y₈、z₈、x₁₀、y₁₀、z₁₀分别是左手腕、左拇指、左中指关键点的x、y、z坐标。

利用坐标轴两两相互垂直，得到XOZ平面的法向量h：

x_g、y_g、z_g是方向向量g的x、y、z坐标，i、j、k为相互垂直的三个单位向量；

将向量h，e，g归一化，得到手部坐标系相对于人体坐标系的姿态变换矩阵R_p：

步骤2.5：联合归一化的手腕坐标位置信息p₆"、大臂与小臂之间的夹角α、六自由度机器人第二连杆长度a₂和第三连杆长度a₃计算最终的机器人末端位置P_e：

P_e＝L_e·p₆" (22)

其中，L_e是计算得到的机器人坐标系原点到机器人末端的距离。

步骤2.6：联合机器人末端位置P_e和手掌姿态信息R_p，求得机器人末端位姿矩阵T_e：

步骤2.7：将机器人末端位姿矩阵T_e传递给ROS平台的机械臂仿真模型，通过ROS中的IKFast运动学求解器求解出适合机器人稳定运动(即代价最小)的逆解，并获得仿真运动轨迹，其中，代价函数如下：

当前时刻机器人关节s的关节角为θ_s，其中一个可行的逆解为α_i为当前关节连接的下一个关节连杆长度。

步骤S3、运动轨迹数据处理。

如图4所示，步骤S3包括：

步骤3.1：由医师观察机械臂仿真模型的仿真运动轨迹是否准确自然且符合康复知识，同时记录关节角度数据；

步骤3.2：对关节角度数据进行插补处理，采用五次B样条轨迹规划方法对关节点进行轨迹插值：

P(u)是机器人在不同节点插值点上的关节角度序列，Q_i,0≤i≤n为控制多边形曲线的顶点，通常被叫做B样条曲线的型值点，N_i，k(u),0≤i≤n为k(k+1)次阶B样条基函数，u是整个机器人运动过程的关节角度序列，n是机器人运动过程的关节角度序列大小。

步骤3.3：对插补处理后的关节角度数据进行滤波优化，使机械臂可以平滑地模仿人体手臂动作轨迹，采用滑动窗口大小为2n+1的滑动平均法对关节角度数据滤波处理：

y′_k为滤波后的关节角度数据序列，y_k是滤波前的关节角度数据序列，N为要处理的数据总数。

其中，滑动窗口大小的取值为实际使用时效果较好的窗口大小值，在本发明的其中一些实施例中，滑动窗口大小2n+1取值为5。

初始获得的轨迹的各个关节角的变化有明显的波动，连续性和稳定性无法保证，不能直接用于康复训练中，通过插值和滤波优化，可以使轨迹更平滑，保证轨迹连续性和稳定性。

步骤3.4：通过网络套接字将经过插值和滤波优化后的轨迹发送到上肢康复机器人上作为康复机械臂的期望运动轨迹。

在本发明的其中一些实施例中，以一个具体的示例来对本发明的有效性进行说明。

以康复训练时常用的平面画圆为例，规划训练轨迹圆心位于基座标系(200，200，500)处、半径为100mm的标准圆，轨迹函数曲线方程式如式：

首先利用本发明体势示教方法控制机器人在规定时段内完成按预设轨迹进行移动的任务，记录该时段内机械臂的关节位移角度。

原始获得的机器人运动轨迹与预设轨迹存在误差较大、轨迹曲线起伏较大、拐点过渡不够平滑等问题。

根据康复医学知识，将该时段时长定为30s，同时将关节位移角度的采样周期设定为1s，共可得到30个采样示教点。

使用4个插值点对关节角位移进行插值处理。

分别取各关节角位移插值后的数据，不断逐个滑动地取5个相邻数据作加权平均来表示平滑数据。经过滤波后的轨迹平滑，机器人在此运动过程中的运动是平稳的，可后续作为康复训练的预设轨迹使用。

上述实施例为本发明较佳的实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于体势手势示教的康复轨迹获取方法，其特征在于，包括以下步骤：

采集医师人体图像，基于人体图像提取医师三维骨骼关键点，得到相机坐标系下的人体骨骼关键点的三维空间坐标；

进行人体坐标系重建以使人体在人体坐标系的姿态和相机坐标系姿态一致，对人体手臂运动和手掌姿态进行归一化处理，基于归一化处理结果得到机器人末端位姿矩阵T_e，将机器人末端位姿矩阵T_e传递给ROS平台的机械臂仿真模型，获得仿真运动轨迹；

记录关节角度数据，基于关节角度数据对仿真运动轨迹进行平滑，平滑后的运动轨迹即为康复机械臂的期望运动轨迹。

2.根据权利要求1所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，得到得到相机坐标系下的人体骨骼关键点的三维空间坐标的步骤包括：

采用人体骨骼关键点识别算法提取医师人体骨骼关键点像素坐标；

利用双目视差的原理恢复每个人体骨骼关键点的深度y，得到深度图：

式中，f是相机焦距，x_l是人体骨骼关键点在左相机感光区域的横坐标，x_r是人体骨骼关键点在右相机感光区域的横坐标，b是左右相机光心之间的距离，d是人体骨骼关键点在左右相机中的距离；

根据人体骨骼关键点的像素坐标结合深度图得到相机坐标系下的人体骨骼关键点的三维空间坐标。

3.根据权利要求2所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，在人体骨骼关键点识别过程中，若肩部被手掌遮挡，则利用另一肩部关键点坐标对被遮肩部位置坐标进行恢复，经过恢复的肩部位置坐标则可以对人体错误姿态进行矫正。

4.根据权利要求1所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，所述进行人体坐标系重建的步骤包括：

以人体左右两肩关键点所在的空间直线作为人体坐标系的X轴，计算左、右肩关键点的方向向量v：

式中，p₂是相机坐标系下左肩关键点的三维空间坐标，p₃是相机坐标系下右肩关键点的三维空间坐标，x_v、y_v、z_v是左、右肩关键点的方向向量v的x、y、z坐标；

以相机坐标系下的任意坐标点到左肩关键点的方向向量表示任意向量v_n：

v_n＝p_n-p₂(3)

式中，p_n是相机坐标系下编号为n的人体骨骼关键点的三维空间坐标；

计算人体坐标系在相机坐标系中的偏航角θ_z：

根据偏航角计算由相机坐标系下的任意向量v_n变换到XOZ平面的任意向量v_n′：

v_n′＝R(θ_z)×v_n(5)

式中，Rθ_z)是偏航角的旋转变换矩阵；

计算人体坐标系在相机坐标系中的俯仰角θ_y：

式中，x_v′、y_v′是XOZ平面的左、右肩关键点的方向向量v_n′的x、y坐标；

根据俯仰角θ_y计算由XOZ平面的任意向量v_n′变换到人体坐标系下的任意向量v_n″：

v_n″＝R(θ_y)×v_n′(7)

令p_n′表示第n个人体骨骼关键点在人体坐标系中的坐标向量：

p_n′＝v_n”(8)

通过坐标变换，人体在人体坐标系的姿态和相机坐标系姿态一致。

5.根据权利要求1所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，对人体手臂运动进行归一化处理的步骤包括：

通过三维坐标信息求出大臂长度l₁、小臂长度l₂、腕肩距离l₃及大臂、小臂之间的夹角α：

式中，x₂、y₂、z₂、x₄、y₄、z₄、x₆、y₆、z₆分别是左肩、左手肘、左手腕关键点的x、y、z坐标；

计算归一化后的手腕关键点在人体坐标系中的坐标向量p₆"：

式中，是归一化后的手腕坐标值。

6.根据权利要求5所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，得到归一化后的手腕坐标值的步骤包括：将手臂上腕部相对于肩部的坐标进行滤波以减少局部抖动，再归一化，得到归一化后的坐标，其中，滤波方法采用滑动窗口大小为w的滑动均值滤波，获取滤波后的手腕坐标有效值/>

对P₆′进行归一化，得到归一化后的手腕坐标值

7.根据权利要求5所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，对手掌姿态进行归一化处理，包括：

在手掌上建立局部空间坐标系：

式中，p₆、p₈、p₁₀是左手腕、左拇指、左中指关键点的三维空间坐标，x_e、y_e、z_e、x_f、y_f、z_f是方向向量e、f的x、y、z坐标，x₆、y₆、z₆、x₈、y₈、z₈、x₁₀、y₁₀、z₁₀分别是左手腕、左拇指、左中指关键点的x、y、z坐标；

利用坐标轴两两相互垂直，得到XOZ平面的法向量h：

式中，x_g、y_g、z_g是方向向量g的x、y、z坐标，i、j、k为相互垂直的三个单位向量；

将向量h，e，g归一化，得到手部坐标系相对于人体坐标系的姿态变换矩阵R_p：

8.根据权利要求7所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，得到所述机器人末端位姿矩阵T_e的步骤包括：

联合归一化的手腕坐标位置信息p₆"、大臂与小臂之间的夹角β、六自由度机器人第二连杆长度a₂和第三连杆长度a₃计算最终的机器人末端位置P_e:

P_e＝L_e·p₆" (22)

式中，L_e是计算得到的机器人坐标系原点到机器人末端的距离；

联合机器人末端位置P_e和手掌姿态信息R_p，求得机器人末端位姿矩阵T_e：

9.根据权利要求1-8任一所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，所述对仿真运动轨迹进行平滑包括对关节角度数据进行插补和滤波优化处理。

10.根据权利要求9所述的一种基于体势手势示教的康复轨迹获取方法，其特征在于，采用五次B样条轨迹规划方法对关节点进行轨迹插值：

p(u)是机器人在不同节点插值点上的关节角度序列，Q_i,0≤i≤n为控制多边形曲线的顶点，通常被叫做B样条曲线的型值点，N_i，k(u),0≤i≤n为k(k+1)次阶B样条基函数，u是整个机器人运动过程的关节角度序列，n是机器人运动过程的关节角度序列大小；

采用滑动窗口大小为2n+1的滑动平均法对关节角度数据滤波处理：

y′_k为滤波后的关节角度数据序列，y_k是滤波前的关节角度数据序列，N为要处理的数据总数。