CN113843803B - 可翻转物体翻转实时跟随轨迹规划方法及系统 - Google Patents

Abstract

本发明提供了一种可翻转物体翻转实时跟随轨迹规划方法及系统，包括：步骤S1：安装有机械臂的底盘导航至可翻转物体的附近位置；步骤S2：机械臂控制视觉系统拍照，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹，将初始段轨迹设计成理论的圆弧轨迹；步骤S3：初始段理论圆弧轨迹生成之后，机械臂控制器进行速度规划、轨迹插补，下发规划的位置点，采集每个插补周期的实际夹爪抓取位置；步骤S4：每隔固定周期将实际夹爪抓取位置发送给机械臂控制系统，基于该数据点集，内部采用实时跟随轨迹方法，规划得到可翻转物体盖的翻转轨迹。本发明解决可翻转物体自动清扫领域的空白技术；本发明具有智能化，大大提升打扫效率。

Description

可翻转物体翻转实时跟随轨迹规划方法及系统

技术领域

本发明涉及智能机器人清洁领域，具体地，涉及一种可翻转物体翻转实时跟随轨迹规划方法及系统，更为具体地，涉及一种马桶清洁时马桶盖翻转实时跟随轨迹规划方法。

背景技术

现有的轨迹跟踪系统中的先进者采用传感器来跟踪轨迹，例如弧焊机器人系统采用结构光视觉传感器来跟踪轨迹。轨迹跟踪系统中的执行器自身有一个通过示教信息或规划等手段预定的任务轨迹，带工具的执行器能根据该预定的任务轨迹生成驱动信息，使工具沿着预定的任务轨迹运行，而轨迹跟踪系统仅能简单地把传感器采集、提取的轨迹偏差信息直接反馈到执行器中，对预定的任务轨迹进行修正。

对于翻转物体的轨迹规划更是一直是一个难题，几乎没有相关发明可以进行可翻转物体的轨迹规划。

因此在清洁领域，使用智能机器人清扫马桶属于空白领域，马桶清洁流程复杂，基本是人力清扫。使用智能机器人取代人力，对马桶盖进行翻转打扫的技术也是空白。马桶清扫时最大的难点就是马桶盖的翻转，翻转轨迹理论上是一条圆弧，但是由于翻转过程中圆弧轨迹的轴线精度不准确，导致实机翻转和理论圆弧轨迹会有差异，这就需要机器人能够实时跟随调整马桶翻转轨迹，从而完成马桶盖翻转任务。如何提供一种可以实时跟随调整马桶盖翻转的方法，是当前需要解决的技术问题。

针对上述现有技术中的缺陷，本发明要解决的技术问题是：a、结合视觉系统计算得到马桶翻转轨迹的轴心线初始位置以及机械臂的夹持点位，自动计算翻转动作的姿态；b、同时由于翻转过程中，路径点是采用视觉给的局部地图信息计算得到，如果采用简单的圆弧路径规划，可能会拉扯马桶盖导致马桶盖损坏，故翻转动作需要实时调整，此方案采用相机拍照返回翻转轨迹夹爪抓取点的实时位置，机械臂控制系统再根据实时数据拟合预测接下来的轨迹路径，实时跟踪预测轨迹，故该技术采用该实时跟随轨迹方法，解决马桶盖翻转过程中点位精度误差导致的实际轨迹与理论位置偏差较大的技术问题。

现阶段使用机械臂翻转马桶盖的技术为空白。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种可翻转物体翻转实时跟随轨迹规划方法及系统。

根据本发明提供的一种可翻转物体翻转实时跟随轨迹规划方法，包括：

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

步骤S2：机械臂控制视觉系统拍照，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹，将初始段轨迹设计成理论的圆弧轨迹；

步骤S3：初始段理论圆弧轨迹生成之后，机械臂控制器进行速度规划、轨迹插补，下发规划的位置点，采集每个插补周期的实际夹爪抓取位置；

步骤S4：每隔固定周期将实际夹爪抓取位置发送给机械臂控制系统，基于该数据点集，内部采用实时跟随轨迹方法，规划得到可翻转物体盖的翻转轨迹。

