CN113892848B - 可翻转物体的翻转跟随轨迹规划方法、装置及系统 - Google Patents

可翻转物体的翻转跟随轨迹规划方法、装置及系统 Download PDF

Info

Publication number
CN113892848B
CN113892848B CN202111172448.8A CN202111172448A CN113892848B CN 113892848 B CN113892848 B CN 113892848B CN 202111172448 A CN202111172448 A CN 202111172448A CN 113892848 B CN113892848 B CN 113892848B
Authority
CN
China
Prior art keywords
posc
force
posb
mechanical arm
posa
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111172448.8A
Other languages
English (en)
Other versions
CN113892848A (zh
Inventor
谢能达
郭震
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hangzhou Jingwu Intelligent Technology Co ltd
Original Assignee
Hangzhou Jingwu Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hangzhou Jingwu Intelligent Technology Co ltd filed Critical Hangzhou Jingwu Intelligent Technology Co ltd
Priority to CN202111172448.8A priority Critical patent/CN113892848B/zh
Publication of CN113892848A publication Critical patent/CN113892848A/zh
Application granted granted Critical
Publication of CN113892848B publication Critical patent/CN113892848B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • A—HUMAN NECESSITIES
    • A47—FURNITURE; DOMESTIC ARTICLES OR APPLIANCES; COFFEE MILLS; SPICE MILLS; SUCTION CLEANERS IN GENERAL
    • A47K—SANITARY EQUIPMENT NOT OTHERWISE PROVIDED FOR; TOILET ACCESSORIES
    • A47K17/00—Other equipment, e.g. separate apparatus for deodorising, disinfecting or cleaning devices without flushing for toilet bowls, seats or covers; Holders for toilet brushes
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/13—Differential equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Abstract

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

Description

可翻转物体的翻转跟随轨迹规划方法、装置及系统
技术领域
本发明涉及智能机器人清洁领域，具体地，涉及一种可翻转物体的翻转跟随轨迹规划方法、装置及系统，更为具体地，涉及一种马桶清洁时马桶盖翻转跟随轨迹规划方法、装置及系统。
背景技术
现有的轨迹跟踪系统中的先进者采用传感器来跟踪轨迹，例如弧焊机器人系统采用结构光视觉传感器来跟踪轨迹。轨迹跟踪系统中的执行器自身有一个通过示教信息或规划等手段预定的任务轨迹，带工具的执行器能根据该预定的任务轨迹生成驱动信息，使工具沿着预定的任务轨迹运行，而轨迹跟踪系统仅能简单地把传感器采集、提取的轨迹偏差信息直接反馈到执行器中，对预定的任务轨迹进行修正。
对于翻转物体的轨迹跟随更是一直是一个难题，几乎没有相关发明可以精确跟随翻转物体的轨迹跟随。
因此现在在清洁领域，使用智能机器人清扫马桶也属于空白领域，马桶清洁流程复杂，基本是人力清扫。使用智能机器人取代人力，对马桶盖进行翻转打扫的技术也是空白。马桶清扫时最大的难点就是马桶盖的翻转，最大的特点就是需要机器人与马桶盖相接触，这就需要机器人关节具有一定柔性，像人手一样具有力的感知，通过感知末端对马桶盖的作用力，实时地调整翻转时末端位姿从而完成马桶盖翻转任务。如何提供一种可以结合力控实现马桶盖翻转的方法，是当前需要解决的技术问题。
现阶段使用机械臂翻转马桶盖的技术为空白。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种可翻转物体的翻转跟随轨迹规划方法、装置及系统。
根据本发明提供的一种可翻转物体的翻转跟随轨迹规划方法，包括：
步骤S1：安装有机械臂的底盘导航至可翻转物体的附近位置；
步骤S2：机械臂控制视觉系统拍照，根据视觉提供的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体的翻转过程的位置姿态路径；
步骤S3：对生成的路径进行轨迹插补，生成位置控制下的位置点，采用力控制技术和位置控制相结合的阻抗控制方法实现可翻转物体的盖的翻转动作。
优选地，在所述步骤S2中：
机械臂控制视觉拍照，视觉计算得到可翻转物体的附近的局部地图信息，包括位置点，并通过mqtt协议将点位数据发给机械臂控制系统，发送数据包括posA、posB、posC、posE、posF、posG，发送数据包括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。
优选地，在所述步骤S3中：
阻抗控制方法为基于末端的六维力矩传感器与可翻转物体的盖的接触力,通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制；
机械臂末端安装有六维力传感器，感知各方向的力Fe，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系，阻抗控制是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制，末端的六维力矩传感器与可翻转物体的盖的接触力，通过一个二阶阻抗控制模型模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，将位置下发给伺服，完成位置控制。
优选地，具体步骤如下：
a、阻抗控制方法采用的阻抗控制模型如下：
Figure BDA0003293818650000031
其中，
Figure BDA0003293818650000032
为正对角阵，Md为机械臂期望的惯性矩阵，Bd为阻尼矩阵，Kd为刚度矩阵，Fd为机械臂末端执行器作用于环境的期望力，Fe为机械臂末端执行器作用于环境实际力；
Figure BDA0003293818650000033
为实际位置的微分的微分，即实际加速度，
Figure BDA0003293818650000034
为期望位置的微分的微分，即期望加速度，
Figure BDA0003293818650000035
为实际位置的微分，即实际速度，
Figure BDA0003293818650000036
为期望位置的微分，即期望速度，X为实际位置，Xd为期望位置；
b、在机械臂控制系统中引入阻抗控制后，通过控制机器人的位置并且实现力与位置之间的期望关系Z(S)，间接控制交互力；
根据上述阻抗控制模型，得出力与位置之间的期望关系Z(S)如下
Z(S)＝Mds+Bd+Kd/s
其中S为6×6的对角阵，其对角线元素为1或0，它的主要功能是用来确定哪些自由度施加力控，哪些自由度施加位置控制；
在时域中力与位置之间的期望关系用微分方程表示为：
Figure BDA0003293818650000041
其中，Md，Bd，Kd为6×6的正对角阵，分别为惯性矩阵，阻尼矩阵和刚度矩阵。通过设置不同的值达到不同的阻抗控制效果，F(t)是机器人与环境间的接触力，X表示机器人的实际位置，Xd表示机器人的指令位置。为了使机器人具备力跟踪的能力，将期望力Fd加入到目标阻抗中，上式变为：
Figure BDA0003293818650000042
Fd为机械臂末端执行器作用于环境的期望力和Fe为机械臂末端执行器作用于环境的实际力，当期望位置Xd接近实际位置X时，实际接触力Fe将收敛到参考力Xd；
根据阻抗控制关系，位置修正量ΔX与力误差ΔF之间的关系为：
ΔF＝(MdS2+BdS+Kd)ΔX
变为时域的形式：
Figure BDA0003293818650000043
采用向后差分离散可得：
Figure BDA0003293818650000044
Figure BDA0003293818650000045
其中T为采样周期，ΔX(k)为k时刻的位置修正量，即位置偏差，ΔX(k-1)为k-1时刻的位置修正量，即位置偏差，ΔX(k-2)为k-2时刻的位置修正量，即位置偏差，
Figure BDA0003293818650000046
为k时刻的速度修正量，即速度偏差；
Figure BDA0003293818650000047
为k时刻的加速度修正量，即加速度偏差；
式可以变为：
Figure BDA0003293818650000048
即：
Figure BDA0003293818650000051
ΔF(k)为k时刻的力偏差量；
当前位置的修正量与当前周期的力偏差，上个周期的位置修正量和上上个周期的位置修正量相关；
本控制方法采用设置的阻抗系统只包含阻尼系数B，M＝0，K＝0，则当前位置修正量为：
Figure BDA0003293818650000052
期望位置经过选择矩阵I-S后得到，位置控制方向的期望位置和力控方向的期望位置，与当前位置修正量叠加产生Xref，其中I是6×6的单位矩阵；
Xref＝Xd(I-S)+ΔX
Xref为计算得到的当前周期的下发的期望的笛卡尔位置；
经过机械臂逆运动学，可以转到关节位置q，计算得到了插补位置；
c、将新计算得到的插补位置下发个伺服，完成整个控制。
根据本发明提供的一种可翻转物体的翻转跟随轨迹规划系统，包括：
模块M1：安装有机械臂的底盘导航至可翻转物体的附近位置；
模块M2：机械臂控制视觉系统拍照，根据视觉提供的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体的翻转过程的位置姿态路径；
模块M3：对生成的路径进行轨迹插补，生成位置控制下的位置点，采用力控制技术和位置控制相结合的阻抗控制方法实现可翻转物体的盖的翻转动作。
优选地，在所述模块M2中：
机械臂控制视觉拍照，视觉计算得到可翻转物体的附近的局部地图信息，包括位置点，并通过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。
优选地，在所述模块M3中：
阻抗控制方法为基于末端的六维力矩传感器与可翻转物体的盖的接触力,通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制；
机械臂末端安装有六维力传感器，感知各方向的力Fe，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系，阻抗控制是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制，末端的六维力矩传感器与可翻转物体的盖的接触力，通过一个二阶阻抗控制模型模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，将位置下发给伺服，完成位置控制；
具体步骤如下：
a、阻抗控制方法采用的阻抗控制模型如下：
Figure BDA0003293818650000071
其中，
Figure BDA0003293818650000072
为正对角阵，Md为机械臂期望的惯性矩阵，Bd为阻尼矩阵，Kd为刚度矩阵，Fd为机械臂末端执行器作用于环境的期望力，Fe为机械臂末端执行器作用于环境实际力；
Figure BDA0003293818650000073
为实际位置的微分的微分，即实际加速度，
Figure BDA0003293818650000074
为期望位置的微分的微分，即期望加速度，
Figure BDA0003293818650000075
为实际位置的微分，即实际速度，
Figure BDA0003293818650000076
为期望位置的微分，即期望速度，X为实际位置，Xd为期望位置；
b、在机械臂控制系统中引入阻抗控制后，通过控制机器人的位置并且实现力与位置之间的期望关系Z(S)，间接控制交互力；
根据上述阻抗控制模型，得出力与位置之间的期望关系Z(S)如下
Z(S)＝Mds+Bd+Kd/s
其中S为6×6的对角阵，其对角线元素为1或0，它的主要功能是用来确定哪些自由度施加力控，哪些自由度施加位置控制；
在时域中力与位置之间的期望关系用微分方程表示为：
Figure BDA0003293818650000077
其中，Md，Bd，Kd为6×6的正对角阵，分别为惯性矩阵，阻尼矩阵和刚度矩阵。通过设置不同的值达到不同的阻抗控制效果，F(t)是机器人与环境间的接触力，X表示机器人的实际位置，Xd表示机器人的指令位置。为了使机器人具备力跟踪的能力，将期望力Fd加入到目标阻抗中，上式变为：
Figure BDA0003293818650000078
Fd为机械臂末端执行器作用于环境的期望力和Fe为机械臂末端执行器作用于环境的实际力，当期望位置Xd接近实际位置X时，实际接触力Fe将收敛到参考力Fd；
根据阻抗控制关系，位置修正量ΔX与力误差ΔF之间的关系为：
ΔF＝(MdS2+BdS+Kd)ΔX
变为时域的形式：
Figure BDA0003293818650000081
采用向后差分离散可得：
Figure BDA0003293818650000082
Figure BDA0003293818650000083
其中T为采样周期，ΔX(k)为k时刻的位置修正量，即位置偏差，ΔX(k-1)为k-1时刻的位置修正量，即位置偏差，ΔX(k-2)为k-2时刻的位置修正量，即位置偏差，
Figure BDA0003293818650000084
为k时刻的速度修正量，即速度偏差；
Figure BDA0003293818650000085
为k时刻的加速度修正量，即加速度偏差；
式可以变为：
Figure BDA0003293818650000086
即：
Figure BDA0003293818650000087
ΔF(k)为k时刻的力偏差量；
当前位置的修正量与当前周期的力偏差，上个周期的位置修正量和上上个周期的位置修正量相关；
本控制方法采用设置的阻抗系统只包含阻尼系数B，M＝0，K＝0，则当前位置修正量为：
Figure BDA0003293818650000088
期望位置经过选择矩阵I-S后得到，位置控制方向的期望位置和力控方向的期望位置，与当前位置修正量叠加产生Xref，其中I是6×6的单位矩阵；
Xref＝Xd(I-S)+ΔX
Xref为计算得到的当前周期的下发的期望的笛卡尔位置；
经过机械臂逆运动学，可以转到关节位置q，计算得到了插补位置；
c、将新计算得到的插补位置下发个伺服，完成整个控制。
根据本发明提供的一种可翻转物体的翻转跟随轨迹规划装置，所述可翻转物体的翻转跟随轨迹规划装置运行时实现上述任一项所述的方法的步骤；或者，实现上述任一项所述的系统。
与现有技术相比，本发明具有如下的有益效果：
1、解决马桶自动清扫领域的空白技术；
2、机械臂与底盘系统结合，保证机械臂能够到达各个区域；视觉与机械臂系统结合，机械臂能够自动规划路径，无需示教引导，机械臂根据视觉的点位，能够自动搜索计算翻转物体的位置和姿态，具有智能化，大大提升打扫效率；
3、机械臂翻转可翻转物体的采用力控制和位置控制相结合的阻抗控制方法，有效保护了可翻转物体的盖不被损坏；
4、结合视觉系统计算得到防碰撞的路径，能够保证环境适应性，可以根据周围环境空间大小，自动计算翻转动作的姿态；
5、机械臂结合视觉系统，提供可翻转物体的盖翻转过程的轨迹跟随规划方法和系统，能够保证运动轨迹的精确性；
6、同时由于翻转过程中，路径点是采用视觉给的局部地图信息计算得到，如果采用位置控制下简单的圆弧路径规划，可能会拉扯可翻转物体的盖导致可翻转物体的盖损坏，故翻转动作不能采用纯位置控制，此方案采用位置控制结合力控制，故该技术采用结合位置控制和力控制的导纳控制方法，可以应用于不同环境下可翻转物体的盖翻转，具有普适性。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为模块示意图；
图2为自动路径规划计算位置点示意图；
其中，图中各组件表示为：
地面1 马桶盖2
马桶后墙壁3 马桶后盖4
马桶侧面墙壁5
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例1：
根据本发明提供的一种可翻转物体的翻转跟随轨迹规划方法，如图1-图2所示，包括：
步骤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。
具体地，在所述步骤S3中：
阻抗控制方法为基于末端的六维力矩传感器与可翻转物体的盖的接触力,通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制；
机械臂末端安装有六维力传感器，感知各方向的力Fe，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系，阻抗控制是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制，末端的六维力矩传感器与可翻转物体的盖的接触力，通过一个二阶阻抗控制模型模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，将位置下发给伺服，完成位置控制。
具体地，具体步骤如下：
a、阻抗控制方法采用的阻抗控制模型如下：
Figure BDA0003293818650000111
其中，
Figure BDA0003293818650000121
为正对角阵，Md为机械臂期望的惯性矩阵，Bd为阻尼矩阵，Kd为刚度矩阵，Fd为机械臂末端执行器作用于环境的期望力，Fe为机械臂末端执行器作用于环境实际力；
Figure BDA0003293818650000122
为实际位置的微分的微分，即实际加速度，
Figure BDA0003293818650000123
为期望位置的微分的微分，即期望加速度，
Figure BDA0003293818650000124
为实际位置的微分，即实际速度，
Figure BDA0003293818650000125
为期望位置的微分，即期望速度，X为实际位置，Xd为期望位置；
b、在机械臂控制系统中引入阻抗控制后，通过控制机器人的位置并且实现力与位置之间的期望关系Z(S)，间接控制交互力；
根据上述阻抗控制模型，得出力与位置之间的期望关系Z(S)如下
Z(S)＝Mds+Bd+Kd/s
其中S为6×6的对角阵，其对角线元素为1或0，它的主要功能是用来确定哪些自由度施加力控，哪些自由度施加位置控制；
在时域中力与位置之间的期望关系用微分方程表示为：
Figure BDA0003293818650000126
其中，Md，Bd，Kd为6×6的正对角阵，分别为惯性矩阵，阻尼矩阵和刚度矩阵。通过设置不同的值达到不同的阻抗控制效果，F(t)是机器人与环境间的接触力，X表示机器人的实际位置，Xd表示机器人的指令位置。为了使机器人具备力跟踪的能力，将期望力Fd加入到目标阻抗中，上式变为：
Figure BDA0003293818650000127
Fd为机械臂末端执行器作用于环境的期望力和Fe为机械臂末端执行器作用于环境的实际力，当期望位置Xd接近实际位置X时，实际接触力Fe将收敛到参考力Fd；
根据阻抗控制关系，位置修正量ΔX与力误差ΔF之间的关系为：
ΔF＝(MdS2+BdS+Kd)ΔX
变为时域的形式：
Figure BDA0003293818650000128
采用向后差分离散可得：
Figure BDA0003293818650000129
Figure BDA00032938186500001210
其中T为采样周期，ΔX(k)为k时刻的位置修正量，即位置偏差，ΔX(k-1)为k-1时刻的位置修正量，即位置偏差，ΔX(k-2)为k-2时刻的位置修正量，即位置偏差，
Figure BDA00032938186500001211
为k时刻的速度修正量，即速度偏差；
Figure BDA0003293818650000131
为k时刻的加速度修正量，即加速度偏差；
式可以变为：
Figure BDA0003293818650000132
即：
Figure BDA0003293818650000133
ΔF(k)为k时刻的力偏差量；
当前位置的修正量与当前周期的力偏差，上个周期的位置修正量和上上个周期的位置修正量相关；
本控制方法采用设置的阻抗系统只包含阻尼系数B，M＝0，K＝0，则当前位置修正量为：
Figure BDA0003293818650000134
期望位置经过选择矩阵I-S后得到，位置控制方向的期望位置和力控方向的期望位置，与当前位置修正量叠加产生Xref，其中I是6×6的单位矩阵；
Xref＝Xd(I-S)+ΔX
Xref为计算得到的当前周期的下发的期望的笛卡尔位置；
经过机械臂逆运动学，可以转到关节位置q，计算得到了插补位置；
c、将新计算得到的插补位置下发个伺服，完成整个控制。
根据本发明提供的一种可翻转物体的翻转跟随轨迹规划装置，所述可翻转物体的翻转跟随轨迹规划装置运行时实现上述任一项所述的方法的步骤。
实施例2：
实施例2为实施例1的优选例，以更为具体地对本发明进行说明。
本领域技术人员可以将本发明提供的一种可翻转物体的翻转跟随轨迹规划方法，理解为可翻转物体的翻转跟随轨迹规划系统的具体实施方式，即所述可翻转物体的翻转跟随轨迹规划系统可以通过执行所述可翻转物体的翻转跟随轨迹规划方法的步骤流程予以实现。
根据本发明提供的一种可翻转物体的翻转跟随轨迹规划系统，包括：
模块M1：安装有机械臂的底盘导航至可翻转物体的附近位置；
模块M2：机械臂控制视觉系统拍照，根据视觉提供的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体的翻转过程的位置姿态路径；
模块M3：对生成的路径进行轨迹插补，生成位置控制下的位置点，采用力控制技术和位置控制相结合的阻抗控制方法实现可翻转物体的盖的翻转动作。
具体地，在所述模块M2中：
机械臂控制视觉拍照，视觉计算得到可翻转物体的附近的局部地图信息，包括位置点，并通过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。
具体地，在所述模块M3中：
阻抗控制方法为基于末端的六维力矩传感器与可翻转物体的盖的接触力,通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制；
机械臂末端安装有六维力传感器，感知各方向的力Fe，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系，阻抗控制是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制，末端的六维力矩传感器与可翻转物体的盖的接触力，通过一个二阶阻抗控制模型模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，将位置下发给伺服，完成位置控制；
具体步骤如下：
a、阻抗控制方法采用的阻抗控制模型如下：
Figure BDA0003293818650000151
其中，
Figure BDA0003293818650000152
为正对角阵，Md为机械臂期望的惯性矩阵，Bd为阻尼矩阵，Kd为刚度矩阵，Fd为机械臂末端执行器作用于环境的期望力，Fe为机械臂末端执行器作用于环境实际力；
Figure BDA0003293818650000153
为实际位置的微分的微分，即实际加速度，
Figure BDA0003293818650000154
为期望位置的微分的微分，即期望加速度，
Figure BDA0003293818650000155
为实际位置的微分，即实际速度，
Figure BDA0003293818650000156
为期望位置的微分，即期望速度，X为实际位置，Xd为期望位置；
b、在机械臂控制系统中引入阻抗控制后，通过控制机器人的位置并且实现力与位置之间的期望关系Z(S)，间接控制交互力；
根据上述阻抗控制模型，得出力与位置之间的期望关系Z(S)如下
Z(S)＝Mds+Bd+Kd/s
其中S为6×6的对角阵，其对角线元素为1或0，它的主要功能是用来确定哪些自由度施加力控，哪些自由度施加位置控制；
在时域中力与位置之间的期望关系用微分方程表示为：
Figure BDA0003293818650000161
其中，Md，Bd，Kd为6×6的正对角阵，分别为惯性矩阵，阻尼矩阵和刚度矩阵。通过设置不同的值达到不同的阻抗控制效果，F(t)是机器人与环境间的接触力，X表示机器人的实际位置，Xd表示机器人的指令位置。为了使机器人具备力跟踪的能力，将期望力Fd加入到目标阻抗中，上式变为：
Figure BDA0003293818650000162
Fd为机械臂末端执行器作用于环境的期望力和Fe为机械臂末端执行器作用于环境的实际力，当期望位置Xd接近实际位置X时，实际接触力Fe将收敛到参考力Fd；
根据阻抗控制关系，位置修正量ΔX与力误差ΔF之间的关系为：
ΔF＝(MdS2+BdS+Kd)ΔX
变为时域的形式：
Figure BDA0003293818650000163
采用向后差分离散可得：
Figure BDA0003293818650000164
Figure BDA0003293818650000165
其中T为采样周期，ΔX(k)为k时刻的位置修正量，即位置偏差，ΔX(k-1)为k-1时刻的位置修正量，即位置偏差，ΔX(k-2)为k-2时刻的位置修正量，即位置偏差，
Figure BDA0003293818650000166
为k时刻的速度修正量，即速度偏差；
Figure BDA0003293818650000167
为k时刻的加速度修正量，即加速度偏差；
式可以变为：
Figure BDA0003293818650000168
即：
Figure BDA0003293818650000169
ΔF(k)为k时刻的力偏差量；
当前位置的修正量与当前周期的力偏差，上个周期的位置修正量和上上个周期的位置修正量相关；
本控制方法采用设置的阻抗系统只包含阻尼系数B，M＝0，K＝0，则当前位置修正量为：
Figure BDA0003293818650000171
期望位置经过选择矩阵I-S后得到，位置控制方向的期望位置和力控方向的期望位置，与当前位置修正量叠加产生Xref，其中I是6×6的单位矩阵；
Xref＝Xd(I-S)+ΔX
Xref为计算得到的当前周期的下发的期望的笛卡尔位置；
经过机械臂逆运动学，可以转到关节位置q，计算得到了插补位置；
c、将新计算得到的插补位置下发个伺服，完成整个控制。
根据本发明提供的一种可翻转物体的翻转跟随轨迹规划装置，所述可翻转物体的翻转跟随轨迹规划装置运行时实现上述任一项所述的系统。
实施例3：
实施例3为实施例1的优选例，以更为具体地对本发明进行说明。
1、该平台是机械臂安装放置在底盘上，机械臂底部是底盘系统，底盘能够自主导航至马桶附近位置；
2、机械臂控制视觉系统拍照，然后机械臂根据视觉给的周围环境位置，采用自动路径规划方法，自动计算生成马桶翻转过程的位置姿态路径；
3、路径生成之后，轨迹插补，轨迹插补生成的是位置控制下的位置点，同时会结合力控制技术，即采用力控制和位置控制相结合的阻抗控制方法，实现马桶盖的翻转动作；
进一步地，步骤2中，所述自动路径规划方法的步骤为：如图1所示，机械臂会控制视觉拍照，视觉会得到马桶附近的局部地图信息，包括各个位置点，其中posA表示马桶盖前方的一个位置点；posB表示马桶盖的翻转轴线上第一个点；posC表示马桶盖翻转轴线上第二个点；posE和PosF表示马桶后盖外边沿的两个角点；posD为posE与posF的中间点；posG为马桶侧边墙壁面和posE、posF形成的直线的交点；
a)机械臂控制视觉拍照，视觉计算得到马桶附近的位置点，并通过mqtt协议将点位数据发给机械臂控制系统，发送数据包括posA、posB、posC、posE、posF、posG；
b)posC为马桶盖前方点位，也是机械臂末端抓取点位，首先计算翻转之后的圆弧末端点posC_1；其计算过程如下：如图2所示，首先计算角度posC，posB和posC_1之间的夹角α，根据点位posA、posB、posC组成一个平面1，posA、posB、posD组成一个平面2，α即为平面1与平面2的夹角；然后根据posC与轴心线posA_posB以及夹角α，计算得到posC_1；c)；然后计算翻转过程中posC，posC_1的姿态，姿态计算首先确定末端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；
c)计算确定了圆弧路径的始末位置和姿态之后，接着进行速度规划以及轨迹插补，即可生成中间位置点；速度规划方法采用传统的T型速度规划或S型速度规划；
进一步的，步骤3中所述的阻抗控制方法，机械臂末端安装有六维力传感器，可以感知各方向的力Fe，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系。阻抗控制的目标，是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制。末端的六维力矩传感器与马桶盖的接触力,通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制。其实现具体步骤如下：
a)上述描述的阻抗控制方法采用的阻抗控制模型如下：
Figure BDA0003293818650000191
其中，
Figure BDA0003293818650000192
为正对角阵，Md为机械臂期望的惯性矩阵，Bd为阻尼矩阵，Kd为刚度矩阵。Fd和Fe分别代表机械臂末端执行器作用于环境的期望力和实际力。
b)在机械臂控制系统中引入阻抗控制后，通过控制机器人的位置并且实现力与位置之间的期望关系Z(S)，间接控制交互力。
根据上述阻抗控制模型，得出力与位置之间的期望关系Z(S)如下
Z(S)＝Mds+Bd+Kd/s
其中S为6×6的对角阵，其对角线元素为1或0，它的主要功能是用来确定哪些自由度施加力控，哪些自由度施加位置控制。
在时域中力与位置之间的期望关系用微分方程表示为：
Figure BDA0003293818650000193
其中，Md，Bd，Kd为6×6的正对角阵，分别为惯性矩阵，阻尼矩阵和刚度矩阵。通过设置不同的值达到不同的阻抗控制效果，F(t)是机器人与环境间的接触力，X表示机器人的实际位置，Xd表示机器人的指令位置。为了使机器人具备力跟踪的能力，将期望力Fd加入到目标阻抗中，上式变为：
Figure BDA0003293818650000194
Fd和Fe分别代表机械臂末端执行器作用于环境的期望力和实际力。当期望位置Xd接近实际位置X时，实际接触力Fe将收敛到参考力Fd。
根据阻抗控制关系，位置修正量ΔX与力误差ΔF之间的关系为：
ΔF＝(MdS2+BdS+Kd)ΔX
变为时域的形式：
Figure BDA0003293818650000195
采用向后差分离散可得：
Figure BDA0003293818650000196
Figure BDA0003293818650000197
其中T为采样周期，式可以变为：
Figure BDA0003293818650000198
即：
Figure BDA0003293818650000201
当前位置的修正量与当前周期的力偏差，上个周期的位置修正量和上上个周期的位置修正量相关。
本控制方法采用设置的阻抗系统只包含阻尼系数B，M＝0，K＝0，则当前位置修正量为：
Figure BDA0003293818650000202
期望位置经过选择矩阵I-S后得到，位置控制方向的期望位置和力控方向的期望位置，与当前位置修正量叠加产生Xref。其中I是6×6的单位矩阵。
Xref＝Xd(I-S)+ΔX
经过机械臂逆运动学，可以转到关节位置q，则计算得到了插补位置。
c)将新计算得到的插补位置下发个伺服，完成整个控制。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (6)

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中：
阻抗控制方法为基于末端的六维力矩传感器与可翻转物体的盖的接触力,通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制；
机械臂末端安装有六维力传感器，感知各方向的力Fe，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系，阻抗控制是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制。
3.根据权利要求2所述的可翻转物体的翻转跟随轨迹规划方法，其特征在于，
具体步骤如下：
a、阻抗控制方法采用的阻抗控制模型如下：
Figure FDA0003718572380000021
其中，
Figure FDA0003718572380000022
为正对角阵，Md为机械臂期望的惯性矩阵，Bd为阻尼矩阵，Kd为刚度矩阵，Fd为机械臂末端执行器作用于环境的期望力，Fe为机械臂末端执行器作用于环境实际力；
Figure FDA0003718572380000023
为实际位置的微分的微分，即实际加速度，
Figure FDA0003718572380000024
为期望位置的微分的微分，即期望加速度，
Figure FDA0003718572380000025
为实际位置的微分，即实际速度，
Figure FDA0003718572380000026
为期望位置的微分，即期望速度，X为实际位置，Xd为期望位置；
b、在机械臂控制系统中引入阻抗控制后，通过控制机器人的位置并且实现力与位置之间的期望关系Z(S)，间接控制交互力；
根据上述阻抗控制模型，得出力与位置之间的期望关系Z(S)如下
Z(S)＝Mds+Bd+Kd/s
其中S为6×6的对角阵，其对角线元素为1或0，它的主要功能是用来确定哪些自由度施加力控，哪些自由度施加位置控制；
在时域中力与位置之间的期望关系用微分方程表示为：
Figure FDA0003718572380000031
其中，Md，Bd，Kd为6×6的正对角阵，分别为惯性矩阵，阻尼矩阵和刚度矩阵，通过设置不同的值达到不同的阻抗控制效果，F(t)是机器人与环境间的接触力，X表示机器人的实际位置，Xd表示机器人的指令位置，为了使机器人具备力跟踪的能力，将期望力Fd加入到目标阻抗中，上式变为：
Figure FDA0003718572380000032
Fd为机械臂末端执行器作用于环境的期望力和Fe为机械臂末端执行器作用于环境的实际力，当期望位置Xd接近实际位置X时，实际接触力Fe将收敛到参考力Fd；
根据阻抗控制关系，位置修正量ΔX与力误差ΔF之间的关系为：
ΔF＝(MdS2+BdS+Kd)ΔX
变为时域的形式：
Figure FDA0003718572380000033
采用向后差分离散可得：
Figure FDA0003718572380000034
Figure FDA0003718572380000035
其中T为采样周期，ΔX(k)为k时刻的位置修正量，即位置偏差，ΔX(k-1)为k-1时刻的位置修正量，即位置偏差，ΔX(k-2)为k-2时刻的位置修正量，即位置偏差，
Figure FDA0003718572380000036
为k时刻的速度修正量，即速度偏差；
Figure FDA0003718572380000037
为k时刻的加速度修正量，即加速度偏差；
式可以变为：
Figure FDA0003718572380000038
即：
Figure FDA0003718572380000041
ΔF(k)为k时刻的力偏差量；
当前位置的修正量与当前周期的力偏差，上个周期的位置修正量和上上个周期的位置修正量相关；
阻抗控制方法采用设置的阻抗系统只包含阻尼系数B，M＝0，K＝0，则当前位置修正量为：
Figure FDA0003718572380000042
期望位置经过选择矩阵I-S后得到，位置控制方向的期望位置和力控方向的期望位置，与当前位置修正量叠加产生Xref，其中I是6×6的单位矩阵；
Xref＝Xd(I-S)+ΔX
Xref为计算得到的当前周期的下发的期望的笛卡尔位置；
经过机械臂逆运动学，可以转到关节位置q，计算得到了插补位置；
c、将新计算得到的插补位置下发个伺服，完成整个控制。
4.一种可翻转物体的翻转跟随轨迹规划系统，其特征在于，包括：
模块M1：安装有机械臂的底盘导航至可翻转物体的附近位置；
模块M2：机械臂控制视觉系统拍照，根据视觉提供的周围环境位置，采用自动路径规划方法，自动计算生成可翻转物体的翻转过程的位置姿态路径；
模块M3：对生成的路径进行轨迹插补，生成位置控制下的位置点，采用力控制技术和位置控制相结合的阻抗控制方法实现可翻转物体的盖的翻转动作；
在所述模块M2中：
机械臂控制视觉拍照，视觉计算得到可翻转物体的附近的局部地图信息，包括位置点，并通过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。
5.根据权利要求4所述的可翻转物体的翻转跟随轨迹规划系统，其特征在于，在所述模块M3中：
阻抗控制方法为基于末端的六维力矩传感器与可翻转物体的盖的接触力,通过一个二阶阻抗控制模型，生成一个附加的位置，此附加位置再去修正预先设定的位置轨迹，最终位置下发给伺服，完成最终的位置控制；
机械臂末端安装有六维力传感器，感知各方向的力Fe，阻抗是指机械臂末端位置偏差与环境作用力之间的动态关系，阻抗控制是为了调节机械臂末端位置偏差与环境作用力之间的动态关系从而实现主动柔顺控制；
具体步骤如下：
a、阻抗控制方法采用的阻抗控制模型如下：
Figure FDA0003718572380000061
其中，
Figure FDA0003718572380000062
为正对角阵，Md为机械臂期望的惯性矩阵，Bd为阻尼矩阵，Kd为刚度矩阵，Fd为机械臂末端执行器作用于环境的期望力，Fe为机械臂末端执行器作用于环境实际力；
Figure FDA0003718572380000063
为实际位置的微分的微分，即实际加速度，
Figure FDA0003718572380000064
为期望位置的微分的微分，即期望加速度，
Figure FDA0003718572380000065
为实际位置的微分，即实际速度，
Figure FDA0003718572380000066
为期望位置的微分，即期望速度，X为实际位置，Xd为期望位置；
b、在机械臂控制系统中引入阻抗控制后，通过控制机器人的位置并且实现力与位置之间的期望关系Z(S)，间接控制交互力；
根据上述阻抗控制模型，得出力与位置之间的期望关系Z(S)如下
Z(S)＝Mds+Bd+Kd/s
其中S为6×6的对角阵，其对角线元素为1或0，它的主要功能是用来确定哪些自由度施加力控，哪些自由度施加位置控制；
在时域中力与位置之间的期望关系用微分方程表示为：
Figure FDA0003718572380000067
其中，Md，Bd，Kd为6×6的正对角阵，分别为惯性矩阵，阻尼矩阵和刚度矩阵，通过设置不同的值达到不同的阻抗控制效果，F(t)是机器人与环境间的接触力，X表示机器人的实际位置，Xd表示机器人的指令位置，为了使机器人具备力跟踪的能力，将期望力Fd加入到目标阻抗中，上式变为：
Figure FDA0003718572380000068
Fd为机械臂末端执行器作用于环境的期望力和Fe为机械臂末端执行器作用于环境的实际力，当期望位置Xd接近实际位置X时，实际接触力Fe将收敛到参考力Fd；
根据阻抗控制关系，位置修正量ΔX与力误差ΔF之间的关系为：
ΔF＝(MdS2+BdS+Kd)ΔX
变为时域的形式：
Figure FDA0003718572380000071
采用向后差分离散可得：
Figure FDA0003718572380000072
Figure FDA0003718572380000073
其中T为采样周期，ΔX(k)为k时刻的位置修正量，即位置偏差，ΔX(k-1)为k-1时刻的位置修正量，即位置偏差，ΔX(k-2)为k-2时刻的位置修正量，即位置偏差，
Figure FDA0003718572380000074
为k时刻的速度修正量，即速度偏差；
Figure FDA0003718572380000075
为k时刻的加速度修正量，即加速度偏差；
式可以变为：
Figure FDA0003718572380000076
即：
Figure FDA0003718572380000077
ΔF(k)为k时刻的力偏差量；
当前位置的修正量与当前周期的力偏差，上个周期的位置修正量和上上个周期的位置修正量相关；
阻抗控制方法采用设置的阻抗系统只包含阻尼系数B，M＝0，K＝0，则当前位置修正量为：
Figure FDA0003718572380000078
期望位置经过选择矩阵I-S后得到，位置控制方向的期望位置和力控方向的期望位置，与当前位置修正量叠加产生Xref，其中I是6×6的单位矩阵；
Xref＝Xd(I-S)+ΔX
Xref为计算得到的当前周期的下发的期望的笛卡尔位置；
经过机械臂逆运动学，可以转到关节位置q，计算得到了插补位置；
c、将新计算得到的插补位置下发个伺服，完成整个控制。
6.一种可翻转物体的翻转跟随轨迹规划装置，其特征在于，所述可翻转物体的翻转跟随轨迹规划装置运行时实现权利要求1至3中任一项所述的方法的步骤；或者，实现权利要求4至5任一项所述的系统。
CN202111172448.8A 2021-10-08 2021-10-08 可翻转物体的翻转跟随轨迹规划方法、装置及系统 Active CN113892848B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111172448.8A CN113892848B (zh) 2021-10-08 2021-10-08 可翻转物体的翻转跟随轨迹规划方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111172448.8A CN113892848B (zh) 2021-10-08 2021-10-08 可翻转物体的翻转跟随轨迹规划方法、装置及系统

