[发明专利]一种基于动量观测器的闭环机械臂系统碰撞检测方法

权利要求书

1.一种基于动量观测器的闭环机械臂系统碰撞检测方法，其特征在于：具体步骤为：

步骤一：在闭环机械臂系统中配置运动传感器，所述运动传感器用于采集关节的转角及转角速度在闭环机械臂系统中配置六维力或六维力矩传感器，所述六维力或六维力矩传感器用于采集每一闭环约束中任一关节处作用的约束力或约束力矩大小，记为

步骤二：建立闭环机械臂系统动力学模型；

根据第一类拉格朗日方程理论得到闭环机械臂系统动力学模型：

其中，M(q)为系统的质量特性矩阵；q、为各关节处的转角、角速度、角加速度，即系统的广义坐标、广义速率以及广义加速度；A(q)、分别为系统中六维力或六维力矩传感器配置关节的约束雅各比矩阵及其导数；μ为拉格朗日乘子，实际物理意义为六维力或六维力矩传感器配置关节处对应的约束力或约束力矩；F^C、F^gc、g(q)分别为关节驱动电机输入对各关节的广义主动力、碰撞引入各关节的广义碰撞力、各体运动耦合产生的广义惯性力非线性项以及重力对各关节的广义力；

步骤三：设计动量观测器用于观测各关节的广义碰撞力；

定义系统广义动量为同时由多体动力学理论得：则根据式(1)所述闭环机械臂系统动力学模型，广义动量的导数表述为：

其中中间变量构造动量观测器如下：

其中，为广义动量及其导数的观测量；带有^符号的量均为对应物理量的观测量或测量量；γ为观测器输出；K_MO为观测器增益，用于调节观测器跟踪性能和灵敏度；

步骤四：对闭环机械臂系统进行碰撞检测；

将步骤一实时测量的关节约束力或约束力矩关节的转角转角速度以及由关节驱动电机的电流值估算的带入步骤三所设计的动量观测器，则可通过观测器输出γ对闭环机械臂系统进行碰撞检测；由于实际系统内部、外部存在摩擦以及环境的不确定因素干扰，需根据各类干扰的具体情况以及控制精度需求设定判断碰撞是否发生的阈值γ_C；当γ≤γ_C时，认为观测到的信息是由不确定因素干扰引起的；当γ＞γ_C时，认为碰撞发生，且观测到的关节的广义碰撞力为

2.如权利要求1所述的一种基于动量观测器的闭环机械臂系统碰撞检测方法，其特征在于：所述观测器能够用于观测的校验方法为：

根据式(3)以及式(2)，有

当测量量足够准确，有且同时若则有此时式(4)表示为

则能够看出理论上观测器的输出γ以指数接近广义碰撞力F^gc；通过调节观测器增益K_MO可调节观测器跟踪性能和灵敏度。