(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210439861.4 (22)申请日 2022.04.25 (71)申请人 国网湖南省电力有限公司 地址 410004 湖南省长 沙市新韶东路398号 申请人 国网湖南省电力有限公司电力科 学 研究院　 国家电网有限公司 (72)发明人 隆晨海　苗力　马国梁　李金亮　 郭健　杨淼　陈思铭　皮奥　 朱吉然　邹妍晖　刘蛟蛟　 (74)专利代理 机构 湖南兆弘专利事务所(普通 合伙) 43008 专利代理师 廖元宝 (51)Int.Cl. B25J 19/02(2006.01)B25J 9/16(2006.01) (54)发明名称 一种连续体机械臂避障系统、 连续体机械臂 及避障方法 (57)摘要 本发明公开了一种连续体机械臂避障系统、 避障方法及连续体机械臂， 所述避障系统包括多 个测量单元， 多个所述测量单元间隔布置于连续 体机械臂上， 各所述测量单元均包括检测处理模 块和多个超声波检测模块； 所述检测处理模块安 装于机械臂上且垂直于机械臂的中心杆， 多个超 声波检测模块分布于检测处理模块的周侧且测 量方向位于垂直中心杆的平面上， 所述检测处理 模块与各所述超声波检测模块相连， 用于根据各 超声波检测模块的检测信号控制机械臂以实现 避障。 本发明具有结构简单、 通用性强、 避障精准 可靠等优点。 权利要求书2页 说明书7页 附图4页 CN 115179329 A 2022.10.14 CN 115179329 A 1.一种连续体机械臂避障系统， 其特征在于， 包括多个测量单元， 多个所述测量单元间 隔布置于连续体机械臂上， 各所述测量单元均包括检测处理模块(9)和多个超声波检测模 块(6)； 所述检测处理模块(9)安装于机械臂上且垂 直于机械臂的中心杆(2)， 多个超声波检 测模块(6)分布于检测处理模块(9)的周 侧且测量方向位于垂直中心杆(2)的平面上， 所述 检测处理模块(9)与各所述超声波检测模块(6)相连， 用于根据各超声波检测模块(6)的检 测信号控制机 械臂以实现避障。 2.根据权利要求1所述的连续体机械臂避障系统， 其特征在于， 所述检测处理模块(9) 包括电路板(8)和主控芯片， 所述主控芯片安装于电路板(8)上， 所述电路板(8)呈圆环状， 所述超声 波检测模块(6)的数量 为四个， 均匀分布于电路板(8)的圆周侧。 3.根据权利要求1或2所述的连续体机械臂避 障系统， 其特征在于， 所述超声波检测模 块(6)为超声 波传感器。 4.根据权利要求1或2所述的连续体机械臂避 障系统， 其特征在于， 所述检测 处理模块 (9)包括具有ArmCortex ‑M4内核的STM32G431C6Tx核心处理芯片、 一个SWD调试接 口、 一个 USART串口、 一个CAN总线以及多个输入输 出IO； 其中STM32G431C6的PA0、 PA 3、 PA6和PB0复用 为GPIO_MODE_IN_FLOATING浮空输入， 作为接收超声波检测模块(6)电平信号的引脚； PA1、 PA3、 PA7和PB8复用为GPIO_MODE_OUT_PP推挽输出， 用来给超声波检测模块(6)提供10us的 激励信号； PA9、 PA10复用为USART1的RX和TX串口用于调试时和上级控制系统通信和前期测 试； PA11、 PA12复用为CAN1的RX和TX， 留有CAN接口， 用于向上级控制系统通信汇报处理后的 信息； PA13、 PA14复用为SWDIO和SWDCL用于调试阶段使用SWD接口向检测处理模块(9)烧入 程序和后期开发； PC14、 PC15为外部低速时钟， 外接一个稳定的32KHZ晶振； PG10为复用为复 位功能， 供开发时按键复位和SWD烧入时使用； PF0、 PF1为外部高速时钟信号， 外接一个稳定 的12MHZ晶振。 5.一种连续体机械臂， 包括驱动箱(1)、 柔性中心杆(2)、 多个控制盘(4)和多组驱动绳 组(3)， 多个所述控制盘(4)固定于柔性中心杆(2)上， 各所述 驱动绳组(3)与所述控制盘(4) 一一对应连接， 驱动箱(1)与各所述驱动绳组(3)相连， 其特征在于， 还包括如权利要求 1～4 中任意一项所述的连续体机械臂避障系统， 所述检测处理模块(9)安装于所述控制盘(4) 上。 6.根据权利要求5所述的连续体机械臂， 其特征在于， 所述中心杆(2)的末端设置有视 觉检测模块(7)。 7.根据权利要求5或6所述的连续体机械臂， 其特征在于， 各所述控制盘(4)之间的中心 杆(2)上固定有间隔盘(5)。 8.一种基于权利要求1～4中任意一项所述的连续体机械臂避障系统 的避障方法， 其特 征在于， 包括 步骤： 1)多个所述超声 波检测模块(6)检测机 械臂径向方向上障碍物的距离； 2)所述检测处理模块(9)将障碍物的距离与预设最小接近距离进行对比； 当障碍物的 距离小于预设最小接 近距离， 则进入步骤3)； 3)在机械臂运动环境中构造虚拟的斥力场， 对障碍物与机械臂产生的斥力进行计算， 得到当前机械臂段在垂直中心杆(2)平面上的建议运动方向和距离； 其中斥力场随机械臂 与障碍物 距离的增大而减小， 指向远离障碍物的方向；权　利　要　求　书 1/2 页 2 CN 115179329 A 24)根据建议 运动方向和距离控制当前机 械臂段的运动， 实现机 械臂的避障。 9.根据权利要求8所述的避障方法， 其特征在于， 步骤3)的具体过程为： 建立斥力势场， 在斥力势场作用域内， 臂体离障碍物越远， 所受斥力越大， 反 之越小， 其斥力场定义 为： 当ρ(q,qobs)≤ρo时： 当ρ(q,qobs)>ρo时： Urep(q)＝0 其中： Urep(q)为障碍物的斥力场； n为斥力势场增益系数； ρ(q,qobs)表示臂体与障碍物 位置之间的相对距离； ρo为障碍物的影响半径， 表示斥力势场作用距离； 其中斥力为斥力势场的负梯度： 斥力的合力Freq为当前机械臂段受到的合力F， 合力F方向为当前机械臂段在垂直中心 杆(2)平面上的建议运动方向， 合力F大小为机械臂段在垂直中心杆(2)平面上的建议运动 距离。 10.根据权利要求8或9所述的避障方法， 其特征在于， 步骤1)的具体过程为： 所述检测 处理模块(9)向各超声波检测模块(6)发送 高电平激励信号， 各所述超声波检测模块(6)发 出方波信号并检测有 无信号返回； 当检测处理模块(9)检测Echo口电平是否出现高电平， 出 现高电平则计时， 等待Echo变回低电平停止计时， 得到发送和接收的时间差t， 即求出距离 s ＝Ct/2， 式 中C为超声 波在空气中的传播速度。权　利　要　求　书 2/2 页 3 CN 115179329 A 3