一种少自由度闭链空间机构运动解算方法技术

本发明专利技术涉及一种少自由度闭链空间机构运动解算方法，包括如下步骤：步骤一：对机构进行运动学分析，建立主开链机构模型和从开链机构模型，并描述各个部件之间的位置关系；步骤二：进行耦合运动学分析，建立主开链机构末端和从开链机构末端位姿之间的耦合运动关系式；步骤三：指定主开链机构期望轨迹，然后对主开链机构进行正运动学解算，获得主开链机构末端位姿；步骤四：获得从开链机构末端位姿，然后进行逆运动学解算获得从开链机构各个关节值；步骤五：使用在线轨迹生成方法进行轨迹规划，然后使用闭环控制方法对少自由度主从式单闭链机构中的各个电机实施位置控制。本发明专利技术实现了少自由度主从式单闭链机构的实时运动解算，并实现同步运动控制。现同步运动控制。现同步运动控制。

【技术实现步骤摘要】
一种少自由度闭链空间机构运动解算方法


[0001]本专利技术涉及机构运动控制领域，具体地说是一种少自由度闭链空间机构运动解算方法。

技术介绍

[0002]目前许多场合应用的机械机构都设计成开链式结构，对此类开链式机构的运动学和动力学研究都已趋于成熟，然而实际应用中的机械装置考虑到控制性能、结构刚性、任务需求等原因，经常只需要部分的自由度，例如2～4自由度来满足使用要求，这类自由度少于6的闭链机构被称为少自由闭链空间机构，这类机械结构相比于开链式机械结构具有更大的设计多样性，但可控自由度较少，因此其运动学和静力分析也更加复杂，存在难以解算和精确控制的问题。

技术实现思路

