机器人关节激励轨迹生成方法、机器人关节动力学参数辨识方法与流程

本发明涉及机器人动力学参数辨识，具体涉及一种机器人关节激励轨迹生成方法、机器人关节动力学参数辨识方法。

背景技术：

1、机器人动力学模型用于描述机器人关节的运动状态与关节驱动力矩之间的关系，是机器人轨迹控制、重力补偿、碰撞检测、拖动示教及力控制等功能的基础。为获得动力学模型中的参数，通常控制机器人关节执行预先设计的激励轨迹，采集关节角度、关节角速度、关节角加速度及关节驱动力矩等数据，并利用所采集的数据辨识机器人动力学参数。现有机器人动力学参数辨识方法通常将刚体动力学模型整理为与惯性参数或最小动力学参数集线性相关的形式，并根据该线性化模型对应的观测矩阵评价和设计激励轨迹。

2、其中，机器人关节在启动和低速运动过程中还具有表征静摩擦与库仑摩擦差异的非线性摩擦特性。为描述该非线性摩擦特性，现有技术还在机器人关节动力学模型中引入stribeck非线性摩擦模型。然而，现有技术中通常将动力学模型中的刚体动力学参数与非线性摩擦参数分别进行处理，例如先依据线性化的刚体动力学模型设计激励轨迹并辨识刚体动力学参数，再根据实测关节驱动力矩与刚体动力学模型所计算力矩之间的差值，进一步估计非线性摩擦参数。

3、但是，由于在激励轨迹的设计生成阶段，非线性摩擦参数未与刚体动力学参数共同作为轨迹评价对象，所设计的激励轨迹主要用于激发惯性、重力等刚体动力学参数，难以为主要在低速过渡状态下产生明显影响的非线性摩擦参数提供充分的辨识信息。并且，后续用于辨识非线性摩擦参数的力矩差值，不仅包含实际摩擦力矩，还可能混入前一阶段刚体动力学参数的辨识误差、模型误差及测量噪声。上述误差可能被进一步吸收到非线性摩擦参数的辨识结果中，使刚体动力学参数与非线性摩擦参数的误差相互传递，导致部分参数辨识结果不稳定，并影响动力学模型在机器人启动及低速运动状态下的关节驱动力矩预测准确性。

技术实现思路

1、申请人发现，在沿用基于线性化刚体动力学模型的固定观测矩阵评价候选激励轨迹时，惯性参数、质量矩等刚体动力学参数通常能够以线性形式参与关节驱动力矩的计算。在候选激励轨迹确定后，可以根据对应的关节角度、关节角速度和关节角加速度建立不依赖于相应待辨识参数取值的固定观测矩阵，并据此评价候选激励轨迹。与此不同，至少部分非线性摩擦参数以非线性方式参与关节驱动力矩的计算。例如，stribeck特征速度位于指数函数内部。将该类非线性摩擦参数与刚体动力学参数共同纳入动力学模型后，动力学模型难以继续整理成由固定观测矩阵与待辨识参数向量相乘得到关节驱动力矩的线性形式；关节驱动力矩相对于该类非线性摩擦参数的参数灵敏度还可能随关节运动状态及相应参数的取值发生变化。因此，难以直接沿用传统的固定线性观测矩阵评价候选激励轨迹。

2、基于此，本发明的第一目的在于，改善现有激励轨迹所生成的数据对以非线性方式参与关节驱动力矩计算的非线性摩擦参数的辨识信息覆盖程度，使机器人执行目标激励轨迹后获得的数据能够更充分地反映非线性摩擦参数对关节驱动力矩的影响。

3、申请人进一步发现，将刚体动力学参数和非线性摩擦参数共同作为激励轨迹所服务的辨识对象后，参数数量的增加并不是主要困难。更重要的是，两类参数对应的物理作用不同，其对关节驱动力矩产生明显影响所需的关节运动状态也不同。例如，惯性参数主要在关节角加速度发生变化时产生明显影响，而stribeck特征速度等非线性摩擦参数主要在启动及低速过渡状态下产生明显影响。因此，同一候选激励轨迹所生成的数据未必能够以相同程度反映各待辨识参数的作用。部分待辨识参数在相应候选激励轨迹下形成的参数灵敏度可能较弱，不同待辨识参数变化所引起的关节驱动力矩变化规律也可能较为接近。若候选激励轨迹不能兼顾不同待辨识参数对关节运动状态的不同需求，即使增加数据量或者采用更复杂的参数估计方法，仍可能出现部分参数辨识信息不足、不同参数的作用难以区分以及辨识结果容易受到测量误差影响的问题。