优选地，在所述步骤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。

优选地，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。

优选地，在所述步骤S3中：

进行速度规划以及轨迹插补，生成每个伺服周期的下发位置点；初始路径的速度规划方法采用T型速度规划或S型速度规划；当运动至act_alpha角度之后，初始路径结束；在机械臂运动过程中，机械臂控制系统会控制相机拍照，相机会采集每个周期的夹爪抓取点位置posC(i)，每隔固定周期发送给机械臂控制系统。

优选地，在所述步骤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＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

其中q_t为t时刻的位置，t为当前的时刻，a0，a1，a2，a3，a4，a5即为该描述方程的一些待求解的参数；

其求解过程即为求解a₀、a₁、a₂、a₃、a₄、a₅；

本方法中的约束条件初始时刻：

q₀＝S₀

结束位置：

q_f＝S₁

S₀为每个移动窗口初始时刻机械臂的末端位置，V₀为每个移动窗口初始时刻机械臂的末端速度，A₀为每个移动窗口初始时刻机械臂的末端加速度，t＝0时刻的参数；q₀是初始时刻的位置；q_f是每个移动窗口结束时刻的位置；S₁为每个移动窗口结束时刻机械臂的末端位置，V₁为每个移动窗口结束时刻机械臂的末端速度，A₁为每个移动窗口结束时刻机械臂的末端加速度；为初始位置的导数，也是速度，为初始速度的导数，也是初始加速度；为结束位置的导数，也是结束位置的速度，为结束速度的导数，也是结束时的加速度；

根据约束条件推导，可得：

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

上述方程联立求解，求得：

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

其中t_f为固定值和伺服插补周期相乘得到；

对于每个移动窗口，S₀为每个移动窗口的posC_init，V₀为posC_init位置时机械臂的末端速度，A₀为posC_init位置时机械臂的的末端加速度；S₁为每个移动窗口的posC_n，V₁为设置的期望的末端速度，A₁为设置的期望的末端加速度；

根据上述描述的实时跟随轨迹规划方法，在每个插补周期生成轨迹插补位置，将生成的插补位置已下发给伺服，实现实时调整可翻转物体盖的翻转轨迹，完成可翻转物体盖的翻转动作。

根据本发明提供的一种可翻转物体翻转实时跟随轨迹规划系统，包括：

模块M1：安装有机械臂的底盘导航至可翻转物体的附近位置；

模块M2：机械臂控制视觉系统拍照，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹，将初始段轨迹设计成理论的圆弧轨迹；

模块M3：初始段理论圆弧轨迹生成之后，机械臂控制器进行速度规划、轨迹插补，下发规划的位置点，采集每个插补周期的实际夹爪抓取位置；

模块M4：每隔固定周期将实际夹爪抓取位置发送给机械臂控制系统，基于该数据点集，内部采用实时跟随轨迹方法，规划得到可翻转物体盖的翻转轨迹。

优选地，在所述模块M2中：

在可翻转物体附近之后机械臂控制视觉系统拍照，机械臂根据视觉给的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹；初始段轨迹设计成理论的圆弧轨迹，运动角度为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。

优选地，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。

优选地，在所述模块M3中：

进行速度规划以及轨迹插补，生成每个伺服周期的下发位置点；初始路径的速度规划方法采用T型速度规划或S型速度规划；当运动至act_alpha角度之后，初始路径结束；在机械臂运动过程中，机械臂控制系统会控制相机拍照，相机会采集每个周期的夹爪抓取点位置posC(i)，每隔固定周期发送给机械臂控制系统。

优选地，在所述模块M4中：

运动初始路径时，相机采集了每个周期的夹爪抓取点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＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

其中q_t为t时刻的位置，t为当前的时刻，a0，a1，a2，a3，a4，a5即为该描述方程的一些待求解的参数；

其求解过程即为求解a₀、a₁、a₂、a₃、a₄、a₅；

本方法中的约束条件初始时刻：

