[发明专利]一种适应真实形状的组合体连续式规避势场构造方法

说明书

本发明公开了一种适应真实形状的组合体连续式规避势场构造方法，通过将组合体障碍物拆分成若干基本几何单元，基于对给定点到各基本几何单元距离及最近邻点计算来构建组合体障碍物人工势场，解析计算该几何单元在给定点处的人工势函数及人工势场值，进而构造解析形式的组合体人工势场。本发明能够基于对障碍物的局部认知信息，为不规则障碍物构造人工势场。并且本发明因为仅依赖于给定点到各基本几何单元距离及最近邻点信息进行人工势场解算，便于在实际障碍环境中的执行。本发明中的人工势函数及势场在除组合体内部的区域是连续的，消除了个体在组合体外部不同区域交界处的运动抖振现象，能够取得较平稳的避障运动控制效果。

技术领域

本发明涉及机器人/智能体运动控制领域，涉及一种适应真实形状的组合体连续式规避势场构造方法。

背景技术

在将控制理论用于无人车、无人机或航天器等真实对象运动控制时，由于真实环境存在复杂的障碍物约束，为了保证个体的安全运行，有必要在控制律中考虑对障碍物的规避。在规划层面上，可以通过使个体跟踪一条与障碍物保持足够距离的路径来实现；在控制层面上，可以通过在个体原有控制律的基础上添加合适的碰撞规避项来实现。对于具有复杂几何结构的障碍物，由于不便于为其构造人工势场，现有的避障方案大多从规划角度予以解决，这不仅为闭环系统稳定性和收敛性证明带来了挑战，而且不适用于对运动障碍物(如其他个体)或即时发现的障碍物的规避。尽管也可以在利用球面或椭球面近似或逼近障碍物外形后，利用人工势场法设计碰撞规避力，但控制精度较为有限，可能会使个体离障碍物局部区域过近或过远，进而限制了在障碍区域附近精细操作任务的应用。虽然某些改进的人工势场法能够解决复杂外形障碍物的碰撞规避，但或者由于不满足势场条件(即碰撞规避力并非所设计势函数的梯度)，缺乏闭环系统稳定性证明，或者存在不连续性，容易在区域边界处造成抖振现象。

发明内容

本发明目的在于提供一种适应真实形状的组合体连续式规避势场构造方法，以克服现有技术存在的问题，

为实现上述目的，本发明提供如下技术方案：

一种适应真实形状的组合体连续式规避势场构造方法包括：

S1：获取组合体障碍物模型、测试点的坐标、碰撞规避检测距离阈值和人工势函数系数；

S2：将S1中组合体障碍物模型分解成若干个基本几何单元；

S3：判断S2中几何单元中的一部分能否构成相对独立的子结构，如果S2中几何单元中的一部分能构成相对独立的子结构，则将几何单元按所属的子结构分别划入到若干个包围盒中，如果S2中几何单元中的任何一部分都不能构成相对独立的子结构，则将所有几何单元整体视为一个子结构，并用一个包围盒覆盖所有几何单元；

S4：根据S2得到的基本几何单元的大小、形状和位置，构造测试点到每一基本几何单元的距离公式；

S5：利用S4中构造的测试点到基本几何单元距离公式，根据S1获得的碰撞规避检测距离阈值和人工势函数系数，构造每一基本几何单元的势函数公式；

S6：通过对S5中构造的势函数公式求负梯度，构造每一基本几何单元的势场公式；

S7：判断测试点所属的包围盒，并返回这些包围盒中几何单元集合的并集；

S8:分别利用S5的势函数公式和S6中的势场公式，计算S7中返回的几何单元集合的并集中各个几何单元在测试点处的人工势函数及人工势场，并分别叠加，得到组合体障碍物模型在测试点处的人工势函数及人工势场。

优选地，将组合体障碍物模型分解成若干个基本几何单元的公式为

其中，B为组合体障碍物模型，B₁,B₂,…,为基本几何单元。

优选地，基本几何单元为凸基本几何单元。