一种基于激光点和障碍函数的运动体鲁棒安全控制方法

申请号 CN202410277509.4 申请日 2024-03-12 公开(公告)号 CN118011832A 公开(公告)日 2024-05-10
申请人 东北大学; 发明人 刘腾飞; 刘飞跃; 吴思; 徐茜;
摘要 本 发明 涉及一种基于激光点 云 和障碍函数的运动体鲁棒安全控制方法,该方法包括:S1:建立带有乘性干扰的全向运动体 运动学模型 ;S2:根据运动体 位置 和 激光雷达 测量方向建立障碍物测量模型;S3:对障碍物测量模型进行正则化处理,得到正则化测量模型;S4:基于正则化测量模型建立候选过零障碍函数并基于运动学模型构建安全约束;S5:利用正基 修改 安全约束,并结合二次规划、过零障碍函数和标称速度构造一个以求解约束优化问题的鲁棒安全 控制器 ,通过实时求解该优化问题的解得到这一时刻运动体的控制量v。其有益效果是,满足运动体在正常情况下以及存在测量误差与运动学乘性干扰的情况下自主避障需求,确保安全控制器的控制量具有李普希茨连续性。
权利要求

1.一种基于激光点和障碍函数的运动体鲁棒安全控制方法,其特征在于,包括:
S1:建立带有乘性干扰的全向运动体运动学模型;依据任务要求确定运动体的起点和终点;在起点和终点之间规划全局运动路径;根据全局运动路径确定标称速度;
S2:根据运动体位置激光雷达测量方向建立障碍物测量模型;
S3:对障碍物测量模型进行正则化处理,得到正则化测量模型;
S4:基于正则化测量模型建立候选过零障碍函数并基于运动学模型构建安全约束;
S5:利用正基修改安全约束,并结合二次规划、过零障碍函数和标称速度构造一个以求解约束优化问题的鲁棒安全控制器,通过实时求解该优化问题的解得到这一时刻运动体的控制量v;
S6:判断运动体是否到达目标点,若是,则结束;若否,则返回S3。
2.根据权利要求1所述的基于激光点云和障碍函数的运动体鲁棒安全控制方法,其特征在于,所述运动学模型为:
n
其中,p∈R表示运动体的位置,在二维世界坐标系中p=(x,y), 为运动体位置的一阶n
导数代表速度,v∈R ,v为运动体的线速度,二维坐标系中速度v可分解为(vx,vy),kv为一个未知且有界的正实数,表示运动体模型的动态不确定性,kv>0,
3.根据权利要求1所述的基于激光点云和障碍函数的运动体鲁棒安全控制方法,其特征在于,所述标称速度通过路径追踪算法确定或人为设定。
4.根据权利要求1所述的基于激光点云和障碍函数的运动体鲁棒安全控制方法,其特征在于,所述障碍物测量模型为:
T
rω(p,l,ω)=min{|o+ω‑p|,o∈O∪B(p,DB),l(o‑p)=|o‑p|}
n n n
其中,p∈R 表示运动体的位置,l∈R表示测量方向且|l|=1,ω(p,l,t)∈R表示在时刻t和测量方向为l的测量误差,并且满足
DB是传感器最大测量范围,集合O代表所
n
有障碍物点的位置集合,o∈O∪B(p,DB)是障碍物点,B(p,DB)={o∈R:|o‑p|=DB},B(p,DB)表示在传感器最大测量边缘上的点集。
5.根据权利要求1所述的基于激光点云和障碍函数的运动体鲁棒安全控制方法,其特征在于,所述对障碍物测量模型进行正则化处理,得到正则化测量模型,包括:
通过增加一个非负的非线性项 对障碍物测量模型进行正则化,得到一个具有李普希茨连续性的正则化测量模型rωc(p,l0,ω):
n n
其中,p∈R表示运动体的位置,l0∈R 是任意一个测量方向,|l0|=1;rωc(p,l0,ω)为在任意一个l0方向上建立一个虚拟障碍物点,该虚拟障碍点到运动体的距离是rωc(p,l0,ω); 是一个非负、李普希茨连续、递减的函数,当 时,
6.根据权利要求1所述的基于激光点云和障碍函数的运动体鲁棒安全控制方法,其特征在于,
所述过零障碍函数为:
hω(p,l0,ω)=rωc(p,l0,ω)‑Ds
其中,Ds是安全裕度;
所述安全约束通过hω(p,l0,ω)对位置p进行求导得到:
7.根据权利要求1所述的基于激光点云和障碍函数的运动体鲁棒安全控制方法,其特征在于,所述正基 满足如下条件:
L中向量元素的个数m>n,n是系统被控状态的维数;
L中每个向量元素li是单位向量,|li|=1;
L中任意n个向量是线性独立的;
n
任意一个单位向量l∈R都可以表示为集合 的
元素的正线性组合,该集合的基数不小于n。
8.根据权利要求1所述的基于激光点云和障碍函数的运动体鲁棒安全控制方法,其特征在于,所述鲁棒安全控制器 具体形式如下所示:
2
v=argmin|v‑vp|
s.t.Av≤b(p)
n n T
其中,vp∈R 是标称速度,v∈R是实际输出的控制量,矩阵A=[l1,…,lm]是一组正基,函数 是一个扩展 类
函数。
9.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时实现权利要求1至7中任一项所述基于激光点云和障碍函数的运动体鲁棒安全控制方法。
10.一种存储设备,包括存储介质和处理器,所述存储介质存储有计算机程序,其特征在于,所述处理器执行所述计算机程序时实现权利要求1至7中任一项所述基于激光点云和障碍函数的运动体鲁棒安全控制方法。

