三维肢体镜像康复机器人的控制方法及康复机器人技术

本发明专利技术提供一种三维肢体镜像康复机器人的控制方法及康复机器人，将计算和平滑健侧上肢关节角度集成到无迹卡尔曼滤波一步中，算法集成度更高，造成的延迟更小，以及基于模型预测控制算法计算机器人各关节的角速度，由于预测模型的存在使得算法具有一定的预见性，从而能显著减小算法延迟以及由延迟造成的健侧肢体与机器人带动的偏瘫侧肢体各关节的角度误差。差。差。

【技术实现步骤摘要】
三维肢体镜像康复机器人的控制方法及康复机器人


[0001]本专利技术涉及康复机器人
，具体而言，涉及一种三维肢体镜像康复机器人的控制方法及康复机器人。

技术介绍

[0002]现有的三维上肢镜像康复机器人，从深度视觉传感器读取人体健侧上肢关节中心三维坐标，然后利用几何关系计算出健侧上肢各关节角度，滤波平滑后再将健侧上肢各关节角度发送给机器人，由机器人带动偏瘫侧上肢各关节运动至与健侧手臂镜像的位置。
[0003]由于控制算法及通信等造成的延迟，主动运动的健侧肢体与机器人带动的偏瘫侧肢体各关节存在着较大的角度误差。

技术实现思路

