[发明专利]一种受约束的机械臂零力控制系统及其控制方法

权利要求书

1.一种受约束的机械臂零力控制系统，其特征在于：包括：

动力学补偿控制器，用于根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩；

速度限制控制器，根据笛卡尔空间及关节空间的速度限制分别计算得到笛卡尔空间及关节空间的速度限制力矩，并将笛卡尔空间的速度限制力矩映射到关节空间得到根据笛卡尔空间的速度限制对参与运动的每个关节提供的反向力矩，通过该反向力矩与关节空间的速度限制力矩之间的加权计算得到为各关节电机输出的速度限制力矩；

位置限制控制器，根据笛卡尔空间及关节空间的位置限制分别计算得到笛卡尔空间及关节空间的位置限制力矩，并将笛卡尔空间的位置限制力矩映射到关节空间得到根据笛卡尔空间的位置限制对参与运动的每个关节提供的反向力矩，通过该反向力矩与关节空间的位置限制力矩之间的加权计算得到为各关节电机输出的位置限制力矩；

机械臂各关节电机在前述三个控制器的控制下输出合力矩。

2.根据权利要求1所述的机械臂零力控制系统，其特征在于：所述动力学补偿控制器根据关节自身重力、所受摩擦力控制各关节电机输出补偿力矩具体如下：

通过牛顿欧拉动力学或拉格朗日动力学计算各个关节所需的克服关节重力所需的补偿力矩Mg和克服摩擦力所需的补偿力矩Mf：

Mg＝g(q)

其中，g(q)表示关节重力矩，表示关节的转动速度，表示关节转动的粘滞摩擦力矩，表示关节转动的库伦摩擦力矩。

3.根据权利要求1所述的机械臂零力控制系统，其特征在于：所述速度限制控制器为各关节提供速度限制力矩具体如下：

假定笛卡尔空间速度为其中，分别表示机械臂的笛卡尔空间速度和角速度；

当分别表示设定的机械臂的笛卡尔空间速度最大值和角速度最大值；则提供笛卡尔空间限制力矩将笛卡尔空间力矩映射到关节空间得到根据笛卡尔空间的速度限制对参与运动的每个关节提供的反向力矩其中K₁表示笛卡尔空间速度限制权重，J表示雅可比矩阵；

假定某个关节实时速度为θ，当|θ|θ_max，提供关节空间限制力矩其中，θ_max表示设定的关节速度最大值，k₁表示关节空间速度限制权重；

据此得到速度限制力矩为

4.根据权利要求1所述的机械臂零力控制系统，其特征在于：所述位置限制控制器为各关节提供位置限制力矩具体如下：

假设在笛卡尔空间触发限制的边界位置为当机械臂在笛卡尔空间的实际位置且机械臂的运动方向依然有加大其在笛卡尔空间的实际位置超出边界位置的趋势时，则提供笛卡尔空间限制力矩将笛卡尔空间力矩映射到关节空间根据笛卡尔空间的位置限制对参与运动的每个关节提供的反向力矩其中，P_limx、P_limy、P_limz分别表示笛卡尔空间X、Y、Z轴方向上的限制边界，(P_x,P_y,P_z)分别表示机械臂在笛卡尔空间的实际位置坐标，K₂表示笛卡尔空间位置限制权重；J表示雅可比矩阵；

假定某个关节实时位置为p，当某个关节的实时位置|p|p_lim且运动方向依然有加大其在关节空间内的实时位置超出其位置边界的趋势时，提供关节空间限制力矩其中，p_lim表示在关节空间内某个关节的位置边界，k₂表示关节空间位置限制权重；

据此得到用于位置限制的合力矩为

5.根据权利要求1所述的机械臂零力控制系统，其特征在于：还包括力矩限制控制器，用于根据前述三个控制器控制各关节电机输出力矩的合力矩及对应关节电机的最大允许输出力矩控制各关节电机输出力矩。