CN115556110B - 基于主动半监督迁移学习的机器人位姿误差感知方法 - Google Patents

Abstract

本发明属于机器人加工相关技术领域，公开了一种基于主动半监督迁移学习的机器人位姿误差感知方法。该方法包括下列步骤：S1在机器人运动空间内选择多个位置，计算运动学参数误差；S2对所开展的任务，构建仿真域数据集；S3对应仿真域数据集，构建测量域数据集；S4利用仿真域数据集构建预训练模型，采用半监督迁移学习方法微调所构建的模型，得到最终模型；S5利用最终模型预测测量域中未被选择关节角对应的位姿误差并进行补偿，以此实现所开展任务的高精度运行。通过本发明，解决常规基于参数标定的位姿误差预测精度低的问题，并将少量点精确测量的精度等级迁移到所有任务点上，实现低测量成本下的位姿误差高精度预测和补偿。

Description

基于主动半监督迁移学习的机器人位姿误差感知方法

技术领域

本发明属于机器人加工相关技术领域，更具体地，涉及一种基于主动半监督迁移学习的机器人位姿误差感知方法。

背景技术

机器人位姿误差是由于机器人各连杆加工误差，装配误差，各电机齿轮传动间隙等因素共同引起的，是制约机器人高精度应用的共性且重要的原因。由于该因素的存在，在实际的工程应用中，机器人常出现绝对定位精度差、空间轨迹精度差等问题。因此对机器人空间运动在离线层面的位姿误差预先感知是控制机器人位姿误差的重要手段。

现有的对于机器人位姿误差感知的方法主要可以分为两大类，其中第一类是直接通过测量获得，这种方法虽然对于误差的获取是最直接的，但这种方式带来的问题是测量成本高，测量环境要求高，点位连续测量难度大；第二类方法是借助智能学习算法进行预测，通常采集空间中的部分点位后，训练映射模型进行新位置的预测，但这些映射模型通常未能考虑不同位置处数据分布的差异性，因此表现出来的预测精度有限。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于主动半监督迁移学习的机器人位姿误差感知方法，解决机器人位姿预测精度低的问题。

为实现上述目的，按照本发明，提供了一种基于主动半监督迁移学习的机器人位姿误差感知方法，该方法包括下列步骤：

S1在机器人运动空间内选择多个位置，控制机器人运动至所选择的位置并采集运动过程中的数据，以此计算机器人运动学参数的误差；

S2对所需开展的机器人任务，设定机器人运动过程中的理论关节角，利用该理论关节角计算理论空间位姿，结合步骤S1中获得的运动学参数误差计算在所述理论关节角下的仿真空间位姿，利用所述理论空间位姿和仿真空间位姿计算位姿误差，以此获得所述理论关节角与位姿误差之间一一对应的数据集，即仿真域数据集；

S3按照预先构建的方法从所述理论关节角中选取出部分数据，机器人按照选取出来的关节角运动并执行测量，计算测量结果与S2中理论空间位姿之间的偏差，即获得每个选取出来的理论关节角对应的测量位姿误差，以此形成所选理论关节角与测量位姿误差之间一一对应的数据集，形成测量域数据集；

S4利用所述仿真域数据集构建预训练神经网络，采用半监督迁移学习的方法利用所述测量域数据集对所述神经网络进行微调，获得微调后的神经网络；

S5利用微调后的神经网络预测所述测量域中未被选择的关节角对应的位姿误差，利用该位姿误差对机器人位姿进行补偿，以此实现所需开展的任务的高精度运行。

进一步优选地，在步骤S1中，所述机器人运动学参数的误差按照下列关系是计算：

δ＝([B₁,B₂,…,B_i,…,B₅₀]^T)⁺[⁶D₁,⁶D₂,…,⁶D_i,…,⁶D₅₀]^T

其中，Bi是第i个测量点处，机器人所有连杆误差与机器人末端误差之间的变换矩阵，其规模为6×24，⁶D_i是第i个测量点位测量得到的微分运动矢量，上角标T是对矩阵进行转置，上角标+是对矩阵求广义逆。

