可翻转物体的翻转跟随轨迹规划方法、装置及系统制造方法及图纸

本发明专利技术提供了一种可翻转物体的翻转跟随轨迹规划方法、装置及系统，包括：步骤S1：安装有机械臂的底盘导航至可翻转物体的附近位置；步骤S2：机械臂控制视觉系统拍照，根据视觉提供的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体的翻转过程的位置姿态路径；步骤S3：对生成的路径进行轨迹插补，生成位置控制下的位置点，采用力控制技术和位置控制相结合的阻抗控制方法实现可翻转物体的盖的翻转动作。本发明专利技术解决马桶自动清扫领域的空白技术；保证机械臂能够到达各个区域；机械臂能够自动规划路径，无需示教引导，机械臂根据视觉的点位，能够自动搜索计算翻转可翻转物体的的位置和姿态，具有智能化，大大提升打扫效率。率。率。

【技术实现步骤摘要】
可翻转物体的翻转跟随轨迹规划方法、装置及系统


[0001]本专利技术涉及智能机器人清洁领域，具体地，涉及一种可翻转物体的翻转跟随轨迹规划方法、装置及系统，更为具体地，涉及一种马桶清洁时马桶盖翻转跟随轨迹规划方法、装置及系统。

技术介绍

[0002]现有的轨迹跟踪系统中的先进者采用传感器来跟踪轨迹，例如弧焊机器人系统采用结构光视觉传感器来跟踪轨迹。轨迹跟踪系统中的执行器自身有一个通过示教信息或规划等手段预定的任务轨迹，带工具的执行器能根据该预定的任务轨迹生成驱动信息，使工具沿着预定的任务轨迹运行，而轨迹跟踪系统仅能简单地把传感器采集、提取的轨迹偏差信息直接反馈到执行器中，对预定的任务轨迹进行修正。
[0003]对于翻转物体的轨迹跟随更是一直是一个难题，几乎没有相关专利技术可以精确跟随翻转物体的轨迹跟随。
[0004]因此现在在清洁领域，使用智能机器人清扫马桶也属于空白领域，马桶清洁流程复杂，基本是人力清扫。使用智能机器人取代人力，对马桶盖进行翻转打扫的技术也是空白。马桶清扫时最大的难点就是马桶盖的翻转，最大的特点就是需要机器人与马本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种可翻转物体的翻转跟随轨迹规划方法，其特征在于，包括：步骤S1：安装有机械臂的底盘导航至可翻转物体的附近位置；步骤S2：机械臂控制视觉系统拍照，根据视觉提供的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体的翻转过程的位置姿态路径；步骤S3：对生成的路径进行轨迹插补，生成位置控制下的位置点，采用力控制技术和位置控制相结合的阻抗控制方法实现可翻转物体的盖的翻转动作。2.根据权利要求1所述的可翻转物体的翻转跟随轨迹规划方法，其特征在于，在所述步骤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型速度规划。3.根据权利要求2所述的可翻转物体的翻转跟随轨迹规划方法，其特征在于：计算翻转之后的圆弧末端点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。4.根据权利要求1所述的可翻转物体的翻转跟随轨迹规划方法，其特征在于，在所述步骤S3中：阻抗控制方法为基于末端的六维力矩传感器与可翻转物体的盖的接触力，通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制；机械臂末端安装有六维力传感器，感知各方向的力F
e
，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系，阻抗控制是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制，末端的六维力矩传感器与可翻转物体的盖的接触力，通过一个二阶阻抗控制模型模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，将位置下发给伺服，完成位置控制。5.根据权利要求4所述的可翻转物体的翻转跟随轨迹规划方法，其特征在于，：具体步骤如下：a、阻抗控制方法采用的阻抗控制模型如下：其中，为正对角阵，M
d
为机械臂期望的惯性矩阵，B
d
为阻尼矩阵，K
d
为刚度矩阵，F
d
为机械臂末端执行器作用于环境的期望力，F
e
为机械臂末端执行器作用于环境实际力；为实际位置的微分的微分，即实际加速度，为期望位置的微分的微分，即期望加速度，为实际位置的微分，即实际速度，为期望位置的微分，即期望速度，X为实际位置，X
d
为期望位置；b、在机械臂控制系统中引入阻抗控制后，通过控制机器人的位置并且实现力与位置之间的期望关系Z(S)，间接控制交互力；根据上述阻抗控制模型，得出力与位置之间的期望关系Z(S)如下Z(S)＝M
d
s+B
d
+K
d
/s其中S为6
×
6的对角阵，其对角线元素为1或0，它的主要功能是用来确定哪些自由度施加力控，哪些自由度施加位置控制；在时域中力与位置之间的期望关系用微分方程表示为：其中，M
d
，B
d
，K
d
为6
×
6的正对角阵，分别为惯性矩阵，阻尼矩阵和刚度矩阵。通过设置不同的值达到不同的阻抗控制效果，F(t)是机器人与环境间的接触力，X表示机器人的实际位置，X
d
表示机器人的指令位置。为了使机器人具备力跟踪的能力，将期望力F
d
加入到目标阻抗中，上式变为：F
d
为机械臂末端执行器作用于环境的期望力和F
e
为机械臂末端执行器作用于环境的实际力，当期望位置X
d
接近实际位置X时，实际接触力F
e
将收敛到参考力F
d
；根据阻抗控制关系，位置修正量ΔX与力误差ΔF之间的关系为：
ΔF＝(M
d
S2+B
d
S+K
d
)ΔX变为时域的形式：采用向后差分离散可得：采用向后差分离散可得：其中T为采样周期，ΔX(k)为k时刻的位置修正量，即位置偏差，ΔX(k
‑
1)为k
‑
1时刻的位置修正量，即位置偏差，ΔX(k
‑
2)为k
‑
2时刻的位置修正量，即位置偏差，为k时刻的速度修正量，即速度偏差；为k时刻的加速度修正量，即加速度偏差；式可以变为：即：ΔF(k)为k时刻的力偏差量；当前位置的修正量与当前周期的力偏差，上个周期的位置修正量和上上个周期的位置修正量相关；本控制方法采用设置的阻抗系统只包含阻尼系数B，M＝0，K＝0，则当前位置修正量为：期望位置经过选择矩阵I
‑
S后得到，位置控制方向的期望位置和力控方向的期望位置，与当前位置修正量叠加产生X
ref
，其中I是6
×
6的单位矩阵；X
ref
＝X
d
(I
‑
S)+ΔXX
ref
为计算得到的当前周期的下发的期望的笛卡尔位置；经过机械臂逆运动学，可以转到关节位置q，计算得到了插补位置；c、将新计算得到的插补位置下发个伺服，完成整个控制。6.一种可翻转物体的翻转跟随轨迹规划系统，其特征在于，包括：模块M1：...

【专利技术属性】
技术研发人员：谢能达，郭震，
申请(专利权)人：上海景吾智能科技有限公司，
类型：发明
国别省市：