CN112223290B - 一种关节空间连续点轨迹过渡方法及装置 - Google Patents
一种关节空间连续点轨迹过渡方法及装置 Download PDFInfo
- Publication number
- CN112223290B CN112223290B CN202011132697.XA CN202011132697A CN112223290B CN 112223290 B CN112223290 B CN 112223290B CN 202011132697 A CN202011132697 A CN 202011132697A CN 112223290 B CN112223290 B CN 112223290B
- Authority
- CN
- China
- Prior art keywords
- transition
- joint space
- bezier
- speed
- point
- 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
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
Abstract
本发明公开了一种关节空间连续点轨迹过渡方法及装置，所述方法包括：获取关节空间的开始位置、中间位置以及结束位置的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度和出射速度；若预设的百分比不为零，则进行贝塞尔过渡；获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；获取关节空间末速度；对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；做位置姿态时间同步，关节空间连续点轨迹过渡；本发明的优点在于：减少时间成本以及能耗成本。
Description
技术领域
本发明涉及机器人运动控制领域，更具体涉及一种关节空间连续点轨迹过渡方法及装置。
背景技术
机械臂运动空间分为关节空间和笛卡尔空间，当前工业机械臂在单独关节空间和笛卡尔空间运动已经十分容易实现。关节空间过渡的主要方法是将关节空间通过正向运动学转到笛卡尔空间，再将笛卡尔空间与笛卡尔空间指令做轨迹过渡，比如在交点处做降速为零的处理，然后切换空间再进行运动，这样在实际生产中会大大增加时间成本和能耗成本。例如，对于通用机器人的关节空间规划，也就是PTP，每个关节从起始点位到终止点位，往往是单独规划运动，各个关节能在同一时间启动，能到达指定点位。这样简单的关节运动对于运动过程控制要求不高，并且有快速定位要求的作业是适用的。但是对于连续关节空间规划运动，如果采用各关节独立运动，会出现各关节从运动起始点，到中间点，最后到目标点，速度从0加速，再到0，再加速，再到0，这种连续启停运动，每出现一个中间点和目标点，各关节就要启停一次。这样会造成运动过程不可控，各关节无法同步到中间点位置，也就是无法实现避开固定点障碍物功能，效率会大大降低，增加了时间成本和能耗成本，不适合实际作业的要求。
现有关节空间和笛卡尔空间的混合空间过渡的主要方法还包括映射到笛卡尔空间后做圆弧过渡算法，但是圆弧过渡算法曲率是常量，会有曲率突变，造成向心加速度的冲击，降低运动精度。
发明内容
本发明所要解决的技术问题在于关节空间连续点轨迹过渡方法时间成本和能耗成本高的问题。
本发明通过以下技术手段实现解决上述技术问题的：一种关节空间连续点轨迹过渡方法，所述方法包括：
步骤一：获取关节空间的开始位置、中间位置以及结束位置的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度和出射速度，其中，贝塞尔过渡的入射速度和出射速度相等；
步骤二：若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；
步骤三：获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；
步骤四：获取关节空间末速度；
步骤五：实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤一至步骤四，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；
步骤六：利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；
步骤七：获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；
步骤八：将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。
本发明采用贝塞尔过渡法，对关节空间末速度进行修正，使得最终过渡段可以保证加速度、速度连续，圆弧相切，不需要速度从0加速，再到0，再加速，再到0这种连续启停运动，减少时间成本以及能耗成本。贝塞尔过渡的入射速度和出射速度、过渡半径以及起始点终点等均可修正，因此曲率可以设置，防止过大的连接速度对机械结构产生冲击，减少能耗成本。
优选的，所述步骤二包括：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的位置信息组成开始位置点q1'、中间位置点q2'以及结束位置点q3'，由开始位置点q1'与中间位置点q2'构建第一向量q2'q1'，由中间位置点q2'以及结束位置点q3'构建第二向量q2'q3'，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径。
优选的，所述步骤三还包括：
利用公式q′2P0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点P0，
利用公式q′2P5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点P5。
优选的，所述步骤四包括：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。
优选的，所述步骤五包括：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；
修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤一至步骤四，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。
优选的，所述步骤七包括：分别取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置Q1与中间位置Q2之间的夹角，θ2为中间位置Q2与结束位置Q3之间的夹角；
利用公式q′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qssin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为θs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。
本发明还提供一种关节空间连续点轨迹过渡装置，所述装置包括：
第一获取模块，用于获取关节空间的开始位置、中间位置、结束位置以及各位置允许运行的速度中的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度，其中，贝塞尔过渡的入射速度和出射速度相等；
判断模块，用于若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；
第二获取模块，用于获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；
第三获取模块，用于获取关节空间末速度；
修正模块，用于实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；
第四获取模块，用于利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；
时间同步模块，用于获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；
规划模块，用于将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。
优选的，所述判断模块还用于：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的位置信息组成开始位置点q1'、中间位置点q2'以及结束位置点q3'，由开始位置点q1'与中间位置点q2'构建第一向量q2'q1'，由中间位置点q2'以及结束位置点q3'构建第二向量q2'q3'，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径。
优选的，所述第二获取模块还用于：
利用公式q′2P0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点P0，
利用公式q′2P5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点P5。
优选的，所述第三获取模块还用于：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。
优选的，所述修正模块还用于：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；
修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。
优选的，所述时间同步模块还用于：分别取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置Q1与中间位置Q2之间的夹角，θ2为中间位置Q2与结束位置Q3之间的夹角；
利用公式q′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qssin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为qs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。
本发明的优点在于：本发明采用贝塞尔过渡法，对关节空间末速度进行修正，使得最终过渡段可以保证加速度、速度连续，圆弧相切，前一段与后一段关节运动速度连续，不存在到达中间点或者目标点的时候为了与下一段关节运动速度保持一致将速度降为0，再在下一段开始时重新启动，因此不需要速度从0加速，再到0，再加速，再到0这种连续启停运动，减少时间成本以及能耗成本。贝塞尔过渡的入射速度和出射速度、过渡半径以及起始点终点等均可修正，因此曲率可以设置，防止过大的连接速度对机械结构产生冲击，减少能耗成本。
附图说明
图1为本发明实施例所公开的一种关节空间连续点轨迹过渡方法的流程示意图；
图2为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中五阶贝塞尔过渡段速度曲线示意图；
图3为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中关节空间AB点过渡前半段示意图；
图4为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中关节空间AB点过渡后半段示意图；
图5为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中关节空间BC点过渡前半段示意图；
图6为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中关节空间BC点过渡后半段示意图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例1
如图1和图2所示，一种关节空间连续点轨迹过渡方法，所述方法包括：
步骤S1：获取关节空间的开始位置、中间位置以及结束位置的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度和出射速度，其中，贝塞尔过渡的入射速度和出射速度相等；
步骤S2：若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；具体过程为：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的位置信息组成开始位置点q1'、中间位置点q2'以及结束位置点q3'，由开始位置点q1'与中间位置点q2'构建第一向量q2'q1'，由中间位置点q2'以及结束位置点q3'构建第二向量q2'q3'，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径，||为向量的模的符号，x1为第一贝塞尔直线长度，x2为第二贝塞尔直线长度，per为预设的百分比。
步骤S3：获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；具体过程为：
利用公式q′2P0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点P0，
利用公式q′2P5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点P5。
以下简要介绍贝塞尔过渡计算过程，相邻两条直线之间采用五次贝塞尔曲线作为过渡轨迹，五次贝塞尔曲线的6个控制器点P0，P1，P2，P3，P4，P5中，起始点P0和终点P5为以两条直线的交点为圆心，过渡精度为半径画圆，相交形成的交点，两条直线形成的交点作为另外两个控制点P2，P3，即P2与P3重合，P1是P0与P2的中点，P4是P3与P5的中点。将6个控制点坐标(Px，Py，Pz)，代入以下公式：
B(t)＝P0(1-t)5+5P1t(1-t)4+10P2t2(1-t)3+10P3t3(1-t)2+5P4t4(1-t)+P5t5t∈[0，1]
可得到贝塞尔曲线的表达式在各个分量的坐标。
Bx(t)＝P0x(1-t)5+5P1xt(1-t)4+10P2xt2(1-t)3+10P3xt3(1-t)2+5P4xt4(1-t)+P5xt5
By(t)＝P0y(1-t)5+5P1yt(1-t)4+10P2yt2(1-t)3+10P3yt3(1-t)2+5P4yt4(1-t)+P5yt5
Bz(t)＝P0z(1-t)5+5P1zt(1-t)4+10P2zt2(1-t)3+10P3zt3(1-t)2+5P4zt4(1-t)+P5zt5
其中P0x、P0y、P0z为P0点在x轴、y轴、z轴的投影点，同理可得P1、P2、P3、P4、P5点的投影点。
步骤S4：获取关节空间末速度；具体过程为：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。
步骤S5：实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤S1至步骤S4，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；具体过程为：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；
修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤S1至步骤S4，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。
步骤S6：利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；
步骤S7：获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；具体过程为：分别取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置Q1与中间位置Q2之间的夹角，θ2为中间位置Q2与结束位置Q3之间的夹角；
利用公式θ′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qssin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为qs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。
步骤S8：将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。
如图3为本发明的关节空间AB点过渡前半段示意图，图4为关节空间AB点过渡后半段示意图，图5为关节空间BC点过渡前半段示意图，图6关节空间BC点过渡后半段示意图，图中可以看出，本发明的轨迹过渡方法使得轨迹连续，圆弧相切。
通过以上技术方案，本发明提供的一种关节空间连续点轨迹过渡方法，采用五次贝塞尔过渡法，对关节空间末速度进行修正，使得最终过渡段可以保证加速度、速度连续，圆弧相切，不需要速度从0加速，再到0，再加速，再到0这种连续启停运动，减少时间成本以及能耗成本。贝塞尔过渡的入射速度和出射速度、过渡半径以及起始点终点等均可修正，因此曲率可以设置，防止过大的连接速度对机械结构产生冲击，减少能耗成本。
实施例2
与实施例1相对应的，本发明实施例2还提供一种关节空间连续点轨迹过渡装置，所述装置包括：
第一获取模块，用于获取关节空间的开始位置、中间位置以及结束位置的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度和出射速度，其中，贝塞尔过渡的入射速度和出射速度相等；
判断模块，用于若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；
第二获取模块，用于获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；
第三获取模块，用于获取关节空间末速度；
修正模块，用于实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；
第四获取模块，用于利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；
时间同步模块，用于获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；
规划模块，用于将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。
具体的，所述判断模块还用于：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的位置信息组成开始位置点q1'、中间位置点q2'以及结束位置点q3'，由开始位置点q1'与中间位置点q2'构建第一向量q2'q1'，由中间位置点q2'以及结束位置点q3'构建第二向量q2'q3'，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径。
具体的，所述第二获取模块还用于：
利用公式q′2P0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点P0，
利用公式q′2P5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点P5。
具体的，所述第三获取模块还用于：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。
具体的，所述修正模块还用于：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；
修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。
具体的，所述时间同步模块还用于：分别取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置Q1与中间位置Q2之间的夹角，θ2为中间位置Q2与结束位置Q3之间的夹角；
利用公式q′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qssin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为θs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。
以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (10)
1.一种关节空间连续点轨迹过渡方法，其特征在于，所述方法包括：
步骤一：获取关节空间的开始位置、中间位置、结束位置以及各位置允许运行的速度中的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度，其中，贝塞尔过渡的入射速度和出射速度相等；
步骤二：若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；
步骤三：获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；
步骤四：获取关节空间末速度；
步骤五：实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤一至步骤四，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；其中，所述关节空间末速度是将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求得；
步骤六：利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；
步骤七：获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；
步骤八：将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。
2.根据权利要求1所述的一种关节空间连续点轨迹过渡方法，其特征在于，所述步骤二还包括：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的位置信息组成开始位置点q1′、中间位置点q2′以及结束位置点q3′，由开始位置点q1′与中间位置点q2′构建第一向量q2′q1′，由中间位置点q2′以及结束位置点q3′构建第二向量q2′q3′，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径，per为预设的百分比。
3.根据权利要求2所述的一种关节空间连续点轨迹过渡方法，其特征在于，所述步骤三包括：
利用公式q′2P0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点P0，
利用公式q′2P5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点P5。
4.根据权利要求1所述的一种关节空间连续点轨迹过渡方法，其特征在于，所述步骤四包括：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。
5.根据权利要求1所述的一种关节空间连续点轨迹过渡方法，其特征在于，所述步骤五包括：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；
修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤一至步骤四，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。
6.根据权利要求2所述的一种关节空间连续点轨迹过渡方法，其特征在于，所述步骤七包括：分别取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置Q1与中间位置Q2之间的夹角，θ2为中间位置Q2与结束位置Q3之间的夹角；
利用公式q′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qs sin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为qs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。
7.一种关节空间连续点轨迹过渡装置，其特征在于，所述装置包括：
第一获取模块，用于获取关节空间的开始位置、中间位置、结束位置以及各位置允许运行的速度中的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度，其中，贝塞尔过渡的入射速度和出射速度相等；
判断模块，用于若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；
第二获取模块，用于获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；
第三获取模块，用于获取关节空间末速度；
修正模块，用于实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；其中，所述关节空间末速度是将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求得；
第四获取模块，用于利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；
时间同步模块，用于获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行S形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；
规划模块，用于将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。
8.根据权利要求7所述的一种关节空间连续点轨迹过渡装置，其特征在于，所述判断模块还用于：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置Q1、中间位置Q2以及结束位置Q3的位置信息组成开始位置点q1′、中间位置点q2′以及结束位置点q3′，由开始位置点q1′与中间位置点q2′构建第一向量q2′q1′，由中间位置点q2′以及结束位置点q3′构建第二向量q2′q3′，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径，per为预设的百分比。
9.根据权利要求8所述的一种关节空间连续点轨迹过渡装置，其特征在于，所述第二获取模块还用于：
利用公式q′2P0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点P0，
利用公式q′2P5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点P5。
10.根据权利要求7所述的一种关节空间连续点轨迹过渡装置，其特征在于，所述第三获取模块还用于：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011132697.XA CN112223290B (zh) | 2020-10-21 | 2020-10-21 | 一种关节空间连续点轨迹过渡方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011132697.XA CN112223290B (zh) | 2020-10-21 | 2020-10-21 | 一种关节空间连续点轨迹过渡方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112223290A CN112223290A (zh) | 2021-01-15 |
| CN112223290B true CN112223290B (zh) | 2021-11-05 |
Family
ID=74108934
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011132697.XA Active CN112223290B (zh) | 2020-10-21 | 2020-10-21 | 一种关节空间连续点轨迹过渡方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112223290B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113103239B (zh) * | 2021-04-29 | 2022-09-13 | 哈尔滨工业大学 | 一种机器人姿态轨迹生成方法、装置及存储介质 |
| CN113448348B (zh) * | 2021-06-07 | 2023-02-17 | 北方天途航空技术发展（北京）有限公司 | 基于贝塞尔曲线的无人机转向飞行路径规划方法 |
| CN113478481B (zh) * | 2021-06-23 | 2022-08-23 | 佛山智能装备技术研究院 | 一种工业机器人位置和姿态运动同步方法 |
| CN113967917B (zh) * | 2021-11-24 | 2022-12-23 | 伯朗特机器人股份有限公司 | 机械臂多关节轨迹时间同步方法、系统及存储介质 |
| CN114781278B (zh) * | 2022-06-17 | 2022-09-27 | 天津理工大学 | 一种基于数据驱动的机电设备寿命预测方法及系统 |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH09146621A (ja) * | 1995-11-01 | 1997-06-06 | Mitsubishi Electric Corp | ロボットのシミュレーション装置 |
| CN105500354B (zh) * | 2016-02-02 | 2017-05-17 | 南京埃斯顿机器人工程有限公司 | 一种工业机器人应用的过渡轨迹规划方法 |
| CN109623820B (zh) * | 2018-12-25 | 2021-09-14 | 哈工大机器人（合肥）国际创新研究院 | 一种机器人空间轨迹过渡方法 |
| CN111791236B (zh) * | 2020-07-17 | 2021-06-01 | 广东拓斯达科技股份有限公司 | 一种工业机器人笛卡尔空间轨迹过渡方法 |
-
2020
- 2020-10-21 CN CN202011132697.XA patent/CN112223290B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN112223290A (zh) | 2021-01-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112223290B (zh) | 一种关节空间连续点轨迹过渡方法及装置 | |
| JP6766186B2 (ja) | ロボット関節空間におけるポイントツーポイント移動の軌跡計画方法 | |
| CN105500354B (zh) | 一种工业机器人应用的过渡轨迹规划方法 | |
| CN107966907B (zh) | 一种应用于冗余度机械臂的障碍物躲避解决方法 | |
| CN110900612B (zh) | 一种位姿同步的六轴工业机器人轨迹平顺方法 | |
| CN108594815B (zh) | 一种分阶段的轮式机器人移动路径规划方法 | |
| CN112757306A (zh) | 一种机械臂逆解多解选择和时间最优轨迹规划算法 | |
| CN112828359B (zh) | 基于势场法多约束的机器人铣削加工姿态规划方法和系统 | |
| CN109794943B (zh) | 一种拐角过渡路径及确定方法 | |
| CN109605369B (zh) | 机械臂奇异点控制方法及系统 | |
| CN110653137B (zh) | 一种保持喷头垂直于喷涂面的喷涂方法 | |
| CN108406765B (zh) | 一种开链式多臂机器人阻抗控制方法 | |
| CN112975992B (zh) | 一种误差可控的机器人轨迹同步优化方法 | |
| CN114670191B (zh) | 一种七自由度肘偏置机械臂轨迹规划方法 | |
| WO2015098085A1 (ja) | 動作プログラム作成方法およびロボットの制御方法 | |
| CN111309002A (zh) | 一种基于矢量的轮式移动机器人避障方法及系统 | |
| CN111633668A (zh) | 一种用于机器人加工三维自由曲面的运动控制方法 | |
| CN113111404B (zh) | 一种加工轨迹的空间连续小线段圆弧与直线拟合方法 | |
| CN113771035A (zh) | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 | |
| CN112192575B (zh) | 一种相邻关节空间轨迹过渡方法及装置 | |
| CN117301063A (zh) | 工业机器人在线前瞻轨迹规划方法、电子设备及存储介质 | |
| CN112356032A (zh) | 一种姿态平滑过渡方法及系统 | |
| CN115167429A (zh) | 一种移动机器人位置和姿态同时规划方法 | |
| CN107016209B (zh) | 一种工业机器人和导轨协同规划方法 | |
| CN113276116B (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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220915 Address after: 236000 room 304, building 3, Zone C, intelligent equipment science and Technology Park, 3963 Susong Road, Hefei Economic and Technological Development Zone, Anhui Province Patentee after: Hefei Hagong Tunan intelligent control robot Co.,Ltd. Address before: 236000 area C, intelligent equipment science and Technology Park, 3963 Susong Road, Hefei Economic and Technological Development Zone, Anhui Province Patentee before: HRG INTERNATIONAL INSTITUTE FOR RESEARCH & INNOVATION |