进一步优选地，在步骤S2中，所述利用该理论关节角计算理论空间位姿采用机器人正运动学的方法计算。

进一步优选地，在步骤S3中，所述预先构建的方法按照下列进行：选取三组数据，分别为：第1个关节角样本，第2～6个关节角样本，第7～t个关节角样本，其中，t是正整数，满足t≥min(20,dimension)，dimension为关节角的维度。

进一步优选地，在步骤S3中，所述三组数据按照下列方式进行选择：

其中，d(·,·)是欧式距离度量函数。θ_i是仿真域中第i个样本的关节角，θ_C是仿真域中所有样本的关节角通过聚类得到的聚类中心对应的关节角，是仿真域中未被选择的样本中的第i个对应的关节角，是测量域中的第j个样本对应的关节角，f_p(·)是利用已选择出的关节角和对应测量得到的位姿误差训练的一个从关节空间到位姿误差空间的弱学习器，P是输出的维度，是测量域对于已经选择出来的关节角测量得到的对应的位姿误差，i是仿真域数据的索引，j是测量域数据的索引，t是测量域需要被测量的关节角数目，n是仿真域数据的总体数目。

进一步优选地，在步骤S4中，所述半监督迁移学习的分布适配按照下列进行：

其中，D_MDA(θ_S,θ_M)是边缘分布差异的度量函数，θ_S,θ_M分别是仿真域和测量域中样本的关节角；D_CDA(D_S,D_M)是条件分布差异的度量函数，D_S,D_M分别是仿真域和测量域中样本的关节角及其对应的位姿误差；λ是权重因子，反映仿真域和测量域在边缘分布差异和条件分布差异上的比重。

进一步优选地，所述半监督迁移学习的融合损失函数按照下列进行：

其中，是回归损失函数；是加权后的分布差异适配函数；W^TW是模型参数的正则化项；α是回归损失函数加权系数；β是加权后的分布差异适配函数加权系数，γ是正则化系数。

进一步优选地，所述半监督迁移学习的回归损失函数按照下列进行：

其中，是仿真域数据的回归损失函数，是测量域数据的回归损失函数，g(·)是关节角与位姿误差的映射函数，θ_S,θ_M分别是仿真域和测量域中样本的关节角；e_S,e_M分别是仿真域和测量域中样本对应的位姿误差。上角标i和j分别为样本的序号标识，n是仿真域所有样本的数目，t是测量域所有被选择出来的样本的数目。

进一步优选地，在步骤S4中，对所述神经网络进行微调时，最小化所述融合损失函数以此实现微调过程。

进一步优选地，在步骤S5中，所述补偿按照下列关系式进行：

其中，finetune(·)是微调后的神经网络模型，是利用微调后的神经网络模型对第i个关节角θ_i的位姿误差的预测结果。P_{Compensation-i}为第i个关节角经过补偿之后的实际位姿。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，具备下列有益效果：

1.本发明中采用的基于主动半监督迁移学习的方法预测机器人的位姿误差的方法，与现有技术相比，该方法将机器人标定得到的运动学参数利用仿真域的构建作为提供位姿误差规律的中间环节，之后利用分段选择的策略选择部分点位进行高精度的点位测量，将少数机器人关节角下的机器人位姿误差测量的精度水平迁移到全部机器人关节角上，实现了少数位姿误差测量下的全局位姿误差智能感知。一方面节省了测量全部点位带来的时间浪费，同时也可以实现对于仿真域位姿误差分布规律的知识挖掘；

2.本发明中在构建测量域数据集时，将关节角的选择分为三个阶段，相比随机选择部分关节角进行测量，分段选择目的是从信息论的角度出发，选择所有关节角中蕴含信息量最大的关节角，确保所选择出来的关节角可以充分表征所有关节角的规律；

3.本发明中在构建最终位姿误差预测神经网络时采用半监督迁移学习的方法，由于测量域中仅有t组关节角被选择并进行了实际的测量得到了对应的位姿误差，因此若想得到测量域中所有关节角对应的位姿误差，需要利用已测量的少量样本对利用仿真域构建的预训练模型进行微调，这属于半监督学习的范畴，而由于两个域在位姿误差的获取手段上是存在差异的，因此数据分布存在差异，需要借助迁移学习的技术手段进行模型训练，固总体上需要采用半监督迁移学习对预训练模型进行微调；