q₀＝S₀

结束位置：

q_f＝S₁

S₀为每个移动窗口初始时刻机械臂的末端位置，V₀为每个移动窗口初始时刻机械臂的末端速度，A₀为每个移动窗口初始时刻机械臂的末端加速度，t＝0时刻的参数；q₀是初始时刻的位置；q_f是每个移动窗口结束时刻的位置；S₁为每个移动窗口结束时刻机械臂的末端位置，V₁为每个移动窗口结束时刻机械臂的末端速度，A₁为每个移动窗口结束时刻机械臂的末端加速度；为初始位置的导数，也是速度，为初始速度的导数，也是初始加速度；为结束位置的导数，也是结束位置的速度，为结束速度的导数，也是结束时的加速度；

根据约束条件推导，可得：

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

上述方程联立求解，求得：

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

其中t_f为固定值和伺服插补周期相乘得到；

对于每个移动窗口，S₀为每个移动窗口的posC_init，V₀为posC_init位置时机械臂的末端速度，A₀为posC_init位置时机械臂的的末端加速度；S₁为每个移动窗口的posC_n，V₁为设置的期望的末端速度，A₁为设置的期望的末端加速度；

根据上述描述的实时跟随轨迹规划方法，在每个插补周期生成轨迹插补位置，将生成的插补位置已下发给伺服，实现实时调整可翻转物体盖的翻转轨迹，完成可翻转物体盖的翻转动作。

与现有技术相比，本发明具有如下的有益效果：

1、本发明解决可翻转物体自动清扫领域的空白技术；

2、本发明机械臂与底盘系统结合，保证机械臂能够到达各个区域；视觉与机械臂系统结合，机械臂能够自动规划路径，无需示教引导，机械臂根据视觉的点位，能够自动搜索计算翻转马桶的位置和姿态，具有智能化，大大提升打扫效率；

3、本发明采用该实时跟随轨迹方法，解决马桶盖翻转过程中点位精度误差导致的实际轨迹与理论位置偏差较大的技术问题；

4、本发明机械臂翻转马桶过程，采用实时跟随在线调整轨迹，有效保护了马桶盖不被损坏。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为马桶点位示意图；

图2为马桶翻转轨迹规划位置点示意图；

图3为实时跟随轨迹规划位置点示意图。

其中，图中各组件表示为：

地面1 马桶盖2

马桶后墙壁3 马桶后盖4

马桶侧面墙壁5

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例1：

根据本发明提供的一种可翻转物体翻转实时跟随轨迹规划方法，如图1-图3所示，包括：

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

步骤S2：机械臂控制视觉系统拍照，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹，将初始段轨迹设计成理论的圆弧轨迹；

步骤S3：初始段理论圆弧轨迹生成之后，机械臂控制器进行速度规划、轨迹插补，下发规划的位置点，采集每个插补周期的实际夹爪抓取位置；

步骤S4：每隔固定周期将实际夹爪抓取位置发送给机械臂控制系统，基于该数据点集，内部采用实时跟随轨迹方法，规划得到可翻转物体盖的翻转轨迹。

具体地，在所述步骤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。

具体地，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。

具体地，在所述步骤S3中：

进行速度规划以及轨迹插补，生成每个伺服周期的下发位置点；初始路径的速度规划方法采用T型速度规划或S型速度规划；当运动至act_alpha角度之后，初始路径结束；在机械臂运动过程中，机械臂控制系统会控制相机拍照，相机会采集每个周期的夹爪抓取点位置posC(i)，每隔固定周期发送给机械臂控制系统。

具体地，在所述步骤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＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

其中q_t为t时刻的位置，t为当前的时刻，a0，a1，a2，a3，a4，a5即为该描述方程的一些待求解的参数；

其求解过程即为求解a₀、a₁、a₂、a₃、a₄、a₅；

本方法中的约束条件初始时刻：

q₀＝S₀

结束位置：

q_f＝S₁

