一种具有人机交互动力学模型的外骨骼式上肢康复机器人

技术领域

本发明涉及上肢康复机器人的运动控制和延伸应用技术领域，特别提 供了一种具有人机交互动力学模型的外骨骼式上肢康复机器人。

背景技术

脑卒中是老年人发病率较高的疾病之一，其导致的肢体运动功能障碍 严重影响了老年人的健康生活。传统的治疗方法需要专业康复医师一对一 地进行物理治疗，其治疗效率有限，极大浪费治疗资源，对患者的家庭和 社会也是不小的经济负担。适当的康复运动训练可以促进脑卒中患者发病 后肢体活动功能的恢复。机器人参与辅助上肢康复运动训练更具有针对性， 持续时间也更长，可重复性更高。许多研究表明，机器人辅助的康复训练 比传统方法的疗效显著(参见文献1：KwakkelG,KollenBJ,KrebsHI.Effects ofRobot-assistedTherapyonUpperLimbRecoveryAfterStroke:aSystematic Review[J].NeurorehabilitationandNeuralRepair,2007,22(2):111-121；同时 参见文献2：FazekasG,HorvathM,TroznaiT,etal.Robot-mediatedUpperLimb PhysiotherapyforPatientswithSpasticHemiparesis:aPreliminaryStudy[J]. JournalofRehabilitationMedicine,2007,39(7):580-582.)。(译文：参考文献 1：KwakkelG,KollenBJ,KrebsHI.脑卒中后机器人辅助上肢治疗康复效果综 述[J].神经康复与神经修复，2007,22(2):111-121；参考文献2：Fazekas G,HorvathM,TroznaiT,etal.上肢痉挛偏瘫辅理疗康复机器人的研究.康复器 械[J],2007,39(7):580-582.)，现在很多机构都在研究上肢康复机器人用于 辅助康复医师进行上肢康复训练，从而将康复医师从繁重的体力劳动中解 脱出来，提高康复效率，减轻患者经济负担。申请号为2014101552790、 2014101591028、2014201927316、2014201927354的专利申请文件中提及一 种外骨骼式上肢康复训练系统(如图1所示)，它是辅助康复医师完成康复 训练的一种医疗设备，可以实现从大关节到小关节的大范围单关节运动及 多关节复合运动，真实再现患者的日常生活动作训练。它由两部分组成： 一台带有显示器的PC机和一组上肢可穿戴外骨骼式机械设备。外骨骼式机 械设备具有五个自由度，分别是：肩部外展/内收，肩部俯/仰，肘部俯/仰， 腕部外展/内收，腕部俯/仰。基座和五根不规则的刚性连杆通过活动关节连 接而成，每个关节通过电机驱动。每个连杆的结构并不规则，其质心在杆 件外部，活动关节连接两个连杆的旋转方向也不一致，其运行空间为X-Y-Z 三维(triaxial)坐标系。考虑康复训练的安全因素，每个关节的旋转角度也 有一定的限制。

在外骨骼式上肢康复机器人进行康复训练的过程中，腕部两个自由度 活动范围较小，对整个动力学模型影响也不大，所以在动力学建模过程中 忽略腕部的两个自由度。在仿真验证时，为确保上肢的活动范围与外骨骼 康复器械运动相匹配，保证腕部两个关节固定不变，同时，取肩部外展/内 收和肩部屈/伸两个自由度实现在三维空间中的活动，再加上肘部屈/伸，这 样人体上肢在动力学建模中就可以看作是一个具有三个自由度的两个连 杆。五自由度外骨骼式康复机器人变成了具有三个大范围运动自由度的不 规则外骨骼式康复器械。

在上肢康复训练中，患者主动运动康复训练是很必要的康复过程。若 要实现主动康复训练，就要能够准确判断患者上肢的运动意图。因为进行 主动训练的患者上肢肌力不具备支撑手臂及康复器械的能力，需要康复器 械施加一定的力矩辅助患者运动。相比于被动康复训练，主动康复训练被 认为对上肢运动功能恢复更为有效。为了实现主动康复训练就需要判断患 者的运动意向。在许多文献中，大多采用肌电信号来实现判断人体主动运 动意向的。然而，由于每个人的实际情况不同，所产生的肌电信号强弱不 一致，肌电信号的干扰因素很多，所以在实际过程中要经过反复检测才能 使用，增加了检测成本。

对于人体上肢关节力矩的测量，有的文献中采用测量末端力矩再通过 雅可比矩阵转换的方法间接计算得到各关节力矩，这样不仅需要固定上肢 末端而且测得数据经过转换，且很难反映出各个关节的真实情况。然而只 要人体上肢固定在外骨骼上，通过调整连杆长度使各关节的旋转中心与人 体上肢各关节旋转中心一致，使其运动角度、角速度和角加速度均与外骨 骼一致，就可以通过外骨骼上的传感器直接测量得到人体上肢各关节力矩。 人体上肢各关节的力矩估计值可由安装在外骨骼各个关节的力矩传感器测 量得到的人体上肢与外骨骼在各个关节处合力矩，减去估计的各关节力矩 值得到。

因此，人们迫切希望获得一种能够通过确定控制参数估计关节力矩与 实测力矩的比较值从而判断患者运动意图的具有人机交互动力学模型的外 骨骼式上肢康复机器人。

发明内容

本发明的目的是提供一种具有人机交互动力学模型的外骨骼式上肢康 复机器人，其关键点是针对外骨骼式上肢康复机器人建立人机交互动力学 模型，以此模型为基础的外骨骼式上肢康复机器人具备以下突出创新点： 其所建立的数学模型能够在一定程度上体现外骨骼机器人与人体之间的动 力学交互关系。通过确定控制参数就可以用估计出的关节力矩与实测力矩 的比较值从而判断患者的运动意图。