4.本发明将机器人位姿误差感知问题定义为迁移学习的范式，考虑了数据分布差异对预测效果的影响，因此可以实现更高的预测精度；借助信息论的理论，利用主动学习策略，实现全局点位中少数位置的机器人位姿误差测量，极大的节省了测量成本和跟踪测量带来的限制；采用基于半监督迁移学习方法，将上述过程统筹起来，利用深度学习模型训练，实现最终机器人空间位姿误差高精度感知；

5.本发明提供的方法一方面实现了空间机器人少量关节配置点位中少数点位精确测量的主动选择，另一方面，将精确测量实现的精度水平迁移到所有的空间点位上，实现空间点位的位姿误差精确感知。不仅可以确保空间点位位姿误差的感知精度，还可以通过少数点位的选择，极大的减少点位测量的时间成本，为提升机器人位姿误差提供理论基础。

附图说明

图1是按照本发明的优选实施例所构建的基于主动半监督迁移学习的机器人位姿误差感知方法的工作流程图；

图2是按照本发明的优选实施例所构建的仿真域数据集构建的示意图；

图3是按照本发明的优选实施例所构建的基于半监督迁移学习方法中模型初始化部分的示意图；

图4是按照本发明的优选实施例所构建的机器人测量系统的示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

一种基于主动半监督迁移学习的机器人位姿误差感知方法，如图1所示，包括：

1.1机器人运动学参数粗标定

在机器人运动空间内随机选择若干位置，这里一般选择50个不同的位置，控制机器人到达所选择的位置，并且对所选择的位置开展激光跟踪仪测量，机器人测量系统的示意图如图4所示，包括机器人本体、安装在机器人末端法兰盘上的反射物，激光跟踪仪。

以史陶比尔机器人为例，相邻关节的变换矩阵可以表示为：

其中，s表示sin，c表示cos。由于各个运动学参数存在误差，相邻关节奇次变换矩阵的微分变换量可以表示为：

其中a_i、d_i、α_i、θ_i为第i连杆的运动学参数，Δa_i、Δd_i、Δα_i、Δθ_i为对应运动参数的误差。计算对应的偏导数，上述相邻关节奇次变换矩阵的微分变换量可以表示为：

又因为微分运动矢量对应的反对称矩阵ⁱΔ与相邻关节奇次变换矩阵的微分变换量之间存在如下关系：

则对应的微分运动矢量ⁱD可以表示为：

那么第i个连杆的误差就可以转换到末端的误差，具体的转换关系可以表示为：

其中为速度伴随矩阵，规模为6×6，ⁱD为第i个连杆的微分运动矢量，规模为6×1。把上面的ⁱD带入到上式中，可以表示为：

其中δ_i表示第i个运动学参数误差组成的向量，即[Δa_i Δd_i Δα_i Δθ_i]^T。

那么，将所有连杆的误差都映射到末端，可以表示为：

其中，B的规模为6×24，δ的规模为24×1。将所选择的50个不同的空间联合表达，对上式增广可以得到：

[⁶D₁,⁶D₂,…,⁶D_t,…,⁶D₅₀]^T＝[B1,B₂,…,B_t,…,B₅₀]^T·δ

之后利用最小二乘法即可得到机器人所有的运动学参数误差，表达为：

δ＝([B₁,B₂,…,B_i,…,B₅₀]^T)⁺[⁶D₁,⁶D₂,…,⁶D_i,…,⁶D₅₀]^T

现有的方法到这里就结束了，但这种方法所能实现的精度等级有限，而且对于空间点位的选择是严格的要求的，利用这样方式得到的机器人运动学参数不能确保全局一致性，也就是说，如果实际机器人运动的位置与标定所在的空间差异很大，就会导致很差的效果。本发明利用这样的粗标定，提供机器人末端误差的大致趋势，后续借助这种趋势实现准确的机器人位姿误差感知。