S₀为每个移动窗口初始时刻机械臂的末端位置，V₀为每个移动窗口初始时刻机械臂的末端速度，A₀为每个移动窗口初始时刻机械臂的末端加速度，t＝0时刻的参数；q₀是初始时刻的位置；q_f是每个移动窗口结束时刻的位置；S₁为每个移动窗口结束时刻机械臂的末端位置，V₁为每个移动窗口结束时刻机械臂的末端速度，A₁为每个移动窗口结束时刻机械臂的末端加速度；为初始位置的导数，也是速度，为初始速度的导数，也是初始加速度；为结束位置的导数，也是结束位置的速度，为结束速度的导数，也是结束时的加速度；

根据约束条件推导，可得：

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

上述方程联立求解，求得：

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

其中t_f为固定值和伺服插补周期相乘得到；

对于每个移动窗口，S₀为每个移动窗口的posC_init，V₀为posC_init位置时机械臂的末端速度，A₀为posC_init位置时机械臂的的末端加速度；S₁为每个移动窗口的posC_n，V₁为设置的期望的末端速度，A₁为设置的期望的末端加速度；

根据上述描述的实时跟随轨迹规划方法，在每个插补周期生成轨迹插补位置，将生成的插补位置已下发给伺服，实现实时调整可翻转物体盖的翻转轨迹，完成可翻转物体盖的翻转动作。

实施例2：

实施例2为实施例1的优选例，以更为具体地对本发明进行说明。

本领域技术人员可以将本发明提供的一种可翻转物体翻转实时跟随轨迹规划方法，理解为可翻转物体翻转实时跟随轨迹规划系统的具体实施方式，即所述可翻转物体翻转实时跟随轨迹规划系统可以通过执行所述可翻转物体翻转实时跟随轨迹规划方法的步骤流程予以实现。

根据本发明提供的一种可翻转物体翻转实时跟随轨迹规划系统，包括：

模块M1：安装有机械臂的底盘导航至可翻转物体的附近位置；

模块M2：机械臂控制视觉系统拍照，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹，将初始段轨迹设计成理论的圆弧轨迹；

模块M3：初始段理论圆弧轨迹生成之后，机械臂控制器进行速度规划、轨迹插补，下发规划的位置点，采集每个插补周期的实际夹爪抓取位置；

模块M4：每隔固定周期将实际夹爪抓取位置发送给机械臂控制系统，基于该数据点集，内部采用实时跟随轨迹方法，规划得到可翻转物体盖的翻转轨迹。

具体地，在所述模块M2中：

在可翻转物体附近之后机械臂控制视觉系统拍照，机械臂根据视觉给的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹；初始段轨迹设计成理论的圆弧轨迹，运动角度为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。

具体地，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。

具体地，在所述模块M3中：

进行速度规划以及轨迹插补，生成每个伺服周期的下发位置点；初始路径的速度规划方法采用T型速度规划或S型速度规划；当运动至act_alpha角度之后，初始路径结束；在机械臂运动过程中，机械臂控制系统会控制相机拍照，相机会采集每个周期的夹爪抓取点位置posC(i)，每隔固定周期发送给机械臂控制系统。

具体地，在所述模块M4中：

运动初始路径时，相机采集了每个周期的夹爪抓取点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＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

其中q_t为t时刻的位置，t为当前的时刻，a0，a1，a2，a3，a4，a5即为该描述方程的一些待求解的参数；

其求解过程即为求解a₀、a₁、a₂、a₃、a₄、a₅；

本方法中的约束条件初始时刻：

q₀＝S₀

结束位置：

q_f＝S₁

S₀为每个移动窗口初始时刻机械臂的末端位置，V₀为每个移动窗口初始时刻机械臂的末端速度，A₀为每个移动窗口初始时刻机械臂的末端加速度，t＝0时刻的参数；q₀是初始时刻的位置；q_f是每个移动窗口结束时刻的位置；S₁为每个移动窗口结束时刻机械臂的末端位置，V₁为每个移动窗口结束时刻机械臂的末端速度，A₁为每个移动窗口结束时刻机械臂的末端加速度；为初始位置的导数，也是速度，为初始速度的导数，也是初始加速度；为结束位置的导数，也是结束位置的速度，为结束速度的导数，也是结束时的加速度；

