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

本发明涉及机构运动控制领域，具体地说是一种少自由度闭链空间机构运动解算方法。

背景技术：

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

技术实现思路

1、本发明的目的在于提供一种少自由度闭链空间机构运动解算方法，能够实现少自由度主从式单闭链机构的实时运动解算，并通过在线轨迹生成及闭环控制实现少自由度主从式单闭链机构的同步运动控制。

2、本发明的目的是通过以下技术方案来实现的：

3、一种少自由度闭链空间机构运动解算方法，包括如下步骤：

4、步骤一：对少自由度主从式单闭链机构进行运动学分析，建立主开链机构模型和从开链机构模型，并描述各个部件之间的位置关系；

5、步骤二：对主开链机构末端和从开链机构末端进行耦合运动学分析，建立主开链机构末端和从开链机构末端位姿之间的耦合运动关系式；

6、步骤三：指定主开链机构期望轨迹，然后对主开链机构进行正运动学解算，获得主开链机构末端位姿；

7、步骤四：依据步骤二中的耦合运动关系式和步骤三中的主开链机构末端位姿，获得从开链机构末端位姿，然后进行逆运动学解算获得从开链机构各个关节值；

8、步骤五：使用在线轨迹生成方法进行轨迹规划，然后使用闭环控制方法对少自由度主从式单闭链机构中的各个电机实施位置控制。

9、步骤一中，定义固定坐标系w作为世界坐标系，然后测量各部件参数，建立主开链机构各部件坐标系模型和从开链机构各部件坐标系模型，并采用仅需四个参数的denabit-hartenberg(dh)方法描述各部件相对位置关系，获得相邻两部件坐标系之间的位姿关系矩阵

10、

11、上式(7)中，αi-1表示绕坐标系{i-1}的x轴旋转的角度，ai-1表示沿着坐标系{i-1}的x轴移动的距离，θi表示绕坐标系{i}的z轴旋转的角度，di表示沿着坐标系{i}的z轴移动的距离。

12、步骤一中，相邻两部件坐标系之间的位姿变换矩阵表示为：

13、

14、上式(2)中，rot(x，αi-1)表示绕坐标系{i-1}的x轴旋转ai-1角度，trans(x，ai-1)表示沿着坐标系{i-1}的x轴移动ai-1距离，rot(z，θi)表示绕坐标系{i}的z轴旋转θi角度，trans(z，di)表示沿着坐标系{i}的z轴移动di距离，另外空间坐标系的平移和旋转变换矩阵表达式如下：

15、

16、

17、

18、

19、将上式(3)-(6)代入式(2)中，获得：

20、

21、步骤二中，由于主开链机构末端和从开链机构末端位姿重合，因此：

22、

23、上式(8)中，w为世界坐标系，ai为主开链机构第i个坐标系，pi为从开链机构第i个坐标系，t表示一组相邻部件的坐标系位姿变换关系，也即上式(7)。。

24、步骤三中，指定主开链机构期望轨迹，即获得主开链机构各关节值θai，i＝1，2，3...，代入下式进行正运动学解算：

25、

26、获得主开链机构末端相对于世界坐标系的位姿

27、步骤四中，从开链机构末端相对于世界坐标系的位姿关系对下式：

28、进行逆运动学解算推导出从开链机构各个关节值θpi，i＝1，2，3...。

29、步骤四中，假设用坐标向量x及其正向运动学方程x＝f(θ)表示末端坐标，得到一个关节坐标到末端坐标的非线性向量方程，其中f：rn→rm可微，令xd为要求解的末端坐标系，即xd为从开链机构末端位姿，根据牛顿-拉夫森法方程g(θ)＝xd-f(θ)，求解目标为求解下述方程的根：

30、g(θ)＝xd-f(θ)＝0  (17)；

31、获得求解关节角的迭代表达式：

32、θi+1＝θi+δθi  (22)；

33、不断重复迭代上式(22)，产生一系列的θ值{θ0，θ1，θ2...}，最终在关节解θd处收敛。