Publications (2)

Publication Number Publication Date
CN113892848A CN113892848A (zh) 2022-01-07
CN113892848B true CN113892848B (zh) 2022-10-14

Family

ID=79190446

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111172448.8A Active CN113892848B (zh) 2021-10-08 2021-10-08 可翻转物体的翻转跟随轨迹规划方法、装置及系统

Country Status (1)

Country Link
CN (1) CN113892848B (zh)

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101332604A (zh) * 2008-06-20 2008-12-31 哈尔滨工业大学 人机相互作用机械臂的控制方法
CN203237791U (zh) * 2013-04-25 2013-10-16 云南众诚士德柔性自动化设备有限公司 一种能够自动拆垛与翻转的多功能机械抓取搬运装置
CN106003033A (zh) * 2016-06-16 2016-10-12 哈尔滨工程大学 一种具有力控制的六自由度机械臂标准汉字书写方法
CN106695797A (zh) * 2017-02-22 2017-05-24 哈尔滨工业大学深圳研究生院 基于双臂机器人协同操作的柔顺控制方法及系统
CN106843213A (zh) * 2017-02-10 2017-06-13 中国东方电气集团有限公司 一种基于移动机器人的移动和操作路径自动规划的方法
CN107300100A (zh) * 2017-05-22 2017-10-27 浙江大学 一种在线cad模型驱动的级联式机械臂视觉引导逼近方法
EP3402634A1 (de) * 2016-01-15 2018-11-21 KUKA Deutschland GmbH Greifer mit einer kraftmessvorrichtung
CN109366488A (zh) * 2018-12-07 2019-02-22 哈尔滨工业大学 一种面向机器人装配的叠加振荡力笛卡尔阻抗控制方法
CN109571457A (zh) * 2018-12-26 2019-04-05 河南机电职业学院 一种自动记录拖拽轨迹的机器人揭锅盖系统
CN109691935A (zh) * 2019-03-06 2019-04-30 南京工程学院 一种智能卫生间清洁机器人及其使用方法
DE102018003269A1 (de) * 2018-04-23 2019-10-24 Franka Emika Gmbh Verfahren zum Einfügen eines Gegenstands in eine Gegenstandsaufnahme mittels eines Robotermanipulators
CN110924340A (zh) * 2019-11-25 2020-03-27 武汉思睿博特自动化系统有限公司 一种用于智能捡垃圾的移动机器人系统与实现方法
CN112372630A (zh) * 2020-09-24 2021-02-19 哈尔滨工业大学（深圳） 一种多机械臂协同打磨力柔顺控制方法和系统
CN112777555A (zh) * 2021-03-23 2021-05-11 江苏华谊广告设备科技有限公司 一种智能加油装置及方法
CN113044029A (zh) * 2021-03-19 2021-06-29 北京理工大学 一种保证无人车在三维地形上安全行驶的运动规划方法

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101332604A (zh) * 2008-06-20 2008-12-31 哈尔滨工业大学 人机相互作用机械臂的控制方法
CN203237791U (zh) * 2013-04-25 2013-10-16 云南众诚士德柔性自动化设备有限公司 一种能够自动拆垛与翻转的多功能机械抓取搬运装置
EP3402634A1 (de) * 2016-01-15 2018-11-21 KUKA Deutschland GmbH Greifer mit einer kraftmessvorrichtung
CN106003033A (zh) * 2016-06-16 2016-10-12 哈尔滨工程大学 一种具有力控制的六自由度机械臂标准汉字书写方法
CN106843213A (zh) * 2017-02-10 2017-06-13 中国东方电气集团有限公司 一种基于移动机器人的移动和操作路径自动规划的方法
CN106695797A (zh) * 2017-02-22 2017-05-24 哈尔滨工业大学深圳研究生院 基于双臂机器人协同操作的柔顺控制方法及系统
CN107300100A (zh) * 2017-05-22 2017-10-27 浙江大学 一种在线cad模型驱动的级联式机械臂视觉引导逼近方法
DE102018003269A1 (de) * 2018-04-23 2019-10-24 Franka Emika Gmbh Verfahren zum Einfügen eines Gegenstands in eine Gegenstandsaufnahme mittels eines Robotermanipulators
CN109366488A (zh) * 2018-12-07 2019-02-22 哈尔滨工业大学 一种面向机器人装配的叠加振荡力笛卡尔阻抗控制方法
CN109571457A (zh) * 2018-12-26 2019-04-05 河南机电职业学院 一种自动记录拖拽轨迹的机器人揭锅盖系统
CN109691935A (zh) * 2019-03-06 2019-04-30 南京工程学院 一种智能卫生间清洁机器人及其使用方法
CN110924340A (zh) * 2019-11-25 2020-03-27 武汉思睿博特自动化系统有限公司 一种用于智能捡垃圾的移动机器人系统与实现方法
CN112372630A (zh) * 2020-09-24 2021-02-19 哈尔滨工业大学（深圳） 一种多机械臂协同打磨力柔顺控制方法和系统
CN113044029A (zh) * 2021-03-19 2021-06-29 北京理工大学 一种保证无人车在三维地形上安全行驶的运动规划方法
CN112777555A (zh) * 2021-03-23 2021-05-11 江苏华谊广告设备科技有限公司 一种智能加油装置及方法