1.2仿真域数据集的构建

仿真域数据集的构建整体上的流程如图2所示。首先对于机器人运动过程的所有位置，都会对应理论的关节角。那么按照没有误差的理论运动学参数可以计算出理论关节角对应的理论空间位姿。与之对应的，结合计算得到的运行学参数误差，按照粗标定后的运动学参数，可以计算出仿真的空间位姿。由于运动学误差的存在，理论空间位姿与仿真空间位姿之间存在偏差。

定义机器人运动过程中的所有点位对应的理论关节角为n为理论关节角的数目。利用理论运动学参数进行机器人正运动学计算后，可以得到第i组关节角对应的理论空间位姿为P_Ti，同样的，利用叠加了粗标定后运动学参数误差δ的运动学参数进行机器人正运动学计算后，可以得到第i组关节角对应的仿真空间位姿为P_Si，两者之间的偏差可以定义为e_Si＝P_Si-P_Ti，将所有的关节角和对应的末端位姿误差组成的数据组合起来，即可得到仿真域数据集，定义为

1.3测量域数据集的构建

对于测量域数据的构建，总体上可以分为三个阶段，①第1组关节配置(值一组机器人关节角，包含每个轴的关节角数据)的选择；②第2～6个关节配置的选择；③第7～t个样本的选择。

为了防止标的样本过少无法获取仿真域的分布规律，同时避免过多的样本标注带来较高的标准成本，需要进行精确测量的样本总数t应满足t≥min(20,dimension)，其中dimension为关节角的维度，这里以六自由度机器人为例，dimension取6，因此需要进行精确测量的样本总数t应该大于6。对于t的上限应结合总体样本数目，按照一定的百分比选择，通常设置为总体样本的20％。

第1个关节配置。

选择离仿真域数据输入特征聚类中心最近的关节角配置。定义关节空间的聚类中心为θ_C，定义样本欧式距离度量函数为d()，则最小化所有样本与聚类中心θ_C之间的距离度量，可以得到对应的位置索引为：

2～6个关节配置。

不失一般性地，假设已经选择了k个样本，第k+1个样本的选择依据如下。首先计算剩余样本到所选择样本之间的空间距离，表示为：

按照行求最小值，得到最后对求最大值，得到对应的样本位置索引，表示为：

在完成前6个关节配置的选择之后，控制机器人到达所选择的6个位置，开展机器人位姿误差的测量。

7～t个关节配置。

不失一般性的，同样介绍第k+1个关节配置的选择依据，首先利用前面选择的k个样本训练一个从关节空间到位姿误差空间的弱学习器，定义为这里可以简单的利用支持向量回归算法实现。其中p为位姿误差空间分量的维度，前三维表示位置误差，后三维表示姿态误差，p∈[1,2,3,4,5,6]。利用所训练的弱学习器对未标注的样本进行预测后，度量在标签空间下，各个未标注样本预测结果与已标注样本之间的距离，可以表示为：

最小化关节角空间的距离度量矩阵和位姿误差空间的距离度量矩阵的hadamardproduct，表达式为：对的结果取最大值，可以得到对应的样本位置索引，表达为：

总体上归纳起来，对于测量域机器人关节角配置索引的的选择，遵循如下规则：

获得了机器人关节角配置的索引后，对应测量域的机器人关节配置可以表示为其中j＝1,…,t。对选择出来的关节角配置开展点位稳定性测量，最终测量得到的测量域数据可表示为：至此完成了测量域机器人关节配置的选择和有限位置处机器人末端位姿误差的获取，即完成了测量域数据集的构建。

1.4基于半监督迁移学习的机器人位姿误差感知

具体而言，分为以下几个步骤；

①在仿真域上预先训练一个神经网络，并且固定前两层的网络参数，如图三所示。对于后四层的参数，仿真域训练好的模型仅提供初始值。图三中通过不同的填充图案对固定参数和调整参数进行了区分。神经网络的相关参数如下表所示。

输入层节点数目	6
		隐藏层1节点数目	16
