✅ 博主简介：擅长数据搜集与处理、建模仿真、程序设计、仿真代码、论文写作与指导，毕业论文、期刊论文经验交流。
✅ 如需沟通交流，扫描文章底部二维码。
（1）改进双向RRT与B样条优化路径规划：
提出了一种改进双向RRT(Bi-RRT)算法用于四旋翼三维环境路径规划。在扩展过程中引入目标点导向偏置采样，50%的概率直接朝向目标点扩展，40%概率朝向对方树的最新节点扩展，仅10%概率随机采样，加速了树木连接。步长采用自适应策略：在空旷区域步长设为2.5m，当检测到扩展方向存在障碍物时步长减小至0.8m。找到初始路径后，采用节点拒绝采样进行路径缩短优化，再对路径点使用三次B样条曲线进行平滑处理，生成曲率连续的飞行走廊。在三维复杂障碍场(100m×100m)中，算法平均规划时间0.43秒，路径长度较传统RRT缩短了17.2%。生成路径随后进行固定时间步长0.05秒的插值，转化为时间序列轨迹作为跟踪控制参考。
（2）指数收敛干扰观测器与自适应非奇异终端滑模跟踪控制器：
针对四旋翼受到的风扰和模型不确定性，设计了一种指数收敛干扰观测器(EDO)实时估计集总扰动。EDO的观测器增益为指数型时变函数，初期增益较大以快速逼近扰动，5秒后逐渐衰减至稳态值，避免了稳态抖振。将扰动估计值作为前馈补偿给控制器。位置外环和姿态内环分别采用自适应非奇异终端滑模控制(ANTSMC)，滑模面设计为避免奇异的终端吸引子形式。控制器增益根据干扰估计值的大小自适应调整，保证跟踪误差在有限时间收敛且无抖振。李雅普诺夫分析证明闭环系统稳定。仿真表明，在5级风扰动下，位置跟踪误差均方根值为0.08m，姿态角误差小于0.03rad，相比传统PID，跟踪精度提高了约65%。
（3）PX4SITL硬件在环仿真与飞行实验：
将规划器和控制器集成到PX4自驾仪仿真环境(PX4SITL)中，在Gazebo中构建了带树木和建筑物的仿真场景。EDO和ANTSMC以C++实现并通过ROS2节点与PX4通信。进行了20次自主避障飞行实验，无人机均成功穿越障碍区到达目标点，平均耗时18.7秒，最差跟踪误差0.15m。在此基础上，利用自研四旋翼平台进行户外飞行验证，搭载NVIDIA Jetson Nano用于规划实时解算，飞行速度2m/s，在3-4级风中完成了路径规划和紧密跟踪任务，证实了整套方法的有效性和鲁棒性。
import numpy as np
from scipy.interpolate import splprep, splev
自适应步长双向RRT*简化
class BiRRTStar:
def init(self, start, goal, obstacles):
self.start = start; self.goal = goal; self.obs = obstacles
self.tree_a = [start]; self.tree_b = [goal]
def extend(self, tree, target, step):
寻找最近节点扩展，省略细节
return new_node
def plan(self):
for i in range(500):
if np.random.rand() < 0.5: target = self.goal
elif np.random.rand() < 0.4: target = self.tree_b[-1]
else: target = np.random.rand(3)*100
step = 2.5 if self.is_free(target) else 0.8
new_node = self.extend(self.tree_a, target, step)
… 连接检查与连接
path = self.get_path()
return path
B样条平滑
def bspline_smooth(path_points, num_points=200):
tck, u = splprep(path_points.T, s=1.0, k=3)
u_new = np.linspace(0,1,num_points)
smooth_path = np.array(splev(u_new, tck)).T
return smooth_path
指数收敛干扰观测器
class ExponentialDisturbObserver:
def init(self, gain0=10, decay=0.5):
self.gain0 = gain0; self.decay = decay
self.z_hat = np.zeros(6) # 扰动估计状态
def estimate(self, t, control_input, acceleration_meas):
gain = self.gain0 * np.exp(-self.decay * t) + 1.0
d_hat = self.z_hat # 简化
self.z_hat += 0.01 * (gain * (acceleration_meas – control_input – self.z_hat))
return d_hat
自适应非奇异终端滑模控制（俯仰通道示意）
def antsmc_pitch(theta, theta_d, theta_dot, dist_est, params):
e = theta – theta_d
de = theta_dot – desired_theta_dot
s = de + 2.0 * np.sign(e) * np.abs(e)**0.6 # 终端滑模面
gain = 1.5 + 0.5 * np.abs(dist_est[4])
u = – gain * np.sign(s) – 1.0 * s – dist_est[4]
return u
如有问题，可以直接沟通
👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇👇
