[发明专利]基于轴不变量的闭链机器人动力学建模与解算方法

权利要求书

1.一种基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，

定义自然坐标系：若多轴系统D处于零位，所有笛卡尔体坐标系方向一致，且体坐标系原点位于运动轴的轴线上，则该坐标系统为自然坐标系；

在系统处于零位时，所有杆件的自然坐标系与底座或世界系的方向一致；系统处于零位即角位置时，自然坐标系绕轴矢量转动角度将转至F^[l]；在下的坐标矢量与在F^[l]下的坐标矢量恒等，即有

或不依赖于相邻的坐标系及F^[l]；称或为轴不变量；

约定：表示属性占位；

为投影符，表示矢量或二阶张量对参考基的投影矢量或投影序列，即坐标矢量或坐标阵列；则，位置矢量在坐标系F^[k]中的投影矢量记为

为叉乘符；则，是轴不变量的叉乘矩阵；

给定多轴刚体系统D＝A,K,T,NT,F,B，惯性系记为F^[i]，A为轴序列，F为杆件参考系序列，B为杆件体序列，K为运动副类型序列，NT为约束轴的序列即非树；除了重力外，作用于轴u的合外力及力矩在上的分量分别记为及轴k的质量及质心转动惯量分别记为m_k及轴k的重力加速度为驱动轴u的双边驱动力及驱动力矩在上的分量分别记为及环境i对轴l的作用力及作用力矩分别为及ⁱτ_l；轴u对轴u′的广义约束力记为则闭链刚体系统的Ju-Kane动力学方程为：

【1】轴u及轴u′的Ju-Kane动力学规范方程为

其中：及是3×3的分块矩阵，及是3D矢量；

【2】非树约束副^uk_u′的约束代数方程为

其中：

式中：及是3×3的分块矩阵，及是3D矢量；k_I表示杆k质心I；轴k的质量及质心转动惯量分别记为m_k及为转动轴u的惯性矩阵；为平动轴u的惯性矩阵；h_R为转动轴u的非惯性矩阵；h_P为平动轴u的非惯性矩阵；为平动关节角速度；为转动关节角速度；P、R均是表示运动副，R为转动副，P为棱柱副。

2.根据权利要求1所述的基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，

基于轴不变量的约束力求解步骤为：

对于无功率损耗的运动轴u，记其约束力及约束力矩矢量分别为则有

上式表示运动轴矢量与运动轴约束力具有自然正交补的关系；

若及为运动副的两个正交约束轴，且约束轴与运动轴正交，即

记为约束轴轴矢量，得

其中：

根据关节加速度由式(130)得到关节约束力大小约束力矩大小当时，由式(130)得且式(130)中同一时刻具有相同的运动状态及内外力；仅在运动轴向上出现力及力矩的平衡；而在约束轴向，动力学方程不满足，即力与力矩不一定平衡；

由式(130)可以得到关节约束力大小及约束力矩大小及若记运动轴径向力矢量及力矩矢量则有

若记运动轴径向力大小为及力矩大小为由式(133)得

至此，完成了轴径向约束广义力的计算。

3.根据权利要求2所述的基于轴不变量的闭链机器人动力学建模与解算方法，其特征是，

由式(130)至式(134)计算运动轴u的径向约束力大小及约束力矩大小时，不考虑运动轴的广义内摩擦力及粘滞力。