隐藏层2节点数目	32
		隐藏层3节点数目	64
隐藏层4节点数目	32
		隐藏层5节点数目	16
输出层节点数目	6
		层间激活函数	Relu
网络损失函数指标	MSE

②利用测量域数据对步骤①后的模型进行领域分布适配。具体而言，对于边缘分布的适配，可以通过如下表达进行度量。

其中为关节角空间到高维希尔伯特核空间的映射函数。

对于条件分布的适配，可以通过如下表达进行度量。

其中，

式中矩阵对应位置处的元素可以表示为：其中k(·,·)表示使用的核函数，本专利中设置为径向基核函数。

为防止单一适配无法完全适配数据分布之间的差异，本发明将上述两部分领域分布适配的度量函数进行加权，加权后的分布适配可以表达为：

其中λ为权重因子，反映了仿真域和测量域两种分布差异的权重，通过在区间[0,1]中以0.1的间隔均匀取样后，选择预测效果最好的取值。

③将一般模型训练的回归损失函数度量和上述加权后的领域分布适配度量联合表达，可以表示为半监督迁移学习的融合损失函数：

其中为回归损失函数，其表达式为：

g(·)表示关节角到末端位姿误差的映射关系，W^TW为模型参数组成的正则化项，为了防止过拟合，α和β是调和系数，用于调整回归损失和领域适配损失在数值上的关系，避免模型参数向着数值大的方向更新，γ是正则化系数。

模型微调过程为模型的自学习过程，通过最小化上述半监督迁移学习的融合损失函数借助Adam优化器实现模型参数的确定。

④完成步骤②后，即可对仿真域的所有关节配置下的末端位姿进行预测。完成预测后即完成了对于机器人空间点位位姿误差的感知。后续可以根据感知到的结果对空间点位进行预先的修改以实现准确的机器人空间运动。

1.5基于感知结果的机器人位姿误差补偿

在完成1.4后，即完成了对于所需开展的机器人任务中各个关节角下的位姿误差感知，定义最终位姿误差预测模型可表示为函数finetune(·)，对于第i个关节角，预测结果可以表示为：

由于机器人在执行运动的时候是按照理论运动学参数进行执行的，因此对应的空间位姿为P_Ti，则补偿后的第i个关节角对应的机器人空间位姿可以表示为：

将对应的P_{Compensation-i}写入到机器人运行程序中，即完成了对感知到位姿误差的补偿。

本发明提供的方法首先对机器人的运动学误差进行粗略的标定，基于标定结果，将机器人位姿误差的感知问题定义为迁移学习的问题，其中源域为仿真域，所描述的机器人位姿误差是机器人理论运动学参数下的位姿与利用标定后运动学参数计算得到的位姿之间的偏差。目标域为测量域，所描述的机器人位姿误差是机器人理论运动学参数下的位姿与利用激光跟踪仪测量得到的位姿之间的偏差。其中目标域样本是利用主动学习策略主动选择得到的，明确了空间全部点位中需要测量的机器人关节角配置。后续利用半监督迁移学习算法，将仿真域的领域知识迁移到测量域上，实现了机器人任务中各关节角下机器人位姿误差的精确感知。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，该方法包括下列步骤：

S1在机器人运动空间内选择多个位置，控制机器人运动至所选择的位置并采集运动过程中的数据，以此计算机器人运动学参数的误差；

S2对所需开展的机器人任务，设定机器人运动过程中的理论关节角，利用该理论关节角计算理论空间位姿，结合步骤S1中获得的运动学参数误差计算在所述理论关节角下的仿真空间位姿，利用所述理论空间位姿和仿真空间位姿计算位姿误差，以此获得所述理论关节角与位姿误差之间一一对应的数据集，即仿真域数据集；

S3按照预先构建的方法从所述理论关节角中选取出部分数据，机器人按照选取出来的关节角运动并执行测量，计算测量结果与S2中理论空间位姿之间的偏差，即获得每个选取出来的理论关节角对应的测量位姿误差，以此形成所选理论关节角与测量位姿误差之间一一对应的数据集，形成测量域数据集；