4、因此，本发明的第二目的在于，提高同一目标激励轨迹所采集的数据对于刚体动力学参数和非线性摩擦参数联合辨识的有效性和稳定性，降低因部分参数辨识信息不足或者不同参数的作用难以区分而产生的辨识不稳定，并有利于提高动力学模型在机器人启动及低速运动状态下的关节驱动力矩预测准确性。

5、为达成上述目的，本发明采用如下技术方案：

6、本发明的第一方面提供一种机器人关节激励轨迹生成方法，包括：在机器人关节运动约束下，基于评价值迭代更新轨迹参数，直至满足预设终止条件，并从迭代过程中确定具有最小评价值的轨迹参数，基于所述轨迹参数输出目标激励轨迹；其中，所述评价值通过以下步骤在每次轨迹参数更新过程中生成：根据当前轨迹参数确定候选激励轨迹对应的关节运动状态序列；所述轨迹参数为用于表示所述候选激励轨迹的关节角度随时间变化规律的参数化表达中的待优化参数；所述关节运动状态序列包括所述候选激励轨迹在多个采样时刻下的关节角度、关节角速度和关节角加速度；根据待辨识机器人关节的动力学模型和所述关节运动状态序列，确定在所述多个采样时刻下，所述动力学模型输出的关节驱动力矩相对于所述动力学模型中各待辨识参数的参数灵敏度；所述动力学模型包括刚体动力学部分和摩擦部分，所述摩擦部分包括非线性摩擦项；所述待辨识参数包括所述刚体动力学部分对应的刚体动力学参数和所述摩擦部分对应的摩擦参数，所述摩擦参数包括以非线性方式参与所述关节驱动力矩计算的非线性摩擦参数；根据所述多个采样时刻下各待辨识参数的参数灵敏度建立参数信息矩阵；所述参数信息矩阵中与任意两个待辨识参数对应的元素，由该两个待辨识参数在所述多个采样时刻下的参数灵敏度乘积累加得到；根据所述参数信息矩阵与正则化矩阵之和的行列式获得第一评价结果，并根据所述参数信息矩阵的条件数获得第二评价结果；所述正则化矩阵与所述参数信息矩阵同阶，所述正则化矩阵的各对角元素为相同的正的预设正则化量，且其余元素为零；对所述第一评价结果作单调递减映射，获得第一评价量，并对所述第二评价结果作单调递增映射，获得第二评价量，使所述第一评价量和所述第二评价量均以数值减小的方向表征所述候选激励轨迹的优化，并减小所述第一评价量和所述第二评价量之间的数值变化范围差异；根据所述第一评价量和所述第二评价量形成与所述当前轨迹参数对应的评价值，所述评价值越小，表征与所述当前轨迹参数对应的候选激励轨迹越优。

7、采用上述技术方案后，本发明基于同时包括刚体动力学部分和摩擦部分的动力学模型，在候选激励轨迹的多个采样时刻下，分别确定动力学模型输出的关节驱动力矩相对于各待辨识参数的参数灵敏度。所述参数灵敏度用于反映在相应关节运动状态下，待辨识参数的变化对动力学模型所输出关节驱动力矩的影响程度。由此，即使摩擦部分中的部分非线性摩擦参数以非线性方式参与关节驱动力矩的计算，不能与刚体动力学参数共同形成仅由关节运动状态确定的固定线性观测矩阵，该类非线性摩擦参数仍能够通过关节驱动力矩相对于其自身的参数灵敏度进入候选激励轨迹的评价过程，从而使激励轨迹的生成不再仅服务于刚体动力学参数的辨识。