34、步骤四中，g(θ)＝xd-f(θ)＝0求解过程具体如下：

35、已知初始关节值θ0，运动学方程f(θ)写为泰勒展开的形式：

36、

37、上式(18)只截取到泰勒级数第一项，并且等效于θ0处的雅可比j(θ0)∈rm×n，则上式(18)可进一步简化为：

38、j(θ0)δθ＝xd-f(θ0)  (19)；

39、如果j(θ0)为方阵且可逆，采用下式求解δθ：

40、δθ＝j-1(θ0)xd-f(θ0)  (20)；

41、否则使用伪逆代替j-1(θ)，其中通过下式计算：

42、j为n＞m时

43、j为n＜m时；

44、将伪逆代替j-1(θ)后，上式(20)变为：

45、

46、最后得到求解关节角的迭代表达式：

47、θi+1＝θi+δθi  (22)。

48、步骤五中，使用在线轨迹生成方法进行轨迹规划，获得主开链机构和从开链机构各个关节从当前值到目标值之间插值关于时间的位置函数p(t)，然后将步骤三中预期的主开链机构各个关节值和步骤四中得到的从开链机构各个关节值作为轨迹规划输入值，轨迹规划输出值则输入闭环位置控制器实现对电机的闭环控制。

49、本发明的优点与积极效果为：

50、1、本发明以少自由度单闭链机构为研究对象，在分析其运动形式的基础上，将少自由度单闭链机构拆分为末端固连的主开链与从开链机构，并得到联系主开链和从开链的耦合运动关系式，进而依据耦合运动关系式和通过对主开链运动控制要求分析，采用主开链各关节期望位置作为已知条件，对主开链进行正运动学计算和对从开链进行逆运动学计算，从而实现少自由度主从式单闭链机构的实时运动解算。

51、2、本发明能够将解算的机构期望位置实时发出，并通过在线轨迹生成及闭环控制实现少自由度主从式单闭链机构同步运动控制。

技术特征：

1.一种少自由度闭链空间机构运动解算方法，其特征在于：包括如下步骤：

2.根据权利要求1所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤一中，定义固定坐标系w作为世界坐标系，然后测量各部件参数，建立主开链机构各部件坐标系模型和从开链机构各部件坐标系模型，并采用仅需四个参数的denabit-hartenberg(dh)方法描述各部件相对位置关系，获得相邻两部件坐标系之间的位姿关系矩阵

3.根据权利要求2所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤一中，相邻两部件坐标系之间的位姿变换矩阵表示为：

4.根据权利要求2所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤二中，由于主开链机构末端和从开链机构末端位姿重合，因此：

5.根据权利要求4所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤三中，指定主开链机构期望轨迹，即获得主开链机构各关节值θai，i＝1，2，3...，代入下式进行正运动学解算：

6.根据权利要求5所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤四中，从开链机构末端相对于世界坐标系的位姿关系对下式：

7.根据权利要求1或6所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤四中，假设用坐标向量x及其正向运动学方程x＝f(θ)表示末端坐标，得到一个关节坐标到末端坐标的非线性向量方程，其中f：rn→rm可微，令xd为要求解的末端坐标系，即xd为从开链机构末端位姿，根据牛顿-拉夫森法方程g(θ)＝xd-f(θ)，求解目标为求解下述方程的根：

8.根据权利要求7所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤四中，g(θ)＝xd-f(θ)＝0求解过程具体如下：

9.根据权利要求1所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤五中，使用在线轨迹生成方法进行轨迹规划，获得主开链机构和从开链机构各个关节从当前值到目标值之间插值关于时间的位置函数p(t)，然后将步骤三中预期的主开链机构各个关节值和步骤四中得到的从开链机构各个关节值作为轨迹规划输入值，轨迹规划输出值则输入闭环位置控制器实现对电机的闭环控制。

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

技术研发人员：赵忆文,罗阳,江超,姜运祥,李英立,魏仁松,胡林涛,赵新刚
受保护的技术使用者：中国科学院沈阳自动化研究所
技术研发日：
技术公布日：2024/1/15