Also Published As

Publication number Publication date
CN113892848A (zh) 2022-01-07

Similar Documents

Publication Publication Date Title
JP5114019B2 (ja) エフェクタの軌道を制御するための方法
Reiser et al. Care-O-bot® 3-creating a product vision for service robot applications by integrating design and technology
US11241796B2 (en) Robot system and method for controlling robot system
JP3207728B2 (ja) 冗長マニピュレータの制御方法
JP4759660B2 (ja) ロボットアーム制御用の装置、方法、プログラム及び集積電子回路、並びに、組立ロボット
JP5765355B2 (ja) ロボットピッキングシステム及び被加工物の製造方法
JP3094091B2 (ja) 宇宙ロボットの制御装置
CN107085428B (zh) 智能移动方法、装置、机器人及存储介质
WO1999064208A1 (fr) Robot et procede de commande de son attitude
JP2008302496A (ja) ロボットアームの制御装置及び制御方法、ロボット、及びロボットアームの制御プログラム
CN108499054A (zh) 一种基于slam的车载机械臂捡球系统及其捡球方法
Patidar et al. Survey of robotic arm and parameters
WO1989008878A1 (en) Method of controlling tool attitude of a robot
JP2007168053A (ja) 垂直多関節型ロボットのティーチング方法および産業用ロボット装置
WO1998033100A1 (fr) Procede et systeme de commande de robot
CN111515928B (zh) 机械臂运动控制系统
CN113892848B (zh) 可翻转物体的翻转跟随轨迹规划方法、装置及系统
Vahrenkamp et al. Visual servoing for dual arm motions on a humanoid robot
Ellekilde et al. Control of mobile manipulator using the dynamical systems approach
CN110545965B (zh) 多关节机器人以及多关节机器人系统
CN114378827B (zh) 一种基于移动机械臂整体控制的动态目标跟踪抓取方法
JP3328414B2 (ja) ロボットハンドの姿勢制御装置
JPH0693209B2 (ja) ロボツトの円弧補間姿勢制御装置
Leonard et al. Dynamic visibility checking for vision-based motion planning
CN113843803A (zh) 可翻转物体翻转实时跟随轨迹规划方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB02 Change of applicant information
CB02 Change of applicant information

Address after: 311231 building 3, No. 477, Hongxing Road, Qiaonan block, economic and Technological Development Zone, Xiaoshan District, Hangzhou City, Zhejiang Province

Applicant after: Hangzhou Jingwu Intelligent Technology Co.,Ltd.

Address before: Room 12, 3rd floor, No.2 Lane 1446, Yunguan Road, Lingang New District, Pudong New Area pilot Free Trade Zone, Shanghai, 201306

Applicant before: Shanghai Jingwu Intelligent Technology Co.,Ltd.

GR01 Patent grant
GR01 Patent grant