本发明公开了一种机器人运动学参数标定位姿优化选取方法，包括：1、采用运动学参数误差雅可比矩阵的能观指数作为优化指标，2、使用迭代优化方法在机器人末端动平台的工作空间中全局采集位姿测量点作为初始解集，3、在工作空间中进行全局优化求解得到最优的位姿解集使得优化指标最优，4、剔除位姿解集中冗余解使得解集中优化指标最优和需要采集的位姿点数量最少。本发明通过对机器人运动学参数误差标定过程中测量位姿的优化选取，从而提高参数标定的精度和鲁棒性。
1.一种机器人运动学参数标定位姿优化选取方法，其特征是按如下步骤进行：步骤1、对所需要标定的机器人建立运动学模型，记为P＝f(x，y)，其中x表示机器人的驱动关节的变化参数，y表示机器人结构的运动学参数，P表示机器人末端动平台在工作空间中所对应的位姿；
步骤2、根据机器人的运动学模型P，利用式(1)求解运动学参数误差的雅可比矩阵J：采用奇异值分解法对雅可比矩阵J进行分解，得到J＝U∑V，并有：其中，σi表雅可比矩阵J的奇异值，且σ1≥σ2…≥σL≥0；则σL表示最小的奇异值，σ1表示最大奇异值；
利用式(2)得到参数辨识雅可比矩阵能观指数O，并作为优化指标：式(2)中，N表示需要辨识的运动学参数的数目；
步骤3.1、在机器人末端动平台的工作空间中随机选择一组NM个标定测量的位姿作为初始位姿，记作 最大迭代次数为NI，当前迭代次数NI′＝0；
步骤3.2、如果当前迭代次数NI′≥NI，跳出当前迭代循环过程，输出优化后的位姿集否则，调用步骤4，并接受步骤4输出的位姿步骤3.3、将 与集合 合并，从而更新步骤3.4、调用步骤5，剔除η+中的冗余位置 并将NI′+1赋值给NI′后，返回步骤3.2；
步骤4、利用禁忌优化算法优化选取机器人末端动平台的位姿 使得参数辨识雅可比矩阵的能观指数O最优；
步骤4.1、初始化禁忌列表长度为L，领域解个数为k，可行解集 禁忌列表集领域收缩因子为w，最大迭代次数为INI，当前最优解集 位姿的下限ηlower和上限ηuper；
令当前最优解值 其中 表示在机器人末端动平台的工作空间中随机采集一个位姿 作为当前解，J(η,rand(ηM+1)表示在初始位姿η和随机位姿rand(ηM+1)处运动学参数误差的雅可比矩阵；O(J(η,rand(ηM+1)))表示在初始位姿η和随机位姿rand(ηM+1)处运动学参数误差的雅可比矩阵J(η,rand(ηM+1)的能观指数，步骤4.2、如果当前迭代次数INI′≥INI，则跳出当前循环，将 输出；否则利用式(3)产生k个领域解：式(3)中， 表示位姿 随机产生的第i个领域解，且有i＝1,2,...,k；
步骤4.3、将步骤4.2产生的所有k个领域解与可行解集进行合并，并将赋值给OL；
步骤4.3.1、如果当前可行解集 跳出循环，输出当前最优解BSF，进行步骤4.4；
否则，从可行解集OL中取出一个领域解 如果 则令并且从可行解集OL中剔除所取出的领域解 后，返回步骤4.3.1；否则执行步骤4.3.2；
步骤4.3.2、对所取出的领域解 求得能观指数 并将最大的能观指数记作当前最优值 再将所取出的领域解放入当前最优解集BSF中和禁忌列表集TL，从而更新禁忌列表集TL；
步骤4.3.3、如果更新后的禁忌列表集TL的长度大于L，则将禁忌列表清空，即 并返回步骤4.3.1；否则，执行步骤4.4；
步骤4.4、如果当前最优解集 则将当前解 放入禁忌列表TL中，从而更新禁忌列表TL为TL∪ηM+1，将INI′+1赋值给INI′后返回步骤3.2；
步骤5.1、调用步骤3产生的位姿解集η+，初始化最大迭代次数RNI＝NM+1，当前迭代次数为RNI′＝1；能观指数列表为 最大能观指数为O(J)＝Max{O(J(η,rand(ηM+1)))}；
步骤5.2、如果当前迭代次数RNI′≥NM+1，则跳出当前循环，输出冗余位置η；否则，从位姿解集η+中取一个位姿 并计算当前能观指数步骤5 .3、如果当前能 观指数 则更新最大能观指数更新能观指数列表ObL为 并获得最小能观指数 在能观指数列表ObL中所对应的序号RNI′，将位姿解集η+中剔除序号-
RNI′所对应的位姿 后赋值给冗余位置η，再执行步骤5.4；否则，直接执行步骤5.4；
步骤5.4、将RNI′+1赋值给RNI′，并返回步骤5.2。
一种机器人运动学参数标定位姿优化选取方法
技术领域
[0001] 本发明涉及机器人运动学参数标定领域，具体涉及一种机器人运动学参数标定的位姿选取方法。
背景技术
[0002] 机器人在生产加工过程中优化加工、装配等因素造成机器人结构参数产生误差，会导致机器人的精度降低，所以在出厂之前必须要对机器人的运动学参数进行标定。目前采用的标定方法均是采集多组末端位姿，利用理论和实际的误差来进行优化辨识。由于机器人参数非常多，需要采集非常多位姿点来提高机器人运动学参数的辨识精度，这样导致参数标定需要花费非常多的资源。机器人标定需要采集的位姿点遍布机器人末端动平台的工作空间中，这样才能保证标定结果的准确性。由于采集的位姿点数非常多，传统的优化算法不能一次优化所有的标定点，而且计算难度大，传统的优化方法需要花费更多的计算资源来进行优化。在机器人工作空间中随机采集测量位姿会使参数误差辨识雅可比矩阵的能观指数降低，使得参数误差辨识雅可比矩阵对运动学误差参数不敏感，辨识结果鲁棒性差，辨识精度差。生产过程中必须对所有机器人进行标定，亟需要提高位姿采集的速度，提高标定效率；并且，需要保证辨识结果的鲁棒性和精度，需要位姿测量点使得参数误差辨识雅可比矩阵的能观指数最优。
发明内容
[0003] 本发明针对参数标定位姿优化选取的难题，提供一种机器人运动学参数标定位姿优化选取方法，以期通过对机器人运动学参数误差标定过程中测量位姿的优化选取，使得参数误差辨识雅可比矩阵的能观指数最优，从而提高参数标定的鲁棒性和精度。
[0004] 本发明为达到上述发明目的，采用如下技术方案：
[0005] 本发明一种机器人运动学参数标定位姿优化选取方法的特点是按如下步骤进行：
[0006] 步骤1、对所需要标定的机器人建立运动学模型，记为P＝f(x，y)，其中x表示机器人的驱动关节的变化参数，y表示机器人结构的运动学参数，P表示机器人末端动平台在工作空间中所对应的位姿；
[0007] 步骤2、根据机器人的运动学模型P，利用式(1)求解运动学参数误差的雅可比矩阵J：
[0008]
[0009] 采用奇异值分解法对雅可比矩阵J进行分解，得到J＝U∑V，并有：
[0010] 其中，σi表雅可比矩阵J的奇异值，且σ1≥σ2…≥σL≥0；则σL表示最小的奇异值，σ1表示最大奇异值；
[0011] 利用式(2)得到参数辨识雅可比矩阵能观指数O，并作为为优化指标：
[0012]
[0013] 式(2)中，N表示需要辨识的运动学参数的数目；
[0014] 步骤3、随机采集位姿并迭代优化；
[0015] 步骤3.1、在机器人末端动平台的工作空间中随机选择一组NM个标定测量的位姿作为初始位姿，记作 最大迭代次数为NI，当前迭代次数N′I＝0；
[0016] 步骤3.2、如果当前迭代次数N′I≥NI，跳出当前迭代循环过程，输出优化后的位姿集 否则，调用步骤4，并接受步骤4输出的位姿
[0017] 步骤3.3、将 与集合 合并，从而更新
[0018] 步骤3.4、调用步骤5，剔除η+中的冗余位置 并将N′I+1赋值给N′I后，返回步骤3.2；
[0019] 步骤4、利用禁忌优化算法优化选取机器人末端动平台的位姿 使得参数辨识雅可比矩阵的能观指数O最优；
[0020] 步骤4.1、初始化禁忌列表长度为L，领域解个数为k，可行解集 禁忌列表集 领域收缩因子为w，最大迭代次数为INI，当前最优解集 位姿的下限ηlower和上限ηuper；
[0021] 令当前最优解值 其中 表示在机器人末端动平台的工作空间中随机采集一个位姿 作为当前解，J(η,rand(ηM+1)表示在初始位姿η和随机位姿rand(ηM+1)处运动学参数误差的雅可比矩阵；O(J(η,rand(ηM+1)))表示在初始位姿η和随机位姿rand(ηM+1)处运动学参数误差的雅可比矩阵J(η,rand(ηM+1)的能观指数，[0022] 步骤4.2、如果当前迭代次数IN′I≥INI，则跳出当前循环，将 输出；否则利用式(3)产生k个领域解：
[0023]
[0024] 式(3)中， 表示位姿 随机产生的第i个领域解，且有i＝1,2,...,k；rand表示随机生成(0,1)之间的浮点数；
[0025] 如果 且
[0026] 则有
[0027] 如果 且
[0028] 则有
[0029] 步骤4.3、将步骤4.2产生的所有k个领域解与可行解集进行合并，并将赋值给OL；
[0030] 步骤4.3.1、如果当前可行解集 跳出循环，输出当前最优解BSF，进行步骤4.4；否则，从可行解集OL中取出一个领域解 如果 则令
并且从可行解集OL中剔除所取出的领域解 后，返回步骤4.3.1；否则
执行步骤4.3.2；
[0031] 步骤4.3.2、对所取出的领域解 求得能观指数并将最大的能观指数记作当前最优值 再将所取出的领
域解 放入当前最优解集BSF中和禁忌列表集TL，从而更新禁忌列表集TL；
[0032] 步骤4.3.3、如果更新后的禁忌列表集TL的长度大于L，则将禁忌列表清空，即并返回步骤4.3.1；否则，执行步骤4.4；
[0033] 步骤4.4、如果当前最优解集 则将当前解 放入禁忌列表TL中，从而更新禁忌列表TL为TL∪ηM+1，将IN′I+1赋值给IN′I后返回步骤3.2；
[0034] 步骤5、剔除解集中的冗余位姿；
[0035] 步骤5.1、调用步骤3产生的位姿解集η+，初始化最大迭代次数RNI＝NM+1，当前迭代次数为RN′I＝1；能观指数列表为 最大能观指数为O(J)＝Max{O(J(η,rand(ηM+1)))}；
[0036] 步骤5.2、如果当前迭代次数RN′I≥NM+1，则跳出当前循环，输出冗余位置η-；否则，从位姿解集η+中取一个位姿 并计算当前能观指数
[0037] 步骤5 .3、如果当前能观指数 则更新最大能观指数更新能观指数列表 并获得最小能观
指数 在能观指数列表ObL中所对应的序号RN′I，将位姿解集η+中剔除序号
-
RN′I所对应的位姿 后赋值给冗余位置η，再执行步骤5.4；否则，直接执行步骤5.4；
[0038] 步骤5.4、将RN′I+1赋值给RN′I，并返回步骤5.2。
[0039] 与已有技术相比，本发明的有益效果体现在：
[0040] 1、本发明采用运动学参数误差雅可比矩阵的能观指数作为优化指标并在机器人工作空间中全局选取位姿点，利用全局优化算法对选择的位姿点进行优化，使得优化指标最优，最后剔除优化位姿解集中的冗余解，从而保证了迭代优化后的解集最优，减少测量位姿点的数目，提高辨识速度和辨识结果的鲁棒性；
[0041] 2、本发明采用运动学参数误差雅可比矩阵的能观指数作为优化指标，选择位姿使得优化指标最优，提高了辨识结果的鲁棒性和精度；
[0042] 3、本发明采用全局优化算法，在机器人工作空间中全局选取优化位姿，解决了传统优化算法的不足，降低了算法的计算量，提高了算法效率；
[0043] 4、本发明选取优化的位姿用于标定机器人的运动学参数误差，可以使得测量位姿点的数目最小，同时保证了辨识结果的鲁棒性和精度，大大提高了标定速度和生产效率。
具体实施方式
[0044] 本实施例中，一种新型参数标定位姿选取系统，是用于机器人运动学参数误差标定中，优化选取测量位姿，使得参数雅可比矩阵的能观指数最优。在参数标定位置选取系统中设置有随机位姿采集模块，智能优化算法位姿优化选取姿态，冗余位姿剔除模块。
[0045] 参数标定随机位置姿态采集模块采用迭代优化的方法，在机器人的工作空间中随机采集一组用于标定测的位置姿态集；将生成的随机测量位置姿态迭代传给禁忌优化算法模块，并将优化选取后的测量位置姿态设置在一组待测量的位置姿态集中，从而得到优化后的位置姿态集。
[0046] 禁忌优化算法模块用于优化采集的位置姿态集，使得辨识参数雅可比矩阵的能观指数参数最优。
[0047] 冗余位置姿态剔除模块用于对优化选取的位置姿态集中剔除冗余的位置姿态，保证测量的位置姿态集中的所有位置姿态都是最优。
[0048] 本实施例中，一种机器人运动学参数标定位姿优化选取方法，是按如下步骤进行：
[0049] 步骤1、对所需要标定的机器人建立运动学模型，记为P＝f(x，y)，其中x表示机器人的驱动关节的变化参数，y表示机器人结构的运动学参数，P表示机器人末端动平台在工作空间中所对应的位姿；机器人结构的运动学参数一般是指机器人结构的尺寸参数，电机与减速机的减速比，末端动平台相对机器人的参考关系。这些参数的辨识精度主要受标定选用的位姿点的影响，所以在辨识前选定最优的测量位姿点。
[0050] 步骤2、根据机器人的运动学模型P，利用式(1)求解运动学参数误差的雅可比矩阵J：
[0051]
[0052] 采用奇异值分解法对雅可比矩阵J进行分解，得到J＝U∑V，并有：
[0053] 其中，σi表雅可比矩阵J的奇异值，且σ1≥σ2…≥σL≥0；则σL表示最小的奇异值，σ1表示最大奇异值；
[0054] 利用式(2)得到参数辨识雅可比矩阵能观指数O，并作为为优化指标：
[0055]
[0056] 式(2)中，N表示需要辨识的运动学参数的数目，根据实际标定需要来确定；
[0057] 步骤3、随机采集位姿并迭代优化；
[0058] 步骤3.1、在机器人末端动平台的工作空间中随机选择一组NM个标定测量的位姿作为初始位姿，记作 NM取值和机器人的运动学模型能够提供的约束方程个数fn以及需要辨识的参数个数N有关，一般取值NM·fn≥2N；最大迭代次数为NI，可以取值NI＝2NM，当前迭代次数N′I＝0；
[0059] 步骤3.2、如果当前迭代次数N′I≥NI，跳出当前迭代循环过程，输出优化后的位姿集 否则，调用步骤4，并接受步骤4输出的位姿
[0060] 步骤3.3、将 与集合 合并，位姿解集的合并在算法上体现在存储方式上的合并，可以采用队列存储的方式，存储在前一项解集的队列后面；从而更新[0061] 步骤3.4、调用步骤5，剔除η+中的冗余位置 位姿剔除也是在存储队
列中将冗余的位姿删除即可；并将N′I+1赋值给N′I后，返回步骤3.2；
[0062] 步骤4、利用禁忌优化算法优化选取机器人末端动平台的位姿 使得参数辨识雅可比矩阵的能观指数O最优；
[0063] 步骤4.1、初始化禁忌列表长度为L，取值L＝2NM，领域解个数为k，取值k＝4NM可行解集 禁忌列表集 领域收缩因子为w，一般取值w＝0.998，可以根据算法的收敛速度进行调整，如果收敛速度慢，可以减小收缩因子，最大迭代次数为INI，迭代次数需要比较大，尽量能够收敛到全局最优解，可以根据解集规模来确定，可以取值INI＝50NM，当前最优解集 位姿的下限ηlower和上限ηuper；上下限的取值根据机器人的工作空间来确定，为了保证能够遍历整个机器人的工作空间，一般取值在工作空间中最大内接立方体的边界处；
[0064] 令当前最优解值 其中 表示在机器人末端动平台的工作空间中随机采集一个位姿 作为当前解，J(η,rand(ηM+1)表示在初始位姿η和随机位姿rand(ηM+1)处运动学参数误差的雅可比矩阵；O(J(η,rand(ηM+1)))表示在初始位姿η和随机位姿rand(ηM+1)处运动学参数误差的雅可比矩阵J(η,rand(ηM+1)的能观指数，这是一个函数表示式，表示上式中式(1)和式(2)最终求得的能观指数，可以根据该式对能观指数进行求解；
[0065] 步骤4.2、如果当前迭代次数IN′I≥INI，则跳出当前循环，将 输出；否则利用式(3)产生k个领域解：
[0066]
[0067] 式(3)中， 表示位姿 随机产生的第i个领域解，且有i＝1,2,...,k；rand表示随机生成(0,1)之间的浮点数；
[0068] 如果 且
[0069] 则有
[0070] 如果 且
[0071] 则有
[0072] 步骤4.3、将步骤4.2产生的所有k个领域解与可行解集进行合并，并将赋值给OL；这里对解集进行合并和剔除冗余解的操作都是在存储队列中
对数据进行保存和删除操作，解集列表即是一个存储列表；
[0073] 步骤4.3.1、如果当前可行解集 跳出循环，输出当前最优解BSF，进行步骤4.4；否则，从可行解集OL中取出一个领域解 如果 则令
并且从可行解集OL中剔除所取出的领域解 后，返回步骤4.3.1；否则
执行步骤4.3.2；
[0074] 步骤4.3.2、对所取出的领域解 求得能观指数并将最大的能观指数记作当前最优值 再将所取出的领
域解 放入当前最优解集BSF中和禁忌列表集TL，从而更新禁忌列表集TL；
[0075] 步骤4.3.3、如果更新后的禁忌列表集TL的长度大于L，则将禁忌列表清空，即并返回步骤4.3.1；否则，执行步骤4.4；
[0076] 步骤4.4、如果当前最优解集 则将当前解 放入禁忌列表TL中，从而更新禁忌列表TL为TL∪ηM+1，将IN′I+1赋值给IN′I后返回步骤3.2；
[0077] 步骤5、剔除解集中的冗余位姿；
[0078] 步骤5.1、调用步骤3产生的位姿解集η+，初始化最大迭代次数RNI＝NM+1，当前迭代次数为RN′I＝1；能观指数列表为 最大能观指数为O(J)＝Max{O(J(η,rand(ηM+1)))}；
[0079] 步骤5.2、如果当前迭代次数RN′I≥NM+1，则跳出当前循环，输出冗余位置η-；否则，从位姿解集η+中取一个位姿 并计算当前能观指数
[0080] 步骤5 .3、如果当前能观指数 则更新最大能观指数更新能观指数列表ObL为 并获得最小能观
指数 在能观指数列表ObL中所对应的序号RN′I，将位姿解集η+中剔除序号
RN′I所对应的位姿 后赋值给冗余位置η-，再执行步骤5.4；否则，直接执行步骤5.4；
[0081] 步骤5.4、将RN′I+1赋值给RN′I，并返回步骤5.2。
[0082] 在算法优化过程中总是随机产生位姿点以及总是使得优化到使得能观指数最优，所以算法在概率上完备有效，总是能够收敛到最优的位姿点。算法的收敛时间随着迭代次数和需要辨识参数个数，以及需要测量的位姿点数的增大而增大，不过总是能够收敛到最优的一组位姿解集。