[0004]本专利技术解决的问题是镜像康复训练时主动运动的健侧肢体与机器人带动的偏瘫侧肢体各关节存在着较大的角度误差。
[0005]为解决上述问题，本专利技术提供一种三维肢体镜像康复机器人的控制方法，所述方法包括：获取健侧肢体的多个关节的中心的三维坐标；基于所述三维坐标及无迹卡尔曼滤波算法，计算和平滑所述健侧肢体的关节角度，得到多个所述关节的角度、角速度；基于多个所述关节的角度、角速度及模型预测控制算法，计算得到外骨骼的相应关节的角速度；根据所述外骨骼的相应关节的角速度控制所述外骨骼运动，以使所述外骨骼带动肢体运动至与所述健侧肢体镜像的位置。
[0006]可选地，所述关节的中心包括以下至少一项：健侧上肢肩关节中心、健侧上肢肘关节中心、健侧上肢腕关节中心。
[0007]可选地，所述基于所述三维坐标及无迹卡尔曼滤波算法，计算和平滑所述健侧肢体的关节角度，得到多个所述关节的角度、角速度，包括：将当前时刻的所述健侧上肢肩关节中心、所述健侧上肢肘关节中心、所述健侧上肢腕关节中心的三维坐标，输入无迹卡尔曼滤波算法计算和平滑所述健侧上肢的关节角度，输出所述健侧上肢肩关节外展/内收角度、肩关节前屈/后伸角度、肘关节屈/伸角度以及所述健侧上肢肩关节外展/内收角速度、肩关节前屈/后伸角速度、肘关节屈/伸角速度。
[0008]可选地，所述健侧上肢系统的状态变量如下：
[0009][0010]其中，q1(k)，q2(k)，q3(k)分别表示当前时刻k健侧上肢肩关节外展/内收角度、肩关节前屈/后伸角度、肘关节屈/伸角度；dq1(k)，dq2(k)，dq3(k)分别表示健侧上肢肩关节外展/内收角速度、肩关节前屈/后伸角速度、肘关节屈/伸角速度；x1(k)，y1(k)，z1(k)分别表示健侧上肢肩关节中心S的三维坐标；dx1(k)，dy1(k)，dz1(k)分别表示健侧上肢肩关节中心S的三维平移速度；L1(k)，L2(k)分别表示k时刻健侧上肢的大臂长度、小臂长度；
[0011]假设健侧上肢各关节匀速运动并受到随机扰动，则健侧上肢系统的离散状态方程为：
[0012][0013]其中，k
‑
1表示上一时刻；T表示控制周期；W(k)＝[w1(k)，w2(k)，w3(k)，w4(k)，w5(k)，w6(k)]T
表示系统受到的随机扰动，均服从零均值的正态分布；
[0014]基于正运动学得到系统观测方程
[0015][0016]其中，Z(k)＝[Z1(k)，Z2(k)，Z3(k)，Z4(k)，Z5(k)，Z6(k)，Z7(k)，Z8(k)，Z9(k)]T
表示系统观测变量；
[0017]V(k)＝[v1(k)，v2(k)，v3(k)，v4(k)，v5(k)，v6(k)，v7(k)，v8(k)，v9(k)]T
表示系统测量误差，均服从零均值的正态分布；
[0018]系统实际观测量由所述健侧上肢肩关节中心的三维坐标[x1(k)，y1(k)，z1(k)]、所述健侧上肢肘关节中心的三维坐标[x2(k)，y2(k)，z2(k)]以及所述健侧上肢腕关节中心
的三维坐标[x3(k)，y3(k)，z3(k)]组成；
[0019]设定状态变量的初始值X(0)、随机扰动W(k)的方差和测量误差V(k)的方差，将所述离散状态方程、所述系统观测方程和所述实际观测量[x1(k)，y1(k)，z1(k)，x2(k)，y2(k)，z2(k)，x3(k)，y3(k)，z3(k)]T
代入预设的无迹卡尔曼算法进行求解，求解出滤波后的健侧上肢肩关节外展/内收角度q1(k)、肩关节前屈/后伸角度q2(k)、肘关节屈/伸角度q3(k)以及健侧上肢肩关节外展/内收角速度dq1(k)、肩关节前屈/后伸角速度dq2(k)、肘关节屈/伸角速度dq3(k)。
[0020]可选地，基于多个所述关节的角度、角速度及模型预测控制算法，计算得到外骨骼的相应关节的角速度，包括：将当前时刻的所述健侧上肢肩关节外展/内收角度、所述肩关节前屈/后伸角度、所述肘关节屈/伸角度以及所述健侧上肢肩关节外展/内收角速度、所述肩关节前屈/后伸角速度、所述肘关节屈/伸角速度，输入模型预测控制算法，输出所述外骨骼的肩关节外展/内收角速度、肩关节前屈/后伸角速度、肘关节屈/伸角速度。
[0021]可选地，外骨骼系统的状态变量X(k)＝[θ1(k)，θ2(k)，θ3(k)]T
；其中，θ1(k)，θ2(k)，θ3(k)分别表示k时刻外骨骼肩关节外展/内收角度、肩关节前屈/后伸角度和肘关节屈/伸角度；
[0022]外骨骼系统预测模型如下：
[0023]X(k+1)＝A*X(k)+B*U(k)
[0024]其中是3*3的单位矩阵；T表示控制周期；U(k)＝[dθ1(k)，dθ2(k)，dθ3(k)]T
表示系统输入的外骨骼各关节角速度；
[0025]模型预测控制等价于求解以下二次优化方程：
[0026][0027]其中，p是预设的预测步长；X(k+i)
ref
＝[q1(k)+i*dq1(k)，q2(k)+i*dq2(k)，q3(k)+i*dq3(k)]T
，i＝1...p是由滤波后的健侧上肢肩关节外展/内收角度q1(k)、肩关节前屈/后伸角度q2(k)、肘关节屈/伸角度q3(k)以及健侧上肢肩关节外展/内收角速度dq1(k)，肩关节前屈/后伸角速度dq2(k)，肘关节屈/伸角速度dq3(k)构成的参考轨迹；Q
i
∈R
p
×
p
和R
i
∈R
p
×
p
是预设的对角权重矩阵；U(k+i)＝[dθ1(k+i)，dθ2(k+i)，dθ3(k+i)]T
需满足约束条件其中dθ1
min
，dθ1
max
分别表示外骨骼肩关节外展/内收允许的最小和最大角速度，dθ2
min
，dθ2
max
分别表示外骨骼肩关节前屈/后伸允许的最小和最大角速度，dθ3
min
，dθ3
max
分别表示外骨骼肘关节屈/伸角允许的最小和最大角速度；
[0028]设定预测步长p，权重矩阵Q
i...

【技术保护点】