8、进一步地，本发明根据各待辨识参数在多个采样时刻下的参数灵敏度建立参数信息矩阵。所述参数信息矩阵中与任意两个待辨识参数对应的元素，由该两个待辨识参数在多个采样时刻下的参数灵敏度乘积累加得到。因此，该参数信息矩阵不仅能够汇总各待辨识参数在整条候选激励轨迹下对关节驱动力矩所产生影响的总体强度，还能够反映不同待辨识参数的参数灵敏度随候选激励轨迹变化时所形成的关联关系。由于参数信息矩阵同时对应刚体动力学参数和摩擦参数，候选激励轨迹是否包含能够有效反映非线性摩擦参数作用的关节运动状态，以及不同待辨识参数在该轨迹下是否形成易于区分的力矩变化规律，均能够体现在该参数信息矩阵中。由此，根据所述参数信息矩阵评价和更新轨迹参数，有利于使目标激励轨迹所包含的关节角度、关节角速度和关节角加速度变化同时适应刚体动力学参数与非线性摩擦参数的辨识需求，改善后续采集数据对非线性摩擦参数的辨识信息覆盖程度。

9、同时，刚体动力学参数与摩擦参数对应的物理作用不同，其产生明显力矩影响所需的关节运动状态也不同。例如，惯性参数通常在关节角加速度变化时产生较明显的力矩影响，而部分非线性摩擦参数主要在启动及低速过渡状态下产生较明显的力矩影响。因此，同一候选激励轨迹下可能同时存在辨识信息较强的参数方向和辨识信息较弱的参数方向，或者存在不同参数的灵敏度变化规律较为接近的情况。若仅侧重提高全部待辨识参数所形成的总体辨识信息，评价结果可能主要受辨识信息较强的参数方向影响，使部分摩擦参数仍未得到充分激励；若仅侧重减小不同参数方向之间的信息差异，又可能得到不同参数方向相对均衡但总体辨识信息不足的候选激励轨迹。

10、为此，本发明根据参数信息矩阵与正则化矩阵之和的行列式获得第一评价结果，并根据参数信息矩阵的条件数获得第二评价结果。第一评价结果能够从整体上反映各待辨识参数共同形成的辨识信息规模，有利于避免目标激励轨迹所提供的总体辨识信息不足；第二评价结果能够反映参数信息矩阵在不同参数方向上的辨识信息不均衡程度，有利于降低部分参数方向过强而部分参数方向过弱，或者不同参数的作用难以区分所导致的数值病态问题。由此，通过第一评价结果和第二评价结果共同评价候选激励轨迹，可以在提高总体辨识信息规模的同时，兼顾不同参数方向上的辨识信息分布，降低评价结果被少数强信息参数方向过度主导的可能。

11、进一步地，正则化矩阵与参数信息矩阵同阶，并具有相同的正对角元素，因而在候选激励轨迹对部分待辨识参数激励不足、参数信息矩阵接近奇异时，仍能够降低行列式为零或者数值过小所造成的评价失效风险，提高第一评价结果在轨迹迭代过程中的数值稳定性。该正则化处理并不改变利用参数信息矩阵评价各待辨识参数辨识信息的基本关系，而是使不同质量的候选激励轨迹均能够获得可用于比较的第一评价结果，从而有利于维持轨迹参数迭代更新过程的连续性和稳定性。

12、此外，参数信息矩阵的行列式通常越大，表示总体辨识信息规模越大，而参数信息矩阵的条件数通常越小，表示不同参数方向上的辨识信息越均衡，二者具有不同的优劣方向和数值变化范围。为此，本发明对第一评价结果作单调递减映射，对第二评价结果作单调递增映射，使第一评价量和第二评价量均以数值减小的方向表征候选激励轨迹的优化，并减小二者之间的数值变化范围差异。由此，可以避免两个评价结果因优劣方向相反而难以共同参与轨迹评价，并降低其中一个评价结果因数值变化范围较大而掩盖另一个评价结果的可能，使总体辨识信息规模和不同参数方向上的辨识信息不均衡程度均能够实际影响与当前轨迹参数对应的评价值。

