(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211147769.7 (22)申请日 2022.09.19 (71)申请人 哈尔滨工业大 学 地址 150001 黑龙江省哈尔滨市南岗区西 大直街92号 (72)发明人 金弘哲　鞠枫嘉　赵杰　刘玉斌　 葛明达　 (74)专利代理 机构 哈尔滨华夏松花江知识产权 代理有限公司 23213 专利代理师 孟宪会 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种针对动态障碍物的冗余度机械臂避障 模型的构建方法 (57)摘要 一种针对动态障碍物的冗余度机械臂避障 模型的构建方法， 属于冗余度机械臂的运动规划 及动态避障领域， 本发明为解决现有技术中缺乏 能够体现机械臂的几何构型， 并且 具备对动态障 碍物是否与机械臂发生碰撞的预测判断的能力 的连续避障模 型， 从而导致对机械臂在对动态障 碍物碰撞预判能力较弱的问题， 本申请基于在三 维空间中相邻两个连杆之间的三角形关系保持 不变的特性， 将冗余机械臂的几何构型用三角形 的运动和变形来描述， 故将机械臂在动态环境中 的自适应调整转化为三角形的动态避障。 将机械 臂连杆看作三角形的边， 而不是离散为点集或球 集， 并结合梯度投影法的自运动项计算冗余机械 臂的运动学逆解， 解决了机械臂在任务执行过程 中的动态避障问题。 权利要求书2页 说明书9页 附图15页 CN 115464650 A 2022.12.13 CN 115464650 A 1.一种针对动态障碍物的冗余度机械臂避障模型的构建方法， 其特征在于： 所述方法 是通过以下步骤实现的： 步骤一： 选择机械臂中两个相邻的连杆作为三角形的两条边， 将第一条边的起点和第 二条边的终点连接起来， 构成机械臂连杆在三维空间中运动面的三角形， 同时基于三角形 几何构型， 提出了运动状态代 价、 迎面碰撞代价和迫近时间代价三种代价函数， 构成三角形 碰撞面避障模型， 用来判断三角形和障碍物之间的相对运动关系； 步骤二： 分析构建机 械臂连杆在三维空间中形成三角形的运动状态 代价； 步骤三： 分析构建机 械臂连杆在三维空间中形成三角形的迎面碰撞 代价； 步骤四： 分析构建机 械臂连杆在三维空间中形成三角形的迫近时间代价； 步骤五： 基于步骤二至步骤四中的得到的运动状态代价、 迎面碰撞代价和迫近时间代 价三个代价值， 提出了三角形碰撞面模型的总代价值H， 只有运动状态代价、 迎面碰撞代价 和迫近时间代价 三个代价 值满足都不为0的情况， 才 会发生碰撞。 2.根据权利要求1所述的一种针对动态障碍物的冗余度机械臂避障模型的构建方法， 其特征在于： 所述 步骤二中运动状态 代价的具体构建方法如下： 步骤二一： 计算障碍物运动 速度方向与平面P的单位法向量Ni的内积 ηi， 即夹角γi的余 弦值： ηi＝W·(Li×Li+1)                                (1) 式中： W为障碍物运动轨迹的单位切向量， Li和Li+1分别为机械臂第i和i+1个臂杆的向 量； 步骤二二： 基于平面P将三维空间分为的区域， 其中规定法向量Ni指向的区域为正面， 另 一侧为反面， 构建正 面的运动状态 代价CMSf的表达式： 式中α 为运动比例系数； 步骤二三： 基于平面P将三维空间分为的区域， 其中规定法向量Ni指向的区域为正面， 另 一侧为反面， 构建反面的运动状态 代价CMSb的表达式： 式中α 为运动比例系数； 步骤二四： 根据步骤二一计算得到的内积ηi和障碍物在三维空间的区域选择运动状态 代价CMS的表达式， 障碍物在三维空间的区域指障碍物位于三角形碰撞面的正 面或反面。 3.根据权利要求2所述的一种针对动态障碍物的冗余度机械臂避障模型的构建方法， 其特征在于： 所述 步骤三中迎面碰撞 代价的具体构建方法如下： 步骤三一： 构建三角形碰撞面模型 所在空间平面P的平面方程： Aix+Biy+Ciz+Di＝0                           (4) 式中： Ai,Bi和Ci分别是平面P的单位法向量Ni在x,y,z方向上的三个坐标分量， Di可以通 过顶点Vi的三维坐标(xi,yi,zi)求出， Di＝‑(Aixi+Biyi+Cizi)； 步骤三二： 构建经 过障碍物位置Xobs且与障碍物运动方向平行的直线方程： (x‑xobs)/m＝(y‑yobs)/n＝(z‑zobs)/p             (5)权　利　要　求　书 1/2 页 2 CN 115464650 A 2式中： (xobs,yobs,zobs)是障碍物位置Xobs的三维坐标， m,n,p是障碍物运动轨迹的单位切 向量W在x,y,z方向上的三个坐标分量； 步骤三三： 对步骤三一所构建的三角形碰撞面模型进行松弛， 将原三角形按照∈的比 例进行松弛， ∈为松弛系数， 使其应对实际机 械臂具备的实际几何体积： 式中： 为原三角形碰撞面顶点 位置向量； 为松弛三角形碰撞面顶点 位置向量； 步骤三四： 基于步骤三二所得的障碍物位置Xobs且与障碍物运动方向平行的直线方程 和和步骤三 三松弛后的三角形碰撞面模型定义迎面碰撞 代价： 式中： Oi为三角形碰撞面的重心， 位置偏差δi为障碍物沿运动方向在平面P的投影点Ci 与Oi之间的距离， μi为Ci与三个三角形顶点之间的最大距离， β 为碰撞比例系数。 4.根据权利要求3所述的一种针对动态障碍物的冗余度机械臂避障模型的构建方法， 其特征在于： 所述 步骤四中迫近时间代价的具体构建方法如下： 式中： di为障碍物位置Xobs与平面P之间存在距离， 为障碍物的运动速度大小， ρ 为 迫近时间比例系数。 5.根据权利要求4所述的一种针对动态障碍物的冗余度机械臂避障模型的构建方法， 其特征在于： 所述 步骤五中总代价 值H的具体构建方法如下： 第i个三角形碰撞面的总代价值Hi采用运动状态代价、 迎面碰撞代价和迫近时间代价三 个代价值的乘积的方式获得， 其表达式如下： Hi＝CMS·CHOC·CAT                               (9) 式中： CMS表示运动状态 代价， CHOC表示迎面碰撞 代价， CAT表示迫近时间代价； 当冗余度机械臂 的臂杆数量较多， 所构成的三角形碰撞面多于一个时， 将每个三角形 碰撞面的总代价 值Hi进行叠加， 获取整个机 械臂的总代价 值H。 其中， Hi表示第i个三角形碰撞面的总 代价值， H表示机械臂的总 代价值， wi表示Hi占H的 比例， wi取1。权　利　要　求　书 2/2 页 3 CN 115464650 A 3