说明书全文

一种基于激光点和障碍函数的运动体鲁棒安全控制方法

技术领域

[0001] 本发明涉及运动体安全控制技术领域,尤其涉及一种基于激光点云和障碍函数的运动体鲁棒安全控制方法。

背景技术

[0002] 本发明方法属于移动机器人安全控制领域,在这里,安全指的是移动机器人与障碍物之间尽可能保持足够远的距离。安全控制的目标是在不违反安全约束的情况下,使机器人在充满障碍物的环境中安全导航到指定目标点。然而,实际应用的机器人通常需要更复杂的方法来与环境进行交互以完成任务,而不仅仅从A点安全导航至B点。安全控制是实现复杂行为能基础,可能还需要实时算法来根据实时感知信息做出相应的反应。此外,连续且平稳的运动也是机器人的核心要求之一。
[0003] 根据环境中障碍物信息是否被充分描述,安全控制可大致划分为离线操作和在线操作。当空间中的障碍物的形状、位置和方位等信息被充分描述的时候,安全控制的主要方法是使用全局路径规划方法,如Dijkstra、A*和快速搜索随机树等为运动体在地图中搜索一条能避开障碍物的安全路径,并使用诸如三次样条曲线等轨迹优化方法得到可以直接用于跟踪的光滑路径,该过程一般是离线操作。处理完整信息的一个明显优势是可以轻松引入任何优化标准,例如寻找最短路径、最短时间路径、最安全路径,难点在于如何根据各种约束获得一个计算效率高的方案。当环境信息不能被充分描述的时候,机器人必须配备传感器去实时探索周围环境并在线做出反应,该类方法主要有动态窗口法、向量直方图法、人工势场法。这些方法的关键思想是利用评估函数来确定基于当前感知输入的情况下,最佳的下一步动作,其优势在于可以高频率生成新的命令以实时应对障碍物环境,缺点是因为无法获知潜在可以被利用的全局环境信息而可能缺乏最优性,在线操作方法的难点在于如何降低算法的计算复杂度以达到实时性的要求,以及如何在有传感器误差、系统模型不确定性的情况下保证运动体的安全性和稳定性
[0004] 二次规划方法在实时安全控制中已被证明是一种有效的解决问题方法。然而,在基于优化的多目标控制中,优化算法产生的解有可能是非李普希茨连续的,非李普希茨连续的解有可能会引起运动体执行机构的抖颤,影响其在动态不确定条件下执行动作的李普希茨连续性,进而影响到系统的稳定性和可靠性。例如,在路径规划中,曲率不连续会导致运动体过冲和不稳定现象。在巡航控制中,控制输入的切换会导致乘客的不适和安全隐患。同时,在复杂环境中,障碍物位置的不连续反馈会导致感知结果的非李普希茨连续性,从而使基于梯度的算法(如基于势函数的方法)和标准的优化算法(包括模型预测控制)也可能产生非李普希茨连续的解。实际系统中可能也会出现传感器误差、系统模型不确定性等问题,这些都会影响系统的安全性。此外,还需要考虑嵌入式平台的计算能力对算法执行速度的影响,当算法复杂度较高而处理器计算能力满足不了避障算法的实时求解要求时,很可能导致运动体与障碍物发生碰撞。

发明内容