13、因此，本发明在机器人关节运动约束下，基于每轮迭代重新生成的评价值更新轨迹参数，并从迭代过程中确定具有最小评价值的轨迹参数，能够在保证目标激励轨迹符合机器人关节运动范围及执行能力的基础上，使所述目标激励轨迹兼顾刚体动力学参数和摩擦参数的总体辨识信息需求以及不同参数作用的可区分性。机器人后续执行所述目标激励轨迹时，所采集的同一批关节运动状态数据和驱动力矩数据能够更充分地反映刚体动力学参数及以非线性方式参与关节驱动力矩计算的非线性摩擦参数的影响，从而有利于降低因部分参数激励不足、不同参数作用难以区分或者信息矩阵数值病态而产生的辨识不稳定风险，并有利于提高动力学模型在机器人启动及低速运动状态下的关节驱动力矩预测准确性。

14、在本发明提供的机器人关节激励轨迹生成方法的至少一个实施例中，优选地，所述刚体动力学部分包括惯性项和重力项，所述摩擦部分包括库仑摩擦项、表征静摩擦与库仑摩擦差异的stribeck非线性摩擦项和粘性摩擦项；所述刚体动力学参数包括转动惯量参数、质量矩参数和重力偏置角，所述摩擦参数包括库仑摩擦参数、静摩擦参数与库仑摩擦参数的差值（可正可负）、stribeck特征速度和粘性摩擦参数，其中，所述stribeck特征速度以非线性方式参与所述关节驱动力矩的计算。

15、采用上述技术方案后，通过惯性项、重力项及不同摩擦项共同描述关节驱动力矩，并将对应参数共同作为待辨识参数，可以使目标激励轨迹同时面向惯性、重力及非线性摩擦特性的辨识需求，提高其对机器人启动和低速过渡状态的适用性。

16、在本发明提供的机器人关节激励轨迹生成方法的至少一个实施例中，优选地，采用有限项傅里叶级数作为所述候选激励轨迹的参数化表达，并将所述有限项傅里叶级数中的正弦项系数、余弦项系数和常数偏置项作为所述轨迹参数；通过对所述有限项傅里叶级数进行解析求导，获得所述候选激励轨迹对应的关节角速度和关节角加速度；所述机器人关节运动约束包括关节角度约束、关节角速度约束、关节角加速度约束和轨迹周期边界连续性约束；所述轨迹周期边界连续性约束包括轨迹周期起止时刻的关节角度、关节角速度和关节角加速度分别相等。

17、采用上述技术方案后，通过有限项傅里叶级数将连续轨迹的优化转换为有限个轨迹参数的优化，并通过解析求导获得相互一致的关节角度、关节角速度和关节角加速度。配合机器人关节运动约束和周期边界连续性约束，可以使目标激励轨迹在满足机器人执行能力的同时保持周期衔接平稳。

18、在本发明提供的机器人关节激励轨迹生成方法的至少一个实施例中，优选地，采用所述动力学模型输出的关节驱动力矩相对于各所述待辨识参数的偏导数，作为对应待辨识参数在对应采样时刻下的所述参数灵敏度；将所述多个采样时刻下各待辨识参数的参数灵敏度按照采样时刻为行、待辨识参数为列进行排列，形成参数灵敏度矩阵；所述参数信息矩阵由所述参数灵敏度矩阵的转置矩阵与所述参数灵敏度矩阵相乘得到，所述参数信息矩阵为与各所述待辨识参数对应的fisher信息矩阵。

19、采用上述技术方案后，通过偏导数定量表示各参数对关节驱动力矩的影响程度，并利用参数灵敏度矩阵建立fisher信息矩阵，可以将各参数的辨识信息强度及参数之间的关联关系汇总至同一矩阵，为利用行列式和条件数评价候选激励轨迹提供明确的计算基础。

20、在本发明提供的机器人关节激励轨迹生成方法的至少一个实施例中，优选地，对所述第一评价结果取负对数，获得所述第一评价量；对所述第二评价结果取正对数，获得所述第二评价量；将所述第一评价量与所述第二评价量相加，获得与所述当前轨迹参数对应的所述评价值。

21、采用上述技术方案后，负对数处理能够将增大总体辨识信息规模的目标转换为减小第一评价量的目标，正对数处理能够保持减小条件数的优化方向，并压缩两个指标的数值变化范围。将二者相加后，可以得到能够供轨迹优化过程直接比较的统一评价值。

