[发明专利]一种机器人关节位置控制方法、装置和机器人

权利要求书

1.一种机器人关节位置控制方法，其特征在于，包括：

当机器人关节沿着预设参考轨迹运动至当前位姿点时，分别获取所述机器人关节在所述当前位姿点处的当前力测量值、当前力参考值、当前插补位姿矩阵和当前伺服控制参数；

根据所述当前力测量值、所述当前力参考值、所述当前插补位姿矩阵和所述当前伺服控制参数计算出期望伺服控制量；

根据所述期望伺服控制量控制所述机器人关节从所述当前位姿点运动至期望位姿点；

根据所述当前力测量值、所述当前力参考值、所述当前插补位姿矩阵和所述当前伺服控制参数计算出期望伺服控制量，具体包括：

基于力估计模型对所述当前力测量值进行估算，得到力估计值；

基于所述力估计值和所述当前力参考值计算力补偿量；

基于所述力补偿量和所述当前插补位姿矩阵计算期望位姿矩阵；

应用逆动力学模型对所述期望位姿矩阵进行求解，得到期望伺服控制参数；

基于所述期望伺服控制参数和所述当前伺服控制参数计算所述期望伺服控制量；

所述力估计模型具体为：

其中，表示在接触点参考坐标系a中的所述力估计值，表示从所述接触点参考坐标系a转换到力测量参考坐标系b中的旋转变换矩阵，表示与所述接触点参考坐标系a的原点在所述力测量参考坐标系b的坐标所对应的反对称算子，0^3×3表示3行3列的零矩阵，表示从所述接触点参考坐标系a转换到所述力测量参考坐标系b中的所述当前力测量值，表示与所述机器人关节连接的工具的重心o在所述接触点参考坐标系a中的坐标，表示所述重心o在所述力测量参考坐标系b中的坐标所对应的反对称算子，表示与所述重心o在所述力测量参考坐标系b中的坐标，表示所述重心o在所述力测量参考坐标系b中改变所述当前力测量值的改变值；

获取所述当前插补位姿矩阵，具体包括：

将所述机器人关节运动至所述当前位姿点的时刻确定为当前运动时刻；

从所述预设参考轨迹中查找与所述当前运动时刻对应的当前参考位姿矩阵以及与所述当前参考位姿矩阵相邻的后一个参考位姿矩阵；

从设定时段中确定在所述当前运动时刻之后的后一运动时刻，以及确定在所述当前运动时刻和所述后一运动时刻之间的中间时刻，其中，所述设定时段是以所述当前运动时刻和所述后一运动时刻为参数表示，且用以指示所述机器人关节从所述当前位姿点运动至所述后一个参考位姿矩阵表示的后一个位姿点的时段；

计算所述中间时刻与所述当前运动时刻之间的时间差；

基于线性插补模型对所述当前参考位姿矩阵、所述后一个参考位姿矩阵、所述设定时段的时长和所述时间差进行线性计算，得到所述当前插补位姿矩阵。

2.根据权利要求1所述的机器人关节位置控制方法，其特征在于，基于所述力估计值和所述当前力参考值计算力补偿量，具体包括：

计算所述力估计值与所述当前力参考值之间的力偏差值；

应用增量式PID控制模型对所述力偏差值进行计算，得到所述力补偿量；

所述增量式PID控制模型具体为：

其中，u(t_m)表示所述中间时刻t_m所对应的所述力补偿量，k_p表示比例系数，k_i表示积分系数，k_d表示微分系数，e(t_m)表示所述中间时刻t_m所对应的所述力偏差值，dt_m表示所述中间时刻t_m的增量，de(t_m)/dt_m表示所述力偏差值e(t_m)对所述中间时刻t_m求导得到的微分商；

所述力偏差值e(t_m)具体为：

其中，ω_ref表示所述当前力参考值。