【技术特征摘要】
1.一种三维肢体镜像康复机器人的控制方法，其特征在于，所述方法包括：获取健侧肢体的多个关节的中心的三维坐标；基于所述三维坐标及无迹卡尔曼滤波算法，计算和平滑所述健侧肢体的关节角度，得到多个所述关节的角度、角速度；基于多个所述关节的角度、角速度及模型预测控制算法，计算得到外骨骼的相应关节的角速度；根据所述外骨骼的相应关节的角速度控制所述外骨骼运动，以使所述外骨骼带动肢体运动至与所述健侧肢体镜像的位置。2.根据权利要求1所述方法，其特征在于，所述关节的中心包括以下至少一项：健侧上肢肩关节中心、健侧上肢肘关节中心、健侧上肢腕关节中心。3.根据权利要求2所述方法，其特征在于，所述基于所述三维坐标及无迹卡尔曼滤波算法，计算和平滑所述健侧肢体的关节角度，得到多个所述关节的角度、角速度，包括：将当前时刻的所述健侧上肢肩关节中心、所述健侧上肢肘关节中心、所述健侧上肢腕关节中心的三维坐标，输入无迹卡尔曼滤波算法计算和平滑所述健侧上肢的关节角度，输出所述健侧上肢肩关节外展/内收角度、肩关节前屈/后伸角度、肘关节屈/伸角度以及所述健侧上肢肩关节外展/内收角速度、肩关节前屈/后伸角速度、肘关节屈/伸角速度。4.根据权利要求3所述方法，其特征在于，所述健侧上肢系统的状态变量如下：其中，q1(k),q2(k),q3(k)分别表示当前时刻k健侧上肢肩关节外展/内收角度、肩关节前屈/后伸角度、肘关节屈/伸角度；dq1(k),dq2(k),dq3(k)分别表示健侧上肢肩关节外展/内收角速度、肩关节前屈/后伸角速度、肘关节屈/伸角速度；x1(k),y1(k),z1(k)分别表示健侧上肢肩关节中心S的三维坐标；dx1(k),dy1(k),dz1(k)分别表示健侧上肢肩关节中心S的三维平移速度；L1(k),L2(k)分别表示k时刻健侧上肢的大臂长度、小臂长度；假设健侧上肢各关节匀速运动并受到随机扰动，则健侧上肢系统的离散状态方程为：
其中，k
‑
1表示上一时刻；T表示控制周期；W(k)＝[w1(k),w2(k),w3(k),w4(k),w5(k),w6(k)]
T
表示系统受到的随机扰动，均服从零均值的正态分布；基于正运动学得到系统观测方程其中，Z(k)＝[Z1(k),Z2(k),Z3(k),Z4(k),Z5(k),Z6(k),Z7(k),Z8(k),Z9(k)]
T
表示系统观测变量；系统实际观测量由所述健侧上肢肩关节中心的三维坐标[x1(k),y1(k),z1(k)]、所述健侧上肢肘关节中心的三维坐标[x2(k),y2(k),z2(k)]以及所述健侧上肢腕关节中心的三维坐标[x3(k),y3(k),z3(k)]组成；V(k)＝[v1(k),v2(k),v3(k),v4(k),v5(k),v6(k),v7(k),v8(k),v9(k)]
T
表示系统测量误差，均服从零均值的正态分布；
设定状态变量的初始值X(0)、随机扰动W(k)的方差和测量误差V(k)的方差，将所述离散状态方程、所述系统观测方程和所述实际观测量[x1(k),y1(k),z1(k),x2(k),y2(k),z2(k),x3(k),y3(k),z3(k)]
T
代入预设的无迹卡尔曼算法进行求解，求解出滤波后的健侧上肢肩关节外展/内收角度q1(k)、肩关节前屈/后伸角度q2(k)、肘关节屈/伸角度q3(k)以及健侧上肢肩关节外展/内收角速度dq1(k)、肩关节前屈/后伸角速度dq2(k)、肘关节屈/伸角速度dq3(k)。5.根据权利要求3或4所述方法，其特征在于，基于多个所述关节的角度、角速度及模型预测控制算法，计算得到外骨骼的相应关节的角速度，包括：将当前时刻的所述健侧上肢肩关节外展/内收角度、所述肩关节前屈/后伸角度、...

【专利技术属性】
技术研发人员：吴剑煌，孙维，黄冠，
申请(专利权)人：深圳华鹊景医疗科技有限公司，
类型：发明
国别省市：