22、在本发明提供的机器人关节激励轨迹生成方法的至少一个实施例中，优选地，采用粒子群优化算法基于所述评价值迭代更新所述轨迹参数，包括：初始化多组轨迹参数以及各组轨迹参数对应的参数更新量，并将每组轨迹参数作为一个粒子的当前位置；在每轮迭代中，根据各粒子的当前位置所对应的评价值，更新各粒子对应的个体历史最优轨迹参数，并根据各粒子的个体历史最优轨迹参数确定全局最优轨迹参数；针对各粒子，将对应的个体历史最优轨迹参数减去该粒子的当前位置得到第一参数差值，并将所述全局最优轨迹参数减去该粒子的当前位置得到第二参数差值；根据该粒子上一轮的参数更新量、所述第一参数差值、所述第二参数差值以及分别与所述第一参数差值和所述第二参数差值对应的随机系数，确定该粒子当前轮的参数更新量；根据所述当前轮的参数更新量更新对应粒子的当前位置，并将更新后的当前位置作为下一轮迭代中的轨迹参数；所述预设终止条件包括达到预设最大迭代次数和连续多轮迭代所得最优评价值的变化量小于预设收敛阈值中的至少一种。

23、采用上述技术方案后，各粒子能够根据自身历史最优结果和粒子群全局最优结果调整轨迹参数，并利用随机系数保持搜索的多样性，从而在轨迹参数空间内持续搜索评价值较小的候选解。通过预设迭代次数或收敛条件停止优化，可以兼顾轨迹优化效果和计算量。

24、本发明的第二方面提供一种机器人关节动力学参数辨识方法，包括：控制待辨识机器人关节执行目标激励轨迹，采集所述待辨识机器人关节在多个采样时刻下的关节运动状态数据和实测驱动力矩数据；所述目标激励轨迹由前述任一机器人关节激励轨迹生成方法得到，所述关节运动状态数据包括关节角度、关节角速度和关节角加速度；根据待辨识机器人关节的动力学模型中各待辨识参数的物理取值范围，确定各所述待辨识参数的参数搜索范围；所述动力学模型为生成所述目标激励轨迹时用于确定参数灵敏度的动力学模型，所述动力学模型包括刚体动力学部分和摩擦部分，所述摩擦部分包括非线性摩擦项，所述待辨识参数包括所述刚体动力学部分对应的刚体动力学参数和所述摩擦部分对应的摩擦参数；在所述参数搜索范围内确定多组参数候选值，每组所述参数候选值均包括各所述刚体动力学参数和各所述摩擦参数对应的候选值；分别将所述关节运动状态数据和各组参数候选值输入所述动力学模型，获得各组参数候选值对应的预测驱动力矩数据；根据多个采样时刻下的所述实测驱动力矩数据与各组参数候选值对应的预测驱动力矩数据之间的残差，分别形成各组参数候选值对应的标量残差评价结果，并以减小所述标量残差评价结果为目标，在所述参数搜索范围内对所述刚体动力学参数和所述摩擦参数对应的参数候选值进行全局联合更新，获得各所述待辨识参数的参数初始估计值；根据所述参数初始估计值对应的各采样时刻的残差，确定用于减小异常残差对参数求解影响的采样数据权重；以所述参数初始估计值作为参数更新的初始值，根据所述采样数据权重形成加权残差评价结果，并以减小所述加权残差评价结果为目标，对所述刚体动力学参数和所述摩擦参数进行局部联合更新，直至满足参数辨识终止条件，输出各所述待辨识参数的辨识结果。

25、采用上述技术方案后，本发明以同一完整动力学模型对刚体动力学参数和摩擦参数进行全局联合初估及局部联合精估，避免将刚体动力学参数和摩擦参数分阶段单独求解，从而降低前一阶段参数误差向后一阶段传递的风险。通过先进行全局联合更新获得参数初始估计值，再依据残差确定采样数据权重并进行局部联合更新，可以同时降低非线性参数求解对初始值的依赖以及异常采样数据对最终辨识结果的影响，提高完整参数联合辨识的稳定性。