根据约束条件推导，可得：

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

上述方程联立求解，求得：

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

其中t_f为固定值和伺服插补周期相乘得到；

对于每个移动窗口，S₀为每个移动窗口的posC_init，V₀为posC_init位置时机械臂的末端速度，A₀为posC_init位置时机械臂的的末端加速度；S₁为每个移动窗口的posC_n，V₁为设置的期望的末端速度，A₁为设置的期望的末端加速度；

根据上述描述的实时跟随轨迹规划方法，在每个插补周期生成轨迹插补位置，将生成的插补位置已下发给伺服，实现实时调整可翻转物体盖的翻转轨迹，完成可翻转物体盖的翻转动作。

实施例3：

实施例3为实施例1的优选例，以更为具体地对本发明进行说明。

步骤1：该技术所采用的平台机械臂安装放置在移动底盘上，移动底盘可以带着机械臂能够自主导航至马桶附近位置；

步骤2：在马桶附近之后机械臂控制视觉系统拍照，然后机械臂根据视觉给的周围环境位置，采用自动路径规划方法，自动计算生成马桶翻转过程初始段轨迹；初始段轨迹设计成理论的圆弧轨迹，运动角度为act_alpha，其值基于总的理论圆弧角度，本方案取act_alpha＝0.1*α；其中α的计算求解在后文描述，α是总的理论圆弧角度，即是posC至posC_1的轨迹的理论角度

步骤3：初始段理论总路径即理论的圆弧轨迹生成之后，机械臂控制器进行速度规划，轨迹插补，下发规划的位置点，同时在机械臂开始运动之后，机械臂会控制视觉系统拍照，采集每个插补周期的实际夹爪抓取位置posC(i)；posC(i)表示第i个周期的夹持点posC；POSC是马桶盖前方点位，也是机械臂末端抓取点位；

步骤4：每隔固定周期10*servo_cycle_time，其中servo_cycle_time是伺服插补周期；视觉系统将相机拍照数据posC(i)发送给机械臂控制系统，机械臂控制系统基于该数据点集，内部采用实时跟随轨迹方法，最终规划得到马桶盖的翻转轨迹。

进一步地，步骤2中，所述自动路径规划方法的步骤为：如图1所示，机械臂会控制视觉拍照，视觉会得到马桶附近的局部地图信息，包括各个位置点，其中posA表示马桶盖的翻转轴线上第一个点；posB表示马桶盖翻转轴线上第二个点；posC表示马桶盖外边沿的夹持点，即机械臂末端夹持点；posD表示马桶盖前方的一个位置点；

机械臂控制视觉拍照，视觉计算得到马桶附近的位置点，并通过mqtt协议将点位数据发给机械臂控制系统，发送数据包括posA、posB、posC、posE、posF、posG；其中posE和PosF表示马桶后盖外边沿的两个角点；posD为posE与posF的中间点，即posD＝0.5*(posE+posF)；posG为马桶侧边墙壁面和posE、posF形成的直线的交点；

步骤2.1然后计算理论圆弧末端点posC_1；其中posC为马桶盖前方点位，也是机械臂末端抓取点位，首先计算翻转完成之后整个圆弧的总的角度α，点位posA、posB、posC组成一个平面1，posA、posB、posD组成一个平面2，平面1与平面2的夹角即为α，其详细计算步骤不在推导赘述；之后计算理论的圆弧末端点posC_1，根据posC与轴心线posA-posB以及夹角α，计算得到posC_1；

步骤2.2然后计算翻转过程中posC，posC_1的姿态，姿态计算首先确定末端rz，rz表示机械臂末端姿态信息，rz表示沿Z轴的旋转矩阵，ry表示沿Y轴的旋转矩阵，rx表示沿X轴的旋转矩阵；考虑到防碰撞，采用自动搜索计算，先计算马桶离墙壁的距离d＝posF-posG；再结合机械臂本身的机械结构参数，姿态进行搜索计算，首先计算确定rz,其计算步骤如下：