[0003]本专利技术的目的在于提供一种少自由度闭链空间机构运动解算方法，能够实现少自由度主从式单闭链机构的实时运动解算，并通过在线轨迹生成及闭环控制实现少自由度主从式单闭链机构的同步运动控制。
[0004]本专利技术的目的是通过以下技术方案来实现的：
[0005]一种少自由度闭链空间机构运动解算方法，包括如下步骤：
[0006]步骤一：对少自由度主从式单闭链机构进行运动学分析，建立主开链机构模型和从开链机构模型，并描述各个部件之间的位置关系；
[0007]步骤二：对主开链机构末端和从开链机构末端进行耦合运动学分析，建立主开链机构末端和从开链机构末端位姿之间的耦合运动关系式；
[0008]步骤三：指定主开链机构期望轨迹，然后对主开链机构进行正运动学解算，获得主开链机构末端位姿；
[0009]步骤四：依据步骤二中的耦合运动关系式和步骤三中的主开链机构末端位姿，获得从开链机构末端位姿，然后进行逆运动学解算获得从开链机构各个关节值；
[0010]步骤五：使用在线轨迹生成方法进行轨迹规划，然后使用闭环控制方法对少自由度主从式单闭链机构中的各个电机实施位置控制。
[0011]步骤一中，定义固定坐标系w作为世界坐标系，然后测量各部件参数，建立主开链机构各部件坐标系模型和从开链机构各部件坐标系模型，并采用仅需四个参数的Denabit
‑
Hartenberg(DH)方法描述各部件相对位置关系，获得相邻两部件坐标系之间的位姿关系矩阵
[0012][0013]上式(7)中，α
i
‑1表示绕坐标系{i
‑
1}的x轴旋转的角度，a
i
‑1表示沿着坐标系{i
‑
1}的x轴移动的距离，θ
i
表示绕坐标系{i}的z轴旋转的角度，d
i
表示沿着坐标系{i}的z轴移动的距离。
[0014]步骤一中，相邻两部件坐标系之间的位姿变换矩阵表示为：
[0015][0016]上式(2)中，Rot(x，α
i
‑1)表示绕坐标系{i
‑
1}的x轴旋转a
i
‑1角度，Trans(x，a
i
‑1)表示沿着坐标系{i
‑
1}的x轴移动a
i
‑1距离，Rot(z，θ
i
)表示绕坐标系{i}的z轴旋转θ
i
角度，Trans(z，d
i
)表示沿着坐标系{i}的z轴移动d
i
距离，另外空间坐标系的平移和旋转变换矩阵表达式如下：
[0017][0018][0019][0020][0021]将上式(3)
‑
(6)代入式(2)中，获得：
[0022][0023]步骤二中，由于主开链机构末端和从开链机构末端位姿重合，因此：
[0024][0025]上式(8)中，w为世界坐标系，ai为主开链机构第i个坐标系，pi为从开链机构第i个坐标系，T表示一组相邻部件的坐标系位姿变换关系，也即上式(7)。。
[0026]步骤三中，指定主开链机构期望轨迹，即获得主开链机构各关节值θ
ai
，i＝1，2，3...，代入下式进行正运动学解算：
[0027][0028]获得主开链机构末端相对于世界坐标系的位姿
[0029]步骤四中，从开链机构末端相对于世界坐标系的位姿关系对下式：
[0030]进行逆运动学解算推导出从开链机构各个关节值θ
pi
，i＝1，2，3...。
[0031]步骤四中，假设用坐标向量x及其正向运动学方程x＝f(θ)表示末端坐标，得到一个关节坐标到末端坐标的非线性向量方程，其中f：R
n
→
R
m
可微，令x
d
为要求解的末端坐标系，即x
d
为从开链机构末端位姿，根据牛顿
‑
拉夫森法方程g(θ)＝x
d
‑
f(θ)，求解目标为求解下述方程的根：
[0032]g(θ)＝x
d
‑
f(θ)＝0
ꢀꢀ
(17)；
[0033]获得求解关节角的迭代表达式：
[0034]θ
i+1
＝θ
i
+Δθ
i
ꢀꢀ
(22)；
[0035]不断重复迭代上式(22)，产生一系列的θ值{θ0，θ1，θ2...}，最终在关节解θ
d
处收敛。
[0036]步骤四中，g(θ)＝x
d
‑
f(θ)＝0求解过程具体如下：
[0037]已知初始关节值θ0，运动学方程f(θ)写为泰勒展开的形式：
[0038][0039]上式(18)只截取到泰勒级数第一项，并且等效于θ0处的雅可比J(θ0)∈R
m
×
n
，则上式(18)可进一步简化为：
[0040]J(θ0)Δθ＝X
d
‑
f(θ0)
ꢀꢀ
(19)；
[0041]如果J(θ0)为方阵且可逆，采用下式求解Δθ：
[0042]Δθ＝J
‑1(θ0)X
d
‑
f(θ0)
ꢀꢀ
(20)；
[0043]否则使用伪逆代替J
‑1(θ)，其中通过下式计算：
[0044]J为n＞m时
[0045]J为n＜m时；
[0046]将伪逆代替J
‑1(θ)后，上式(20)变为：
[0047][0048]最后得到求解关节角的迭代表达式：
[0049]θ
i+1
＝θ
i
+Δθ
i
ꢀꢀ
(22)。
[0050]步骤五中，使用在线轨迹生成方法进行轨迹规划，获得主开链机构和从开链机构各个关节从当前值到目标值之间插值关于时间的位置函数p(t)，然后将步骤三中预期的主开链机构各个关节值和步骤四中得到的从开链机构各个关节值作为轨迹规划输入值，轨迹规划输出值则输入闭环位置控制器实本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种少自由度闭链空间机构运动解算方法，其特征在于：包括如下步骤：步骤一：对少自由度主从式单闭链机构进行运动学分析，建立主开链机构模型和从开链机构模型，并描述各个部件之间的位置关系；步骤二：对主开链机构末端和从开链机构末端进行耦合运动学分析，建立主开链机构末端和从开链机构末端位姿之间的耦合运动关系式；步骤三：指定主开链机构期望轨迹，然后对主开链机构进行正运动学解算，获得主开链机构末端位姿；步骤四：依据步骤二中的耦合运动关系式和步骤三中的主开链机构末端位姿，获得从开链机构末端位姿，然后进行逆运动学解算获得从开链机构各个关节值；步骤五：使用在线轨迹生成方法进行轨迹规划，然后使用闭环控制方法对少自由度主从式单闭链机构中的各个电机实施位置控制。2.根据权利要求1所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤一中，定义固定坐标系w作为世界坐标系，然后测量各部件参数，建立主开链机构各部件坐标系模型和从开链机构各部件坐标系模型，并采用仅需四个参数的Denabit
‑
Hartenberg(DH)方法描述各部件相对位置关系，获得相邻两部件坐标系之间的位姿关系矩阵方法描述各部件相对位置关系，获得相邻两部件坐标系之间的位姿关系矩阵上式(7)中，α
i
‑1表示绕坐标系{i
‑
1}的x轴旋转的角度，a
i
‑1表示沿着坐标系{i
‑
1的x轴移动的距离，θ
i
表示绕坐标系{i}的z轴旋转的角度，d
i
表示沿着坐标系{i}的z轴移动的距离。3.根据权利要求2所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤一中，相邻两部件坐标系之间的位姿变换矩阵表示为：上式(2)中，Rot(x，α
i
‑1)表示绕坐标系{i
‑
1}的x轴旋转a
i
‑1角度，Trans(x，a
i
‑1)表示沿着坐标系{i
‑
1}的x轴移动a
i
‑1距离，Rot(z，θ
i
)表示绕坐标系{i}的z轴旋转θ
i
角度，Trans(z，d
i
)表示沿着坐标系{i}的z轴移动d
i
距离，另外空间坐标系的平移和旋转变换矩阵表达式如下：下：
将上式(3)
‑
(6)代入式(2)中，获得：4.根据权利要求2所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤二中，由于主开链机构末端和从开链机构末端位姿重合，因此：上式(8)中，w为世界坐标系，ai为主开链机构第i个坐标系，pi为从开链机构第i个坐标系，T表示一组相邻部件的坐标系位姿变换关系，也即上式(7)。。5.根据权利要求4所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤三中，指定主开链机构期望轨迹，即获得主开链机构各关节值θ
ai

【专利技术属性】
技术研发人员：赵忆文，罗阳，江超，姜运祥，李英立，魏仁松，胡林涛，赵新刚，
申请(专利权)人：中国科学院沈阳自动化研究所，
类型：发明
国别省市：