26、在本发明提供的机器人关节动力学参数辨识方法的至少一个实施例中，优选地，所述刚体动力学部分包括惯性项和重力项，所述摩擦部分包括库仑摩擦项、表征静摩擦与库仑摩擦差异的stribeck非线性摩擦项和粘性摩擦项；所述刚体动力学参数包括转动惯量参数、质量矩参数和重力偏置角，所述摩擦参数包括库仑摩擦参数、静摩擦参数与库仑摩擦参数的差值（可正可负）、stribeck特征速度和粘性摩擦参数；所述预测驱动力矩由所述惯性项、所述重力项、所述库仑摩擦项、所述stribeck非线性摩擦项和所述粘性摩擦项共同确定；所述残差为对应采样时刻的实测驱动力矩与预测驱动力矩之间的差值。

27、采用上述技术方案后，通过完整模型共同计算惯性力矩、重力力矩和摩擦力矩，并以实测总驱动力矩与预测总驱动力矩之间的残差更新全部参数，可以使刚体动力学参数和摩擦参数共同解释实测数据，降低刚体动力学模型误差被固定吸收到摩擦参数中的风险。

28、在本发明提供的机器人关节动力学参数辨识方法的至少一个实施例中，优选地，在形成各组参数候选值对应的所述标量残差评价结果之前，对所述实测驱动力矩数据进行零相位低通滤波，并剔除关节角速度处于预设零速死区内的采样数据，将滤波后且未被剔除的实测驱动力矩数据及其对应的关节运动状态数据作为参数辨识数据；所述预设零速死区用于剔除关节角速度方向难以稳定确定的零速附近采样数据；根据所述参数辨识数据中各采样时刻的实测驱动力矩数据与预测驱动力矩数据之间的残差平方和，形成各组参数候选值对应的所述标量残差评价结果；根据所述参数初始估计值对应的各采样时刻的残差确定残差尺度，并根据各采样时刻的残差相对于所述残差尺度的大小确定对应采样数据的所述采样数据权重；残差处于预设范围内的采样数据具有第一权重，残差超出所述预设范围的采样数据具有小于所述第一权重的采样数据权重，且所述采样数据权重随对应残差相对于所述残差尺度的增大而减小；根据各采样数据的所述采样数据权重对各采样时刻的残差进行加权，并根据加权后的各采样时刻的残差形成所述加权残差评价结果。

29、采用上述技术方案后，零相位低通滤波能够在抑制高频噪声的同时减少数据相位错位，剔除零速死区内速度方向不稳定的数据能够降低摩擦方向判断误差。根据残差尺度降低较大残差对应数据的权重，可以减少异常测量值或瞬时冲击对局部参数更新的影响，提高参数辨识的鲁棒性。

30、在本发明提供的机器人关节动力学参数辨识方法的至少一个实施例中，优选地，采用粒子群优化算法，以减小所述参数辨识数据对应的实测驱动力矩数据与预测驱动力矩数据之间的残差平方和为目标，对所述刚体动力学参数和所述摩擦参数进行所述全局联合更新，获得所述参数初始估计值；根据所述参数初始估计值对应的残差，按照中位数绝对偏差确定所述残差尺度，并根据所述残差尺度确定huber阈值；将残差绝对值未超过所述huber阈值的采样数据对应的huber权重确定为所述第一权重，并根据所述huber阈值与残差绝对值之间的比值，确定残差绝对值超过所述huber阈值的采样数据对应的huber权重；以所述参数初始估计值作为初始值，采用信赖域反射算法求解huber加权非线性最小二乘问题，对所述刚体动力学参数和所述摩擦参数进行所述局部联合更新，获得各所述待辨识参数的辨识结果。

31、采用上述技术方案后，粒子群优化能够在参数搜索范围内获得较合理的完整参数初始估计值，降低局部求解对初始值的敏感性；中位数绝对偏差及huber权重能够根据实际残差水平降低异常数据的影响；信赖域反射算法则能够在参数初始估计值附近进一步减小加权残差，从而兼顾全局搜索能力、局部求解精度和异常数据鲁棒性。