本发明一种具有人机交互动力学模型的外骨骼式上肢康复机器人，其 特征在于：首先，在康复训练中将人体上肢看作具有三个自由度的二连杆 机械臂(肩部两个自由度，肘部一个自由度)，将其动力学模型与外骨骼机 器人动力学模型联合，形成外骨骼式上肢康复机器人人机交互动力学模型； 其中：人体上肢的关节角度与力矩可通过安装在外骨骼活动关节上的角度 与力矩传感器测量得到；所述外骨骼式上肢康复机器人人机交互动力学模 型是基于伪惯量矩阵采用拉格朗日算法的具有19个参数的人机交互动力学 模型；通过建立机器人系统的图示化模型并与控制器及其动态系统相连进 行综合仿真比较，表明运用该方法建立的数学模型能够在一定程度上体现 外骨骼机器人与人体之间的动力学交互关系；建模之后再确定控制参数根 据关节力矩估计值与实测力矩值的比较判断患者的运动意图；

从图2中可以看出，当外骨骼式上肢康复机器人各关节力矩传感器测 得的力矩实际值T_m与人机交互模型计算出的各关节角度力矩估算值T_c相比 较所得偏差值d_T小于0时，表明人体上肢向此关节运动正方向施加了力矩， 人体上肢的运动意图为向正向旋转；反之，如果偏差值d_T大于0时，则表 明人体上肢向此关节运动反方向施加了力矩，人体上肢的运动意图为向反 向旋转；而当偏差值d_T等于0时，表明人体上肢没有施加任何力矩，没有 运动意图。

在实际应用中，设定一定的阈值，用以调节判断运动意图的灵敏度。 还可以对d_T值进行一定的处理，用以生成控制外骨骼运动的指令C。本发明 所述具有人机交互动力学模型的外骨骼式上肢康复机器人，其特征在于： 建立外骨骼式上肢康复机器人动力学模型，具体要求是：

根据D-H参数法，外骨骼式上肢康复机器人坐标变换关系如图3所示， 并由此获得坐标变换D-H参数表(如表1所示)；

表1上肢康复外骨骼坐标变换D-H参数表

忽略摩擦力的影响，利用拉格朗日方法建立外骨骼式上肢康复机器人 动力学模型，模型的标准形式如式(1)所示：

$\tau =M\left(q\right)\stackrel{··}{q}+C(q,\stackrel{·}{q})+G\left(q\right)---\left(1\right)$

式(1)中分别表示关节i的角度、角速度和角加速度；M(q)是一 个3×3的惯性矩阵；为3×1的非线性哥氏力和向心力矢量；G(q)为 3×1的重力矢量；τ是一个3×1的关节控制输入力矩矢量；

M(q)、和G(q)的元素包含了各相关杆件的惯性参数，即刚体的十 个常量，列于式(2)所示：

F_i＝[m_i,I_xx,i,I_yy,i,I_zz,i,I_xy,i,I_xz,i,I_yz,i,x_i,y_i,z_i](i＝1,2,3)(2)

式(2)中，m_i为连杆i的质量，I_xx，i，I_yy，i，I_zz，i为相对于坐标系{i}的三维 质量惯性矩，I_xy，i，I_xz，i，I_yz，i为相对于坐标系{i}的三维质量惯性积，x_i,y_i,z_i为 连杆质心的三维坐标；

将式(1)所示外骨骼式上肢康复机器人动力学模型用式(2)所示各 杆件的惯性参数表示，在对其进行非线性组合并整理归纳将模型转化为如 式(3)所示的线性形式：

${\Phi }_{exo}(q_{exo},{\stackrel{·}{q}}_{exo},{\stackrel{··}{q}}_{exo})P_{exo}={\tau }_{exo}---\left(3\right)$

式(3)中，τ_exo是一个3×1的矢量，表示外骨骼各关节的力矩；Φ_exo是 一个3×19的矩阵，代表回归变量；P_exo是一个19×1的矢量，代表动力学模 型的惯性参数；

Φ_exo与P_exo中的元素分别定义如下：

${\Phi }_{1,1}^{exo}={\stackrel{··}{q}}_1;$