i1：首先选择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导致碰撞，跳i3步骤继续计算rz，如果不会碰撞，则确定了rz＝posA-posC；

i3:重新计算rz，改为通过向量posC至posA与posB的中间点计算，即向量rz＝0.5*(posA+posB)-posC；然后rz再归一化，即确定了rz如果机械臂4，5，6轴处关节连接处的设计尺寸仍会大于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；

进一步地，步骤3中，计算确定了总的理论圆弧路径的始末位置和姿态之后，接着进行速度规划以及轨迹插补，即可生成每个伺服周期的下发位置点；初始路径的速度规划方法采用传统的T型速度规划或S型速度规划；posC至posC_1的轨迹，理论角度为α；此处初始路径至运动至角度为act_alpha＝0.1*α；当运动至act_alpha角度之后，本方案中的初始路径即结束。在机械臂运动过程中，机械臂控制系统会控制相机拍照，相机会采集每个周期的夹爪抓取点位置，每隔固定周期10*servo_cycle_time，发送给机械臂控制系统，接着采用步骤4；

进一步的，步骤4中所述的实时跟随轨迹方法。其实现具体步骤如下：

步骤4.1基于上一步描述的，运动初始路径时，相机采集了每个周期的夹爪抓取点posC(i)，并将数据发送给了机械臂控制器系统，机械臂控制器系统根据这些数据点集，采用最小二乘法，拟合得到一个圆轨迹，拟合的圆轨迹示意图如图3所示；posC至posC_init之间的位置点是相机给的位置点，posC_init至posC_n是拟合完成之后，往后续周期预测50个插补周期位置，这50个位置点是基于拟合圆生成的位置点；接着采用实时跟随调整方法，去计算规划位置；

理论的圆弧轨迹，圆弧角度为act_alpha；圆弧起点是posC，则根据圆弧角度act_alpha，计算得到圆弧末端点为posC_init；

采集了运行过程中的数据之后重新拟合了一个新的圆弧圆弧会经过posC和posC_init，在posC_init后再取距离posC_init点50个周期的位置，该位置即为posC_n；posC_posC_init，posC_n都是拟合圆弧上的点位；

步骤4.2机械臂运动过程中，相机一直在采集数据。该描述的实时跟随调整方法即是，第i步，i为从1开始的步序，采用机械臂到达posC_init之后，再将posC_init至posC_n之间的理论圆弧位置做为输入，每次取50个周期的位置做为一个移动窗口，机械臂末端去实时跟随这些位置，同时每个周期判断posC_n，posA和posB平面与posA，posB，posC平面的夹角progres_alpha，当两个平面的夹角progres_alpha大于等于α时，即认为路径结束，即此时的posC_n即为结束点。如果progres_alpha小于α，则跳下一步i+1步，在第i+1步，第i步运动完，将上一步相机采集的新的数据和先前的相机数据结合，重新拟合会生成新的拟合圆弧，生成新的posC_init；然后重复步骤i；直至产生结束点posC_n。

步骤4.3每次取50个周期的位置做为一个移动窗口，机械臂取实时跟随这些位置点，实时跟随轨迹规划为基于五次多项式的实时跟随轨迹规划方法，其具体描述如下：

所采用的五次多项式方程描述如下：

q_t＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

其中q_t为t时刻的位置，t为当前的时刻，a0，a1，a2，a3，a4，a5即为该描述方程的一些待求解的参数；

其求解过程即为求解a₀、a₁、a₂、a₃、a₄、a₅。

本方法中的约束条件初始时刻：q₀＝S₀

结束位置：q_f＝S₁

S₀为上述描述的每个移动窗口初始时刻机械臂的末端位置，V₀为上述描述的每个移动窗口初始时刻机械臂的末端速度，A₀为上述描述的每个移动窗口初始时刻机械臂的末端加速度；即t＝0时刻的参数；q0是初始时刻的位置；q_f是每个移动窗口结束时刻的位置；

