(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211246426.6 (22)申请日 2022.10.12 (71)申请人 深圳市智绘科技有限公司 地址 518000 广东省深圳市南 山区西丽 街 道西丽社区打石一路深圳国际创新谷 六栋 （万科云城六期二栋） A座3001研 发用房至 3004研发用房 (72)发明人 张亮　雷龙　周心其　熊伟成　 (74)专利代理 机构 深圳智汇远见知识产权代理 有限公司 4 4481 专利代理师 赵烁 (51)Int.Cl. B25J 9/16(2006.01) B25J 13/00(2006.01) (54)发明名称 机器人路径的优化方法及装置、 电子设备 (57)摘要 本发明公开了一种机器人路径的优化方法 及装置、 电子设备， 其中， 该方法包括： 确定前端 可行路径， 并划分所述前端可行路径的待求解空 间， 其中， 所述待求解空间为部分约束的轨迹路 径范围； 计算所述轨迹路径范围的预通行时间； 采用预通行时间构建所述待求解空间的优化目 标矩阵和约束矩阵， 其中， 所述优化目标矩阵用 于表征轨迹优化的最佳指标， 所述约束矩阵用于 表征轨迹优化满足的约束条件； 根据所述优化目 标矩阵和所述约束矩 阵在所述待求解空间中求 解最优轨迹， 并将所述最优轨迹确定为机器人的 导航轨迹。 通过本发明， 解决了相关技术在复杂 场景中求解光滑路径的效率低的技术问题， 提高 了特殊应用场景 下的适用性和鲁棒 性。 权利要求书3页 说明书16页 附图6页 CN 115302520 A 2022.11.08 CN 115302520 A 1.一种机器人路径的优化方法， 其特 征在于， 包括： 确定前端可行路径， 并划分所述前端可行路径的待求解空间， 其中， 所述待求解空间为 部分约束的轨 迹路径范围； 计算所述轨 迹路径范围的预通行时间； 采用预通行时间构建所述待求解空间的优化目标矩阵和约束矩阵， 其中， 所述优化目 标矩阵用于表征轨 迹优化的最佳指标， 所述约束矩阵用于表征轨 迹优化满足的约束条件； 根据所述优化目标矩阵和所述约束矩阵在所述待求解空间中求解最优轨迹， 并将所述 最优轨迹确定为机器人的导 航轨迹。 2.根据权利要求1所述的方法， 其特征在于， 划分所述前端可行路径的待求解空间包 括： 对所述前端可 行路径进离 散化处理， 得到连续的离 散路径点； 分别生成以所述离散路径点为圆心 的外接圆的正方形， 其中， 每个正方向对应一个路 径段； 将多个正方 形占用的空间确定为所述前端可 行路径的待求 解空间。 3.根据权利要求1所述的方法， 其特征在于， 划分所述前端可行路径的待求解空间包 括： 对所述前端可 行路径进离 散化处理， 得到连续的离 散路径点； 遍历所述离散路径点上的每个路径点,将在目标段直线外侧且投影距离低于预设值的 路径点投影到所述目标段上， 以确定沿所述目标段运行的中间路径， 其中， 所述目标段直线 为所述机器人的必要约束路径； 在所述离散路径点的起点到所述中间路径起点之间生成第 一路径， 在所述中间路径的 终点到所述离 散路径点的终点之间生成第二路径； 采用所述中间路径， 所述第 一路径和所述第 二路径生成所述前端可行路径的待求解空 间。 4.根据权利要求3所述的方法， 其特征在于， 采用所述中间路径， 所述第一路径和所述 第二路径生成所述前端可 行路径的待求 解空间包括： 对所述第一路径和所述第二路径进行离散化处理， 并生成以每个离散点为圆心， 预定 半径的圆为外接圆的正方形， 得到第一矩形队列， 沿所述中间路径生成小于第二预设值的 目标矩形； 去除所述第一矩形队列中重复连接的矩形， 得到第二矩形队列； 检测所述第 二矩形队列中是否存在互相不连接且相邻的矩形对： 若存在互相不连接且 相邻的矩形对， 向外膨胀所述矩形对中的两个矩形， 直至互相连接， 并采用膨胀后的矩形将 所述第二矩形队列更新 为第三矩形队列； 检测所述目标矩形是否超出所述第三矩形队列的矩形块范围， 若存在， 修剪所述目标 矩形的超出部 分， 将修剪后的目标矩形和所述第三矩形队列占用的轨迹路径范围确定为所 述待求解空间。 5.根据权利要求1所述的方法， 其特征在于， 计算所述轨迹路径范围的预通行时间包 括： 针对轨迹路径范围的每个路径段， 计算路径段的路径长度L， 其中， 所述轨迹路径范围权　利　要　求　书 1/3 页 2 CN 115302520 A 2包括若干条依次邻接的路径段； 确定所述机器人的限制速度vmax和限制加速度amax； 采用以下公式分别计算所述机器人的最大预估速度vmax*和最大预估加速度amax*： vmax*= m*vmax， amax*=n*amax， 其中， 0＜m＜1， 0＜n＜1； 采用所述最大预估速度和所述最大预估加速度计算在所述路径段上加速到所述最大 预估速度所用的加速时间t1； 采用所述加速时间计算所述机器人的最大加速距离； 根据所述 最大加速距离和所述路径长度计算所述路径段的预通行时间。 6.根据权利要求5所述的方法， 其特征在于， 根据 所述最大加速距离和所述路径长度计 算所述路径段的预通行时间包括： 判断所述路径长度L是否小于2*最大加速距离s1； 若所述路径长度L小于2 *最大加速距离s1， 采用以下 公式计算所述路径段的预通行时间 tsum: 若所述路径长度L大于或等于2*最大加速距离s1， 采用以下公式计算所述路径段的预 通行时间tsum: 。 7.根据权利要求1所述的方法， 其特征在于， 采用预通行时间构建所述待求解空间的优 化目标矩阵包括： 确定所述轨迹路径范围的每个路径段的目标优化指标， 并采用若干个所述目标优化指 标构建系数矩阵， 其中， 所述系数矩阵的阶数与所述目标优化指标的项数相同； 基于所述系数矩阵构建所述系数矩阵的多 项式曲线表达式； 计算所述多 项式曲线表达式的k阶导数， 其中， k 为大于0的整数； 采用所述多 项式曲线表达式和所述 k阶导数计算得到优化目标函数； 将所述预通行时间确定为所述多项式曲线的总时间， 基于所述总时间将所述优化目标 函数中多项式曲线转换为相 应阶次的贝塞尔曲线， 得到贝塞尔控制点矩阵和 转换矩阵， 其 中， 所述贝塞尔控制点矩阵中的控制点为最优轨迹的待求解变量， 所述贝塞尔曲线的轨迹 与所述最优轨迹相同； 采用所述贝 塞尔控制点矩阵和所述 转换矩阵构建优化目标矩阵。 8.根据权利要求1所述的方法， 其特征在于， 采用预通行时间构建所述待求解空间的约 束矩阵包括： 为所述轨 迹路径范围的每 个路径段构建位置矩阵， 速度矩阵， 以及加速度矩阵； 分别设置所述位置矩阵， 所述速度矩阵， 以及所述加速度矩阵的上界值和下界值， 其 中， 所述上界值和所述下界值用于计算所述 最优轨迹的位置参数， 速度参数， 加速度参数； 为所述轨迹路径范围的每个路径段构建位置连续性矩阵， 速度连续性矩阵， 以及加速权　利　要　求　书 2/3 页 3 CN 115302520 A 3