简介路径规划是机器人自主导航的核心技术之一。当环境中存在雷达探测区、禁飞区等威胁区域时路径不仅要避免碰撞还需满足动力学约束和实时性要求。这类问题天然具有非凸性直接求解困难。凸优化凭借全局最优解和高效求解能力成为处理此类问题的有力工具。本文从凸优化的基本原理出发介绍将威胁规避约束转化为凸问题的三种工程方法惩罚函数、凸安全走廊和混合整数凸优化。重点展示一种基于A*粗路径生成凸走廊再以CVXPY建模、OSQP求解二次规划QP的完整流程使无人机在满足速度、加速度限制的同时安全绕开威胁区。该方法计算速度快、稳定性好适用于无人机离线规划与在线重规划场景。同时总结了仿真中走廊不连通、目标函数非凸等易出错的细节为路径规划工程实践提供参考。 前两天整理硬盘翻出一个标注着“使用凸优化在存在威胁区域的情况下对无人机进行路径规划.zip”的压缩包那是之前复现过的路径规划项目。压缩包不算大但里面夹杂着论文截图、MATLAB代码和一份放飞自我的实验结果记录。今天想把这套东西的来龙去脉摊开说清楚所谓威胁区域下的无人机路径规划到底是怎么把“别飞进雷达探测区、禁飞区”这种躲猫猫要求翻译成一个凸优化问题并求解的。如果你是做机器人或无人机路径规划的学生、工程师或者想了解凸优化在真实场景里怎么落地这篇东西应该能给你一些参考。1. 威胁区把问题变成了什么从非凸约束到可求解模型1.1 问题定义不是“绕开障碍物”这么简单无人机路径规划最常见说法是找一条从起点到终点的路径不碰障碍物。但一旦把“威胁区域”放进问题里目标往往就变了不只是“不碰”而是“尽量别靠近”或者“穿过威胁区的暴露时间要短”。这个差别很重要直接决定了用什么优化框架。威胁区域在工程上通常被抽象成圆形或凸多边形。比如一个圆形威胁区可以写成C_j { p : ||p - c_j|| r_j }其中因为一旦进入这个区域代价很大甚至直接失败所以路径需要尽可能避开。但与此同时你还要让路径尽量短、飞行时间尽量少、控制能量尽量小。于是最自然的数学表达是输入起点 p0终点 pT威胁区集合 {C_j}输出连续轨迹 γ(t)目标最小化飞行时间、路径长度或威胁暴露约束γ(0)p0γ(T)pT且对所有 t 和 j 满足 γ(t) ∉ C_j。这里最大的难点就藏在最后一条约束里。||p - c_j|| r_j左边是凸函数但约束是“凸函数大于等于常数”。它的可行集是圆的外部而圆的外部不是凸集。你站在航线上看这条约束等于是在整个平面上挖了一个洞洞周围的可行域是弯折的凹进去的。严格说整个问题是非凸的。1.2 凸优化的“真香”来自哪里那为什么大家还是前赴后继地用凸优化答案非常实际一旦问题能写成凸优化求解器给的是全局最优解而且计算快、稳定性好。凸优化的两个核心条件是目标函数是凸函数可行域是凸集。凸函数只有一个全局最低点没有一堆局部极小值来坑你。你可以把它想象成一个只有单个底部的大碗从碗沿任何位置松手球都会滚到同一个碗底。非凸问题则像布满小坑的高尔夫球场球很容易滚进某个浅坑就出不来你甚至不知道这个坑是不是最深的。对于无人机这种讲究实时性的场景这个性质太重要了。离线规划慢一点没关系在线重规划必须毫秒级。凸优化配合 OSQP、Gurobi 这类成熟求解器很多小规模问题能在几毫秒内求解。这对后续做机载计算、动态重规划都很有价值。1.3 威胁区的“洞”必须另想办法所以想在保持凸优化框架的同时处理威胁区核心思路不是“用凸优化直接解非凸约束”而是“把问题重新表述成凸问题”。常见的手段我后面会展开这里先提三个最主流的方向把威胁区变成惩罚项塞进目标函数把自由空间切割成若干个凸胞让路径在凸胞内走引入 0/1 整数变量判断路径从威胁区的哪一侧绕过去。我以前也幼稚地直接把非凸约束写进 CVXPY结果求解器直接报错或者给出一堆没法看的轨迹。后来才明白所谓“用凸优化做带威胁区的路径规划”几乎都是用了某种改造术而不是硬刚。2. 我为什么没有直接从障碍物边缘做几何规划2.1 几何规划看起来很直接但坑不少最早我拿到这个项目时第一反应是干嘛不用 A* 或者 RRTA* 在栅格地图上搜一条避障路径然后用 B 样条平滑不是也能飞吗确实能飞但做了一段时间之后我发现了几个很烦的问题。第一几何搜索只得到一条折线路径完全不考虑无人机动力学。四旋翼有最大速度、最大加速度、最小转弯半径一条在几何上很漂亮的路径可能根本飞不出来。第二平滑阶段容易把路径拉回威胁区。很多平滑算法只关心曲线光顺不感知障碍物。B 样条一拉很容易把路径拉进禁飞区你还得再去做碰撞检测重新修。第三几何路径很难处理“威胁暴露最小”这种连续指标。如果目标是绕开某个圆形威胁区你可以做但如果目标是权衡路径长度和暴露风险几何搜索就力不从心了。第四是扩展性问题。威胁区一多栅格分辨率稍微提高一点A* 的搜索空间膨胀非常快RRT 虽能随机采样但结果每次都不一样而且不一定最优。我后来才意识到凸优化不是要去替代几何搜索而是应该和几何搜索配合先用几何方法找到一条粗略的拓扑路径再用凸优化在满足动力学和安全约束的前提下把轨迹磨平。2.2 三种把威胁区塞进凸优化框架的工程做法在项目里我实际对比了三种主流的改造方式每种都有适用场景。方法核心思想凸性优点问题惩罚函数法把威胁区作为目标函数中的惩罚项离威胁越近代价越大通常需要额外处理才能保持凸实现简单能表达风险权衡容易陷入局部最优权重难调凸安全走廊法将自由空间分解为凸多边形把轨迹约束在走廊内严格凸走廊约束为线性求解稳定适合大场景走廊生成质量直接影响结果混合整数凸优化用 0/1 变量表示从威胁区哪一侧通过用大 M 法线性化整数凸混合整数凸优化表达能力强模型精确小规模可用大规模组合爆炸惩罚函数法最直接。比如你可以在目标函数里加一项J w / (||p - c_j||^2 epsilon)但要注意这个函数在威胁区边缘变化很剧烈直接放进 QP 里大概率不是凸函数。项目里有一个版本就是这么干的结果求解器经常报“problem is not DCP”要不就是路径在威胁区边缘反复震荡。后来我把惩罚项改成线性近似配合信赖域约束才勉强能用。凸安全走廊法是我个人认为最工程化的方案。它的思路分三步用 A* 或者 Dijkstra 在栅格地图上找一条粗路径沿着粗路径生成一串有重叠的凸多边形这些多边形就是安全走廊把无人机运动学离散化求解一个二次规划 QP位置约束是“每个时间步落在对应的凸多边形内”。这套流程的好处是非凸的避障约束被走廊“消化”了。走廊本身是凸的路径又被限制在走廊里所以避障自然满足。剩下的全是线性约束和二次目标QS 又快又稳。混合整数凸优化则更适合小规模、强约束的场景。比如只有两三个威胁区你希望精确建模“路径在威胁区左边还是右边、上边还是下边”。这时候引入 0/1 变量用大 M 法把“不能在威胁区内部”转成一组互斥的线性约束。求解规模小的时候效果非常好但威胁区一多整数变量组合爆炸求解时间指数上升。2.3 我在这个 zip 项目里最终采用的流程翻完压缩包里的代码我发现作者并没有用惩罚函数的那种粗暴写法而是走了一条“A* 粗路径 凸走廊生成 QP 轨迹平滑”的路线。整套数据流是这样的地图上标出威胁区栅格化并对威胁区做膨胀处理A* 搜索一条从起点到终点、完全不碰威胁区的粗路径沿粗路径按照固定步长生成一系列凸多边形相邻多边形必须有重叠将时间轴分成多段每一段对应一个凸多边形把位置约束写成线性不等式用 CVXPY 建模OSQP 求解得到一条满足动力学约束的平滑轨迹。这套流程整体上很稳而且每个环节都能单独验证。我觉得这比强行在目标函数里加非凸惩罚要靠谱得多。3. 把动力学和威胁规避写成凸优化一个可跑的简化算例3.1 先建立二阶积分运动模型要真的跑通凸优化第一步是把无人机模型简化成适合优化的形式。这里我用的是多旋翼水平面内的二阶积分模型状态量是位置和速度x_k [px_k, py_k, vx_k, vy_k]^T控制量是加速度u_k [ax_k, ay_k]^T用前向欧拉法离散化时间步长 dtpx_{k1} px_k vx_k * dt 0.5 * ax_k * dt^2 py_{k1} py_k vy_k * dt 0.5 * ay_k * dt^2 vx_{k1} vx_k ax_k * dt vy_{k1} vy_k ay_k * dt这个模型在低速无人机场景下够用。如果你要飞战斗机一样的机动那得换成更复杂的状态方程但优化形式不变只是约束更多。优化目标我设为最小化控制能量也就是各个时间步加速度的平方和。这样轨迹不会出现不必要的急加减速也利于飞控跟踪。约束包括起点的位置、速度必须等于设定值终点的位置、速度必须等于设定值每个时间步的速度大小要小于最大值每个时间步的加速度大小要小于最大值每个时间步的位置必须落在对应的凸走廊内。前面四条都好理解最后一条是威胁规避的核心。因为威胁区已经在生成走廊时被排除了所以只要位置落在走廊内就一定不会进入威胁区。3.2 CVXPY 实现一个走廊约束下的轨迹优化这里我给出一个最简化的可运行示例。为了方便贴代码我把走廊简化为两个有重叠的凸矩形前一半时间位置落在左下矩形[0,6] x [0,6]后一半时间位置落在右上矩形[4,10] x [4,10]。这两个矩形的重叠区域[4,6] x [4,6]就是路径过渡的地方。import cvxpy as cp import numpy as np # 时间参数 N 40 # 时间步数 dt 0.15 # 时间步长 # 起点和终点位置、速度 p0 np.array([0.0, 0.0]) pT np.array([10.0, 10.0]) v0 np.array([0.0, 0.0]) vT np.array([0.0, 0.0]) # 变量定义 px cp.Variable(N) py cp.Variable(N) vx cp.Variable(N) vy cp.Variable(N) ax cp.Variable(N - 1) ay cp.Variable(N - 1) constraints [] # 初始和终端状态约束 constraints [px[0] p0[0], py[0] p0[1]] constraints [vx[0] v0[0], vy[0] v0[1]] constraints [px[N-1] pT[0], py[N-1] pT[1]] constraints [vx[N-1] vT[0], vy[N-1] vT[1]] # 动力学约束 for k in range(N - 1): constraints [px[k1] px[k] vx[k] * dt 0.5 * ax[k] * dt**2] constraints [py[k1] py[k] vy[k] * dt 0.5 * ay[k] * dt**2] constraints [vx[k1] vx[k] ax[k] * dt] constraints [vy[k1] vy[k] ay[k] * dt] # 走廊约束这里其实是两个凸矩形真实项目中来自上游算法 for k in range(N): if k N // 2: constraints [0 px[k], px[k] 6] constraints [0 py[k], py[k] 6] else: constraints [4 px[k], px[k] 10] constraints [4 py[k], py[k] 10] # 速度和加速度边界 constraints [cp.abs(vx) 2.5, cp.abs(vy) 2.5] constraints [cp.abs(ax) 2.0, cp.abs(ay) 2.0] # 目标函数最小化控制能量 objective cp.Minimize(cp.sum_squares(ax) cp.sum_squares(ay)) # 求解 prob cp.Problem(objective, constraints) prob.solve(solvercp.OSQP, verboseFalse) # 输出结果 if prob.status cp.OPTIMAL: print(求解成功控制能量 , prob.value) print(前 3 个位置, np.vstack([px.value[:3], py.value[:3]]).T) else: print(求解失败状态 , prob.status)注意真实项目中走廊约束不是这样手写的。这里的矩形只是为了让你直观看到“凸约束怎么加进去”。如果上游生成的走廊是一组凸多边形你只需要把每个时刻的约束换成对应的多边形不等式即可。3.3 求解结果和工程上的解读这段代码用 OSQP 求解通常几毫秒就能出结果。你得到的是每个时间步的位置、速度、加速度路径天然满足动力学约束和走廊约束。我第一次跑通的时候异常兴奋因为代码改一行就能让轨迹从直线变成绕过威胁区的曲线。但后来发现真正的挑战从来不是求解器而是你怎么把威胁区准确地变成约束。还有一个细节CVXPY 里面位置约束用的是“每个时间步必须在某个凸多边形内”但如果两个相邻时间步被分配到不同的凸多边形而两个多边形没有重叠那么这一步到下一步可能是跳变优化器会报不可行。所以生成走廊时相邻凸多边形一定要有重叠区。4. 仿真里最容易翻车的三个细节4.1 走廊不连通或太窄QP 直接报 infeasible这是我仿真中遇过最频繁的问题。A* 粗路径往往贴着威胁区边缘走如果你沿着粗路径用固定宽度生成凸多边形在某些拐弯处两个多边形可能只连接在一个点上QP 中就会出现“前一步还在这个区域下一步必须跳到那个区域”的矛盾约束最终求解器告诉你 infeasible。调试方法很简单把每个时刻的走廊约束单独打印出来看看相邻时间点的位置可行域是否有交集。如果没有说明走廊生成环节出问题了。我的实践是在生成走廊时给每个凸多边形加 20% 的膨胀余量强制相邻多边形有足够重叠。膨胀多了会导致路径绕路但至少求解稳定。之后再逐步缩小膨胀系数。4.2 目标函数里乱加威胁暴露项结果反而更危险有人希望路径离威胁区中心越远越好于是把威胁暴露代价直接写进 QP 目标比如J w / (||p - c_j||^2 epsilon)这个想法很自然但问题在于这个函数在威胁区附近是高度非凸的。你把它加进 QP目标函数变成非凸求解器要么直接报错要么给你一个局部最优解。更可怕的是某些求解器强行求解给出的结果看起来没问题实际路径却可能在威胁区边缘画圈比不加还危险。稳妥的做法本文还有配套的精品资源点击获取
简介路径规划是机器人自主导航的核心技术之一。当环境中存在雷达探测区、禁飞区等威胁区域时路径不仅要避免碰撞还需满足动力学约束和实时性要求。这类问题天然具有非凸性直接求解困难。凸优化凭借全局最优解和高效求解能力成为处理此类问题的有力工具。本文从凸优化的基本原理出发介绍将威胁规避约束转化为凸问题的三种工程方法惩罚函数、凸安全走廊和混合整数凸优化。重点展示一种基于A*粗路径生成凸走廊再以CVXPY建模、OSQP求解二次规划QP的完整流程使无人机在满足速度、加速度限制的同时安全绕开威胁区。该方法计算速度快、稳定性好适用于无人机离线规划与在线重规划场景。同时总结了仿真中走廊不连通、目标函数非凸等易出错的细节为路径规划工程实践提供参考。 前两天整理硬盘翻出一个标注着“使用凸优化在存在威胁区域的情况下对无人机进行路径规划.zip”的压缩包那是之前复现过的路径规划项目。压缩包不算大但里面夹杂着论文截图、MATLAB代码和一份放飞自我的实验结果记录。今天想把这套东西的来龙去脉摊开说清楚所谓威胁区域下的无人机路径规划到底是怎么把“别飞进雷达探测区、禁飞区”这种躲猫猫要求翻译成一个凸优化问题并求解的。如果你是做机器人或无人机路径规划的学生、工程师或者想了解凸优化在真实场景里怎么落地这篇东西应该能给你一些参考。1. 威胁区把问题变成了什么从非凸约束到可求解模型1.1 问题定义不是“绕开障碍物”这么简单无人机路径规划最常见说法是找一条从起点到终点的路径不碰障碍物。但一旦把“威胁区域”放进问题里目标往往就变了不只是“不碰”而是“尽量别靠近”或者“穿过威胁区的暴露时间要短”。这个差别很重要直接决定了用什么优化框架。威胁区域在工程上通常被抽象成圆形或凸多边形。比如一个圆形威胁区可以写成C_j { p : ||p - c_j|| r_j }其中因为一旦进入这个区域代价很大甚至直接失败所以路径需要尽可能避开。但与此同时你还要让路径尽量短、飞行时间尽量少、控制能量尽量小。于是最自然的数学表达是输入起点 p0终点 pT威胁区集合 {C_j}输出连续轨迹 γ(t)目标最小化飞行时间、路径长度或威胁暴露约束γ(0)p0γ(T)pT且对所有 t 和 j 满足 γ(t) ∉ C_j。这里最大的难点就藏在最后一条约束里。||p - c_j|| r_j左边是凸函数但约束是“凸函数大于等于常数”。它的可行集是圆的外部而圆的外部不是凸集。你站在航线上看这条约束等于是在整个平面上挖了一个洞洞周围的可行域是弯折的凹进去的。严格说整个问题是非凸的。1.2 凸优化的“真香”来自哪里那为什么大家还是前赴后继地用凸优化答案非常实际一旦问题能写成凸优化求解器给的是全局最优解而且计算快、稳定性好。凸优化的两个核心条件是目标函数是凸函数可行域是凸集。凸函数只有一个全局最低点没有一堆局部极小值来坑你。你可以把它想象成一个只有单个底部的大碗从碗沿任何位置松手球都会滚到同一个碗底。非凸问题则像布满小坑的高尔夫球场球很容易滚进某个浅坑就出不来你甚至不知道这个坑是不是最深的。对于无人机这种讲究实时性的场景这个性质太重要了。离线规划慢一点没关系在线重规划必须毫秒级。凸优化配合 OSQP、Gurobi 这类成熟求解器很多小规模问题能在几毫秒内求解。这对后续做机载计算、动态重规划都很有价值。1.3 威胁区的“洞”必须另想办法所以想在保持凸优化框架的同时处理威胁区核心思路不是“用凸优化直接解非凸约束”而是“把问题重新表述成凸问题”。常见的手段我后面会展开这里先提三个最主流的方向把威胁区变成惩罚项塞进目标函数把自由空间切割成若干个凸胞让路径在凸胞内走引入 0/1 整数变量判断路径从威胁区的哪一侧绕过去。我以前也幼稚地直接把非凸约束写进 CVXPY结果求解器直接报错或者给出一堆没法看的轨迹。后来才明白所谓“用凸优化做带威胁区的路径规划”几乎都是用了某种改造术而不是硬刚。2. 我为什么没有直接从障碍物边缘做几何规划2.1 几何规划看起来很直接但坑不少最早我拿到这个项目时第一反应是干嘛不用 A* 或者 RRTA* 在栅格地图上搜一条避障路径然后用 B 样条平滑不是也能飞吗确实能飞但做了一段时间之后我发现了几个很烦的问题。第一几何搜索只得到一条折线路径完全不考虑无人机动力学。四旋翼有最大速度、最大加速度、最小转弯半径一条在几何上很漂亮的路径可能根本飞不出来。第二平滑阶段容易把路径拉回威胁区。很多平滑算法只关心曲线光顺不感知障碍物。B 样条一拉很容易把路径拉进禁飞区你还得再去做碰撞检测重新修。第三几何路径很难处理“威胁暴露最小”这种连续指标。如果目标是绕开某个圆形威胁区你可以做但如果目标是权衡路径长度和暴露风险几何搜索就力不从心了。第四是扩展性问题。威胁区一多栅格分辨率稍微提高一点A* 的搜索空间膨胀非常快RRT 虽能随机采样但结果每次都不一样而且不一定最优。我后来才意识到凸优化不是要去替代几何搜索而是应该和几何搜索配合先用几何方法找到一条粗略的拓扑路径再用凸优化在满足动力学和安全约束的前提下把轨迹磨平。2.2 三种把威胁区塞进凸优化框架的工程做法在项目里我实际对比了三种主流的改造方式每种都有适用场景。方法核心思想凸性优点问题惩罚函数法把威胁区作为目标函数中的惩罚项离威胁越近代价越大通常需要额外处理才能保持凸实现简单能表达风险权衡容易陷入局部最优权重难调凸安全走廊法将自由空间分解为凸多边形把轨迹约束在走廊内严格凸走廊约束为线性求解稳定适合大场景走廊生成质量直接影响结果混合整数凸优化用 0/1 变量表示从威胁区哪一侧通过用大 M 法线性化整数凸混合整数凸优化表达能力强模型精确小规模可用大规模组合爆炸惩罚函数法最直接。比如你可以在目标函数里加一项J w / (||p - c_j||^2 epsilon)但要注意这个函数在威胁区边缘变化很剧烈直接放进 QP 里大概率不是凸函数。项目里有一个版本就是这么干的结果求解器经常报“problem is not DCP”要不就是路径在威胁区边缘反复震荡。后来我把惩罚项改成线性近似配合信赖域约束才勉强能用。凸安全走廊法是我个人认为最工程化的方案。它的思路分三步用 A* 或者 Dijkstra 在栅格地图上找一条粗路径沿着粗路径生成一串有重叠的凸多边形这些多边形就是安全走廊把无人机运动学离散化求解一个二次规划 QP位置约束是“每个时间步落在对应的凸多边形内”。这套流程的好处是非凸的避障约束被走廊“消化”了。走廊本身是凸的路径又被限制在走廊里所以避障自然满足。剩下的全是线性约束和二次目标QS 又快又稳。混合整数凸优化则更适合小规模、强约束的场景。比如只有两三个威胁区你希望精确建模“路径在威胁区左边还是右边、上边还是下边”。这时候引入 0/1 变量用大 M 法把“不能在威胁区内部”转成一组互斥的线性约束。求解规模小的时候效果非常好但威胁区一多整数变量组合爆炸求解时间指数上升。2.3 我在这个 zip 项目里最终采用的流程翻完压缩包里的代码我发现作者并没有用惩罚函数的那种粗暴写法而是走了一条“A* 粗路径 凸走廊生成 QP 轨迹平滑”的路线。整套数据流是这样的地图上标出威胁区栅格化并对威胁区做膨胀处理A* 搜索一条从起点到终点、完全不碰威胁区的粗路径沿粗路径按照固定步长生成一系列凸多边形相邻多边形必须有重叠将时间轴分成多段每一段对应一个凸多边形把位置约束写成线性不等式用 CVXPY 建模OSQP 求解得到一条满足动力学约束的平滑轨迹。这套流程整体上很稳而且每个环节都能单独验证。我觉得这比强行在目标函数里加非凸惩罚要靠谱得多。3. 把动力学和威胁规避写成凸优化一个可跑的简化算例3.1 先建立二阶积分运动模型要真的跑通凸优化第一步是把无人机模型简化成适合优化的形式。这里我用的是多旋翼水平面内的二阶积分模型状态量是位置和速度x_k [px_k, py_k, vx_k, vy_k]^T控制量是加速度u_k [ax_k, ay_k]^T用前向欧拉法离散化时间步长 dtpx_{k1} px_k vx_k * dt 0.5 * ax_k * dt^2 py_{k1} py_k vy_k * dt 0.5 * ay_k * dt^2 vx_{k1} vx_k ax_k * dt vy_{k1} vy_k ay_k * dt这个模型在低速无人机场景下够用。如果你要飞战斗机一样的机动那得换成更复杂的状态方程但优化形式不变只是约束更多。优化目标我设为最小化控制能量也就是各个时间步加速度的平方和。这样轨迹不会出现不必要的急加减速也利于飞控跟踪。约束包括起点的位置、速度必须等于设定值终点的位置、速度必须等于设定值每个时间步的速度大小要小于最大值每个时间步的加速度大小要小于最大值每个时间步的位置必须落在对应的凸走廊内。前面四条都好理解最后一条是威胁规避的核心。因为威胁区已经在生成走廊时被排除了所以只要位置落在走廊内就一定不会进入威胁区。3.2 CVXPY 实现一个走廊约束下的轨迹优化这里我给出一个最简化的可运行示例。为了方便贴代码我把走廊简化为两个有重叠的凸矩形前一半时间位置落在左下矩形[0,6] x [0,6]后一半时间位置落在右上矩形[4,10] x [4,10]。这两个矩形的重叠区域[4,6] x [4,6]就是路径过渡的地方。import cvxpy as cp import numpy as np # 时间参数 N 40 # 时间步数 dt 0.15 # 时间步长 # 起点和终点位置、速度 p0 np.array([0.0, 0.0]) pT np.array([10.0, 10.0]) v0 np.array([0.0, 0.0]) vT np.array([0.0, 0.0]) # 变量定义 px cp.Variable(N) py cp.Variable(N) vx cp.Variable(N) vy cp.Variable(N) ax cp.Variable(N - 1) ay cp.Variable(N - 1) constraints [] # 初始和终端状态约束 constraints [px[0] p0[0], py[0] p0[1]] constraints [vx[0] v0[0], vy[0] v0[1]] constraints [px[N-1] pT[0], py[N-1] pT[1]] constraints [vx[N-1] vT[0], vy[N-1] vT[1]] # 动力学约束 for k in range(N - 1): constraints [px[k1] px[k] vx[k] * dt 0.5 * ax[k] * dt**2] constraints [py[k1] py[k] vy[k] * dt 0.5 * ay[k] * dt**2] constraints [vx[k1] vx[k] ax[k] * dt] constraints [vy[k1] vy[k] ay[k] * dt] # 走廊约束这里其实是两个凸矩形真实项目中来自上游算法 for k in range(N): if k N // 2: constraints [0 px[k], px[k] 6] constraints [0 py[k], py[k] 6] else: constraints [4 px[k], px[k] 10] constraints [4 py[k], py[k] 10] # 速度和加速度边界 constraints [cp.abs(vx) 2.5, cp.abs(vy) 2.5] constraints [cp.abs(ax) 2.0, cp.abs(ay) 2.0] # 目标函数最小化控制能量 objective cp.Minimize(cp.sum_squares(ax) cp.sum_squares(ay)) # 求解 prob cp.Problem(objective, constraints) prob.solve(solvercp.OSQP, verboseFalse) # 输出结果 if prob.status cp.OPTIMAL: print(求解成功控制能量 , prob.value) print(前 3 个位置, np.vstack([px.value[:3], py.value[:3]]).T) else: print(求解失败状态 , prob.status)注意真实项目中走廊约束不是这样手写的。这里的矩形只是为了让你直观看到“凸约束怎么加进去”。如果上游生成的走廊是一组凸多边形你只需要把每个时刻的约束换成对应的多边形不等式即可。3.3 求解结果和工程上的解读这段代码用 OSQP 求解通常几毫秒就能出结果。你得到的是每个时间步的位置、速度、加速度路径天然满足动力学约束和走廊约束。我第一次跑通的时候异常兴奋因为代码改一行就能让轨迹从直线变成绕过威胁区的曲线。但后来发现真正的挑战从来不是求解器而是你怎么把威胁区准确地变成约束。还有一个细节CVXPY 里面位置约束用的是“每个时间步必须在某个凸多边形内”但如果两个相邻时间步被分配到不同的凸多边形而两个多边形没有重叠那么这一步到下一步可能是跳变优化器会报不可行。所以生成走廊时相邻凸多边形一定要有重叠区。4. 仿真里最容易翻车的三个细节4.1 走廊不连通或太窄QP 直接报 infeasible这是我仿真中遇过最频繁的问题。A* 粗路径往往贴着威胁区边缘走如果你沿着粗路径用固定宽度生成凸多边形在某些拐弯处两个多边形可能只连接在一个点上QP 中就会出现“前一步还在这个区域下一步必须跳到那个区域”的矛盾约束最终求解器告诉你 infeasible。调试方法很简单把每个时刻的走廊约束单独打印出来看看相邻时间点的位置可行域是否有交集。如果没有说明走廊生成环节出问题了。我的实践是在生成走廊时给每个凸多边形加 20% 的膨胀余量强制相邻多边形有足够重叠。膨胀多了会导致路径绕路但至少求解稳定。之后再逐步缩小膨胀系数。4.2 目标函数里乱加威胁暴露项结果反而更危险有人希望路径离威胁区中心越远越好于是把威胁暴露代价直接写进 QP 目标比如J w / (||p - c_j||^2 epsilon)这个想法很自然但问题在于这个函数在威胁区附近是高度非凸的。你把它加进 QP目标函数变成非凸求解器要么直接报错要么给你一个局部最优解。更可怕的是某些求解器强行求解给出的结果看起来没问题实际路径却可能在威胁区边缘画圈比不加还危险。稳妥的做法本文还有配套的精品资源点击获取
![【关注可白嫖源码】--课程设计--毕业设计--78966形体管理中心系统[编号：project79321]（案件分析）](http://pic.xiahunao.cn/yaotu/【关注可白嫖源码】--课程设计--毕业设计--78966形体管理中心系统[编号：project79321]（案件分析）)
平方复杂度无法被常规优化消除｜TaoToken 统一 Key 通道实测)