S₁为上述描述的每个移动窗口结束时刻机械臂的末端位置，V₁为上述描述的每个移动窗口结束时刻机械臂的末端速度，A₁为上述描述的每个移动窗口结束时刻机械臂的末端加速度，即t＝50*servo_cycle_time时刻的参数；

根据约束条件推导，可得

a₀＝S₀；

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁；

a₁＝V₀；

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁；

2*a₂＝A₀；

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁；

上述6个方程联立求解，求得：

a₀＝S₀；

a₁＝V₀；

a₂＝A₀/2；

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³；

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴；

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵；

其中t_f＝50*servo_cycle_time；

对于每个移动窗口

S₀为每个移动窗口的posC_init，V₀为posC_init位置时机械臂的末端速度，A₀为posC_init位置时机械臂的的末端加速度；

S₁为每个移动窗口的posC_n，V₁为设置的期望的末端速度，A₁为设置的期望的末端加速度；

根据上述描述的实时跟随轨迹规划方法，即可在每个插补周期，生成轨迹插补位置，将生成的插补位置已下发给伺服，最终实现实时调整马桶盖的翻转轨迹，完成马桶盖的翻转动作。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (8)

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

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

步骤S2：机械臂控制视觉系统拍照，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹，将初始段轨迹设计成理论的圆弧轨迹；

步骤S3：初始段理论圆弧轨迹生成之后，机械臂控制器进行速度规划、轨迹插补，下发规划的位置点，采集每个插补周期的实际夹爪抓取位置；

步骤S4：每隔固定周期将实际夹爪抓取位置发送给机械臂控制系统，基于数据点集，内部采用实时跟随轨迹方法，规划得到可翻转物体盖的翻转轨迹；

在所述步骤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＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

其中q_t为t时刻的位置，t为当前的时刻，a0，a1，a2，a3，a4，a5即为该描述方程的一些待求解的参数；

其求解过程即为求解a₀、a₁、a₂、a₃、a₄、a₅；

本方法中的约束条件初始时刻：

q₀＝S₀

结束位置：

q_f＝S₁

S₀为每个移动窗口初始时刻机械臂的末端位置，V₀为每个移动窗口初始时刻机械臂的末端速度，A₀为每个移动窗口初始时刻机械臂的末端加速度，t＝0时刻的参数；q₀是初始时刻的位置；q_f是每个移动窗口结束时刻的位置；S₁为每个移动窗口结束时刻机械臂的末端位置，V₁为每个移动窗口结束时刻机械臂的末端速度，A₁为每个移动窗口结束时刻机械臂的末端加速度；为初始位置的导数，也是速度，为初始速度的导数，也是初始加速度；为结束位置的导数，也是结束位置的速度，为结束速度的导数，也是结束时的加速度；

根据约束条件推导，可得：

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a⁴*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

上述方程联立求解，求得：

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

其中t_f为固定值和伺服插补周期相乘得到；

对于每个移动窗口，S₀为每个移动窗口的posC_init，V₀为posC_init位置时机械臂的末端速度，A₀为posC_init位置时机械臂的的末端加速度；S₁为每个移动窗口的posC_n，V₁为设置的期望的末端速度，A₁为设置的期望的末端加速度；

根据上述描述的实时跟随轨迹规划方法，在每个插补周期生成轨迹插补位置，将生成的插补位置已下发给伺服，实现实时调整可翻转物体盖的翻转轨迹，完成可翻转物体盖的翻转动作。

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.一种可翻转物体翻转实时跟随轨迹规划系统，其特征在于，包括：

模块M1：安装有机械臂的底盘导航至可翻转物体的附近位置；

模块M2：机械臂控制视觉系统拍照，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹，将初始段轨迹设计成理论的圆弧轨迹；

模块M3：初始段理论圆弧轨迹生成之后，机械臂控制器进行速度规划、轨迹插补，下发规划的位置点，采集每个插补周期的实际夹爪抓取位置；

