[发明专利]可翻转物体的翻转跟随轨迹规划方法、装置及系统

权利要求书

1.一种可翻转物体的翻转跟随轨迹规划方法，其特征在于，包括：

步骤S1：安装有机械臂的底盘导航至可翻转物体的附近位置；

步骤S2：机械臂控制视觉系统拍照，根据视觉提供的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体的翻转过程的位置姿态路径；

步骤S3：对生成的路径进行轨迹插补，生成位置控制下的位置点，采用力控制技术和位置控制相结合的阻抗控制方法实现可翻转物体的盖的翻转动作；

在所述步骤S2中：

机械臂控制视觉拍照，视觉计算得到可翻转物体的附近的局部地图信息，包括位置点，并通过mqtt协议将点位数据发给机械臂控制系统，发送数据包括posA、posB、posC、posE、posF、posG，其中posA表示可翻转物体的盖的翻转轴线上第一个点；posB表示可翻转物体的盖翻转轴线上第二个点；posC可翻转物体的盖前方点位,即机械臂末端夹持的点位；posE和posF表示可翻转物体的后盖外边沿的两个角点；posD为posE与posF的中间点；posG为可翻转物体的侧边墙壁面和posE、posF形成的直线的交点；

posC为可翻转物体的盖前方点位，也是机械臂末端抓取点位，计算可翻转物体的盖翻转之后的圆弧末端点posC_1；计算翻转过程中posC，posC_1的姿态；

计算确定了圆弧路径的始末位置和姿态之后，接着进行速度规划以及轨迹插补，生成中间位置点；速度规划方法采用T型速度规划或S型速度规划；

计算翻转之后的圆弧末端点posC_1过程如下：

计算角度posC、posB和posC_1之间的夹角α，根据点位posA、posB、posC组成一个平面1，posA、posB、posD组成一个平面2，α为平面1与平面2的夹角；根据posC与轴心线posA-posB以及夹角α，计算得到posC_1；

计算翻转过程中posC、posC_1的姿态过程如下：

确定机械臂末端姿态信息rz，采用自动搜索计算，先计算可翻转物体的离墙壁的距离d＝posF-posG，再结合机械臂本身的机械结构参数，姿态进行搜索计算，rz表示沿Z轴的旋转矩阵，ry表示沿Y轴的旋转矩阵，rx表示沿X轴的旋转矩阵，d为可翻转物体的离墙壁的距离；姿态搜索计算步骤如下：

i1、选择rz，取向量rz＝posA-posB，该向量方向即是机械臂末端Z轴方向，该向量归一化之后，求解得到旋转矩阵rz；判断此时机械臂的4,5,6轴关节连接处的设计尺寸是否会大于d，如果不大于d，向量rz＝posA-posB，如果会大于d，进行i2步骤继续搜索计算rz；

i2、重新计算rz，取向量rz＝posA-posC；判断此时机械臂的4,5,6轴关节连接处的设计尺寸是否会大于d，如果不大于d，向量rz＝posA-posC，如果大于d，进行i3步骤继续搜索计算rz；

i3、重新计算rz，为通过向量posC至posA与posB的中间点计算，即向量rz＝0.5*(posA+posB)-posC，rz进行归一化，确定rz，判断此时机械臂的4,5,6轴关节连接处的设计尺寸是否会大于d，如果不大于d，向量rz＝0.5*(posA+posB)-posC；如果大于d，进行i4步骤继续搜索计算rz；

i4、重新计算rz，改为通过向量posC至posB，即向量rz＝posB-posC；然后rz再归一化，确定rz；机械设计以及部署时能够保证该方向不会发生碰撞；确定ry为posA、posB、posC组成一个平面1的法向量，ry＝(posA-posB)×(posC-posB)，根据右手准则确定rx，通过上述位置和姿态计算，确定可翻转物体的翻转的圆弧轨迹posC至posC_1。

2.根据权利要求1所述的可翻转物体的翻转跟随轨迹规划方法，其特征在于，在所述步骤S3中：

阻抗控制方法为基于末端的六维力矩传感器与可翻转物体的盖的接触力,通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制；

机械臂末端安装有六维力传感器，感知各方向的力F_e，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系，阻抗控制是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制。