[发明专利]可穿戴机器人和人体的单元化联合建模方法、系统及介质

权利要求书

1.一种可穿戴机器人和人体的单元化联合建模方法，其特征在于，所述方法包括：

通过人体身体部位划分人体单元，对可穿戴机器人划分单元，并设置每个单元包括的参数；

定义各单元之间传递点的连接，获得人体和可穿戴机器人联合的计算模型；

通过计算模型接收各单元的姿态参数，将每个单元计算初始化；

遍历每个单元，计算每个单元的运动学和动力学；

若所有单元的运动学和动力学计算均完成，则保存所有的计算结果；

若有新的姿态数据输入计算模型，则返回通过计算模型接收各单元的姿态参数，将单元计算初始化，并执行后续操作；

若无新的姿态数据输入计算模型，则结束计算，输出所有的计算结果；

所述计算每个单元的运动学和动力学中，计算每个单元的运动学，具体为：

对于每个单元的运动学计算，若该单元的姿态参数已知，并且有一个或一个以上的传递点的运动学已知，则根据下面的公式计算该单元未知传递点的运动学：

p_i＝p_k+R(θ)(h_i-h_k)

v_i＝v_k+ω×R(θ)(h_i-h_k)

a_i＝a_k+ω×(ω×R(θ)(h_i-h_k))+α×R(θ)(h_i-h_k)

其中，i为某个未知传递点，k为某个已知传递点，R为从地面到该单元的旋转矩阵，h_i为该未知传递点的单元坐标系位置，h_k为该已知传递点的单元坐标系位置，p_i为该未知传递点的地面位置，p_k为该已知传递点的地面位置，v_i为该未知传递点的地面速度，v_k为该已知传递点的地面速度，a_i为该未知传递点的地面加速度，a_k为该已知传递点的地面加速度，θ为该单元角度，ω为该单元的角速度，α为该单元的角加速度；

对于存在连接关系的两个传递点，若第一个传递点的运动学已知，且对应连接的第二传递点的运动学未知，则第一个传递点向对应连接的第二传递点进行传递的运动学公式如下：

[p₂ v₂ a₂]＝[p₁ v₁ a₁]

其中，p₁为第一个传递点地面位置，p₂为第二个传递点地面位置，v₁为第一个传递点地面速度，v₂为第二个传递点地面速度，a₁为第一个传递点地面加速度，a₂为第二个传递点地面加速度；

所述计算每个单元的运动学和动力学中，计算每个单元的动力学，具体为：

对于每个单元的动力学计算，若某该单元内所有传递点的运动学已知，并且除了某个传递点外，所有其他传递点的动力学已知，则将该传递点记为未知传递点，根据下面的公式计算该未知传递点的动力学：

其中，o为该未知传递点，f_o为该未知传递点的力，n_o为该未知传递点的力矩，g为重力加速度矢量，-y轴方向，m为该单元的质量，i为某个已知传递点，h_i为该已知传递点的单元坐标系单元坐标系位置，f_i为该已知传递点的力，I为该单元的转动惯量，θ为该单元的角度，ω为该单元的角速度，α为该单元的角加速度；a_cm为质心线加速度，h_cm为质心的单元坐标系位置；

对于存在连接关系的两个传递点，若第一个传递点的动力学已知，且对应连接的第二传递点的动力学未知，则第一个传递点向对应连接的第二传递点进行传递的动力学公式如下：

[f₂ n₂]＝[-f₁ -n₁]

其中，f₁为第一个传递点的力，f₂为第二个传递点的力，n₁为第一个传递点的力矩，n₂为第二个传递点的力矩。