1.本发明涉及自动控制领域，具体是一种机器人状态估计方法。
背景技术：
2.机器人伺服力控技术是实现高动态、高精度闭环的关键，传统力前馈控制通过将期望力以虚功理论和雅克比动力学模型向关节执行器进行映射，从而实现对期望力的前馈输出，而在实际机器人控制系统中由于存在摩擦、接触抖动、力矩映射模型参数摄动等问题，仅采用前馈输出难以保证精度和鲁棒性，因此目前主要通过引入力反馈的控制方法来提高力跟踪精度、降低输出噪声，传统在线力估计需要执行器为力伺服即能直接或间接测量当前力矩，但对于位置、速度等执行器来说无法实现对无外部力矩传感器下的力估计，因此为对异构执行器的力在线估计，需要提出一种新的力反馈估计方法。
技术实现要素：
3.本发明所要解决的技术问题是提供一种异构伺服执行器的力反馈估计方法，其用于仿生机器人支撑相力控中的力反馈控制需求，同时面向位置、速度和力异构执行器类型实现当前力的拟合和估计。
4.本发明的技术方案如下：
5.本发明构建多通道力反馈估计模型(图1)，面向力控制伺服采用其位置、速度和力矩测量结果进行比例加强融合实现反馈估计，对于位置、速度执行器由于其无法精确获取当前力矩测量值，因此基于离线力矩传感器拟合标定的方式，采用辨识算法构建位置、速度与力反馈间的映射模型，获取执行器的虚拟力矩，最终基于虚功原理实现对当前执行器力反馈的在线估计，并进一步用于力伺服控制回路中，本发明方法的主要流程步骤(图2)如下：
6.步骤1，针对不同类型的异构伺服执行器对其原始测量数据进行数字滤波，所述的原始测量数据包括位置、速度和电流测量值；
7.步骤2，构建力伺服执行器的线性估计模型，采集真实扭矩测量数据对上述模型中系数进行拟合求解最优化的模型系数；
8.步骤3，构建位置/速度执行器的非线性估计模型，采集真实扭矩测量数据对上述模型中系数进行拟合求解最优化的模型系数；
9.步骤4，在得到模型系数后，向力伺服反馈线性估计模型、位置/速度伺服执行器扭矩估计模型中分别输入采集的位置、速度和电流测量值，在线估计得到执行器电流测量值转换扭矩估计结果以及位置/速度测量值转换扭矩估计结果
10.步骤5，基于虚功原理和雅克比矩阵计算当前扭矩估计结果对应的力f
est
：
[0011][0012]
其中j为雅克比矩阵。
[0013]
进一步地，所述步骤1中，
[0014]
对于位置测量值，采用微分跟踪滤波器对位置测量值进行滤波，计算输入角度q的角速度增量δq；
[0015]
对于速度增量，采用低通滤波器对速度增量进行滤波得到然后对滤波后的进行积分获得积分角度估计值：
[0016][0017]
最后，对积分角度估计值进行单圈复位和限幅处理，即当角度超过0
°
和360
°
后做零点补偿处理：
[0018][0019]
其中q
base
为单圈复位基准；
[0020]
对于速度与电流测量值仅采用低通滤波器进行处理。
[0021]
进一步地，所述步骤2中，构建的力伺服执行器的线性估计模型如下：
[0022][0023]
其中kq为待标定位置比例系数，kv为待标定速度比例系数，km为已知的电流扭矩转换系数，为滤波后的电流测量值，为模型输出的扭矩估计结果。
[0024]
进一步地，所述步骤2中，采用最小二乘法对力伺服执行器的线性估计模型待求参数进行拟合，如下式使模型估计结果与真实测量扭矩测量值τm间误差最小化，得到最优化的模型参数
[0025][0026]
其中为优化的位置比例系数；为优化的速度比例系数；为优化的电流扭矩转换系数；通过在异构伺服执行器安装相关扭矩传感器对真实扭矩输出τm进行测量。
[0027]
进一步地，所述步骤3中，构建的位置/速度伺服执行器的非线性估计模型如下：
[0028][0029]
其中为模型输出的扭矩估计结果，k
q,i
为待标定位置比例系数，k
v,i
为待标定速度比例系数，n为模型阶次，为积分角度估计值，为滤波后的关节角速度。
[0030]
进一步地，所述步骤3中，采用深度学习或迭代优化方法求解位置/速度伺服执行器非线性力估计模型系数如下式使模型估计结果与真实测量扭矩测量值τm间误差最小化，得到最优化的模型参数
[0031][0032]
其中为优化的位置比例系数，为优化的速度比例系数。
[0033]
进一步地，所述步骤4中，得到最优参数后，向模型中输入采集的位置、速度和电流测量值得到扭矩估计结果
[0034][0035]
本发明的优点：本发明提供的一种异构伺服执行器的力反馈估计方法，其通过引入位置变化和速度变化来估计执行器作用力，从而实现在机器人控制系统中构建力反馈控制器的目的。由于位置、速度执行器无法直接反馈力矩数据，本发明通过对执行器力反馈数据离线拟合和标定的方法，构建位置和速度变化与力反馈估计间的映射模型，为执行器力反馈控制提供虚拟力矩数据，从而解决异构执行器力反馈在线估计的问题，并提高执行器机构存在磨损、静摩擦和接触抖动时力反馈测量的鲁棒性。
附图说明
[0036]
图1是力反馈估计模型框图；
[0037]
图2是所提方法的计算步骤流程框图
具体实施方式
[0038]
下面结合附图对本发明作进一步详细说明。
[0039]
本发明提供的一种异构伺服执行器的力反馈估计方法，其通过引入位置变化和速度变化来估计执行器作用力，从而实现在系统中构建力反馈控制器的目的。由于位置、速度执行器无法直接反馈力矩数据，本发明通过对执行器力反馈数据离线拟合和标定的方法，构建位置和速度变化与力反馈估计间的映射模型，为执行器力反馈控制提供虚拟力矩数据，从而解决异构执行器力反馈在线估计的问题，并提高执行器机构存在磨损、静摩擦和接触抖动时力反馈测量的鲁棒性。
[0040]
其具体步骤如下：
[0041]
步骤1，针对不同类型的异构伺服执行器对其原始测量数据进行数字滤波，所述的原始测量数据包括位置、速度和电流测量值。
[0042]
针对位置、速度和电流测量值采取不同的处理方法：
[0043]
(1)对于位置测量值，位置测量值存在单圈和多圈角度阶跃的问题，从而出现测量值在0
°
和360
°
间的跳变，因此本发明首先采用微分跟踪滤波器对其进行滤波；计算输入角度q的角速度增量δq；
[0044]
(2)对于速度增量，首先采用低通滤波器对速度增量进行滤波得到然后对滤波后的进行积分获得积分角度估计值：
[0045][0046]
最后，对积分角度进行单圈复位和限幅处理，即当角度超过0
°
和360
°
后做零点补偿处理：
[0047][0048]
其中q
base
为单圈复位基准。
[0049]
(3)对于速度与电流测量值，由于不存在周期跳变情况，因此仅采用低通滤波器进行处理。
[0050]
本发明依据异构执行器类型来确定其采用的力反馈估计模型：
[0051][0052][0053]
步骤2，基于步骤1中数字滤波处理后的上述测量值实现对异构伺服执行器作用力的在线估计，对于电流测量来说其可以直接通过电流扭矩系数结合执行器减速比例换算得到，而对于位置与速度测量值本发明通过离线标定拟合的方式获取其变化量与扭矩测量间的映射关系，并最终实现对力反馈的估计。
[0054]
(1)构建力伺服执行器的线性估计模型：
[0055][0056]
其中kq为待标定位置比例系数，kv为待标定速度比例系数，km为已知的电流扭矩转换系数，为滤波后的电流测量值，为模型输出的扭矩估计结果，即由电流测量值换算的扭矩数据。
[0057]
(2)对上述力伺服执行器的线性估计模型进行离线标定，标定中需要在异构伺服执行器安装相关扭矩传感器对真实扭矩输出τm进行测量，给定不同负载、转速和位置下的测试轨迹并实时记录当前的位置、速度和真实力矩测量值，采用最小二乘法对上述力伺服执行器的线性估计模型待求系数进行拟合，如下式使力伺服执行器的线性估计模型的估计结果与真实扭矩测量值τm间误差最小化，来获取最优化的模型参数
[0058]
[0059]
其中为优化的位置比例系数；为优化的速度比例系数；为优化的电流扭矩转换系数，τm为真实扭矩测量值，通过在异构伺服执行器安装相关扭矩传感器对真实扭矩输出进行测量；为模型输出的扭矩估计结果。
[0060]
步骤3，对于位置/速度执行器来说由于其测量数据与扭矩反馈间存在非线性映射关系，因此本发明构建相应的非线性估计模型：
[0061]
(1)构建位置/速度伺服执行器的非线性估计模型：
[0062][0063]
其中为模型输出的扭矩估计结果，k
q,i
为待标定位置比例系数，k
v,i
为待标定速度比例系数，n为模型阶次，为积分角度估计值，为滤波后的关节角速度。
[0064]
(2)对上述非线性估计模型进行离线标定，同样在执行器输出端安装力矩传感器对真实力矩进行测量τm，给定不同负载、转速和位置下的测试轨迹并实时记录当前的位置、速度和真实力矩测量值，采用深度学习或迭代优化等方法求解上述模型系数，如下式使模型估计结果与真实测量扭矩测量值τm间误差最小化来获取最优化的模型参数
[0065][0066]
其中为优化的位置比例系数，为优化的速度比例系数，τm为真实扭矩输出值。
[0067]
步骤4，获取的各模型最优化模型系数后，向力伺服反馈线性估计模型、位置/速度伺服执行器扭矩估计模型中输入采集的位置、速度和电流测量值，在线估计得到执行器电流测量值转换扭矩估计结果以及位置/速度测量值转换扭矩估计结果
[0068]
基于拟合模型实时估计当前的虚拟扭矩：
[0069][0070]
其中，为由电流测量值转换的扭矩估计结果、为由位置与速度测量值转换的扭矩估计结果。
[0071]
步骤5，基于虚功原理和雅克比矩阵计算当前扭矩估计结果对应的力：
[0072][0073]
其中j为雅克比矩阵。
[0074]
本发明异构伺服执行器的力反馈估计方法用于仿生机器人支撑相力控中的力反馈控制需求，同时面向位置、速度和力异构执行器类型实现当前力的拟合和估计。本发明构建多通道力反馈估计模型，面向力控制伺服采用其位置、速度和力矩测量结果进行比例加强融合实现反馈估计，对于位置、速度执行器由于其无法精确获取当前力矩测量值，因此基于离线力矩传感器拟合标定的方式，采用辨识算法构建位置、速度与力反馈间的映射模型，
获取执行器的虚拟力矩，最终基于虚功原理实现对当前执行器力反馈的在线估计，并进一步用于力伺服控制回路中，本发明适用于机器人运动控制领域。