[0005] 要解决的技术问题
[0006] 鉴于现有技术的上述缺点、不足,本发明提供一种基于激光点云和障碍函数的运动体鲁棒安全控制方法,其解决了如何在系统存在动态不确定性的情况下,确保移动机器人能够更平稳、安全地运行的技术问题。
[0007] 技术方案
[0008] 为了达到上述目的,本发明采用的主要技术方案包括:
[0009] 第一方面,本发明提供一种基于激光点云和障碍函数的运动体鲁棒安全控制方法,包括:
[0010] S1:建立带有乘性干扰的全向运动体运动学模型;依据任务要求确定运动体的起点和终点;在起点和终点之间规划全局运动路径;根据全局运动路径确定标称速度;
[0011] S2:根据运动体位置和激光雷达测量方向建立障碍物测量模型;
[0012] S3:对障碍物测量模型进行正则化处理,得到正则化测量模型;
[0013] S4:基于正则化测量模型建立候选过零障碍函数并基于运动学模型构建安全约束;
[0014] S5:利用正基修改安全约束,并结合二次规划、过零障碍函数和标称速度构造一个以求解约束优化问题的鲁棒安全控制器,通过实时求解该优化问题的解得到这一时刻运动体的控制量v;
[0015] S6:判断运动体是否到达目标点,若是,则结束;若否,则返回S3。
[0016] 可选地,所述运动学模型为:
[0017]
[0018] 其中,p∈Rn表示运动体的位置,在二维世界坐标系中p=(x,y), 为运动体位置的n一阶导数代表速度,v∈R ,v为运动体的线速度,二维坐标系中速度v可分解为kv为一个未知且有界的正实数,表示运动体模型的动态不确定
性,kv>0,
[0019] 可选地,所述标称速度通过路径追踪算法确定或人为设定。
[0020] 可选地,所述障碍物测量模型为:
[0021] rω(p,l,ω)=min{|o+ω‑p|,o∈O∪B(p,DB),lT(o‑p)=|o‑p|}
[0022] 其中,p∈Rn表示运动体的位置,l∈Rn表示测量方向且|l|=1,ω(p,l,t)∈Rn表示在时刻t和测量方向为l的测量误差,并且满足DB是传感器最大测量范围,集合O代表所
n
有障碍物点的位置集合,o∈O∪B(p,DB)是障碍物点,B(p,DB)={o∈R:|o‑p|=DB},B(p,DB)表示在传感器最大测量边缘上的点集。
[0023] 可选地,所述对障碍物测量模型进行正则化处理,得到正则化测量模型,包括:
[0024] 通过增加一个非负的非线性项 对障碍物测量模型进行正则化,得到一个具有李普希茨连续性的正则化测量模型rωc(p,l0,ω):
[0025]
[0026] 其中,p∈Rn表示运动体的位置,l0∈Rn是任意一个测量方向,|l0|=1;rωc(p,l0,ω)为在任意一个l0方向上建立一个虚拟障碍物点,该虚拟障碍点到运动体的距离是rωc(p,l0,ω); 是一个非负、李普希茨连续、递减的函数,当 时,
[0027] 可选地,所述过零障碍函数为:
[0028] hω(p,l0,ω)=rωc(p,l0,ω)‑Ds
[0029] 其中,Ds是安全裕度;
[0030] 所述安全约束通过hω(p,l0,ω)对位置p进行求导得到:
[0031]
[0032] 可选地,所述正基 满足如下条件:
[0033] L中向量元素的个数m>n,n是系统被控状态的维数;
[0034] L中每个向量元素li是单位向量,|li|=1;
[0035] L中任意n个向量是线性独立的;
[0036] 任意一个单位向量l∈Rn都可以表示为集合的元素的正线性组合,该集合的基数不小于n。
[0037] 可选地,所述鲁棒安全控制器 具体形式如下所示:
[0038] v=argmin|v‑vp|2
[0039] s.t.Av≤b(p)
[0040] 其中,vp∈Rn是标称速度,v∈Rn是实际输出的控制量,矩阵A=[l1,…,lm]T是一组正基, 函数 是一个扩展类函数。
[0041] 第二方面,本发明提供一种计算机可读存储介质,其上存储有计算机程序,所述程序执行时实现上述第一方面中任一项所述基于激光点云和障碍函数的运动体鲁棒安全控制方法。
[0042] 第三方面,本发明提供一种存储设备,包括存储介质和处理器,所述存储介质存储有计算机程序,所述程序被处理器执行时实现上述第一方面中任一项所述基于激光点云和障碍函数的运动体鲁棒安全控制方法。
[0043] 有益效果
[0044] 本发明的有益效果是:本发明的一种基于激光点云和障碍函数的运动体鲁棒安全控制方法,该方法应用于搭载环境感知工具且具备全向运动能力的运动体,例如搭载激光雷达的麦克纳姆轮无人车,适用于复杂环境中存在避障需求的场景,如工厂车间等。该方法首先通过正则化方法修改障碍物测量模型,在保证运动体安全的同时,引入连续变化的虚拟障碍点以确保感知模型具有李普希茨连续性。随后,通过对正则化的障碍物测量模型构建鲁棒控制障碍函数并生成安全约束。特别是针对测量误差和运动体运动学乘性干扰的情况,本申请利用鲁棒控制障碍函数的性质,在安全约束中增加安全裕度,以消除不确定性对安全性的影响。进一步,使用正基对安全约束进行重构,使非冗余积极约束的约束矩阵是线性无关满足行满秩的要求。最后,将安全约束与二次规划进行整合,设计了一个以求解约束优化问题为表现形式的鲁棒安全控制方法。此方法在实时满足机器人安全需求的同时,还保证了运动状态的李普希茨连续性要求,使其运行更加安全平稳,并且该方法对测量误差和运动体运动学乘性干扰具有鲁棒性。附图说明
[0045] 图1为本发明实施例提供的基于激光点云和障碍函数的运动体鲁棒安全控制方法的流程示意图;
[0046] 图2为本发明实施例提供的整体算法框架
[0047] 图3为本发明实施例提供的方法中用到的物理设备和作用;
[0048] 图4为本发明实施例提供的所使用的受控运动体;
[0049] 图5为本发明实施例提供的感知测量模型的仿真图;
[0050] 图6为本发明实施例提供的正则化之后的感知测量模型的仿真图;
[0051] 图7为本发明实施例提供的仿真效果图;
[0052] 图8为本发明实施例提供物理实验场景;
[0053] 图9为本发明实施例提供的物理实验场景下运动体的轨迹;
[0054] 图10为本发明实施例提供的距离障碍物最小距离对比图;
[0055] 图11为本发明实施例提供的速度导数对比图。

