(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210880071.X (22)申请日 2022.07.25 (71)申请人 西北工业大 学 地址 710072 陕西省西安市碑林区友谊西 路127号 (72)发明人 党朝辉　周昊　张育林　袁建平　 刘培栋　焦博涵　 (74)专利代理 机构 西安通大专利代理有限责任 公司 6120 0 专利代理师 钱宇婧 (51)Int.Cl. G05D 1/02(2020.01) G06F 30/15(2020.01) G06F 30/20(2020.01) G06F 111/04(2020.01)G06F 119/14(2020.01) (54)发明名称 一种凸多面体人工势场及梯度力的连续解 析构造方法 (57)摘要 本发明公开了一种凸多面体人工势场及梯 度力的连续解析构造方法， 本发 明基于凸多面体 模型的解析表达式以及测试点相对于凸多面体 方位的逻辑判断， 给出了点到凸多面体距离d及 其在点P处梯度 ▽d的解析表达式。 然后将其代入 到经典的人工势函数及其梯度表达式中， 得到了 适应于凸多面体形状的人工势函数及其梯度力。 同现有技术相比， 本发明所构造的人工势函数及 梯度力是解析的， 无数值迭代计算过程， 因而能 够为避障运动控制实时精确提供适应于凸多面 体障碍物形状的人工势场排斥力， 并且在先验获 知测试点相对于凸多面体方位的情况下， 该点处 人工势函数及梯度力的表达式是确定的， 省去了 该点相对于凸多面体方位的逻辑判断步骤。 权利要求书4页 说明书13页 附图2页 CN 115033003 A 2022.09.09 CN 115033003 A 1.一种凸多面体人工势场及 梯度力的连续 解析构造方法， 其特 征在于， 包括以下步骤： 步骤1， 建立凸多面体模型、 测试点坐标和参数值； 所述凸多面体模型以面形式表示或 以顶点形式表示； 步骤2， 对以面形式表示的凸面体模型变换获得以顶点形式表示的凸面体模型， 或对以 顶点形式表示的凸面体模型变换获得以面形式表示的凸面体模型； 步骤3， 通过以面形式表示的凸面体模型和以顶点形式表示的凸面体模型， 确定凸多面 体中各面与顶点之间的连接关系 、 顶点之间的连接关系和各面之间的连接关系； 步骤4， 基于以面形式表示的凸多面体模型和以顶点形式表示的凸多面体模型， 结合凸 多面体中各面与顶点之间的连接关系、 顶点之间的连接关系和各面之间的连接关系， 判断 测试点相对于凸多面体的方位； 所述测试点相对于凸多面体的方位包括四种 形式： 含边界 的凸多面体内部、 凸多面体的面上 方、 凸多面体的棱上 方和凸多面体的顶点上 方； 步骤5， 根据测试点相对于凸多面体的方位形式， 解析计算出测试点和凸多面体的距 离， 以及所述距离在测试点处的梯度； 步骤6， 基于所述测试点和凸多面体的距离， 计算出凸多面体在测试点处的人工势函 数； 基于所述测试点和凸多面体的距离， 以及距离在测试点处的梯度， 计算出凸多面体在测 试点处的梯度力。 2.根据权利要求1所述的一种凸多面体人工势场及梯度力的连续解析构造方法， 其特 征在于， 步骤1中， 以面形式表示的凸多面体模型为： 其中， 每一个不 等式的下 标为凸多面体中各个面的编号， nf为凸多面体的面个数； 以顶点形式表示的凸多面体模型为： 其中， Pi(i＝1,2,...,nv)分别为凸多面体各个顶点， nv为凸多面体顶点个数， O为坐标 原点。 3.根据权利要求1所述的一种凸多面体人工势场及梯度力的连续解析构造方法， 其特 征在于， 步骤2中， 对以面形式表示的凸面体模型变换获得以顶点形式表示的凸面体模型的 过程中， 从以面形式表示的凸面体模型中， 选取任意3个不等式变换为等式后求解； 如果无 解， 则3个面中的2个面平行， 如果有解， 验证所述解是否满足凸多面体模型中其他的不等 式， 如果满足则为凸多面体的一个顶点， 如果不满足其他的不等式， 舍去解； 遍历所有不等 式后， 将面形式表示的凸多面体模型改写为顶点形式的凸多面体模型； 对以顶点形式表示的凸面体模型变换获得以面形式表示的凸面体模型的过程中， 从以 顶点形式表示的凸多面体模型中任取三个顶点构造平面约束方程， 将其他点坐标代入， 如 果代入的点坐标使得平面约束方程成立， 则为凸多面体的一个面约束， 如果代入的点坐标 使得平面约束方程不成立， 则重新代入其他点， 得到凸多面体的一个面约束， 直至所有的三权　利　要　求　书 1/4 页 2 CN 115033003 A 2个顶点遍历结束， 得到凸多面体各个面约束， 得到以面形式表示的凸多面体模型。 4.根据权利要求1所述的一种凸多面体人工势场及梯度力的连续解析构造方法， 其特 征在于， 步骤3中， 所述确定连接关系的过程 为： (1)所述凸多面体中各面与顶点关联矩阵为 如果顶点Pj(j＝1,2,..., nv， nv为顶点个数)位于面Fi(i＝1,2,...,nf)上， 则 否则 (2)所述顶点 之间的邻接矩阵为 如果顶点Pi与顶点Pj同时位于多 面体的两 个面上， 则称顶点Pi与顶点Pj相邻接， 对应有 否则有 (3)所述各面之间的邻接矩阵为 如果面Fi与面Fj有两个公共顶点， 则称面 Fi与面Fj相邻接， 对应有 否则有 5.根据权利要求1所述的一种凸多面体人工势场及梯度力的连续解析构造方法， 其特 征在于， 步骤4中， 测试点相对于凸多面体的方位判断过程 为： (1)测试点 位于含边界的凸多面体内部时， 测试点满足下式(1)的约束， 其中， 下标对应凸多面体各个面编号， nf为凸多面体的面个数； (2)测试点位于凸多面体的面上方时， 测试点在平面Fi中的垂足 满足 式(7)中由平面Fi各邻接面Fj形成的约束 其中垂足Hfi的坐标通过如下公式(5)求出 (3)测试点 位于凸多面体的面Fi与面Fj的公共棱上 方时的约束条件为： 其中点Hfi定义及求 法如式(5)所示， 点P1和P2为凸多面体的面Fi与面Fj的公共顶点， Pi(i ∈{1,2,. ..,nv})为点Pi对应的空间向量；权　利　要　求　书 2/4 页 3 CN 115033003 A 3