模块M4：每隔固定周期将实际夹爪抓取位置发送给机械臂控制系统，基于数据点集，内部采用实时跟随轨迹方法，规划得到可翻转物体盖的翻转轨迹；

在所述模块M4中：

运动初始路径时，相机采集了每个周期的夹爪抓取点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＝a₀+a₁*t+a₂*t²+a₃*t³+a₄*t⁴+a₅*t⁵

其中q_t为t时刻的位置，t为当前的时刻，a0，a1，a2，a3，a4，a5即为该描述方程的一些待求解的参数；

其求解过程即为求解a₀、a₁、a₂、a₃、a₄、a₅；

本方法中的约束条件初始时刻：

q₀＝S₀

结束位置：

q_f＝S₁

S₀为每个移动窗口初始时刻机械臂的末端位置，V₀为每个移动窗口初始时刻机械臂的末端速度，A₀为每个移动窗口初始时刻机械臂的末端加速度，t＝0时刻的参数；q₀是初始时刻的位置；q_f是每个移动窗口结束时刻的位置；S₁为每个移动窗口结束时刻机械臂的末端位置，V₁为每个移动窗口结束时刻机械臂的末端速度，A₁为每个移动窗口结束时刻机械臂的末端加速度；为初始位置的导数，也是速度，为初始速度的导数，也是初始加速度；为结束位置的导数，也是结束位置的速度，为结束速度的导数，也是结束时的加速度；

根据约束条件推导，可得：

a₀＝S₀

a₀+a₁*t_f+a₂*t_f ²+a₃*t_f ³+a₄*t_f ⁴+a₅*t_f ⁵＝S₁

a₁＝V₀

a₁+2*a₂*t_f+3*a₃*t_f ²+4*a₄*t_f ³+5*a₅*t_f ⁴＝V₁

2*a₂＝A₀

2*a₂+6*a₃*t_f+12*a₄*t_f ²+20*a₅*t_f ³＝A₁

上述方程联立求解，求得：

a₀＝S₀

a₁＝V₀

a₂＝A₀/2

a₃＝(A₁-3*A₀)/(2*t_f)-(4*V₁+6*V₀)/t_f ²+10*(S₁-S₀)/t_f ³

a₄＝(3*A₀-2*A₁)/(2*t_f ²)+(7*V₁+8*V₀)/t_f ³-15*(S₁-S₀)/t_f ⁴

a₅＝(A₁-A₀)/(2*t_f ³)-3*(V₁+V₀)/t_f ⁴+6*(S₁-S₀)/t_f ⁵

其中t_f为固定值和伺服插补周期相乘得到；

对于每个移动窗口，S₀为每个移动窗口的posC_init，V₀为posC_init位置时机械臂的末端速度，A₀为posC_init位置时机械臂的的末端加速度；S₁为每个移动窗口的posC_n，V₁为设置的期望的末端速度，A₁为设置的期望的末端加速度；

根据上述描述的实时跟随轨迹规划方法，在每个插补周期生成轨迹插补位置，将生成的插补位置已下发给伺服，实现实时调整可翻转物体盖的翻转轨迹，完成可翻转物体盖的翻转动作。

6.根据权利要求5所述的可翻转物体翻转实时跟随轨迹规划系统，其特征在于，在所述模块M2中：

在可翻转物体附近之后机械臂控制视觉系统拍照，机械臂根据视觉给的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体翻转过程初始段轨迹；初始段轨迹设计成理论的圆弧轨迹，运动角度为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。

7.根据权利要求5所述的可翻转物体翻转实时跟随轨迹规划系统，其特征在于：

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。

8.根据权利要求5所述的可翻转物体翻转实时跟随轨迹规划系统，其特征在于，在所述模块M3中：

进行速度规划以及轨迹插补，生成每个伺服周期的下发位置点；初始路径的速度规划方法采用T型速度规划或S型速度规划；当运动至act_alpha角度之后，初始路径结束；在机械臂运动过程中，机械臂控制系统会控制相机拍照，相机会采集每个周期的夹爪抓取点位置posC(i)，每隔固定周期发送给机械臂控制系统。