(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210796069.4 (22)申请日 2022.07.06 (71)申请人 海南大学 地址 570100 海南省海口市人民大道58号 (72)发明人 郭东生　张卫东　李晓航　王世全　 衣博文　谢威　史颂华　赵斌　 (74)专利代理 机构 上海科盛知识产权代理有限 公司 312 25 专利代理师 杨宏泰 (51)Int.Cl. B25J 9/16(2006.01) B25J 18/00(2006.01) (54)发明名称 一种可容噪的三轮全向移动机械臂重复运 动规划方法 (57)摘要 本发明涉及一种可容噪的三轮全向移动机 械臂重复运动规划方法， 包括以下步骤： 步骤1： 基于负梯度下 降思想推导在速度层上实现重复 运动的性能指标； 步骤2： 基于误差反馈设计具备 容噪特性的新型雅克比矩阵等式； 步骤3： 结合性 能指标和新型等式建立可容噪的速度层重复运 动规划方案； 步骤4： 将速度层重复运动规划方案 转化为二次型优化问题； 步骤5： 采用数值算法求 解二次型优化问题； 步骤6： 三轮全向移动机械臂 的控制器根据求解结果实时驱动移动平台的三 个全向轮和机械臂的关节完成设定的轨迹规划 任务， 并实现重复运动规划。 与现有技术相比， 本 发明具有使得三轮全向移动机械臂在噪声环境 下仍可有效地完成设定任务 以及实现重复运动 规划等优点。 权利要求书3页 说明书7页 附图1页 CN 115107028 A 2022.09.27 CN 115107028 A 1.一种可容噪的三轮全向移动机械臂重复运动规划方法， 其特征在于， 该方法包括以 下步骤： 步骤1： 基于负梯度下降的思想推导在速度层上实现重复运动的性能指标； 步骤2： 基于误差反馈设计具 备容噪特性的新型雅克比矩阵等式； 步骤3： 结合 性能指标和新型雅克比矩阵等式建立可容噪的速度层重复运动规划方案； 步骤4： 将速度层重复运动规划方案转 化为二次型优化问题； 步骤5： 采用数值 算法求解步骤4得到的二次型优化问题； 步骤6： 三轮全向移动机械臂的控制器根据步骤5求解的结果实时驱动三轮全向移动机 械臂完成设定的轨 迹规划任务， 并实现重复运动规划。 2.根据权利要求1所述的一种可容噪的三轮全向移动机械臂重复运动规划方法， 其特 征在于， 所述的步骤1中， 在速度层上实现重复运动的性能指标的表达式为： 其中， 为在速度层上实现重复运动的性能指标， || ·||2表示向量的二范数， 表示三轮全向移动机 械臂的联合速度向量， n表示 三轮全向移动机 械臂 的一个安装在移动平台上的机械臂具有n个自由度， 表示移动平台的三个对称分布 全向驱动轮的旋转角速度， 表示机械臂的关节速度， μ＞0∈R为设计参数， 用以实现 三轮全向移动机械臂的重复运动， c＝[px‑px0； py‑py0； cos(φ)(sin(φ) ‑sin(φ0))； θ‑θ0],c ∈R3+n为系数向量， px∈R和py∈R分别表示移动平台在X轴和Y轴方向上的位置， 即机械臂安 装在移动平台上的位置， θ∈Rn表示机械臂的关节角度， px0∈R、 py0∈R和φ0∈R分别表示移 动平台在X轴方向上位置、 Y轴方向上位置以及朝向角的初始值， θ0∈Rn表示机械臂关节角度 的初始值， D＝[W,0； 0,I],D∈R(3+n)×(3+n)表示系数矩阵， I∈ Rn×n表示单位矩阵， W∈ R3×3表示 由移动平台自身结构参数组成的矩阵， 由移动平台自身结构参数组成的矩阵W的表达式为： 其中， ρ ＞0∈R表示移动平台每个全向驱动轮的半径， d＞0∈R表示移动平台中心点到 全向驱动轮的距离， φ∈R表示移动平台的朝向角。 3.根据权利要求2所述的一种可容噪的三轮全向移动机械臂重复运动规划方法， 其特 征在于， 所述的步骤2中， 设计具 备容噪特性的新型雅克比矩阵等式的过程具体为： 根据三轮全向移动机械臂轨迹规划的需求， 基于末端执行器位置误差及其积分信 息的 反馈， 设计具 备容噪特性的新型雅克比矩阵等式， 具 备容噪特性的新型雅克比矩阵等式为： 权　利　要　求　书 1/3 页 2 CN 115107028 A 2其中， r∈Rm表示末端执行器在m维空间中的位置向量， 表示r的时间导数， J∈Rm ×(3+n)表示三轮全向移动机械臂的雅克比矩阵， e(t)∈Rm表示移动机械臂 末端执行器的位置误差， 为三轮全向移动机械臂的联 合位置向量， f( ·):R3+n→Rm表示一个非线性映射函数， t＞0∈R表示时间， α ＞0∈R和β ＞0 ∈R均表示误差反馈系数， 且 两者需要满足α2＞β， σ ∈R表示积分变 量， δ∈Rm表示三轮全向移 动机械臂在执 行轨迹规划任务时可能遇 到的噪声向量。 4.根据权利要求3所述的一种可容噪的三轮全向移动机械臂重复运动规划方法， 其特 征在于， 所述的步骤3中， 可容噪的速度层重复运动规划方案具体为： 最小化性能指标 受约束于新型雅克比矩阵等式、 移动平台三个全向驱动轮的旋转角度极限、 旋转角速 度极限、 机械臂关节的角度极限和速度极限， 新型雅克比矩阵等式对应于三轮全向移动机 械臂在噪声环境下执行速度层的轨迹规划任务， 新型雅克比矩阵等式、 移动平台三个全向 驱动轮的旋转角度极限、 旋转角速度极限、 机 械臂关节的角度极限和速度极限的表达式为： ω‑≤ω≤ω+ θ‑≤θ ≤θ+ 其中， ω∈R3表示移动平台三个全 向驱动轮的旋转角度， ω±、 θ±和 分别表示驱 动轮旋转角度ω、 驱动轮旋转角速度 机械臂关节角度θ和机械臂关节速度 的上下极 限。 5.根据权利要求4所述的一种可容噪的三轮全向移动机械臂重复运动规划方法， 其特 征在于， 所述的步骤4中， 转 化的二次型优化问题具体为： 最小化xTQx/2+pTx； 受约束于第 一约束条件和第 二约束条件， 第 一约束条件和第 二约束条件的表达 式分别 为： Ax＝b x‑≤x≤x+ 其中， 表示二次型优化 问题的决策变量， Q＝DTD∈R(3+n)×(3+n)和A＝JD∈Rm ×( 3 + n )均 表 示 系 数 矩 阵 ，T 表 示 矩 阵 或 向 量 的 转 置 ，p ＝μDTc ∈ R3 + n和 均表示系数向量， 表示二 次型优化问题的决策变量x的上下极限， v＝[ω； θ],v∈R3+n表示三轮全向移动机械臂的联 合角度向量， v±＝[ω±； θ±]表示联合角度的上下极限， 表示联合速度的上下 极限， γ＞0∈R表示处 理极限的转换参数。权　利　要　求　书 2/3 页 3 CN 115107028 A 3