${\Phi }_{1,2}={\stackrel{··}{q}}_1\left(cos\right(2q_2+q_3)+cos(q_3\left)\right)-(2{\stackrel{·}{q}}_1{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3)sin(2q_2+q_3)-{\stackrel{·}{q}}_1{\stackrel{·}{q}}_{3}sin\left(q_3\right);$

${\Phi }_{1,3}={\stackrel{··}{q}}_{1}cos\left(2q_2\right)-2{\stackrel{·}{q}}_1{\stackrel{·}{q}}_{2}sin\left(2q_2\right);$

${\Phi }_{1,4}={\stackrel{··}{q}}_1\left(sin\right(2q_2+q_3)+sin(q_3\left)\right)+(2{\stackrel{·}{q}}_1{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3)cos(2q_2+q_3)+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_{3}cos\left(q_3\right);$

${\Phi }_{1,5}={\stackrel{··}{q}}_{1}sin\left(2q_2\right)+2{\stackrel{·}{q}}_1{\stackrel{·}{q}}_{2}cos\left(2q_2\right);$

${\Phi }_{1,6}={\stackrel{··}{q}}_{1}cos(2q_2+2q_3)-2({\stackrel{·}{q}}_1{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3)sin(2q_2+2q_3);$

${\Phi }_{1,7}={\stackrel{··}{q}}_{1}sin(2q_2+2q_3)+2({\stackrel{·}{q}}_1{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3)cos(2q_2+2q_3);$

${\Phi }_{1,8}=({\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3)\sin (q_2+q_3)+{({\stackrel{·}{q}}_2+{\stackrel{·}{q}}_3)}^{2}cos(q_2+q_3);$

${\Phi }_{1,9}=({\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3)cos(q_2+q_3)-{({\stackrel{·}{q}}_2+{\stackrel{·}{q}}_3)}^{2}sin(q_2+q_3);$

Φ_1,10＝Φ_1,19＝sin(q₁)sin(q₂+q₃)；

${\Phi }_{1,11}={\stackrel{··}{q}}_{2}sin\left(q_2\right)+{{\stackrel{·}{q}}_2}^{2}cos\left(q_2\right);$

Φ_1,14＝cos(q₁)；

Φ_1,15＝sin(q₁)；

Φ_1,16＝cos(q₂)sin(q₁)；

Φ_1,17＝sin(q₁)sin(q₂)；

Φ_1,18＝sin(q₁)cos(q₂+q₃)；

${\Phi }_{2,2}=(2{\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3)cos\left(q_3\right)+{{\stackrel{·}{q}}_1}^{2}sin(2q_2+q_3)-{\stackrel{·}{q}}_3(2{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_3)sin\left(q_3\right);$

${\Phi }_{2,3}={{\stackrel{·}{q}}_1}^{2}sin\left(2q_2\right);$

${\Phi }_{2,4}=(2{\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3)\sin \left(q_3\right)-{{\stackrel{·}{q}}_1}^{2}cos(2q_2+q_3)+{\stackrel{·}{q}}_3(2{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_3)cos\left(q_3\right);$

${\Phi }_{2,5}=-{{\stackrel{·}{q}}_1}^{2}cos\left(2q_2\right);$

${\Phi }_{2,6}={\Phi }_{3,6}={{\stackrel{·}{q}}_1}^{2}sin(2q_2+2q_3);$

${\Phi }_{2,7}={\Phi }_{3,7}=-{{\stackrel{·}{q}}_1}^{2}cos(2q_2+2q_3);$

${\Phi }_{2,8}={\Phi }_{3,8}={\stackrel{··}{q}}_{1}sin(q_2+q_3);$

${\Phi }_{2,9}={\Phi }_{3,9}={\stackrel{··}{q}}_{1}cos(q_2+q_3);$

Φ_2,10＝Φ_2,19＝Φ_3,10＝Φ₃₁₉＝-cos(q₁)cos(q₂+q₃)；

${\Phi }_{2,11}={\stackrel{··}{q}}_{1}sin\left(q_2\right);$

${\Phi }_{2,12}={\stackrel{··}{q}}_2;$

${\Phi }_{2,13}={\stackrel{··}{q}}_3;$

Φ_2,16＝cos(q₁)sin(q₂)；

Φ_2,17＝-cos(q₁)cos(q₂)；

Φ_2,18＝Φ_3,18＝cos(q₁)sin(q₂+q₃)；

${\Phi }_{3,2}={\stackrel{··}{q}}_{2}cos\left(q_3\right)+{{\stackrel{·}{q}}_1}^{2}sin(2q_2+q_3)/2+({{\stackrel{·}{q}}_1}^2+2{{\stackrel{·}{q}}_2}^2)sin\left(q_3\right)/2;$

${\Phi }_{3,4}={\stackrel{··}{q}}_{2}sin\left(q_3\right)-{{\stackrel{·}{q}}_1}^{2}cos(2q_2+q_3)/2-({{\stackrel{·}{q}}_1}^2+2{{\stackrel{·}{q}}_2}^2)cos\left(q_3\right)/2;$

${\Phi }_{3,13}={\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3;$

Φ_1,12＝Φ_1,13＝Φ_2,1＝Φ_2,14＝Φ_2,15＝0；

Φ_3,1＝Φ_3,3＝Φ_3,5＝Φ_3,11＝Φ_3,12＝Φ_3,14＝Φ_3,15＝Φ_3,16＝Φ_3,17＝0；

$P_1^{exo}=0.5(I_{2xx}+I_{3xx}+2I_{1yy}+I_{2yy}+I_{3yy})+0.5a_2^2{m}_2+0.5(a_2^2+a_3^2)m_3+a_2{m}_2{r}_{2x}+a_3{m}_3{r}_{3x};$

P₂＝a₂a₃m₃+a₂m₃r_3x；

$P_3=-0.5I_{2xx}+0.5I_{2yy}+0.5a_2^2(m_2+m_3)+a_2{m}_2{r}_{2x};$

P₄＝-a₂m₃r_3y；

P₅＝-I_2xy-a₂m₂r_2y；

$P_6=-0.5I_{3xx}+0.5I_{3yy}+0.5a_3^2{m}_3+a_3{m}_3{r}_{3x};$

P₇＝-a₃m₃r_3y-I_3xy；

P₈＝I_3xz+a₃m₃r_3z；

P₉＝I_3yz；

P₁₀＝I_2yz；

P₁₁＝I_2xz+a₂(m₂r_2z+m₃r_3z)；

$P_{12}=I_{2zz}+I_{3zz}+a_2^2{m}_2+(a_2^2+a_3^2)m_3+2a_2{m}_2{r}_{2x}+2a_3{m}_3{r}_{3x};$

$P_{13}=I_{3zz}+a_3^2{m}_3+2a_3{m}_3{r}_{3x},$

P₁₄＝-g(m₁r_1z+m₂r_2z+m₃r_3z)；

P₁₅＝-gm₁r_1x；

P₁₆＝-g(a₂m₂+a₂m₃+m₂r_2x)；

P₁₇＝gm₂r_2y；

P₁₈＝-g(m₃r_3x+a₃m₃)；

P₁₉＝gm₃r_3y；

如上过程能够建立外骨骼式上肢康复机器人的动力学模型。

所述具有人机交互动力学模型的外骨骼式上肢康复机器人，其特征在 于：建立人体上肢动力学模型的具体要求是：

按照与权利要求2所述同样的方法，建立人体上肢动力学模型如式(4)：

${\Phi }_u(q_u,{\stackrel{·}{q}}_u,{\stackrel{··}{q}}_u)P_u={\tau }_u---\left(4\right)$

式(4)中，τ_u是一个3×1的矢量，表示人体上肢各关节的力矩；Φ_u是 一个3×19的矩阵，代表回归变量；P_u是一个19×1的向量，代表动力学模 型的惯性参数；

人体上肢动力学模型与外骨骼式上肢康复机器人动力学模型元素结构 相同，但是参数有别；

人体上肢动力学模型与外骨骼式上肢康复机器人动力学模型之间的区 别除了数值不同以外，人体上肢肩部外展/内收自由度并没有连接连杆，即 i＝1时的惯性参数F₁为零；Φ_u与P_u中的元素也能够分别列写出来。

所述具有人机交互动力学模型的外骨骼式上肢康复机器人，其特征在 于：如图4所示为外骨骼式上肢康复机器人人机交互动力学模型示意图， 从图中可以看出：外骨骼式上肢康复机器人“穿戴”在人体上肢上对人体 上肢起到固定和支撑的作用；由于同时忽略了腕部以下关节的影响，外骨 骼式上肢康复机器人与人体上肢都视为具有三个自由度的机械结构；而外 骨骼式上肢康复机器人的连杆长度可调，每个关节都与人体上肢同轴运动， 他们的旋转中心重合；因此，他们的运动学方程与雅可比矩阵相同，动力 学方程结构也相同，只是参数有别；

把外骨骼式上肢康复机器人的动力学模型与人体上肢的动力学模型叠 加，便是外骨骼式上肢康复机器人人机交互动力学模型；

建立外骨骼式上肢康复机器人人机交互动力学模型的具体要求是：将 外骨骼机器人动力学模型与人体上肢动力学模型相加，即可得到外骨骼式 上肢康复机器人人机交互模型如式(5)：

$J_{exo}^T{J}_u^{-T}{\Phi }_u(q_u,{\stackrel{·}{q}}_u,{\stackrel{··}{q}}_u)P_u+{\Phi }_{exo}(q_{exo},{\stackrel{·}{q}}_{exo},{\stackrel{··}{q}}_{exo})P_{exo}={\tau }_m---\left(5\right)$

式(5)中，τ_m为外骨骼与人体上肢在各个关节处的合力矩，即各个关 节处力矩传感器测量得到的力矩值；l_exo和l_u分别是外骨骼与人体上肢的雅 可比矩阵，即：

$\begin{array}{cc}{J}_{exo}=\left(\begin{array}{ccc}{J}_{1,1}^{exo}& J_{1,2}^{exo}& J_{1,3}^{exo}\\ J_{2,1}^{exo}& J_{2,2}^{exo}& J_{2,3}^{exo}\\ J_{3,1}^{exo}& J_{3,2}^{exo}& J_{3,3}^{exo}\\ J_{4,1}^{exo}& J_{4,2}^{exo}& J_{4,3}^{exo}\\ J_{5,1}^{exo}& J_{5,2}^{exo}& J_{5,3}^{exo}\\ J_{6,1}^{exo}& J_{6,2}^{exo}& J_{6,3}^{exo}\end{array}\right)& J_u=\left(\begin{array}{ccc}{J}_{1,1}^u& J_{1,2}^u& J_{1,3}^u\\ J_{2,1}^u& J_{2,2}^u& J_{2,3}^u\\ J_{3,1}^u& J_{3,2}^u& J_{3,3}^u\\ J_{4,1}^u& J_{4,2}^u& J_{4,3}^u\\ J_{5,1}^u& J_{5,2}^u& J_{5,3}^u\\ J_{6,1}^u& J_{6,2}^u& J_{6,3}^u\end{array}\right)\end{array}---\left(6\right)$

式(6)中：

$J_{1,1}^{exo}=-cos\left(q_1\right)\left(a_2^{exo}cos\right(q_2)+a_3^{exo}cos(q_2+q_3\left)\right),$

$J_{1,2}^{exo}=sin\left(q_1\right)\left(a_2^{exo}sin\right(q_2)+a_3^{exo}sin(q_2+q_3\left)\right),$

$J_{1,3}^{exo}=a_3^{exo}sin\left(q_1\right)sin(q_2+q_3),$

$J_{2,1}^{exo}=-sin\left(q_1\right)\left(a_2^{exo}cos\right(q_2)+a_3^{exo}cos(q_2+q_3\left)\right),$

$J_{2,2}^{exo}=-cos\left(q_1\right)\left(a_2^{exo}\sin \right(q_2)+a_3^{exo}sin(q_2+q_3\left)\right),$

$J_{2,3}^{exo}=-a_3^{exo}cos\left(q_1\right)sin(q_2+q_3),$

$J_{3,1}^{exo}=0,$

$J_{3,2}^{exo}=-a_2^{exo}cos\left(q_2\right)-a_3^{exo}cos(q_2+q_3),$

$J_{3,3}^{exo}=-a_3^{exo}cos(q_2+q_3),$

$J_{4,1}^{exo}=0,$

$J_{4,2}^{exo}=-cos\left(q_1\right),$

$J_{4,3}^{exo}=-cos\left(q_1\right),$

$J_{5,1}^{exo}=0,$

$J_{5,2}^{exo}=-sin\left(q_1\right),$

$J_{5,3}^{exo}=-sin\left(q_1\right),$

$J_{6,1}^{exo}=1,$

$J_{6,2}^{exo}=0,$

$J_{6,3}^{exo}=0.$

$J_{1,1}^u=-cos\left(q_1\right)\left(a_2^{u}cos\right(q_2)+a_3^{u}cos(q_2+q_3\left)\right),$

$J_{1,2}^u=sin\left(q_1\right)\left(a_2^{u}sin\right(q_2)+a_3^{u}sin(q_2+q_3\left)\right),$

$J_{1,3}^u=a_2^{u}sin\left(q_1\right)sin(q_2+q_3),$

$J_{2,1}^u=-sin\left(q_1\right)\left(a_2^{u}cos\right(q_2)+a_3^{u}cos(q_2+q_3\left)\right),$

$J_{2,2}^u=-cos\left(q_1\right)\left(a_2^{u}sin\right(q_2)+a_3^{u}sin(q_2+q_3\left)\right),$

$J_{2,3}^u=-a_3^{u}cos\left(q_1\right)sin(q_2+q_3),$

$J_{3,1}^u=0,$

$J_{3,2}^u=-a_2^{u}cos\left(q_2\right)-a_3^{u}cos(q_2+q_3),$

$J_{3,3}^u=-a_3^{u}cos(q_2+q_3),$

$J_{4,1}^u=0,$

$J_{4,2}^u=-cos\left(q_1\right),$

$J_{4,3}^u=-cos\left(q_1\right),$

$J_{5,1}^u=0,$

$J_{5,2}^u=-sin\left(q_1\right),$

$J_{5,3}^u=-sin\left(q_1\right),$

$J_{6,1}^u=1,$

$J_{6,2}^u=0,$

如前所述，由于外骨骼式上肢康复机器人机械臂连杆长度可调，能够 调节成与人体上肢相同长度，即有l_exo＝J_u；根据上文Φ_u＝Φ_exo，式(5)能 够整理如下：

$\Phi (q,\stackrel{·}{q},\stackrel{··}{q})P={\tau }_m---\left(7\right)$

式(7)中Φ＝Φ_u＝Φ_exo，P＝P_u+P_exo；

为便于验证，写成状态方程的形式：

$\stackrel{··}{q}=M{(q,P)}^{-1}({\tau }_m-C(q,\stackrel{·}{q},P)\stackrel{·}{q}-G(q,P\left)\right)---\left(8\right)$

式(8)中，

M＝[P₁+P₂(cos(2q₂+q₃)+cos(q₃))+P₃cos(2q₂)+P₄(sin(2q₂+q₃)+sin(q₃))+P₅sin(2q₂)+P₆cos(2q₂+2q₃)

+P₇sin(2q₂+2q₃)P₈sin(q₂+q₃)+P₉cos(q₂+q₃)+P₁₀cos(q₂)+P₁₁sin(q₂),P₈sin(q₂+q₃)+P₉cos(q₂+q₃)；

P₈sin(q₂+q₃)+P₉cos(q₂+q₃)+P₁₀cos(q₂)+P₁₁sin(q₂),P₁₂+2P₂cos(q₃)+2P₄sin(q₃),P₁₃+P₂cos(q₃)+P₄sin(q₃)；

P₈sin(q₂+q₃)+P₉cos(q₂+q₃),P₁₃+P₂cos(q₃)+P₄sin(q3),P₁₃]

C＝[-P₆(dq₂+dq₃)sin(2q₂+2q₃)+P₄dq₃/2cos(q₃)+P₇(dq₂+dq₃)cos(2q₂+2q₃)

-P₂dq₃/2sin(q₃)+P₄(dq₂+dq₃/2)cos(2q₂+q₃)-P₂(dq₂+dq₃/2)sin(2q₂+q₃)

+P₅dq₂cos(2q₂)-P₃dq₂sin(2q₂),-P₆dq₁sin(2q₂+2q₃)+P₁₁dq₂cos(q₂)

+P₇dq₁cos(2q₂+2q₃)+P₄dq₁cos(2q₂+q₃)-P₂dq₁sin(2q₂+q₃)+P₅dq₁cos(2q₂)

-P₃dq₁sin(2q₂)+P₈(dq₂+dq₃)cos(q₂+q₃)-P₉(dq₂+dq₃)sin(q₂+q₃)-P₁₀dq₂sin(q₂),

-P₆dq₁sin(2q₂+2q₃)+P₄dq₁/2cos(q₃)+P₇dq₁cos(2q₂+2q₃)-P₂dq₁/2sin(q₃)

+P₄dq₁/2cos(2q₂+q₃)-P₂dq₁/2sin(2q₂+q₃)+P₈(dq₂+dq₃)cos(q₂+q₃)-P₉(dq₂+dq₃)sin(q₂+q₃)；

dq₁(-P₄cos(2q₂+q₃)-P₅cos(2q₂)+P₂sin(2q₂+q₃)+P3sin(2q₂)-P₇cos(2q₂+2q₃)+P₆sin(2q₂+2q₃)),

dq₃(P₄cos(q₃)-P₂sin(q₃)),(dq₂+dq₃)(P₄cos(q₃)-P₂sin(q₃))；dq₁(-P₄/2cos(2q₂+q₃)

+P₂/2sin(2q₂+q₃)-P₇cos(2q₂+2q₃)+P₆sin(2q₂+2q₃)-P₄/2cos(q₃)+P₂/2sin(q₃)),

-dq₂(P₄cos(q₃)-P₂sin(q₃)),0]

G＝[P₁₄cos(q₁)+P₁₅sin(q₁)+P₁₆sin(q₁)cos(q₂)+P₁₇sin(q₁)sin(q₂)+P₁₈(cos(q₂)cos(q₃)

-sin(q₂)sin(q₃))sin(q₁)+P₁₉sin(q₁)(sin(q₂)cos(q₃)+sin(q₃)cos(q₂))；

cos(q₁)(-P₁₉cos(q₂+q₃)+P₁₈sin(q₂+q₃)-P₁₇cos(q₂)+P₁₆sin(q₂))；

cos(q₁)(-P₁₉cos(q₂+q₃)+P₁₈sin(q₂+q₃))]

本发明所述具有人机交互动力学模型的外骨骼式上肢康复机器人，其 特征在于：将数学模型与仿真模型进行对比验证模型准确性，同时表明该 机器人可根据关节力矩估计值与实测力矩值的比较判断患者的运动意图；

具体要求如下：

忽略零件影响保留结构特征，建立外骨骼上肢康复机器人人机交互三 维样机如图4所示，并分别建立外骨骼机械臂与人体上肢的图示化模型； 将两模型输出各关节力矩相加就是安装在外骨骼机械臂各关节力矩传感器 测得的实际力矩值，设定好其它输入输出则完成了外骨骼式上肢康复机器 人人机交互动态系统建模；采用工具软件子系统封装技术，得到的上肢康 复外骨骼人机动交互力学仿真模型如图5所示；

图5中，其中：TR为系统输入力矩，TM为安装在外骨骼各个关节的力 矩传感器测量得到的力矩值，输出的是各个关节的运动状态。通过给 定相同的力矩值，对比数学模型与仿真模型输出的各个关节的运动变化， 可以验证建立的外骨骼式上肢康复机器人人机交互动力学模型的准确性。

本发明提出了一种具有人机交互动力学模型的外骨骼式上肢康复机器 人，使之能够准确识别出患者上肢的运动意图。其核心思想是：首先在康 复训练中将人体上肢看作具有三个自由度的二连杆机械臂，其中：肩部两 个自由度，肘部一个自由度；然后将人体动力学模型与外骨骼式上肢康复 机器人的动力学模型相结合，形成外骨骼式上肢康复机器人人机交互动力 学模型；其中，关节角度与力矩值由安装在外骨骼机械臂中活动关节上的 角度与力矩传感器测量所得；所述外骨骼式上肢康复机器人人机交互动力 学模型采用了基于伪惯量矩阵的拉格朗日动力学建模方法，通过整合化简 得到了一个具有19个参数的人机交互动力学模型；通过建立机器人系统的 图示化模型并与控制器及其动态系统相连进行综合仿真比较，表明运用该 方法建立的数学模型能够在一定程度上体现外骨骼机器人与人体之间的动 力学交互关系；确定控制参数根据关节力矩估计值与实测力矩值的比较判 断患者的运动意图；

本发明相关知识内容补充说明如下：

本发明采用安装在外骨骼康复机械各关节处的位置与力矩传感器来辨 识人体上肢动力学参数并判断运动意向，减少了外部设备的使用，避免了 外界因素的干扰，采用的数据稳定可靠，辨识与判断的结果更加准确。图2 所示为在主动康复训练过程中通过人机交互模型判断人体上肢运动意图结 构图。图中测得各个关节的角度与力矩值都是通过安装在外骨骼各关节上 的传感器测量得到。

附图说明

下面结合附图及实施方式对本发明作进一步详细的说明：

图1为外骨骼式上肢康复机器人康复训练系统；

图2为主动康复训练判断人体上肢运动意图结构图；

图3为外骨骼式上肢康复机器人D-H坐标变换关系图；

图4为外骨骼式上肢康复机器人人机三维模型示意图；

图5为外骨骼式上肢康复机器人人机交互动力学图示化仿真模型结构图；

图6为关节1图示化仿真模型与数学模型轨迹仿真结果对比图；

图7为关节2图示化仿真模型与数学模型轨迹仿真结果对比图；

图8为关节3图示化仿真模型与数学模型轨迹仿真结果对比图。

具体实施方式

实施例1

一种具有人机交互动力学模型的外骨骼式上肢康复机器人，其关键要 点简要说明如下：首先在康复训练中将人体上肢看作具有三个自由度的二 连杆机械臂，其中：肩部两个自由度，肘部一个自由度；之后将其动力学 模型与外骨骼康复机器人动力学模型联合，形成外骨骼式上肢康复机器人 人机交互动力学模型；其中：人体上肢的关节角度与力矩通过安装在外骨 骼活动关节上的角度与力矩传感器间接测量得到；所述外骨骼式上肢康复 机器人人机交互动力学模型具体是基于伪惯量矩阵采用拉格朗日算法的具 有19个参数的人机交互动力学模型；通过建立机器人系统的图示化模型并 与控制器及其动态系统相连进行综合仿真比较，表明运用该方法建立的数 学模型能够在一定程度上体现外骨骼机器人与人体之间的动力学交互关 系；建模之后再确定控制参数根据关节力矩估计值与实测力矩值的比较判 断患者的运动意图

从图2中可以看出，当外骨骼式上肢康复机器人各关节力矩传感器测 得的力矩实际值T_m与人机交互模型计算出的各关节角度力矩估算值T_c相比 较所得偏差值d_T小于0时，表明人体上肢向此关节运动正方向施加了力矩， 人体上肢的运动意图为向正向旋转；反之，如果偏差值d_T大于0时，则表 明人体上肢向此关节运动反方向施加了力矩，人体上肢的运动意图为向反 向旋转；而当偏差值d_T等于0时，表明人体上肢没有施加任何力矩，没有 运动意图。

在实际应用中，设定一定的阈值，用以调节判断运动意图的灵敏度。还 可以对d_T值进行一定的处理，用以生成控制外骨骼运动的指令C。

具体内容要求现顺序说明如下：

一、外骨骼式上肢康复机器人与人体上肢的动力学模型建立

1、所述建立外骨骼式上肢康复机器人的动力学模型的具体要求是：

根据D-H参数法建立外骨骼式上肢康复机器人坐标变换关系(如图3 所示)，并由此获得坐标变换D-H参数表(如表1所示)；

表1外骨骼式上肢康复机器人坐标变换D-H参数表

利用拉格朗日方法忽略摩擦力的影响建立外骨骼式上肢康复机器人动 力学模型，模型的标准形式如式(1)所示：

$\tau =M\left(q\right)\stackrel{··}{q}+C(q,\stackrel{·}{q})+G\left(q\right)---\left(1\right)$

式(1)中分别表示关节i的角度、角速度和角加速度；M(q)是一 个3×3的惯性矩阵；为3×1的非线性哥氏力和向心力矢量；G(q)为 3×1的重力矢量；τ是一个3×1的关节控制输入力矩矢量；

M(q)、和G(q)的元素包含了各相关杆件的惯性参数，即刚体的十 个常量，列于式(2)所示：

F_i＝[m_i,I_xx,i,I_yy,i,I_zz,i,I_xy,i,I_xz,i,I_yz,i,x_i,y_i,z_i](i＝1,2,3)(2)

式(2)中，m_i为连杆i的质量，I_xx，i,I_yy，i,I_zz，i为相对于坐标系{i}的三维 质量惯性矩，I_xy，i,I_xz，i,I_yz，i为相对于坐标系{i}的三维质量惯性积，x_i,y_i,z_i为 连杆质心的三维坐标；

将式(1)所示外骨骼康复机器人动力学模型用式(2)所示各杆件的惯 性参数表示，在对其进行非线性组合并整理归纳将模型转化为如式(3)所 示的线性形式：

${\Phi }_{exo}(q_{exo},{\stackrel{·}{q}}_{exo},{\stackrel{··}{q}}_{exo})P_{exo}={\tau }_{exo}---\left(3\right)$

式(3)中，τ_exo是一个3×1的矢量，表示外骨骼各关节的力矩；Φ_exo是 一个3×19的矩阵，代表回归变量；P_exo是一个19×1的矢量，代表动力学模 型的惯性参数；

Φ_exo与P_exo中的元素分别定义如下：

${\Phi }_{1,1}^{exo}={\stackrel{··}{q}}_1;$

${\Phi }_{1,2}={\stackrel{··}{q}}_1\left(\cos \right(2q_2+q_3)+\cos (q_3\left)\right)-(3{\stackrel{·}{q}}_1{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3)\sin (2q_2+q_3)-{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3\sin \left(q_3\right);$

${\Phi }_{1,3}={\stackrel{··}{q}}_{1}cos\left(2q_2\right)-2{\stackrel{·}{q}}_1{\stackrel{·}{q}}_{2}sin\left(2q_2\right);$

${\Phi }_{1,4}={\stackrel{··}{q}}_1\left(sin\right(2q_2+q_3)+sin(q_3\left)\right)+(2{\stackrel{·}{q}}_1{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3)cos(2q_2+q_3)+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_{3}cos\left(q_3\right);$

${\Phi }_{1,5}={\stackrel{··}{q}}_{1}sin\left(2q_2\right)+2{\stackrel{·}{q}}_1{\stackrel{·}{q}}_{2}cos\left(2q_2\right);$

${\Phi }_{1,6}={\stackrel{··}{q}}_{1}cos(2q_2+2q_3)-2({\stackrel{·}{q}}_1{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3)sin(2q_2+2q_3);$

${\Phi }_{1,7}={\stackrel{··}{q}}_{1}sin(2q_2+2q_3)+2({\stackrel{·}{q}}_1{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_1{\stackrel{·}{q}}_3)cos(2q_2+2q_3);$

${\Phi }_{1,8}=({\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3)sin(q_2+q_3)+{({\stackrel{·}{q}}_2+{\stackrel{·}{q}}_3)}^{2}cos(q_2+q_3);$

${\Phi }_{1,9}=({\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3)cos(q_2+q_3)-{({\stackrel{·}{q}}_2+{\stackrel{·}{q}}_3)}^{2}sin(q_2+q_3);$

Φ_1,10＝Φ_1,19＝sin(q₁)sin(q₂+q₃)；

${\Phi }_{1,11}={\stackrel{··}{q}}_{2}sin\left(q_2\right)+{{\stackrel{·}{q}}_2}^{2}cos\left(q_2\right);$

Φ_1,14＝cos(q₁)；

Φ_1,15＝sin(q₁)；

Φ_1,16＝cos(q₂)sin(q₁)；

Φ_1,17＝sin(q₁)sin(q₂)；

Φ_1,18＝sin(q₁)cos(q₂+q₃)；

${\Phi }_{2,2}=(2{\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3)cos\left(q_3\right)+{{\stackrel{·}{q}}_1}^{2}sin(2q_2+q_3)-{\stackrel{·}{q}}_3(2{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_3)sin\left(q_3\right);$

${\Phi }_{2,3}={{\stackrel{·}{q}}_1}^{2}sin\left(2q_2\right);$

${\Phi }_{2,4}=(2{\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3)sin\left(q_3\right)-{{\stackrel{·}{q}}_1}^{2}cos(2q_2+q_3)+{\stackrel{·}{q}}_3(2{\stackrel{·}{q}}_2+{\stackrel{·}{q}}_3)cos\left(q_3\right);$

${\Phi }_{2,5}=-{{\stackrel{·}{q}}_1}^{2}cos\left(2q_2\right);$

${\Phi }_{2,6}={\Phi }_{3,6}={{\stackrel{·}{q}}_1}^{2}sin(2q_2+2q_3);$

${\Phi }_{2,7}={\Phi }_{3,7}=-{{\stackrel{·}{q}}_1}^{2}cos(2q_2+2q_3);$

${\Phi }_{2,8}={\Phi }_{3,8}={\stackrel{··}{q}}_{1}sin(q_2+q_3);$

${\Phi }_{2,9}={\Phi }_{3,9}={\stackrel{··}{q}}_{1}cos(q_2+q_3);$

Φ_2,10＝Φ_2,19＝Φ_3,10＝Φ₃₁₉＝-cos(q₁)cos(q₂+q₃)；

${\Phi }_{2,11}={\stackrel{··}{q}}_{1}sin\left(q_2\right);$

${\Phi }_{2,12}={\stackrel{··}{q}}_2;$

${\Phi }_{2,13}={\stackrel{··}{q}}_3;$

Φ_2,16＝cos(q₁)sin(q₂)；

Φ_2,17＝-cos(q₁)cos(q₂)；

Φ_2,18＝Φ_3,18＝cos(q₁)sin(q₂+q₃)；

${\Phi }_{3,2}={\stackrel{··}{q}}_{2}cos\left(q_3\right)+{{\stackrel{·}{q}}_1}^{2}sin(2q_2+q_3)/2+({{\stackrel{·}{q}}_1}^2+2{{\stackrel{·}{q}}_2}^2)sin\left(q_3\right)/2;$

${\Phi }_{3,4}={\stackrel{··}{q}}_{2}sin\left(q_3\right)-{{\stackrel{·}{q}}_1}^{2}cos(2q_2+q_3)/2-({{\stackrel{·}{q}}_1}^2+2{{\stackrel{·}{q}}_2}^2)cos\left(q_3\right)/2;$

${\Phi }_{3,13}={\stackrel{··}{q}}_2+{\stackrel{··}{q}}_3;$

Φ_1,12＝Φ_1,13＝Φ_2,1＝Φ_2,14＝Φ_2,15＝0；

Φ_3,1＝Φ_3,3＝Φ_3,5＝Φ_3,11＝Φ_3,12＝Φ_3,14＝Φ_3,15＝Φ_3,16＝Φ_3,17＝0；

$P_1^{exo}=0.5(I_{2xx}+I_{3xx}+2I_{1yy}+I_{2yy}+I_{3yy})+0.5a_2^2{m}_2+0.5(a_2^2+a_3^2)m_3+a_2{m}_2{r}_{2x}+a_3{m}_3{r}_{3x};$

P₂＝a₂a₃m₃+a₂m₃r_3x；

$P_3=-0.5I_{2xx}+0.5I_{2yy}+0.5a_2^2(m_2+m_3)+a_2{m}_2{r}_{2x};$

P₄＝-a₂m₃r_3y；

P₅＝-I_2xy-a₂m₂r_2y；

$P_6=-0.5I_{3xx}+0.5I_{3yy}+0.5a_3^2{m}_3+a_3{m}_3{r}_{3x};$

P₇＝-a₃m₃r_3y-I_3xy；

P₈＝I_3xz+a₃m₃r_3z；

P₉＝I_3yz；

P₁₀＝I_2yz；

P₁₁＝I_2xz+a₂(m₂r_2z+m₃r_3z)；

$P_{12}=I_{2zz}+I_{3zz}+a_2^2{m}_2+(a_2^2+a_3^2)m_3+2a_2{m}_2{r}_{2x}+2a_3{m}_3{r}_{3x};$

$P_{13}=I_{3zz}+a_3^2{m}_3+2a_3{m}_3{r}_{3x};$

P₁₄＝-g(m₁r_1z+m₂r_2z+m₃r_3z)；

P₁₅＝-gm₁r_1x；

P₁₆＝-g(a₂m₂+a₂m₃+m₂r_2x)；

P₁₇＝gm₂r_2y；

P₁₈＝-g(m₃r_3x+a₃m₃)；

P₁₉＝gm₃r_3y；

2、建立人体上肢动力学模型的具体要求是：

按照与权利要求2所述同样的方法，建立人体上肢动力学模型如式(4)：

${\Phi }_u(q_u,{\stackrel{·}{q}}_u,{\stackrel{··}{q}}_u)P_u={\tau }_u---\left(4\right)$

式(4)中，τ_u是一个3×1的矢量，表示人体上肢各关节的力矩；Φ_u是 一个3×19的矩阵，代表回归变量；P_u是一个19×1的向量，代表动力学模 型的惯性参数；

人体上肢动力学模型与外骨骼式上肢康复机器人动力学模型元素结构 相同，但是参数有别；

人体上肢动力学模型与外骨骼式上肢康复机器人动力学模型之间的区 别除了数值不同以外，人体上肢肩部外展/内收自由度并没有连接连杆，即 i＝1时的惯性参数F₁为零；Φ_u与P_u中的元素也能够分别列写出来。

二、外骨骼式上肢康复机器人人机交互动力学模型的建立

1、外骨骼式上肢康复机器人人机交互动力学模型建立

所述具有人机交互动力学模型的外骨骼式上肢康复机器人，其特征在 于：如图4所示为外骨骼式上肢康复机器人人机交互动力学模型示意图， 从图中可以看出：外骨骼式上肢康复机器人“穿戴”在人体上肢上以便对 人体上肢起到固定和支撑的作用；由于同时忽略了腕部以下关节的影响， 外骨骼机器人结构与人体上肢都视为具有三个自由度的机器人；而外骨骼 机械臂连杆长度可调，每个关节都与人体上肢同轴运动，他们的旋转中心 重合；因此，他们的运动学方程与雅可比矩阵相同，动力学方程结构也相 同，只是参数有别；

把外骨骼式上肢康复机器人动力学模型与人体上肢动力学模型叠加， 便是上肢康复外骨骼式康复机器人人机交互动力学模型；

建立外骨骼式上肢康复机器人人机交互动力学模型的具体要求是：将 外骨骼式上肢康复机器人动力学模型与人体上肢动力学模型相加，即可得 到外骨骼式上肢康复机器人人机交互动力学模型如式(5)：

$J_{exo}^T{J}_u^{-T}{\Phi }_u(q_u,{\stackrel{·}{q}}_u{\stackrel{··}{q}}_u)P_u+{\Phi }_{exo}(q_{exo},{\stackrel{·}{q}}_{exo}{\stackrel{··}{q}}_{exo})P_{exo}={\tau }_m---\left(5\right)$

式(5)中，τ_m为外骨骼式上肢康复机器人与人体上肢在各个关节处的 合力矩，即各个关节处力矩传感器测量得到的力矩值；l_exo和l_u分别是外骨 骼式上肢康复机器人与人体上肢的雅可比矩阵，即：

$\begin{array}{cc}{J}_{exo}=\left(\begin{array}{ccc}{J}_{1,1}^{exo}& J_{1,2}^{exo}& J_{1,3}^{exo}\\ J_{2,1}^{exo}& J_{2,2}^{exo}& J_{2,3}^{exo}\\ J_{3,1}^{exo}& J_{3,2}^{exo}& J_{3,3}^{exo}\\ J_{4,1}^{exo}& J_{4,2}^{exo}& J_{4,3}^{exo}\\ J_{5,1}^{exo}& J_{5,2}^{exo}& J_{5,3}^{exo}\\ J_{6,1}^{exo}& J_{6,2}^{exo}& J_{6,3}^{exo}\end{array}\right)& J_u=\left(\begin{array}{ccc}{J}_{1,1}^u& J_{1,2}^u& J_{1,3}^u\\ J_{2,1}^u& J_{2,2}^u& J_{2,3}^u\\ J_{3,1}^u& J_{3,2}^u& J_{3,3}^u\\ J_{4,1}^u& J_{4,2}^u& J_{4,3}^u\\ J_{5,1}^u& J_{5,2}^u& J_{5,3}^u\\ J_{6,1}^u& J_{6,2}^u& J_{6,3}^u\end{array}\right)\end{array}---\left(6\right)$

式(6)中：

$J_{1,1}^{exo}=-cos\left(q_1\right)\left(a_2^{exo}cos\right(q_2)+a_3^{exo}cos(q_2+q_3\left)\right),$

$J_{1,2}^{exo}=sin\left(q_1\right)\left(a_2^{exo}sin\right(q_2)+a_3^{exo}sin(q_2+q_3\left)\right),$

$J_{1,3}^{exo}=a_3^{exo}sin\left(q_1\right)sin(q_2+q_3),$

$J_{2,1}^{exo}=-sin\left(q_1\right)\left(a_2^{exo}cos\right(q_2)+a_3^{exo}cos(q_2+q_3\left)\right),$

$J_{2,2}^{exo}=-cos\left(q_1\right)\left(a_2^{exo}sin\right(q_2)+a_3^{exo}sin(q_2+q_3\left)\right),$

$J_{2,3}^{exo}=-a_3^{exo}cos\left(q_1\right)sin(q_2+q_3),$

$J_{3,1}^{exo}=0,$

$J_{3,2}^{exo}=-a_2^{exo}cos\left(q_2\right)-a_3^{exo}cos(q_2+q_3),$

$J_{3,3}^{exo}=-a_3^{exo}cos(q_2+q_3),$

$J_{4,1}^{exo}=0,$

$J_{4,2}^{exo}=-cos\left(q_1\right),$

$J_{4,3}^{exo}=-cos\left(q_1\right),$

$J_{5,1}^{exo}=0,$

$J_{5,2}^{exo}=-sin\left(q_1\right),$

$J_{5,3}^{exo}=-sin\left(q_1\right),$

$J_{6,1}^{exo}=1,$

$J_{6,2}^{exo}=0,$