具体实施方式

[0056] 为了更好的解释本发明,以便于理解,下面结合附图,通过具体实施方式,对本发明作详细描述。
[0057] 本发明实施例针对在复杂多障碍物环境中工作的全向移动机器人,提出了一种基于激光点云和障碍函数的运动体鲁棒安全控制方法。该方法利用激光雷达传感器实时获取局部环境中的障碍物信息,采用方向‑距离函数描述障碍物的测量模型,并据此构建障碍函数以生成安全约束。通过将二次规划与安全约束相结合,设计了一种以求解约束优化问题为表现形式的安全控制器。在本发明中,针对系统可能存在的动态不确定性以及现有安全控制方法难以确保输出到运动体状态的李普希茨连续性问题,本发明做出了三个重要改进。首先,通过对存在测量误差的障碍物测量模型进行正则化,确保感知模型的李普希茨连续性。其次,利用正基对原始速度可行集进行重塑,以确保非冗余积极约束满足行满秩条件。最后,利用鲁棒控制障碍函数的思想,在安全约束中增加安全裕度进一步抵抗不确定性对安全性造成的影响。这三项改进保证了安全控制器输出到运动体状态的李普希茨连续性,大大增强了对系统动态不确定性的鲁棒性。本发明不仅能够满足运动体在正常情况下以及存在测量误差与运动学乘性干扰的情况下的自主避障需求,同时也能确保安全控制器的控制量具有李普希茨连续性。这些优势使得在充满障碍物的环境中,运动体能够更加安全、平稳地进行导航。
[0058] 为了更好的理解上述技术方案,下面将参照附图更详细地描述本发明的示例性实施例。虽然附图中显示了本发明的示例性实施例,然而应当理解,可以以各种形式实现本发明而不应被这里阐述的实施例所限制。相反,提供这些实施例是为了能够更清楚、透彻地理解本发明,并且能够将本发明的范围完整的传达给本领域的技术人员。
[0059] 第一方面,参照图1,本实施例提供了一种基于激光点云和障碍函数的运动体鲁棒安全控制方法,包括:
[0060] S1:建立带有乘性干扰的全向运动体运动学模型;依据任务要求确定运动体的起点和终点;在起点和终点之间规划全局运动路径;根据全局运动路径确定标称速度;
[0061] S2:根据运动体位置和激光雷达测量方向建立障碍物测量模型;
[0062] S3:对障碍物测量模型进行正则化处理,得到正则化测量模型;
[0063] S4:基于正则化测量模型建立候选过零障碍函数并基于运动学模型构建安全约束;
[0064] S5:利用正基修改安全约束,并结合二次规划、过零障碍函数和标称速度构造一个以求解约束优化问题的鲁棒安全控制器,通过实时求解该优化问题的解得到这一时刻运动体的控制量v;
[0065] S6:判断运动体是否到达目标点,若是,则结束;若否,则返回S3。
[0066] 本实施例公开的一种基于激光点云和障碍函数的运动体鲁棒安全控制方法,目标是确保在系统存在动态不确定性的情况下,使移动机器人能够更平稳、安全地运行。该方法的关键特点是运动体在与环境实时交互过程中,能够始终确保其状态在安全集之内,并且保证该控制方法的输出到运动体状态的李普希茨连续性,同时对于测量误差和运动体运动学的乘性干扰等动态不确定性具有鲁棒性。本实施例使用了一种新颖的正则化方法,通过对原始的障碍物感知模型进行正则化处理,得到变化较为平缓的虚拟障碍点,有利于保证安全控制量的李普希茨连续性,又不会破坏安全性。相关技术通常会为每一个障碍物点构造安全约束,当障碍物点较多的时候会严重影响约束优化问题的求解效率,另外多约束优化问题也可能会产生非李普希茨连续的解。本实施例通过引入正基对速度可行域进行重构,减少了二次规划安全约束的数量,显著提高了求解速度,且不会破坏安全性。实际系统中可能会出现传感器误差以及系统模型不确定性等问题,影响系统的安全性。本发明提出的安全控制方法对这种动态不确定性具有鲁棒性。通过扩大安全裕度将速度可行集向内收缩,有效抵御了传感器误差和系统模型不确定性对运动体安全性带来的不利影响。
[0067] 可选地,所述运动学模型为:
[0068]
[0069] 其中,p∈Rn表示运动体的位置,在二维世界坐标系中p=(z,y), 为运动体位置的n一阶导数代表速度,v∈R ,v为运动体的线速度,二维坐标系中速度v可分解为(vx,vy),kv为一个未知且有界的正实数,表示运动体模型的动态不确定性,kv>0,
[0070] 可选地,所述标称速度通过路径追踪算法确定或人为设定。全局路径和标称速度旨在为运动体提供一个导航路径和速度的参考。
[0071] 可选地,所述障碍物测量模型为:
[0072] rω(p,l,ω)=min{|o+ω‑p|,o∈O∪B(p,DB),lT(o‑p)=|o‑p|}
[0073] 其中,p∈Rn表示运动体的位置,l∈Rn表示测量方向且|l|=1,ω(p,l,t)∈Rn表示在时刻t和测量方向为l的测量误差,并且满足DB是传感器最大测量范围,集合O代表所
n
有障碍物点的位置集合,o∈O∪B(p,DB)是障碍物点,B(p,DB)={o∈R:|o‑p|=DB},B(p,DB)表示在传感器最大测量边缘上的点集。
[0074] 采用一个内点非空的紧集表示所有障碍物点的位置集合,根据运动体位置p和激光雷达测量方向l建立方向‑距离函数描述障碍物的测量模型rω(p,ω,l)。这里激光雷达的工作方式是扫描式,其扫描一周会有产生非常多个测量方向。
[0075] 可选地,所述对障碍物测量模型进行正则化处理,得到正则化测量模型,包括:
[0076] 通过增加一个非负的非线性项 对障碍物测量模型进行正则化,得到一个具有李普希茨连续性的正则化测量模型rωc(p,l0,ω):
[0077]
[0078] 其中,p∈Rn表示运动体的位置,l0∈Rn是任意一个测量方向,|l0|=1;rωc(p,l0,ω)为在任意一个l0方向上建立一个虚拟障碍物点,该虚拟障碍点到运动体的距离是rωc(p,l0,ω); 是一个非负、李普希茨连续、递减的函数,当 时,
[0079] 通过增加一个非线性项 对障碍物测量模型rω(p,ω,l)进行正则化,得到一个具有李普希茨连续性的正则化测量模型rωc(p,ω,l)。障碍物测量模型rω(p,ω,l)并不是一种理想的数学模型而是根据实际的环境干扰包含了测量误差ω。
[0080] 可选地,所述过零障碍函数为:
[0081] hω(p,l0,ω)=rωc(p,l0,ω)‑Ds
[0082] 其中,Ds是安全裕度;
[0083] 所述安全约束通过hω(p,l0,ω)对位置p进行求导得到:
[0084]
[0085] 设计一个候选过零障碍函数hω(p,ω,li)并构建安全约束,以确保运动体在测量方向li上相对于障碍点是安全的。
[0086] 可选地,所述正基 满足如下条件:
[0087] L中向量元素的个数m>n,n是系统被控状态的维数;
[0088] L中每个向量元素li是单位向量,|li|=1;
[0089] L中任意n个向量是线性独立的;
[0090] 任意一个单位向量l∈Rn都可以表示为集合的元素的正线性组合,该集合的基数不小于n。
[0091] 利用正基修改安全约束以达到对原始速度可行集的重塑。
[0092] 可选地,所述鲁棒安全控制器 具体形式如下所示:
[0093] v=argmin|v‑vp|2
[0094] s.t.Av≤b(p)
[0095] 其中,vp∈Rn是标称速度,v∈Rn是实际输出的控制量,矩阵A=[l1,…,lm]T是一组正基, 函数 是一个扩展类函数。
[0096] 下面结合一具体实施例来进一步介绍本申请的一种基于激光点云和障碍函数的运动体鲁棒安全控制方法:
[0097] 本实施例介绍了一种基于激光点云和障碍函数的运动体鲁棒安全控制方法。在图2中展示了该方法的整体算法框架。图3展示了算法的物理实现。运动捕捉系统实时解算并反馈运动体的位置,地面站运行路径跟踪算法提供标称速度vp。同时,如图4所示,激光雷达安装在运动体的上方质心处,其能实时感知运动体周围的障碍物,提供障碍物的距离和方位信息。鲁棒安全控制算法在板载计算机NUC上运行,将激光雷达测量信息和标称速度作为输入,以求解出当前时刻运动体的实际速度指令。随后,将该实际速度指令传送给运动体的运动控制器以驱动运动体的运动。通过这个过程,运动体的位置得以改变,完成了当前时刻的安全控制。详细步骤如下:
[0098] 步骤1:建立带有乘性干扰的全向运动体的运动学模型。本实施例考虑的被控对象是一个具有全向运动能力的运动体,在这里用一辆装有麦克纳姆轮、可以进行全向运动的无人车进行说明,如图4所示。该运动体的运动学模型可以用一个带有乘性干扰的一阶积分器模型表示:
[0099]
[0100] 其中,p∈Rn是运动体的位置,在二维世界坐标系中p=(x,y),是位置的一阶导数n代表速度,v∈R 是运动体的线速度也是系统的控制输入,二维坐标系中速度v可分解为是一个未知且有界的正实数,表示运动体模型的动态不确定性,
需要注意的是,本实施例中运动体的具体形状不在考虑范围之内,因此安全控制算法只对运动体的速度进行控制,不控制运动体的转向θ。此外,需要依据任务要求明确运动体的起点和终点,随后在两者之间规划出一个全局运动路径,并利用路径追踪算法得出一个有界的标称速度 也可以直接设定标称速度vp。标称速度
的作用是为运动体提供一个速度的参考值。
[0101] 步骤2:采用一个内点非空的紧集O代表所有障碍物点的位置集合,根据运动体位置p和激光雷达测量方向l建立方向‑距离函数描述障碍物点的测量模型rω(p,ω,l)。请注意,这里激光雷达的工作方式是扫描式,如图4,激光雷达安装在运动体的上方质心处,运动体看作一个质点,以质点为中心,激光雷达扫描周围障碍物进行感知。由于外界环境的干扰和测量精度等原因,激光雷达会不可避免地存在测量误差,该测量误差有界、随机,具有不确定性。所以由运动体位置p和测量方向l定义具有测量误差的障碍物测量模型为:
[0102] rω(p,l,ω)=min{|o+ω‑p|:o∈O∪B(p,DB),lT(o‑p)=|o‑p|}[0103] 其中,p∈Rn表示运动体的位置,l∈Rn表示测量方向且|l|=1,ω(p,l,t)∈Rn表示在时刻t和测量方向为l的测量误差,并且满足DB是传感器最大测量范围。对于不同的测
量方向l,其对应的测量误差也不同。集合O代表所有障碍物点的位置集合,并假设其为一个n
内点非空的紧集,o∈O∪B(p,DB)是障碍物点,B(p,DB)={o∈R:|o‑p|=DB},B(p,DB)表示在传感器最大测量边缘上的点集,无论是否有真实的障碍物,其因为到达了传感器测量范围的最远端,所以可以等价成障碍物点集合。
[0104] 障碍物测量模型rω(p,l,ω)是一个关于距离的模型,其含义为在激光雷达测量方向l上,障碍点距离运动体位置p的最近距离,需要注意的是,该模型对激光雷达的测量方法进行了数学化建模,实际的rω(p,l,ω)并非经过计算获得,而是可直接从激光雷达实际测量值中获取。这里选择360个激光雷达测量方向去模拟实际激光雷达的
测量方向,障碍物测量模型的仿真模拟效果图如图5。与此同时,在测量方向l上测量到的障碍物点可以表示为:
[0105] Sω(p,l,ω)={p+lrω(p,l,ω):|l|=1}
[0106] 步骤3:正则化障碍物测量模型rω(p,l,ω)。在步骤2的基础上,通过增加一个非负的非线性项 对障碍物测量模型rω(p,l,ω)进行正则化,得到一个具有李普希茨连续性的正则化测量模型rωc(p,l0,ω):
[0107]
[0108] 其中, 表示运动体的状态,在这里也代表位置,l0∈Rn是任意一个测量方向,该测量方向可以是实际的,也可以是虚拟的,且满足|l0|=1。rωc(p,l0,ω)可以理解为在任意一个l0方向上建立一个虚拟障碍物点,该虚拟障碍点到运动体的距离是rωc(p,l0,ω)。 [‑1,1]→R+是一个非负、李普希茨连续、递减的函数,并且当满足的时候, 一个可供选择的 是
请注意,rωc(p,l0,ω)是通过激光雷达实际测量值rω(p,l,
ω)经过计算获得。
[0109] 正则化的作用是产生一系列虚拟障碍点,如图6所示,可以观察到在对激光雷达测量模型rω(p,l,ω)正则化后,虚拟障碍点相比于原始的障碍点变化更加平滑,使得障碍物感知模型rω(p,l,ω)随运动体位置p的变化是李普希茨连续的,进而使得运动体更加安全平稳地运行。
[0110] 步骤4:在步骤3的基础上,根据rωc(p,l0,ω)设计一个候选过零障碍函数hω(p,l0,ω)并构建安全约束,确保运动体在测量方向l0上相对于障碍点是安全的。过零障碍函数如下:
[0111] hω(p,l0,ω)=rωc(p,l0,ω)‑Ds
[0112] 其中,Ds是安全裕度,能够抑制测量误差对系统安全性的影响。基于控制障碍函数的理论,通过hω(p,l0,ω)对位置p进行求导,得到一个安全约束:
[0113]
[0114] 该约束规定了在l0方向上能保证运动体安全的控制量的集合。通常为了保证运动体在所有测量方向l上的安全性,需要为每一个感知到的障碍物点施加同样形式的安全约束,因此约束的数量会急剧增加,优化问题的求解速度变慢,导致算法实时性变差。此外,多约束优化问题难以保证解的李普希茨连续性。这意味着求解出的控制量可能会产生突变,影响受控运动体的稳定性和安全性。针对以上问题,本发明在步骤5中给出了解决办法。
[0115] 步骤5:利用正基修改步骤4中的安全约束以达到对原始速度可行集的重塑,并与二次规划和步骤4中的过零障碍函数hω(p,l0,ω)相结合,构造一个以约束优化问题为表现形式的鲁棒控制器,求解该优化问题得到运动体的实际控制量v。具体而言,对于特定的0
[0116] L中向量元素的个数m>n,n是系统被控状态的维数,在这里,n=2;
[0117] L中每个向量元素li是单位向量,|li|=1;
[0118] L中任意n个向量是线性独立的;
[0119] 任意一个单位向量 都可以表示为集合的元素的正线性组合,该集合的基数不小于n。
[0120] 其次,将正基与二次规划和步骤4中的过零障碍函数hω(p,l0,ω)结合,构造一个鲁棒安全控制器 其具体形式如下所示:
[0121] v=argmin|v‑vp|2
[0122] s.t.Av≤b(p)
[0123] 其中,vp∈Rn是标称控制量,为运动体提供一个速度的参考值,在这里vp=1.5*n T(paim‑p),paim代表路径点位置,v∈R 是实际输出的控制量,矩阵A=[l1,…,lm] 是一组正基,这里选 取21个单位向 量组成一组正 基,即m=21 ,则正基集合 为
函数 是一个扩展
类函数。由于任意一个单位向量 都为集合
的元素的正线性组合,这里选取
[0124] 上述所设计的鲁棒安全控制方法具有以下的特点:
[0125] 当运动体的位置 在初始时刻处于安全区域时,在将来的所有时刻都能使运动体位置保持在安全区域,即能持续保证运动体的安全。
[0126] 控制方法对运动体运动学中的乘性干扰(kv‑1)v和传感器测量误差ω(p,l,t)具有鲁棒性。
[0127] 控制方法的输出到运动体的状态p是李普希茨连续的,保证了运动体在运动过程中不会因为控制量是非李普希茨连续的而发生抖颤,使运动体更加平稳地导航。
[0128] 步骤6:判断运动体是否到达目标点区域,若是,则结束程序;若否,则基于以上步骤执行下一时刻控制。
[0129] 从图7数值仿真效果来看,所发明的鲁棒安全控制方法在充满障碍物的杂乱环境中能从起始点沿着预设路径进行导航,并且当预设路径上存在障碍物的时候,运动体能躲避,最终安全到达目标位置区域。为了进一步验证所设计鲁棒安全控制算法的优点,本发明搭建了一个物理实验系统如图8所示,并将所发明的鲁棒安全控制方法(R)与全障碍安全算法(A)和非光滑安全算法(N)进行对比。图9是在该物理实验环境中,三种方法对应的机器人运动轨迹。
[0130] 图10显示了三种不同安全控制方法与障碍物之间的最小距离随时间的变化。对于控制方法A,其所需的运行时间大约是22s,方法N的运行时间是23.9s。所发明的方法R运行时间大约是26s,由于这种方法减少了速度可行集,导致了运行速度的降低。然而,从安全性角度来看,在某些时间段,方法A和N会使运动体距离安全基线较近,甚至可能跨越安全基线违反安全性要求。相比之下,新方法R始终距离安全基线较远,表明其在安全性方面有更佳的性能。这一结果说明,鲁棒安全控制算法能有效抵抗障碍物感知测量模型的测量误差和运动体运动学的乘性干扰,从而减少对运动体安全性的不利影响,相比其他两种安全控制方法,该方法展现出更高的鲁棒性。
[0131] 图11显示了三种不同安全控制方法对运动体速度的导数随时间的变化。实际控制中,通常为了保证运动体的跟踪性能和鲁棒性能,通常要求控制量表现为具有李普希茨连续性,即其导数在一定范围内保持有限。观察图11可知,在7‑20s的时间段内,非光滑避障算法N的安全控制量变化最为显著和剧烈,这意味着运动体速度发生了难以实际跟踪的突变,可能影响运动体的导航稳定性,同时对执行机构、电机等产生不利影响。全障碍避障算法A相对于方法N有所缓解,但是仍然存在较大的波动。相比之下,本发明所提的鲁棒安全控制方法能够有效地抑制控制量的变化,使其导数保持在一个合理可控且较小的范围内,这一特性表明运动体速度的李普希茨连续性,进一步保证在动态不确定性情况下的安全性和鲁棒性。
[0132] 第二方面,本发明实施例提供一种计算机可读存储介质,其上存储有计算机程序,所述程序执行时实现上述第一方面中任一项所述基于激光点云和障碍函数的运动体鲁棒安全控制方法。
[0133] 第三方面,本发明实施例提供一种存储设备,包括存储介质和处理器,所述存储介质存储有计算机程序,所述程序被处理器执行时实现上述第一方面中任一项所述基于激光点云和障碍函数的运动体鲁棒安全控制方法。
[0134] 本领域内的技术人员应明白,本发明的实施例可提供为方法、系统或计算机程序产品。因此,本发明可采用完全硬件实施例、完全软件实施例,或结合软件和硬件方面的实施例的形式。而且,本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD‑ROM、光学存储器等)上实施的计算机程序产品的形式。
[0135] 显然,本领域的技术人员可以对本发明进行各种修改和变型而不脱离本发明的精神和范围。这样,倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内,则本发明也应该包含这些修改和变型在内。
[0136] 尽管上面已经示出和描述了本发明的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本发明的限制,本领域的普通技术人员在本发明的范围内可以对上述实施例进行改动、修改、替换和变型。
QQ群二维码
意见反馈