所述预先构建的方法按照下列进行：选取三组数据，分别为：第1个关节角样本，第2～6个关节角样本，第7～t个关节角样本，其中，t是正整数，满足t≥min(20,dimension)，dimension为关节角的维度；

S4利用所述仿真域数据集构建预训练神经网络，采用半监督迁移学习的方法利用所述测量域数据集对所述神经网络进行微调，获得微调后的神经网络；

S5利用微调后的神经网络预测所述测量域中未被选择的关节角对应的位姿误差，利用该位姿误差对机器人位姿进行补偿，以此实现所需开展的任务的高精度运行。

2.如权利要求1所述的一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，在步骤S1中，所述机器人运动学参数的误差按照下列关系式计算：

δ＝([B₁,B₂,…,B_i,…,B₅₀]^T)⁺[⁶D₁,⁶D₂,…,⁶D_i,…,⁶D₅₀]^T

其中，B_i是第i个测量点处，机器人所有连杆误差与机器人末端误差之间的变换矩阵，其规模为6×24，⁶D_i是第i个测量点位测量得到的微分运动矢量，上角标T是对矩阵进行转置，上角标+是对矩阵求广义逆。

3.如权利要求1所述的一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，在步骤S2中，所述利用该理论关节角计算理论空间位姿采用机器人正运动学的方法计算。

4.如权利要求1所述的一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，在步骤S3中，所述三组数据按照下列方式进行选择：

其中，d(·,·)是欧式距离度量函数，θ_i是仿真域中第i个样本的关节角，θ_C是仿真域中所有样本的关节角通过聚类得到的聚类中心对应的关节角，是仿真域中未被选择的样本中的第i个对应的关节角，是测量域中的第j个样本对应的关节角，f_p(·)是利用已选择出的关节角和对应测量得到的位姿误差训练的一个从关节空间到位姿误差空间的弱学习器，P是输出的维度，是测量域对于已经选择出来的关节角测量得到的对应的位姿误差，i是仿真域数据的索引，j是测量域数据的索引，t是测量域需要被测量的关节角数目，n是仿真域数据的总体数目，k是已经完成选择的关节角数目。

5.如权利要求1所述的一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，在步骤S4中，所述半监督迁移学习的分布适配按照下列进行：

其中，D_MDA(θ_S,θ_M)是边缘分布差异的度量函数，θ_S,θ_M分别是仿真域和测量域中样本的关节角；D_CDA(D_S,D_M)是条件分布差异的度量函数，D_S,D_M分别是仿真域和测量域中样本的关节角及其对应的位姿误差；λ是权重因子，反映仿真域和测量域在边缘分布差异和条件分布差异上的比重，是加权后的分布差异适配函数。

6.如权利要求5所述的一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，所述半监督迁移学习的融合损失函数按照下列进行：

其中，是回归损失函数；是加权后的分布差异适配函数；W^TW是模型参数的正则化项；α是回归损失函数加权系数；β是加权后的分布差异适配函数加权系数，γ是正则化系数。

7.如权利要求6所述的一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，所述半监督迁移学习的回归损失函数按照下列进行：

其中，是仿真域数据的回归损失函数，是测量域数据的回归损失函数，g(·)是关节角与位姿误差的映射函数，θ_S,θ_M分别是仿真域和测量域中样本的关节角；e_S,e_M分别是仿真域和测量域中样本对应的位姿误差，上角标i和j分别为样本的序号标识，n是仿真域所有样本的数目，t是测量域所有被选择出来的样本的数目。

8.如权利要求6所述的一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，在步骤S4中，对所述神经网络进行微调时，最小化所述融合损失函数以此实现微调过程。

9.如权利要求1所述的一种基于主动半监督迁移学习的机器人位姿误差感知方法，其特征在于，在步骤S5中，所述补偿按照下列关系式进行：

其中，finetune(·)是微调后的神经网络模型，是利用微调后的神经网络模型对第i个关节角θ_i的位姿误差的预测结果，P_{Compensation-i}为第i个关节角经过补偿之后的实际位姿，P_Ti是第i组关节角对应的理论空间位姿。