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

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


背景技术：

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


技术实现要素：

3.本发明的目的在于提供一种少自由度闭链空间机构运动解算方法，能够实现少自由度主从式单闭链机构的实时运动解算，并通过在线轨迹生成及闭环控制实现少自由度主从式单闭链机构的同步运动控制。
4.本发明的目的是通过以下技术方案来实现的：
5.一种少自由度闭链空间机构运动解算方法，包括如下步骤：
6.步骤一：对少自由度主从式单闭链机构进行运动学分析，建立主开链机构模型和从开链机构模型，并描述各个部件之间的位置关系；
7.步骤二：对主开链机构末端和从开链机构末端进行耦合运动学分析，建立主开链机构末端和从开链机构末端位姿之间的耦合运动关系式；
8.步骤三：指定主开链机构期望轨迹，然后对主开链机构进行正运动学解算，获得主开链机构末端位姿；
9.步骤四：依据步骤二中的耦合运动关系式和步骤三中的主开链机构末端位姿，获得从开链机构末端位姿，然后进行逆运动学解算获得从开链机构各个关节值；
10.步骤五：使用在线轨迹生成方法进行轨迹规划，然后使用闭环控制方法对少自由度主从式单闭链机构中的各个电机实施位置控制。
11.步骤一中，定义固定坐标系w作为世界坐标系，然后测量各部件参数，建立主开链机构各部件坐标系模型和从开链机构各部件坐标系模型，并采用仅需四个参数的denabit-hartenberg(dh)方法描述各部件相对位置关系，获得相邻两部件坐标系之间的位姿关系矩阵
12.13.上式(7)中，α
i-1
表示绕坐标系{i-1}的x轴旋转的角度，a
i-1
表示沿着坐标系{i-1}的x轴移动的距离，θi表示绕坐标系{i}的z轴旋转的角度，di表示沿着坐标系{i}的z轴移动的距离。
14.步骤一中，相邻两部件坐标系之间的位姿变换矩阵表示为：
[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，di)表示沿着坐标系{i}的z轴移动di距离，另外空间坐标系的平移和旋转变换矩阵表达式如下：
[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：rn→rm
可微，令xd为要求解的末端坐标系，即xd为从开链机构末端位姿，根据牛顿-拉夫森法方程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)∈rm×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)，然后将步骤三中预期的主开链机构各个关节值和步骤四中得到的从开链机构各个关节值作为轨迹规划输入值，轨迹规划输出值则输入闭环位置控制器实现对电机的闭环控制。
[0051]
本发明的优点与积极效果为：
[0052]
1、本发明以少自由度单闭链机构为研究对象，在分析其运动形式的基础上，将少自由度单闭链机构拆分为末端固连的主开链与从开链机构，并得到联系主开链和从开链的耦合运动关系式，进而依据耦合运动关系式和通过对主开链运动控制要求分析，采用主开链各关节期望位置作为已知条件，对主开链进行正运动学计算和对从开链进行逆运动学计算，从而实现少自由度主从式单闭链机构的实时运动解算。
[0053]
2、本发明能够将解算的机构期望位置实时发出，并通过在线轨迹生成及闭环控制实现少自由度主从式单闭链机构同步运动控制。
附图说明
[0054]
图1为本发明的流程示意图，
[0055]
图2为本发明实施例采用的一种少自由度主从式单闭链机构示意图，
[0056]
图3为图2中机构拆分后的主开链机构示意图，
[0057]
图4为图2中机构拆分后的从开链机构示意图，
[0058]
图5为图3中的主开链机构坐标系模型示意图，
[0059]
图6为图4中的从开链机构坐标系模型示意图，
[0060]
图7为轨迹在线规划示意图，
[0061]
图8为运动控制系统控制过程示意图，
[0062]
图9为验证实验获得的x方向位置偏差图，
[0063]
图10为验证实验获得的y方向位置偏差图，
[0064]
图11为验证实验获得的z方向位置偏差图，
[0065]
图12为验证实验获得的r方向姿态偏差图，
[0066]
图13为验证实验获得的p方向姿态偏差图，
[0067]
图14为验证实验获得的y方向姿态偏差图。
[0068]
其中，1为主基座，101为第一铰链，2为第一连杆，3为第二连杆，301为第二铰链，302为转动电机，4为直线电机，401为虎克铰，402为第三铰链，5为从基座，6为基座连杆。
具体实施方式
[0069]
下面结合附图对本发明作进一步详述。
[0070]
本发明为了解决主动自由度与被动自由度构成的少自由度主从式单闭链机构难以结算和精确控制的问题，下面以图2所示的实施例对本发明方法做进一步说明。
[0071]
如图2所示，本实施例中采用的少自由度主从式单闭链机构包括主基座1、第一连杆2、第二连杆3、直线电机4、从基座5和基座连杆6，其中主基座1上端通过第一铰链101与第一连杆2一端铰接，第一连杆2另一端与通过第二铰链301与第二连杆3一端铰接，第二连杆3另一端则设有转动电机302，所述直线电机4头端设有虎克铰401与所述转动电机302连接，所述直线电机4尾端通过第三铰链402与所述从基座5铰接，所述主基座1和从基座5之间通过基座连杆6相连。
[0072]
本发明方法主要包括如下步骤：
[0073]
步骤一：对少自由度主从式单闭链机构进行运动学分析，建立主开链机构模型和
从开链机构模型，并描述各个部件之间的位置关系。
[0074]
单闭链机构结构通常较为复杂，运动约束较多，其位形空间的自由度数较少，直接进行运动学分析具有挑战。可以采用如下grubler公式计算单闭链机构的位形空间自由度：
[0075][0076]
上式(1)中，m是刚体的自由度数、n是总连杆数、j是关节数、fi是关节i对应的自由度数，以图2所示机构为例，其自由度为：
[0077][0078]
为简化运动学分析和实现少自由单闭链机构同步运动控制，可以依据需要控制的连杆对象如图2所示第二连杆3的末端坐标系，将单闭链划分为主从开链。
[0079]
然后根据运动学分析情况建立如图3所示的主开链机构和图4所示的从开链机构，采用通用的且参数仅需四个的denabit-hartenberg(dh)方法描述各部件相对位置关系，具体为：
[0080]
定义一个固定坐标系w作为世界坐标系，然后测量各部件参数，建立如图5所示的主开链机构各部件坐标系模型和如图6所示的从开链机构各部件坐标系模型。
[0081]
如图5和图6所示的空间中任意两个坐标系的相对位姿关系可由平移和旋转进行描述，可通过特定点在两个不同坐标系下的坐标值确定坐标系之间的相对位姿关系。
[0082]
相邻两部件坐标系之间的位姿变换矩阵表示为：
[0083][0084]
上式(2)中，是坐标系{i-1}和坐标系{i}之间的位姿变换关系，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，di)表示沿着坐标系{i}的z轴移动di距离。
[0085]
空间坐标系的平移和旋转变换矩阵表达式如下所示：
[0086][0087][0088]
[0089][0090]
将上式(3)-(6)代入式(2)中，可得到相邻两部件坐标系之间的位姿变换关系为：
[0091][0092]
上式(7)中，α
i-1
表示绕坐标系{i-1}的x轴旋转的角度，a
i-1
表示沿着坐标系{i-1}的x轴移动的距离，θi表示绕坐标系{i}的z轴旋转的角度，di表示沿着坐标系{i}的z轴移动的距离。
[0093]
步骤二：对主开链机构末端和从开链机构末端进行耦合运动学分析，建立主开链机构末端和从开链机构末端位姿之间的耦合运动关系式。
[0094]
由于主从式单闭链结构的特殊性，即主开链机构末端与从开链机构末端相连，因此各个部件之间的运动存在耦合，使得单闭链的运动学分析具有挑战性。为了简化主从式单闭链机构运动分析，解决运动耦合问题，需要建立主从式单闭链的主开链机构末端和从开链机构末端之间的耦合运动关系式，由图5～6所示的各个部件坐标系的位姿变换关系可知，主开链机构和从开链机构在连接位置存在耦合运动学约束，即主开链机构末端和从开链机构末端位姿重合：
[0095][0096]
上式(8)中，w为表示编号为w的坐标系(世界坐标系)，ai为主开链机构第i个坐标系，pi为从开链机构第i个坐标系，t表示一组相邻部件的坐标系位姿变换关系，也即上式(7)。
[0097]
对于图2-4结构，上式(8)写为：
[0098][0099]
定义图3所示的主开链机构各关节值为θ
ai
，i＝1，2，图4所示的从开链机构各关节值为θ
pi
，i＝1，2，3...，则在设定主开链的期望轨迹情况下，也即主开链机构各个关节值已知的情况下，主从式单闭链机构的运动控制问题转变为根据主开链关节值θ
ai
，i＝1，2和耦合运动关系求解从开链关节值θ
pi
，i＝1，2，3...。
[0100]
上式(8)的等号左侧是主开链的正运动学方程，其在给定的期望控制关节值θ
ai
，i＝1，2下，可计算出主开链的期望末端位姿图2所示实施例为详见下述步骤三。
[0101]
上式(8)的等号右侧是从开链的正运动学方程，在主开链的期望末端位姿已知的情况下，由于主开链末端和从开链末端重合，可知从开链机构末端相对于世界坐标系的位姿关系图2所示实施例为然后根据从开链的正运动学方程求逆解，计算出从开链的理论关节转角θ
pi
，i＝1，2，3...，详见下述步骤四。
[0102]
步骤三：指定主开链机构期望轨迹，即主开链机构各关节值已知，然后进行正运动学解算，从而推到出主开链机构末端相对于世界坐标系的位姿关系。
[0103]
正运动学即给定各个关节角度，计算末端坐标系相对于世界坐标系的齐次变换矩阵，由图5所示的主开链机构各部件坐标系模型可知：
[0104][0105]
以图5所示机构为例，根据实际测量的主开链机构各部件物理属性，可以获得主开链的dh参数如下表1所示：
[0106][0107]
表1主开链机构坐标参数
[0108]
上表1中，ai表示主开链机构各个部件坐标系，α表示绕坐标系{i-1}的x轴旋转的角度，a表示沿着坐标系{i-1}的x轴移动的距离，θ表示绕坐标系{i}的z轴旋转的角度，d表示沿着坐标系{i}的z轴移动的距离。
[0109]
利用上式(7)可计算各个部件坐标系的齐次变换矩阵：
[0110][0111][0112][0113][0114]
将式(10)-(13)代入式(9)，得到主开链机构正运动学公式如下：
[0115]
[0116]
上式(14)中：
[0117]nx
＝0.5*cos(θ1+θ2+1.396)+0.5*cos(θ
2-θ1+1.396)
[0118]
ny＝-sin(θ
2-0.174)sinθ1[0119]
nz＝-cos(θ2-0.174)
[0120]ox
=-cos(θ
2-0.174)cos(θ1)
[0121]
oy＝0.5*cos(θ1+θ2+1.396)-0.5*cos(θ
2-θ1+1.396)
[0122]
oz＝-cos(θ2+1.396
[0123]kx
＝-sin(θ1)
[0124]ky
=cos(θ1)
[0125]
p
x
＝0.106*cos(θ1)cos(θ2)-0.3*sin(θ1)+0.5
[0126]
py＝0.3*cos(θ1)+0.106*cos(θ2)*sin(θ1)-0.3
[0127]
pz＝0.079-0.106*sin(θ2)；
[0128]
因此给定主开链的关节变量θ1，θ2代入上式(14)中，便可以得到主开链机构末端相对于世界坐标系的位姿
[0129]
步骤四：依据步骤二的耦合运动关系式和步骤三中获得的主开链机构末端相对于世界坐标系的位姿关系也即获得从开链机构末端相对于世界坐标系的位姿关系然后对下式：
[0130]
进行逆运动学解算推导出从开链机构各个关节值θ
pi
，i＝1，2，3...。
[0131]
对于通用的n自由度开链机构而言，若其正向运动学写成t(θ)，θ∈rn的形式，则其逆运动学问题可以描述成：给定齐次变换矩阵x∈se(3)，找出满足t(θ)＝x的关节角θ。
[0132]
对于开链机构而言，运动学逆解可能存在多组解，这与运动学正解不同，对于运动学正解，给定一组关节角总是存在唯一的末端位姿与之对应。对于较为通用的开链空间机构，可以采用牛顿-拉夫森方法求解其运动学逆解问题，这个过程本质上是个数值迭代过程，如果所选的关节角初始值接近真实值，计算结果很容易实现收敛。
[0133]
对于图4所示从开链机构，采用牛顿-拉夫森方法求解特定位姿下的关节解，具体过程如下：
[0134]
1、对于给定的微分方程g(θ)：r
→
r，要求数值求解方程g(θ)＝0，假设θ0为初值，g(θ)的泰勒级数在θ0处展开，并只取到第一项：
[0135]
g(θ)＝g(θ0)+g
′
(θ0)(θ-θ0)＝0
[0136]
θ＝θ
0-(g
′
(θ0))-1
g(θ0)
ꢀꢀ
(15)；
[0137]
2、将上式(15)求得的值作为初值，重新代入方程，再次求解，得到下述方程：
[0138]
θ
k+1
＝θ
k-(g
′
(θk))-1
g(θk)
ꢀꢀ
(16)；
[0139]
同样的公式可以扩展到开链空间机构逆运动学数值算法中，假设用坐标向量x及其正向运动学方程x＝f(θ)表示末端坐标，会得到一个关节坐标到末端坐标的非线性向量方程，其中f：rn→rm
可微，令xd为要求解的末端坐标系，即xd为从开链机构的期望末端位姿
根据牛顿-拉夫森法方程g(θ)＝x
d-f(θ)，本发明方法求解目标为求解下述方程的根：
[0140]
g(θ)＝x
d-f(θ)＝0
ꢀꢀ
(17)；
[0141]
与上述式(15)类似，已知初始关节值θ0，运动学方程f(θ)可以写为泰勒展开的形式：
[0142][0143]
上式(18)只截取到泰勒级数第一项，并且等效于θ0处的雅可比j(θ0)∈rm×n，则根据上式(17)可知，上式(18)可进一步简化为：
[0144]
j(θ0)δθ＝x
d-f(θ0)
ꢀꢀ
(19)；
[0145]
如果j(θ0)为方阵且可逆，便可以采用下式求解δθ：
[0146]
δθ＝j-1
(θ0)x
d-f(θ0)
ꢀꢀ
(20)；
[0147]
否则需要使用伪逆代替j-1
(θ)，其中可通过下式计算：
[0148]
j为n＞m时
[0149]
j为n＜m时；
[0150]
将伪逆代替j-1
(θ)后，上式(20)变为：
[0151][0152]
最后得到求解关节角的迭代表达式：
[0153]
θ
i+1
＝θi+δθiꢀꢀ
(22)；
[0154]
不断重复迭代上式(22)，产生一系列的θ值{θ0，θ1，θ2...}，最终在关节解θd处收敛。
[0155]
步骤五：为了实现少自由度主从式单闭链机构的同步运动控制，本发明在上述运动解算基础上使用在线轨迹生成方法进行轨迹规划，得到主开链机构和从开链机构各个关节从当前值到目标值之间插值的关于时间足够光滑的位置函数p(t)，并且遵守与关节速度、加速度相关的限制，上述过程可通过轨迹在线规划器实现，此为本领域公知技术，本实施例规划的轨迹效果示意图如图7所示，然后使用闭环控制方法，利用闭环位置控制器对图2所示机构中的各个电机上的电机驱动器实施位置控制，从而使空间机构能够跟踪给定位置指令。
[0156]
本发明机构的具体运动控制过程如图8所示：用户给定主开链机构期望轨迹，也即给定主开链机构的各个关节值θ
a1
、θ
a2
，将其代入上式(9)进行主开链正运动学解算，得到主开链机构末端相对于世界坐标系的位姿再将作为从开链机构的期望末端位姿代入上式(17)进行从开链逆运动学迭代求解，得到从开链机构特定位姿下的各个关节值θ
pi
，i＝1，2，3...，然后获得的关节值作为输入值θr输入轨迹在线规划器进行规划，并将规划后的值θc输入闭环位置控制器实现对各个电机的闭环控制。所述轨迹在线规划器和闭环位置控制器均为本领域公知技术。
[0157]
本发明通过仿真模拟验证上述方法的可行性，具体为：
[0158]
利用matlab仿真软件建立图2所示的少自由度主从式单闭链机构，其中主开链机构参数如上表1所示，设定主开链机构的各个关节值i＝1，2分别以0.05的步进值从-π/6到π/6遍历，并计算出对应的主开链机构末端位姿，根据耦合运动关系式(8)，主开链机构末端位姿作为从开链机构的末端位姿，通过从开链的逆运动学方程解算出从开链机构的对应关节角θ
pi
，i＝1，2，3...，同时使用matlab下的机器人工具箱构造的主从式单闭链模型绘制所有的路点，并且获得从开链机构在解算出的关节角条件下的末端位姿，并与主开链的末端位姿比较，其中θ
ai
，i＝1，2∈[-10
°
，10
°
]时位移误差最大为41mm，姿态最大误差为0.114rad,其余情况因为主从开链关节值运动角度较大，误差相对较大，最大位移误差为120mm，最大姿态误差为0.302rad，但依然能满足控制要求，具体结果如图9～14所示，由此可见从开链的末端位姿能够跟随主开链的末端位姿，满足对少自由度主从式单闭链机构的控制要求，验证了本发明方法的正确性和可行性。

技术特征：
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
，i＝1，2，3...，代入下式进行正运动学解算：获得主开链机构末端相对于世界坐标系的位姿6.根据权利要求5所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤四中，从开链机构末端相对于世界坐标系的位姿关系对下式：进行逆运动学解算推导出从开链机构各个关节值θ
pi
，i＝1，2，3...。7.根据权利要求1或6所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤四中，假设用坐标向量x及其正向运动学方程x＝f(θ)表示末端坐标，得到一个关节坐标到末端坐标的非线性向量方程，其中f：r
n
→
r
m
可微，令x
d
为要求解的末端坐标系，即x
d
为从开链机构末端位姿，根据牛顿-拉夫森法方程g(θ)＝x
d-f(θ)，求解目标为求解下述方程的根：g(θ)＝x
d-f(θ)＝0
ꢀꢀꢀꢀꢀ
(17)；获得求解关节角的迭代表达式：θ
i+1
＝θ
i
+δθ
i
ꢀꢀꢀꢀ
(22)；不断重复迭代上式(22)，产生一系列的θ值{θ0，θ1，θ2...}，最终在关节解θ
d
处收敛。8.根据权利要求7所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤四中，g(θ)＝x
d-f(θ)＝0求解过程具体如下：已知初始关节值θ0，运动学方程f(θ)写为泰勒展开的形式：
上式(18)只截取到泰勒级数第一项，并且等效于θ0处的雅可比j(θ0)∈r
m
×
n
，则上式(18)可进一步简化为：j(θ0)δθ＝x
d-f(θ0)
ꢀꢀꢀꢀꢀ
(19)；如果j(θ0)为方阵且可逆，采用下式求解δθ：δθ＝j-1
(θ0)x
d-f(θ0)
ꢀꢀꢀꢀ
(20)；否则使用伪逆代替j-1
(θ)，其中通过下式计算：j为n＞m时j为n＜m时；将伪逆代替j-1
(θ)后，上式(20)变为：最后得到求解关节角的迭代表达式：θ
i+1
＝θ
i
+δθ
i
ꢀꢀꢀꢀꢀ
(22)。9.根据权利要求1所述的少自由度闭链空间机构运动解算方法，其特征在于：步骤五中，使用在线轨迹生成方法进行轨迹规划，获得主开链机构和从开链机构各个关节从当前值到目标值之间插值关于时间的位置函数p(t)，然后将步骤三中预期的主开链机构各个关节值和步骤四中得到的从开链机构各个关节值作为轨迹规划输入值，轨迹规划输出值则输入闭环位置控制器实现对电机的闭环控制。

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


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