【技术实现步骤摘要】
可翻转物体翻转实时跟随轨迹规划方法及系统
[0001]本专利技术涉及智能机器人清洁领域，具体地，涉及一种可翻转物体翻转实时跟随轨迹规划方法及系统，更为具体地，涉及一种马桶清洁时马桶盖翻转实时跟随轨迹规划方法。
技术介绍
[0002]现有的轨迹跟踪系统中的先进者采用传感器来跟踪轨迹，例如弧焊机器人系统采用结构光视觉传感器来跟踪轨迹。轨迹跟踪系统中的执行器自身有一个通过示教信息或规划等手段预定的任务轨迹，带工具的执行器能根据该预定的任务轨迹生成驱动信息，使工具沿着预定的任务轨迹运行，而轨迹跟踪系统仅能简单地把传感器采集、提取的轨迹偏差信息直接反馈到执行器中，对预定的任务轨迹进行修正。
[0003]对于翻转物体的轨迹规划更是一直是一个难题，几乎没有相关专利技术可以进行可翻转物体的轨迹规划。
[0004]因此在清洁领域，使用智能机器人清扫马桶属于空白领域，马桶清洁流程复杂，基本是人力清扫。使用智能机器人取代人力，对马桶盖进行翻转打扫的技术也是空白。马桶清扫时最大的难点就是马桶盖的翻转，翻转轨迹理论上是一条圆弧，但是由于翻转过程中圆弧轨迹的轴线精度不准确，导致实机翻转和理论圆弧轨迹会有差异，这就需要机器人能够实时跟随调整马桶翻转轨迹，从而完成马桶盖翻转任务。如何提供一种可以实时跟随调整马桶盖翻转的方法，是当前需要解决的技术问题。
[0005]针对上述现有技术中的缺陷，本专利技术要解决的技术问题是：a、结合视觉系统计算得到马桶翻转轨迹的轴心线初始位置以及机械臂的夹持点位，自动计算翻转动作的姿态；b、同时由...
【技术保护点】
【技术特征摘要】 【专利技术属性】
1.一种可翻转物体翻转实时跟随轨迹规划方法，其特征在于，包括：步骤S1：安装有机械臂的底盘导航至可翻转物体的附近位置；步骤S2：机械臂控制视觉系统拍照，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹，将初始段轨迹设计成理论的圆弧轨迹；步骤S3：初始段理论圆弧轨迹生成之后，机械臂控制器进行速度规划、轨迹插补，下发规划的位置点，采集每个插补周期的实际夹爪抓取位置；步骤S4：每隔固定周期将实际夹爪抓取位置发送给机械臂控制系统，基于该数据点集，内部采用实时跟随轨迹方法，规划得到可翻转物体盖的翻转轨迹。2.根据权利要求1所述的可翻转物体翻转实时跟随轨迹规划方法，其特征在于，在所述步骤S2中：在可翻转物体附近之后机械臂控制视觉系统拍照，机械臂根据视觉给的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹；初始段轨迹设计成理论的圆弧轨迹，运动角度为act_alpha,运动角度的值基于总的理论圆弧角度；自动路径规划方法的步骤为：机械臂控制视觉拍照，视觉得到可翻转物体附近的局部地图信息，包括各个位置点，，并通过mqtt协议将点位数据发给机械臂控制系统，包括posA、posB、posC、posE、posF、posG；其中posA表示可翻转物体盖的翻转轴线上第一个点；posB表示可翻转物体盖翻转轴线上第二个点；posC表示可翻转物体盖前方点位，也是机械臂末端夹持点，运动的起点；posD表示可翻转物体盖前方的一个位置点；其中posE和posF表示可翻转物体后盖外边沿的两个角点；posD为posE与posF的中间点，posD＝0.5*(posE+posF)；posG为可翻转物体侧边墙壁面和posE、posF形成的直线的交点；计算翻转完成之后整个圆弧的总角度α，也是posC至posC_1的轨迹的理论角度；点位posA、posB、posC组成一个平面1，posA、posB、posD组成一个平面2，平面1与平面2的夹角即为α；计算理论的圆弧末端点posC_1，根据posC与轴心线posA
‑
posB以及夹角α，计算得到posC_1；计算翻转过程中posC、posC_1的姿态，姿态计算首先确定机械臂末端姿态信息rz，，rz表示沿Z轴的旋转矩阵，ry表示沿Y轴的旋转矩阵，rx表示沿X轴的旋转矩阵；考虑到防碰撞，采用自动搜索计算，先计算可翻转物体离墙壁的距离d＝posF
‑
posG；再结合机械臂本身的机械结构参数，姿态进行搜索计算，计算确定rz。3.根据权利要求2所述的可翻转物体翻转实时跟随轨迹规划方法，其特征在于：rz计算步骤如下：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中：进行速度规划以及轨迹插补，生成每个伺服周期的下发位置点；初始路径的速度规划方法采用T型速度规划或S型速度规划；当运动至act_alpha角度之后，初始路径结束；在机械臂运动过程中，机械臂控制系统会控制相机拍照，相机会采集每个周期的夹爪抓取点位置posC(i)，每隔固定周期发送给机械臂控制系统。5.根据权利要求1所述的可翻转物体翻转实时跟随轨迹规划方法，其特征在于，在所述步骤S4中：运动初始路径时，相机采集了每个周期的夹爪抓取点posC(i)，并将数据发送给了机械臂控制器系统，机械臂控制器系统根据数据点集，采用最小二乘法拟合得到一个圆轨迹；posC至posC_init之间的位置点是相机给的位置点，posC_init至posC_n是拟合完成之后，在后续周期预测固定插补周期位置，固定数值的位置点是基于拟合圆生成的位置点；采用实时跟随调整方法，计算规划位置；圆弧起点posC根据运动角度act_alpha，计算得到圆弧末端点为posC_init；采集了运行过程中的数据之后重新拟合了一个新的圆弧，圆弧经过posC和posC_init，在posC_init后再取距离posC_init点固定值周期的位置，该位置即为posC_n；posC_posC_init,posC_n都是拟合圆弧上的点位；机械臂运动过程中，相机一直在采集数据，实时跟随调整方法是：第i步：机械臂到达posC_init之后，再将posC_init至posC_n之间的理论圆弧位置做为输入，i为从1开始的步序，每次取固定值周期的位置做为一个移动窗口，机械臂末端实时跟随位置，每个周期判断posC_n，posA和posB平面与posA,posB,posC平面的夹角progres_alpha，当两个平面的夹角progres_alpha大于等于α时，路径结束，此时的posC_n为结束点；如果progres_alpha小于α，进行i+1步；在第i+1步，将第i步相机采集的新的数据和先前的相机数据结合，重新拟合生成新的拟合圆弧，生成新的posC_init；重复步骤，直至两个平面的夹角progres_alpha大于等于α产生结束点posC_n；每次取固定值周期的位置做为一个移动窗口，机械臂实时跟随位置点，实时跟随轨迹规划为基于五次多项式的实时跟随轨迹规划方法，其具体描述如下：所采用的五次多项式方程描述如下：q
t
＝a0+a1*t+a2*t2+a3*t3+a4*t4+a5*t5其中q
t
为t时刻的位置，t为当前的时刻，a0，a1，a2，a3，a4，a5即为该描述方程的一些待求解的参数；其求解过程即为求解a0、a1、a2、a3、a4、a5；
本方法中的约束条件初始时刻：q0＝S
00
结束位置：q
f
＝S
11
S0为每个移动窗口初始时刻机械臂的末端位置，V0为每个移动窗口初始时刻机械臂的末端速度，A0为每个移动窗口初始时刻机械臂的末端加速度，t＝0时刻的参数；q0是初始时刻的位置；q
f
是每个移动窗口结束时刻的位置；S1为每个移动窗口结束时刻机械臂的末端位置，V1为每个移动窗口结束时刻机械臂的末端速度，A1为每个移动窗口结束时刻机械臂的末端加速度；为初始位置的导数，也是速度，为初始速度的导数，也是初始加速度；为结束位置的导数，也是结束位置的速度，为结束速度的导数，也是结束时的加速度；根据约束条件推导，可得：a0＝S0a0+a1*t
f
+a2*t
f2
+a3*t
f3
+a4*t
f4
+a5*t
f5
＝S1a1＝V0a1+2*a2*t
f
+3*a3*t
f2
+4*a4*t
f3
+5*a5*t
技术研发人员：谢能达，郭震，
申请(专利权)人：上海景吾智能科技有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。