CN117806333A - 复杂环境中保持可见性的目标追踪轨迹规划方法及装置 - Google Patents
复杂环境中保持可见性的目标追踪轨迹规划方法及装置 Download PDFInfo
- Publication number
- CN117806333A CN117806333A CN202410223976.9A CN202410223976A CN117806333A CN 117806333 A CN117806333 A CN 117806333A CN 202410223976 A CN202410223976 A CN 202410223976A CN 117806333 A CN117806333 A CN 117806333A
- Authority
- CN
- China
- Prior art keywords
- target
- robot
- probability
- planning
- complex environment
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 78
- 238000001914 filtration Methods 0.000 claims abstract description 16
- 238000005457 optimization Methods 0.000 claims abstract description 13
- 238000004422 calculation algorithm Methods 0.000 claims description 16
- 239000011159 matrix material Substances 0.000 claims description 16
- 230000008569 process Effects 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000009826 distribution Methods 0.000 claims description 8
- 230000000007 visual effect Effects 0.000 claims description 4
- 230000001186 cumulative effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 238000013519 translation Methods 0.000 claims description 3
- 230000002411 adverse Effects 0.000 abstract description 4
- 230000006870 function Effects 0.000 description 18
- 238000005516 engineering process Methods 0.000 description 6
- 230000001133 acceleration Effects 0.000 description 4
- 230000008447 perception Effects 0.000 description 4
- 230000036461 convulsion Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000002474 experimental method Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 241000282472 Canis lupus familiaris Species 0.000 description 1
- 238000000342 Monte Carlo simulation Methods 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004590 computer program Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 238000011144 upstream manufacturing Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明属于控制***领域，为了解决现有技术无法做到实时轨迹规划的问题，本发明提供了复杂环境中保持可见性的目标追踪轨迹规划方法及装置，包括：机器人通过板载传感器检测目标，获得对目标的观测值；通过扩展卡尔曼滤波，获得对目标状态的估计；通过历史的目标状态，预测当前以及未来目标运动的控制输入；采用模型预测控制MPC的框架，通过优化机器人未来N步的控制输入，来最小化目标函数，同时满足约束，并使用顺序凸规划的范式求解优化问题；通过底层的控制器执行轨迹规划中得到的当步控制指令，追踪目标。机器人能够在障碍物杂乱的环境及机器人运动和传感器的噪声较高的不利条件下，对运动高度随机的目标进行实时、安全、稳健的追踪。
Description
技术领域
本发明属于控制***领域，具体而言，涉及一种复杂环境中保持可见性的目标追踪轨迹规划方法及装置。
背景技术
使用自主无人***（如无人车、无人机等）对移动目标进行追踪已在各种重要领域中广泛应用，如无人驾驶汽车对移动汽车的跟踪、无人机航拍摄影、以及水下的目标监视等等。目标追踪技术集合了机器人对环境的感知、状态估计、运动规划和运动控制等技术。机器人需要通过板载的传感器对目标以及周围环境进行感知，并通过感知结果估计目标的真实状态（如位置、速度等），从而规划出无碰撞的目标追踪轨迹，最后，处理器通过执行控制算法，输出机器人驱动单元可执行的指令，从而驱使机器人沿着轨迹运动，实现对目标的追踪。
对目标可见性的保持一直是目标跟踪轨迹规划器的一大重要需求。现有技术中保持目标可见性的方案包括：
（1）一种无人机的轨迹规划器，应用于保持可见性的目标追踪问题。该轨迹规划技术流程包括以下几步：
预处理：根据栅格地图的信息，生成欧几里得符号距离场（Euclidean SignedDistance Field, ESDF），它是一个二维数组，存储二维栅格地图上每个栅格中心点到障碍物的最小距离。
轨迹规划：该方法使用B样条曲线表征目标的未来轨迹，将轨迹规划建模成一个无约束优化的问题，并调用拟牛顿法求解数值优化问题。优化的变量为B样条曲线的参数（又称控制点）和无人机的偏航角，优化的目标函数包括几项代价的加权：
1.距离代价。当目标距离机器人过近或过远时，该项代价增大。
2.角度代价。当目标与机器人位置连线与机器人偏航角的夹角过大时，该项代价增大。
3.遮挡代价。求解该代价时，通过在目标与机器人的连线上取等分点，以每个等分点为球心生成多个球，并以每个球到障碍物的距离作为遮挡代价，距离越小，说明遮挡代价越高。其中，每个球到障碍物的距离可通过球心到障碍物的距离求得，而后者能够直接从预先生成的ESDF中读取。
4.运动学可行性代价。当机器人相邻两步的速度之差、加速度之差、角速度之差、角加速度之差过大时，该项代价增大。
5.轨迹光滑性代价。当机器人相邻两步的加加速度（加速度的导数）之差，角加加速度（角加速度的导数）之差过大时，该项代价增大。
6.避障代价。当机器人距离障碍物过近时，该项代价增大。机器人到障碍物的距离同样可以直接从ESDF中读取。
上述技术的缺点包括：在轨迹规划时，假设机器人与目标的真值已知，并未***性地考虑目标定位的不确定性。而在现实追踪问题中，由于传感器的观测噪声以及机器人和目标运动的过程噪声的存在，机器人对目标位置的估计往往是不准确的。这会导致该技术在***不确定性较大的目标追踪过程中，被不准确的目标位置所误导，生成非最优、且不稳定的追踪轨迹，容易导致目标丢失。而且上述技术仅限用于无人机的目标追踪任务，拓展性较差。
（2）一种应用于目标搜索与追踪的机器人轨迹规划方法，该方法使用高斯分布建模目标运动的过程噪声和传感器的观测噪声，并使用一种卡尔曼滤波（Kalman Filter,KF）的变体对目标的位置进行状态估计。卡尔曼滤波分为两步：
1.预测步：根据目标当前位置的均值、方差以及其动力学模型，预测下一时刻目标位置的均值和方差。
2.更新步：根据机器人对目标的观测，更新目标位置的均值和方差。
该变体意为，当目标处于机器人的视野范围内时，执行卡尔曼滤波的预测和更新步；否则，只执行卡尔曼滤波的预测步。
上述方法将轨迹规划问题建模为一个模型预测控制（Model PredictiveControl, MPC）问题，通过优化机器人未来N步的控制输入，来最小化未来N步目标的不确定性，同时满足约束，并使用顺序凸规划（Sequential Convex Programming，SCP）的范式求解该优化问题。该优化问题具体解释如下：
1.目标函数：该问题使用未来N步目标位置概率分布的熵表征不确定性，并通过放缩的方式，使用目标概率分布的协方差矩阵的迹，以及机器人与目标的距离作为目标函数。
2.约束：机器人与目标状态预测的约束。即，机器人相邻时刻的位置满足其动力学约束；目标相邻时刻的位置均值和方差，要满足卡尔曼滤波的约束关系。
上述机器人轨迹规划方法考虑的是没有障碍物的自由空间，故在有障碍物的追踪任务中无法适用。而且其计算复杂度高，无法做到实时的轨迹规划。
发明内容
本申请实施例提供了一种复杂环境中保持可见性的目标追踪轨迹规划方法及装置，能够在高噪声环境中进行更稳定、高效的追踪。
第一方面，本申请提供了一种复杂环境中保持可见性的目标追踪轨迹规划方法，包括：S101，机器人通过板载传感器检测目标，获得对目标的观测值；S103，机器人通过扩展卡尔曼滤波，获得对目标状态的估计；S105，机器人通过历史的目标状态，预测当前以及未来目标运动的控制输入；S107，采用模型预测控制MPC的框架，通过优化机器人未来N步的控制输入，来最小化目标函数，同时满足约束，并使用顺序凸规划的范式求解优化问题；S109，机器人通过底层的控制器执行轨迹规划中得到的当步控制指令，追踪目标。
进一步，步骤S107中，模型预测控制MPC问题建模如下：
其中包含从时刻a到时刻b的所有时刻；目标置信状态/>为目标状态的概率分布，包括该时刻的均值/>和协方差/>；机器人的置信状态/>编码了机器人状态的均值/>和协方差/>；/>分别代表机器人置信状态预测函数和目标置信状态预测函数，/>代表k+i时刻机器人与第j个障碍物的碰撞风险。
进一步，在步骤S107的MPC问题中，目标函数采用在未来N步内机器人观测到目标的累积概率的负值：
，其中/>表示机器人在k时刻观测到目标的概率。
进一步，在步骤S107的MPC问题中，第一个约束式代表机器人状态预测的约束，其通过扩展卡尔曼滤波的预测步得出，即
其中是机器人运动学模型，/>是机器人运动学的雅克比矩阵，/>是机器人运动过程噪声的协方差矩阵。
进一步，在步骤S107的MPC问题中，第二个约束式通过EKF变体计算，将可见概率引入EKF的框架，目标状态预测方法包括：
预测步：
其中是目标运动学模型，/>是目标运动学的雅克比矩阵，/>是目标过程噪声的协方差矩阵；
估计可见概率；
更新步：
是机器人观测模型的雅克比矩阵，/>分别是传感器噪声的协方差矩阵。
进一步，在步骤S107的MPC问题中，第四个约束式是避障约束，采用机会约束的方式建模避障约束，即，要求机器人与图中障碍物碰撞的概率小于设定的阈值，即第四个约束式可化为/>。
进一步，求解可见概率的过程包括：将可见概率化为两类概率的乘积：一是目标在视野范围内的概率/>，二是机器人与目标的连线与障碍物不碰撞的概率/>，即
其中i代表第i个障碍物；
将表示成两个几何体之间符号距离大于或小于0的满足概率，即
其中sd表示符号距离函数，代表两个几何体之间的有向距离；分别是目标和机器人的位置，/>是机器人的视野范围；/>是视线，即机器人与目标位置的连线线段，/>是第i个障碍物。
进一步，针对，提出了统一的计算范式，流程如下：
通过GJK和EPA算法，求解两个几何体在均值/>处的符号距离以及/>上的最近点/>；
计算两点之间的单位法向量，以及两点分别在/>上的本地坐标/>；
对于任意的，假设/>之间的最近点的本地坐标不变，依然为/>，并通过旋转和平移变换求解得到/>在世界坐标系下的近似最近点坐标/>；
将之间近似的SDF解析表达为/>；
将在/>的均值处做泰勒展开，得到SDF关于/>的线性近似式/>；
使用线性不等式满足概率的求解公式：
来求解目标概率。
第二方面，本申请提供了一种复杂环境中保持可见性的目标追踪轨迹规划装置，该装置被配置为：
机器人通过板载传感器检测目标，获得对目标的观测值；机器人通过扩展卡尔曼滤波，获得对目标状态的估计；机器人通过历史的目标状态，预测当前以及未来目标运动的控制输入；采用模型预测控制MPC的框架，通过优化机器人未来N步的控制输入，来最小化目标函数，同时满足约束，并使用顺序凸规划的范式求解优化问题；机器人通过底层的控制器执行轨迹规划中得到的当步控制指令，追踪目标。
第三方面，本申请提供了一种机器人，包括轨迹规划器，轨迹规划器被配置为执行上述任一项所述复杂环境中保持可见性的目标追踪轨迹规划方法。
本申请实施例复杂环境中保持可见性的目标追踪轨迹规划方法及装置具有如下有益效果：
采用本发明提出的轨迹规划方法，机器人能够在障碍物杂乱的环境及机器人运动和传感器的噪声较高的不利条件下，对运动高度随机的目标进行实时、安全、稳健的追踪。
附图说明
图1为本申请实施例复杂环境中保持可见性的目标追踪轨迹规划方法流程示意图；
图2为本申请实施例复杂环境中保持可见性的目标追踪轨迹规划方法另一种流程示意图；
图3为本申请实施例中机器人与目标的状态预测示意图；
图4为使用本申请的范式求解的流程图。
具体实施方式
下面结合附图和实施例对本申请进行进一步的介绍。
下述介绍提供了本发明的多个实施例，不同实施例之间可以替换或者合并组合，因此本申请也可认为包含所记载的相同和/或不同实施例的所有可能组合。因而，如果一个实施例包含特征A、B、C，另一个实施例包含特征B、D，那么本申请也应视为包括含有特征A、B、C、D的一个或多个所有其他可能的组合的实施例，尽管该实施例可能并未在以下内容中有明确的文字记载。
使用自主无人***（如无人车、无人机等）对移动目标进行追踪已在各种重要领域中广泛应用，如无人驾驶汽车对移动汽车的跟踪、无人机航拍摄影、以及水下的目标监视等等。目标追踪技术集合了机器人对环境的感知、状态估计、运动规划和运动控制等技术。机器人需要通过板载的传感器对目标以及周围环境进行感知，并通过感知结果估计目标的真实状态（如位置、速度等），从而规划出无碰撞的目标追踪轨迹，最后，处理器通过执行控制算法，输出机器人驱动单元可执行的指令，从而驱使机器人沿着轨迹运动，实现对目标的追踪。
对目标可见性的保持一直是目标跟踪轨迹规划器的一大重要需求。“保持可见性”要求机器人在算法层面上考虑实际应用中影响可见性的因素，如传感器有限的观测范围、复杂环境中障碍物对视线的遮挡等，从而生成特定的轨迹来确保对目标连续不间断的观测。本申请的方案关注的便是保持可见性的轨迹规划问题，旨在***性地考虑上述影响追踪效果的不利因素。
目前，目标追踪轨迹规划器考虑的通常都是确定性***，而不完美的***模型和过程噪声等因素，***的不确定性广泛存在于实际的机器人应用中，并可能降低跟踪任务中状态估计的精确度和轨迹的最优性。为了克服这一技术上的缺陷，本发明在概率空间中***地考虑了影响目标追踪可见性的因素：有限的观测范围和障碍物遮挡，并提出了一种考虑目标可见性的轨迹规划器，用于在杂乱环境中安全地跟踪移动的目标。通过仿真实验和实际实验的测试，证明了本申请的方案相比基于确定性***的轨迹规划器，能够在高噪声环境中进行更稳定、高效的追踪。
本发明考虑的是在障碍物杂乱的二维环境中移动目标跟踪问题。假设机器人知道自己精确的状态（如位置、朝向等，这在实际任务中可通过定位***如GPS等获取）。机器人依赖一个观测范围有限且带噪声的传感器来检测随机移动的目标。因此，机器人无法精确知道目标状态（如位置、朝向等），目标状态需进行状态估计。此外，机器人和目标的运动模型都具有过程噪声，这增加了目标跟踪的难度。机器人的目标是规划一条无碰撞的轨迹，以保持对目标的持续检测。
符号定义：分别代表机器人和目标的真实状态，/>分别代表机器人和目标的控制输入。下标r代表机器人（robot），t代表目标（target）。采用离散的时间间隔，即真实的追踪进行时间/>，/>为时间采样间隔。
如图1所示，本申请复杂环境中保持可见性的目标追踪轨迹规划方法包括：S101，机器人通过板载传感器检测目标，获得对目标的观测值；S103，机器人通过扩展卡尔曼滤波，获得对目标状态的估计；S105，机器人通过历史的目标状态，预测当前以及未来目标运动的控制输入；S107，采用模型预测控制MPC的框架，通过优化机器人未来N步的控制输入，来最小化目标函数，同时满足约束，并使用顺序凸规划的范式求解优化问题；S109，机器人通过底层的控制器执行轨迹规划中得到的当步控制指令，追踪目标。
如图1-2所示，在每个时刻，目标追踪流程分为以下几步：
1. 感知。机器人通过板载传感器检测目标，获得对目标的观测值。
2. 状态估计。机器人通过扩展卡尔曼滤波，获得对目标状态的估计。
3. 控制输入估计与预测。机器人通过历史的目标状态，预测当前以及未来目标运动的控制输入。
4. 轨迹规划。采用模型预测控制MPC的框架，通过优化机器人未来N步的控制输入，来最小化目标函数，同时满足约束，并使用顺序凸规划的范式求解该优化问题。
5. 运动控制。机器人通过底层的控制器执行轨迹规划中得到的当步控制指令，追踪目标。本申请采用离散时间模型，例如现在是时刻k，然后通过轨迹规划算法得到了k,k+1,k+2,...,k+N-1时刻的控制输入，在实际执行中只采用k时刻（这个就是当步的意思）的控制输入。
在感知中，我们考虑的是一个通用的观测模型，并不局限于上游传感器的形式。例如，对于相机传感器，观测值可以设置为目标到机器人的距离、方向角以及目标自己的朝向，这些数据都可以通过一些现有的感知算法获取。我们记第k时刻获得的观测为。
在状态估计步，采用扩展卡尔曼滤波器，得到目标状态的均值和方差/>。
在控制输入的估计与预测步，首先通过一个历史窗口的控制输入加权平均得到当前步的控制输入，再通过线性二次高斯控制器，计算一个从前一步目标状态到当前步状态估计后目标状态的最优控制输入，再将二者加权得到当前步的控制输入。将当前步的控制输入作为未来N步控制输入的预测。
轨迹规划是本发明的核心贡献。本发明模型预测控制MPC问题建模如下：
其中包含从时刻a到时刻b的所有时刻；目标置信状态/>为目标状态的概率分布，包括该时刻的均值/>和协方差/>；机器人的置信状态/>编码了机器人状态的均值/>和协方差/>；/>分别代表机器人置信状态预测函数和目标置信状态预测函数，/>代表k+i时刻机器人与第j个障碍物的碰撞风险。
在上述MPC问题中，目标函数采用在未来N步内机器人观测到目标的累积概率的负值：
，其中/>表示机器人在k时刻观测到目标的概率。
在上述MPC问题中，第一个约束式代表机器人状态预测的约束，其通过扩展卡尔曼滤波的预测步得出，即
其中是机器人运动学模型，/>是机器人运动学的雅克比矩阵，/>是机器人运动过程噪声的协方差矩阵。
在上述MPC问题中，如图3所示，第二个约束式通过发明人开发的一种EKF变体计算。直观上说，在未来预测的时域中，机器人看见目标的概率越大，目标的不确定性应该越小。因此，将可见概率引入EKF的框架，提出如下的目标状态预测方案：
a）预测步：
其中是目标运动学模型，/>是目标运动学的雅克比矩阵，/>是目标过程噪声的协方差矩阵；
b）估计可见概率；
c）更新步：
是机器人观测模型的雅克比矩阵，/>分别是传感器噪声的协方差矩阵。
在上述MPC问题中，第四个约束式是避障约束，采用机会约束（chance constraint）的方式建模避障约束，即，要求机器人与图中障碍物碰撞的概率小于设定的阈值，即第四个约束式可化为/>。
在实际应用中，求解可见概率涉及不规则区域的积分，计算复杂度较高，无法满足实时规划需求。本发明提出了一种可见概率的分解方法：将可见概率化为两类概率的乘积：一是目标在视野范围内的概率/>，二是机器人与目标的连线（称为视线）与障碍物不碰撞的概率/>，即
其中i代表第i个障碍物。
计算的方法：本发明提出了基于符号距离函数表征如上概率的方法，即将它们全部表示成两个几何体之间符号距离大于（或小于）0的满足概率，即
其中sd表示符号距离函数，代表两个几何体之间的有向距离；分别是目标和机器人的位置，/>是机器人的视野范围；/>是视线，即机器人与目标位置的连线线段，/>是第i个障碍物。
针对这种概率（），本申请提出了一种统一的计算范式，算法1如下。
算法1：符号距离满足概率计算：
a) 输入：两个几何体，它们由一个高斯分布随机变量/>决定。
b) 输出：的符号距离小于等于0的概率。
流程：
a)通过GJK和EPA算法，求解两个几何体在均值/>处的符号距离以及上的最近点/>；
b)计算两点之间的单位法向量，以及两点分别在/>上的本地坐标/>；
c)对于任意的，假设/>之间的最近点的本地坐标不变，依然为/>，并通过旋转和平移变换求解得到/>在世界坐标系下的近似最近点坐标/>。
d)将之间近似的SDF解析表达为/>；
e)将在/>的均值处做泰勒展开，得到SDF关于/>的线性近似式/>；
f)使用线性不等式满足概率的求解公式：
来求解目标概率。
如图4所示，使用该范式求解，/>可直接通过算法1计算得出。对于可见概率/>，先分别调用算法1，求解出目标在视野范围内的概率/>和不被第/>个障碍物遮挡的概率/>，再进行累乘得到/>。至此，MPC问题便已可解。于是可以使用SCP的框架求解该问题，得到最优的控制输入。采用的解法器是MOSEK。
经过大量的仿真和实际实验，证明：采用本发明提出的轨迹规划方法，机器人能够在障碍物杂乱的环境及机器人运动和传感器的噪声较高的不利条件下，对运动高度随机的目标进行实时、安全、稳健的追踪。本发明能达成上述效果的机理分析如下：
实时性：本发明提出的轨迹规划器单步规划速度约为0.1s，满足机器人实时的规划需求。本方法的实时性主要来源于算法1的高效性。本申请的算法1执行一次仅需不到1ms，这比蒙特卡洛模拟的方法快了100倍左右。
安全：本申请的算法具有较高的安全性保障，这是因为，采用机会约束进行避障，能够保证求解的轨迹与障碍物的碰撞概率足够小。
稳健：由于本文使用高斯分布建模机器人、目标的运动噪声以及传感器噪声，并从概率的意义下最大化对目标的可见性。因此，相比确定性的轨迹规划方法，本发明能够更好地考虑***不确定性对状态估计、轨迹规划的影响，从而生成更稳定的目标轨迹。
求解符号距离和最近点时，除了本发明采用的GJK和EPA算法，在固定地图下，也可以通过先求解欧式符号距离场ESDF，再在规划中读取SDF来实现。状态估计模块，除了扩展卡尔曼滤波，其他滤波器，如无迹卡尔曼滤波（UKF）等，也能实现状态估计功能。本发明提出的是较为通用的轨迹规划方法，能够适用于多种机器人类型（无人车、无人机、机器狗等）以及传感器类型（毫米波雷达、相机等）。本申请中的优化问题可用许多主流的求解器进行求解，如GUROBI，MOSEK，CPLEX等
本申请还提出一种复杂环境中保持可见性的目标追踪轨迹规划装置，该装置被配置为：机器人通过板载传感器检测目标，获得对目标的观测值；机器人通过扩展卡尔曼滤波，获得对目标状态的估计；机器人通过历史的目标状态，预测当前以及未来目标运动的控制输入；采用模型预测控制MPC的框架，通过优化机器人未来N步的控制输入，来最小化目标函数，同时满足约束，并使用顺序凸规划的范式求解优化问题；机器人通过底层的控制器执行轨迹规划中得到的当步控制指令，追踪目标。
本申请中，复杂环境中保持可见性的目标追踪轨迹规划装置实施例与复杂环境中保持可见性的目标追踪轨迹规划方法实施例基本相似，相关之处请参考复杂环境中保持可见性的目标追踪轨迹规划方法实施例的介绍。
本申请还提供了一种机器人，包括轨迹规划器，轨迹规划器被配置为执行上述任一项复杂环境中保持可见性的目标追踪轨迹规划方法。
本发明实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述复杂环境中保持可见性的目标追踪轨迹规划方法步骤。其中，计算机可读存储介质可以包括但不限于任何类型的盘，包括软盘、光盘、DVD、CD-ROM、微型驱动器以及磁光盘、ROM、RAM、EPROM、EEPROM、DRAM、VRAM、闪速存储器设备、磁卡或光卡、纳米***（包括分子存储器IC），或适合于存储指令和/或数据的任何类型的媒介或设备。
以上介绍仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
Claims (10)
1.一种复杂环境中保持可见性的目标追踪轨迹规划方法，其特征在于，包括：S101，机器人通过板载传感器检测目标，获得对目标的观测值；S103，机器人通过扩展卡尔曼滤波，获得对目标状态的估计；S105，机器人通过历史的目标状态，预测当前以及未来目标运动的控制输入；S107，采用模型预测控制MPC的框架，通过优化机器人未来N步的控制输入，来最小化目标函数，同时满足约束，并使用顺序凸规划的范式求解优化问题；S109，机器人通过底层的控制器执行轨迹规划中得到的当步控制指令，追踪目标。
2.根据权利要求1所述复杂环境中保持可见性的目标追踪轨迹规划方法，其特征在于，步骤S107中，模型预测控制MPC问题建模如下：
其中包含从时刻a到时刻b的所有时刻；目标置信状态/>为目标状态的概率分布，包括该时刻的均值/>和协方差/>；机器人的置信状态/>编码了机器人状态的均值/>和协方差/>；/>分别代表机器人置信状态预测函数和目标置信状态预测函数，/>代表k+i时刻机器人与第j个障碍物的碰撞风险。
3.根据权利要求2所述复杂环境中保持可见性的目标追踪轨迹规划方法，其特征在于，在步骤S107的MPC问题中，目标函数采用在未来N步内机器人观测到目标的累积概率的负值：
，其中/>表示机器人在k时刻观测到目标的概率。
4.根据权利要求2所述复杂环境中保持可见性的目标追踪轨迹规划方法，其特征在于，在步骤S107的MPC问题中，第一个约束式代表机器人状态预测的约束，其通过扩展卡尔曼滤波的预测步得出，即
其中是机器人运动学模型，/>是机器人运动学的雅克比矩阵，/>是机器人运动过程噪声的协方差矩阵。
5.根据权利要求2所述复杂环境中保持可见性的目标追踪轨迹规划方法，其特征在于，在步骤S107的MPC问题中，第二个约束式通过EKF变体计算，将可见概率引入EKF的框架，目标状态预测方法包括：
预测步：
其中是目标运动学模型，/>是目标运动学的雅克比矩阵，/>是目标过程噪声的协方差矩阵；
估计可见概率；
更新步：
是机器人观测模型的雅克比矩阵，/>分别是传感器噪声的协方差矩阵。
6. 根据权利要求2所述复杂环境中保持可见性的目标追踪轨迹规划方法，其特征在于，在步骤S107的MPC问题中，第四个约束式是避障约束，采用机会约束的方式建模避障约束，即，要求机器人与图中障碍物碰撞的概率小于设定的阈值，即第四个约束式可化为。
7.根据权利要求5所述复杂环境中保持可见性的目标追踪轨迹规划方法，其特征在于，求解可见概率的过程包括：将可见概率化为两类概率的乘积：一是目标在视野范围内的概率/>，二是机器人与目标的连线与障碍物不碰撞的概率/>，即
其中i代表第i个障碍物；
将表示成两个几何体之间符号距离大于或小于0的满足概率，即
其中sd表示符号距离函数，代表两个几何体之间的有向距离；分别是目标和机器人的位置，/>是机器人的视野范围；/>是视线，即机器人与目标位置的连线线段，是第i个障碍物。
8.根据权利要求7所述复杂环境中保持可见性的目标追踪轨迹规划方法，其特征在于，针对，提出了统一的计算范式，流程如下：
通过GJK和EPA算法，求解两个几何体在均值/>处的符号距离以及/>上的最近点/>；
计算两点之间的单位法向量，以及两点分别在/>上的本地坐标/>；
对于任意的，假设/>之间的最近点的本地坐标不变，依然为/>，并通过旋转和平移变换求解得到/>在世界坐标系下的近似最近点坐标/>；
将之间近似的SDF解析表达为/>；
将在/>的均值处做泰勒展开，得到SDF关于/>的线性近似式/>；
使用线性不等式满足概率的求解公式：
来求解目标概率。
9.一种复杂环境中保持可见性的目标追踪轨迹规划装置，其特征在于，该装置被配置为：
机器人通过板载传感器检测目标，获得对目标的观测值；机器人通过扩展卡尔曼滤波，获得对目标状态的估计；机器人通过历史的目标状态，预测当前以及未来目标运动的控制输入；采用模型预测控制MPC的框架，通过优化机器人未来N步的控制输入，来最小化目标函数，同时满足约束，并使用顺序凸规划的范式求解优化问题；机器人通过底层的控制器执行轨迹规划中得到的当步控制指令，追踪目标。
10.一种机器人，包括轨迹规划器，其特征在于，所述轨迹规划器被配置为执行权利要求1-8中任一项所述复杂环境中保持可见性的目标追踪轨迹规划方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410223976.9A CN117806333B (zh) | 2024-02-29 | 2024-02-29 | 复杂环境中保持可见性的目标追踪轨迹规划方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410223976.9A CN117806333B (zh) | 2024-02-29 | 2024-02-29 | 复杂环境中保持可见性的目标追踪轨迹规划方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117806333A true CN117806333A (zh) | 2024-04-02 |
| CN117806333B CN117806333B (zh) | 2024-05-24 |
Family
ID=90423761
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410223976.9A Active CN117806333B (zh) | 2024-02-29 | 2024-02-29 | 复杂环境中保持可见性的目标追踪轨迹规划方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117806333B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105929844A (zh) * | 2016-04-26 | 2016-09-07 | 哈尔滨工业大学 | 一种地外天体软着陆多障碍约束环境下避障方法 |
| US20210078592A1 (en) * | 2019-09-14 | 2021-03-18 | Honda Motor Co., Ltd. | Trajectory planner |
| CN112923925A (zh) * | 2021-01-07 | 2021-06-08 | 天津大学 | 双模式盘旋追踪地面目标的多无人机协同航迹规划方法 |
| US20210191404A1 (en) * | 2018-02-28 | 2021-06-24 | Five AI Limited | Path planning in mobile robots |
| CN114594785A (zh) * | 2022-01-17 | 2022-06-07 | 北京理工大学 | 基于混合整数二阶锥优化的无人机避障实时轨迹规划方法 |
| CN115509248A (zh) * | 2022-10-09 | 2022-12-23 | 哈尔滨工业大学 | 一种基于序列凸优化和模型预测控制的卫星集群重构控制方法 |
| CN116627154A (zh) * | 2023-06-09 | 2023-08-22 | 上海大学 | 基于位姿预测和轨迹优化的无人机引导降落方法及无人机 |
| CN117369495A (zh) * | 2023-10-16 | 2024-01-09 | 天津大学 | 一种基于模型预测控制的无人机编队轨迹规划方法 |
| CN117401187A (zh) * | 2023-10-12 | 2024-01-16 | 哈尔滨工业大学 | 一种基于代数条件的复杂外形航天器碰撞规避控制方法 |
| CN117472066A (zh) * | 2023-12-27 | 2024-01-30 | 成都流体动力创新中心 | 一种航向角速度局部最优的避障控制方法 |
-
2024
- 2024-02-29 CN CN202410223976.9A patent/CN117806333B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105929844A (zh) * | 2016-04-26 | 2016-09-07 | 哈尔滨工业大学 | 一种地外天体软着陆多障碍约束环境下避障方法 |
| US20210191404A1 (en) * | 2018-02-28 | 2021-06-24 | Five AI Limited | Path planning in mobile robots |
| US20210078592A1 (en) * | 2019-09-14 | 2021-03-18 | Honda Motor Co., Ltd. | Trajectory planner |
| CN112923925A (zh) * | 2021-01-07 | 2021-06-08 | 天津大学 | 双模式盘旋追踪地面目标的多无人机协同航迹规划方法 |
| CN114594785A (zh) * | 2022-01-17 | 2022-06-07 | 北京理工大学 | 基于混合整数二阶锥优化的无人机避障实时轨迹规划方法 |
| CN115509248A (zh) * | 2022-10-09 | 2022-12-23 | 哈尔滨工业大学 | 一种基于序列凸优化和模型预测控制的卫星集群重构控制方法 |
| CN116627154A (zh) * | 2023-06-09 | 2023-08-22 | 上海大学 | 基于位姿预测和轨迹优化的无人机引导降落方法及无人机 |
| CN117401187A (zh) * | 2023-10-12 | 2024-01-16 | 哈尔滨工业大学 | 一种基于代数条件的复杂外形航天器碰撞规避控制方法 |
| CN117369495A (zh) * | 2023-10-16 | 2024-01-09 | 天津大学 | 一种基于模型预测控制的无人机编队轨迹规划方法 |
| CN117472066A (zh) * | 2023-12-27 | 2024-01-30 | 成都流体动力创新中心 | 一种航向角速度局部最优的避障控制方法 |
Non-Patent Citations (2)
| Title |
|---|
| 余家祥 等: "面向目标跟踪的传感器调度方法", 舰船电子工程, no. 12, 20 December 2011 (2011-12-20) * |
| 王林 等: "复杂环境下多无人机协作式地面移动目标跟踪", 控制理论与应用, no. 03, 15 March 2011 (2011-03-15) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117806333B (zh) | 2024-05-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Fulgenzi et al. | Probabilistic navigation in dynamic environment using rapidly-exploring random trees and gaussian processes | |
| Ragi et al. | UAV path planning in a dynamic environment via partially observable Markov decision process | |
| US8706298B2 (en) | Temporal tracking robot control system | |
| Steyer et al. | Grid-based object tracking with nonlinear dynamic state and shape estimation | |
| Ding et al. | New multiple-target tracking strategy using domain knowledge and optimization | |
| Moratuwage et al. | Collaborative multi-vehicle SLAM with moving object tracking | |
| Rohrmuller et al. | Probabilistic mapping of dynamic obstacles using markov chains for replanning in dynamic environments | |
| Tian et al. | Multi-vehicle tracking using an environment interaction potential force model | |
| Chen et al. | Risk-aware trajectory sampling for quadrotor obstacle avoidance in dynamic environments | |
| Ćesić et al. | Detection and tracking of dynamic objects using 3D laser range sensor on a mobile platform | |
| Farmani et al. | Optimal UAV sensor management and path planning for tracking multiple mobile targets | |
| Ahmad et al. | End-to-end probabilistic depth perception and 3d obstacle avoidance using pomdp | |
| GB2564897A (en) | Method and process for motion planning in (un-)structured environments with pedestrians and use of probabilistic manifolds | |
| Hewawasam et al. | Comparative study on object tracking algorithms for mobile robot navigation in gps-denied environment | |
| Calkins et al. | Bearing-only active sensing under merged measurements | |
| Doellinger et al. | Environment-aware multi-target tracking of pedestrians | |
| CN117806333B (zh) | 复杂环境中保持可见性的目标追踪轨迹规划方法及装置 | |
| Quinones-Ramirez et al. | Robot path planning using deep reinforcement learning | |
| Fabiani et al. | Tracking an unpredictable target among occluding obstacles under localization uncertainties | |
| Ding et al. | Moving object tracking in support of unmanned vehicle opeartion | |
| Cui et al. | A Robust and Efficient UAV Path Planning Approach for Tracking Agile Targets in Complex Environments. Machines 2022, 10, 931 | |
| Sajjadi et al. | A Robust and Adaptive Sensor Fusion Approach for Indoor UAV Localization | |
| Abd Rahman et al. | Tracking uncertain moving objects using dynamic track management in multiple hypothesis tracking | |
| Lee et al. | QP Chaser: Polynomial Trajectory Generation for Autonomous Aerial Tracking | |
| Skoglar | UAV path and sensor planning methods for multiple ground target search and tracking-A literature survey |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |