CN112000125B - 一种地铁隧道内四旋翼飞行器自主导航方法 - Google Patents

Abstract

本发明公开了一种地铁隧道内四旋翼飞行器自主导航方法，利用激光雷达及HectorSLAM算法构建离线的隧道地图同时实现实时定位；通过引入回归滤波机制及主干约束的改进快速探索随机树基于离线地图实现航迹规划；利用三次B样条曲线优化离线航迹；根据离线规划的航迹点及四旋翼实时位置设计PID位置控制器求解引力加速度；根据人工势场法求解障碍物的斥力加速度；计算引力及斥力加速度的矢量和控制四旋翼的姿态角偏转及位置，使四旋翼完成起始点至终点的自主导航飞行。本发明解决了弱光、无GPS信号的地铁隧道环境下的四旋翼自主导航问题，使四旋翼自主完成航迹规划及航迹跟踪避障，以实现基于四旋翼的地铁隧道智能巡检。

Description

一种地铁隧道内四旋翼飞行器自主导航方法

技术领域

本发明涉及空中旋翼类飞行器定位与导航领域，具体涉及一种地铁隧道内四旋翼飞行器自主导航方法。

背景技术

目前，对地铁的日常维护多依赖于人工巡检方式，效率低下，且随着地铁线路覆盖率的提高，届时将面临巨大的人员短缺压力。

基于四旋翼的智能巡检手段为地铁巡检问题提供了新型解决方案，目前已广泛应用于铁路轨道检测、高压输电线路检测、桥梁裂缝检测。在以上应用场景中，四旋翼自主导航的实现多依赖于GPS或图像处理技术，可适用于地铁高架路段，然而地铁隧道中无GPS信号且弱光无法适用。

发明内容

本发明的目的在于提供一种地铁隧道内四旋翼飞行器自主导航方法，基于激光雷达的HectorSLAM技术不受光线强弱限制，可实现厘米级的地图构建及同步定位，再配以合适的路径规划算法即可实现地铁隧道内四旋翼的自主导航。

实现本发明目的的技术解决方案为：一种地铁隧道内四旋翼飞行器自主导航方法，具体步骤如下：

步骤1，利用激光雷达及HectorSLAM算法构建离线的隧道地图，并对四旋翼进行实时定位；

步骤2，通过引入回归滤波机制及主干约束的改进快速探索随机树基于离线地图实现航迹规划；

步骤3，利用三次B样条曲线优化离线航迹，包括平滑处理和曲率约束；

步骤4，根据离线规划的航迹点及四旋翼实时位置设计PID位置控制器求解引力加速度；

步骤5，根据人工势场法求解障碍物的斥力加速度；

步骤6，计算引力及斥力加速度的矢量和控制四旋翼的姿态角偏转及位置，使四旋翼完成起始点至终点的自主导航飞行。

本发明与现有技术相比，其显著优点在于：1)采用基于激光雷达的HectorSLAM进行隧道环境地图构建与实时定位，不受光线限制且可达厘米级的定位精度，同时激光雷达的高扫描频率及高测量精度保证了导航避障的及时性与可靠性；2)采用ReT-RRT*进行全局航迹规划，回归滤波机制的引入限制了随机节点的分布密度，减少了在随机采样过程中产生的大量过度搜索节点，有效提高了在隧道地图中规划航迹的收敛速度，主干约束的引入限制了搜索空间为主干的周围空间，降低了采样节点无效生长的概率，进一步提高了航迹规划效率；3)利用三次B样条曲线对离线航迹进行平滑处理和曲率约束，提高了航迹跟踪精度，降低了最大跟踪误差；4)根据离线规划的航迹点及四旋翼实时位置设计PID位置控制器求解引力加速度，由于参考航迹点间隔几近相等，使得引力加速度较为稳定，有利于四旋翼的平稳飞行；5)利用APF求解障碍物的斥力加速度，保障了四旋翼在面对未知障碍时的安全性。

附图说明

图1为自主导航系统总体结构框图。

图2为ReT-RRT*路径规划结果图。

图3为折线航迹平滑处理结果图。

图4为控制点调整策略示意图。

图5为曲率约束前后航迹对比图。

图6为四旋翼控制系统框图。

图7为四旋翼航迹跟踪避障实验结果图。

具体实施方式

下面结合附图和具体实施例，进一步说明本发明方案。

本发明的一种地铁隧道内四旋翼飞行器自主导航方法，具体步骤如下：

步骤1，利用激光雷达及HectorSLAM算法构建离线的隧道地图，并对四旋翼进行实时定位；

建图及定位传感器采用激光雷达HOKUYO UTM-30LX，可适应地铁隧道弱光的环境特点，同时该激光雷达的高扫描频率(40Hz)及高测量精度(30mm，0.25°)保证了导航避障的及时性与可靠性。建图及定位算法采用HectorSLAM，利用高斯牛顿方法来解决扫描匹配问题，消耗较低的计算资源，因此可用于低重量、低功耗和低成本的处理器；同时，该算法可实现精确的环境感知和自定位，精度可达5cm；更为重要的是不依赖于里程计，可适用于空中平台。

步骤2，通过引入回归滤波机制及主干约束的改进快速探索随机树(RRT*withRegression Filtering Mechanism and Trunk Constraint,ReT-RRT*)基于离线地图实现航迹规划；

RRT*因渐近最优特性广泛应用于全局路径规划，但随着场景地图尺寸的进一步扩大需解决路径收敛速度过慢问题。因此提出改进ReT-RRT*算法，其具体实现过程如下。

首先对RRT*引入回归滤波机制，该机制设计了参数：最小探索区域半径ε_threshold，当新生长节点x_new与随机树T上最近节点x_nearest1的距离小于ε_threshold，则认为该节点位于已探索区域，属于过度搜索节点，对其进行回归处理不予生长，通过设定不同ε_threshold值可调整随机树节点的分布密度以适应不同尺寸的场景地图。

其次，对RRT*引入主干约束，缩小无效的搜索空间。定义地图坐标系为O(X,Y)，路径起点为S(x₀,y₀)，路径终点为E(x₁,y₁)，其中，

即为主干方向。定义缩小后搜索空间的坐标系为O'(X',Y')，该坐标系以点S为原点，以

为X’轴，Y’轴是平面上经过原点并与X’轴垂直的任一向量。坐标系O(X,Y)到O'(X',Y')的变换矩阵分为平移变换矩阵

及旋转变换矩阵

其中，α为O(X,Y)旋转至O'(X',Y')的夹角，顺势针为正，逆时针为负。由此，我们可将O(X,Y)中一点(x,y)^T通过变换矩阵H、R转换至O'(X',Y')中一点(x',y')^T，

通过限制(x',y')^T横坐标与纵坐标范围，即可限制搜索空间范围，提高航迹规划效率。

步骤3，利用三次B样条曲线优化离线航迹，包括平滑处理和曲率约束；

首先，由于ReT-RRT*算法所规划的航迹由若干直线段连接而成，在转折点处不光滑，不利于四旋翼的跟踪控制，故采用三次B样条曲线对其进行拟合平滑处理。

为第k段三次B样条曲线(k＝0,1,…,m)方程。其中，0≤t≤1；F_i,3(t)为基函数，其表达式为：

Q_i+k为控制点坐标，由ReT-RRT*生成的规划航迹特征点P_j，j＝0,1,…,m+1，通过下式反解得到。

其次，由于平滑后航迹中部分转折点处曲率过大，致使四旋翼在跟踪控制过程中受惯性影响会产生较大的跟踪误差，故基于三次B样条的局部可重建特性，提出了控制点调整策略。基函数F_i,3(t)与控制点Q_i+k线性无关，且基函数表达式恒定，故三次B样条曲线的各点曲率值由选取的控制点位置决定；同时控制点Q_i+k位置的改变仅影响P_i+k-3至P_i+k+1曲线段内各点曲率值；因此若某点曲率值超出上限值，则对其所属曲线段S_k,3(t)对应的控制点Q_2+k进行位置调整，限制曲率。

步骤4，根据离线规划的航迹点及四旋翼实时位置设计PID位置控制器求解引力加速度；

e(k)＝‖q(k)-p_refer(k)‖

其中，ξ为引力系数，K_p、K_i、K_d为PID控制系数，T_s为采样控制周期，q为四旋翼的当前位置，p_refer为参考航迹点，

为引力加速度的作用方向，为单位矢量。

步骤5，根据人工势场法(Artificial Potential Field Method，APF)求解障碍物的斥力加速度；

其中，η为斥力系数，ρ_obs(q)为四旋翼到障碍物的距离，d为障碍物的作用范围阈值，

为斥力加速度的作用方向，为单位矢量，始终垂直于四旋翼前进方向，且背离障碍物。

步骤6，计算引力及斥力加速度的矢量和控制四旋翼的姿态角偏转及位置，使四旋翼完成起始点至终点的自主导航飞行。

下面结合实施例对本发明进行详细说明。

实施例

为了验证本发明方案的有效性，设计了如图1所示的自主导航系统；搭建四旋翼硬件平台，采用“X”型结构，机架轴距550mm，以树莓派3B作为机载上位机，Pixhawk为底层飞控板，MB1212声呐为高度传感器，HOKUYO UTM-30LX激光雷达为核心传感器；并在室内环境下进行了四旋翼自主导航实验。其中，上位机树莓派3B及飞控板Pixhawk是整个自主导航系统的核心处理器。飞控板Pixhawk基于Ardupilot源码进行二次开发，在原有手动控制高度的定高模式基础上加以扩展实现一键起飞定高功能；并添加辅助开关以切换自主导航模式，该模式下Pixhawk通过串口实时接收上位机位置控制信号并通过姿态控制器实时控制四旋翼位姿。上位机树莓派3B基于ROS系统开发了完整的机载上位机程序，主要包括基于HectorSLAM的地图构建与定位模块、结合ReT-RRT*算法及三次B样条优化的离线航迹规划模块、基于APF的在线避障模块、PID位置控制器模块、以及基于Mavlink协议数据包的串口通信模块。

地铁隧道内四旋翼飞行器自主导航方法，包括如下步骤：

步骤1，基于无线局域网搭建分布式通信网络，利用客户端电脑远程登录到服务端机载上位机树莓派3B；

首先，服务端通过ifconfig指令查看自身IP地址并记录；其次，在客户端环境变量文件～/.bashrc.sh中写入服务端的IP地址及主机名；然后，客户端执行与前面类似的两步操作；最后将服务端与客户端设置在同一网段中，并在服务端上安装支持SSH协议的openssh库。至此，客户端可通过命令“ssh服务端主机名@服务端IP地址”远程登录到服务端。

步骤2，启动激光雷达驱动程序及HectorSLAM功能包构建离线环境地图；

首先，新终端执行“roslaunch mrobot_bringup urg_lidar.launch”启动激光雷达获取点云数据；然后，新终端执行“roslaunch mrobot_navigation hector.launch”启动HectorSLAM构建地图；最后，新终端执行“rosrun map_server map_saver-f～/map/mymap”保存当前环境的栅格地图。

步骤3，基于已构建的离线环境地图，确定路径起始点及终点并运行ReT-RRT*算法进行航迹规划，航迹规划结果如图2所示；

步骤4，利用三次B样条曲线对图2中已规划的航迹进行平滑处理和曲率约束，平滑处理后结果如图3所示，控制点调整策略原理如图4所示，曲率约束后结果如图5所示，并将最终规划所得的航迹点保存待用；

步骤5，将四旋翼放置于路径起始点，遥控器控制四旋翼解锁后切换开关至改进定高模式，一键起飞定高；

该一键起飞定高功能是基于原有Ardupilot定高模式二次开发实现的。Ardupilot是一套支持多旋翼飞行器，传统直升机，固定翼飞机的开源代码，该源码提供了丰富的导航控制相关库与传感器相关库，并实现了一系列功能不同的飞行模式，如定高模式、自动模式、着陆模式、悬停模式、自稳模式等，极大地降低了程序的二次开发复杂度。其原有定高模式需手动控制遥控器油门以调节高度，改进过后该模式支持四旋翼自动起飞并稳定在给定高度，降低操作复杂度。

步骤6，当四旋翼稳定在指定高度时，通过辅助开关切换至自主导航模式；

原定高模式下四旋翼的水平移动受遥控器俯仰通道及横滚通道的输入值控制，因此在遥控器的拨动开关中定义了一个辅助开关用以切换自主导航模式。该模式下，程序将不断读取Pixhawk的SERIAL 4/5串口，接收机载上位机树莓派3B实时传送的位置加速度信号，并通过姿态反解得到姿态控制器的参考姿态角，最后利用姿态控制器的输出实时控制四旋翼位姿，如图6所示。

步骤7，客户端同时启动激光雷达、HectorSLAM、位置控制器模块、避障模块及串口通信模块；

辅助开关切换至自主导航模式后，客户端运行“roslaunch mrobot_navigationhector_control.launch”命令。该launch文件中同时包含激光雷达启动程序“urg_lidar.launch”，HectorSLAM启动程序“hector.launch”，以及自定义功能包的启动程序。该自定义功能包主要包括以下模块：PID位置控制器模块、基于APF的在线避障模块、基于Mavlink协议的串口通信模块。下面将分别叙述。

PID位置控制器模块，根据规划所得的航迹点及HectorSLAM发布的实时定位数据话题“/slam_out_pose”设计PID位置控制器求解引力加速度，计算方法如下：

e(k)＝‖q(k)-p_refer(k)‖

其中，ξ为引力系数，K_p、K_i、K_d为PID控制系数，T_s为采样控制周期，q为四旋翼的当前位置，p_refer为参考航迹点，

为引力加速度的作用方向，为单位矢量。

基于APF的在线避障模块，利用激光雷达点云数据“/laser”实时获取障碍物的相对距离及角度，进而计算出障碍物的相对位置，最后根据人工势场法求解障碍物的斥力加速度，计算方法如下：

其中，η为斥力系数，ρ_obs(q)为四旋翼到障碍物的距离，d为障碍物的作用范围阈值，

为斥力加速度的作用方向，为单位矢量，始终垂直于四旋翼前进方向，且背离障碍物。

基于Mavlink协议的串口通信模块，首先计算

及

的矢量和得到四旋翼俯仰及横滚方向的位置加速度值，然后将两个方向的加速度值转换成补码并打包成Mavlink协议数据包通过串口送至底层飞控板Pixhawk，实现四旋翼对规划航迹的跟踪避障，结果如图7所示。

Claims (4)

1.一种地铁隧道内四旋翼飞行器自主导航方法，其特征在于，包括如下步骤：

步骤1，利用激光雷达及HectorSLAM算法构建离线的隧道地图，并对四旋翼进行实时定位；

步骤2，通过引入回归滤波机制及主干约束的改进快速探索随机树基于离线地图实现航迹规划；具体如下：

首先对RRT*引入回归滤波机制，该机制设计了参数：最小探索区域半径ε_threshold，当新生长节点x_new与随机树T上最近节点x_nearest1的距离小于ε_threshold，则认为该节点位于已探索区域，属于过度搜索节点，对其进行回归处理不予生长，通过设定不同ε_threshold值可调整随机树节点的分布密度；

其次，对RRT*引入主干约束，设定路径起点到终点为主干方向，并限制搜索空间为主干的周围区域，压制无效的采样搜索空间，具体为：

定义地图坐标系为O(X，Y)，路径起点为S(x₀，y₀)，路径终点为E(x₁，y₁)，其中，

即为主干方向；定义缩小后搜索空间的坐标系为O’(X’，Y’)，该坐标系以点S为原点，以

为X’轴，Y’轴是平面上经过原点并与X’轴垂直的任一向量；坐标系O(X，Y)到O’(X’，Y’)的变换矩阵分为平移变换矩阵

及旋转变换矩阵

其中，α为O(X，Y)旋转至O’(X’，Y’)的夹角，顺势针为正，逆时针为负；由此，可将O(X，Y)中一点(x，y)^T通过变换矩阵H、R转换至O’(X’，Y’)中一点(x’，y’)^T；

通过限制(x’，y’)^T横坐标与纵坐标范围，即可限制搜索空间范围；

步骤3，利用三次B样条曲线优化离线航迹，包括平滑处理和曲率约束；其具体过程如下：ReT-RRT*算法所规划的航迹由若干直线段连接而成，采用三次B样条曲线对其进行拟合平滑处理；基于三次B样条的局部可重建特性，通过控制点调整策略进行曲率约束，所述控制点调整策略为：若某点曲率值超出上限值，则对其所属曲线段对应的控制点进行位置调整，限制曲率；

步骤4，根据离线规划的航迹点及四旋翼实时位置设计PID位置控制器求解引力加速度，计算方法为：

e(k)＝||q(k)-p_refer(k)||

其中，ξ为引力系数，K_p、K_i、K_d为PID控制系数，T_s为采样控制周期，q为四旋翼的当前位置，p_refer为参考航迹点，

为引力加速度的作用方向，为单位矢量；

步骤5，根据人工势场法求解障碍物的斥力加速度；

步骤6，计算引力及斥力加速度的矢量和控制四旋翼的姿态角偏转及位置，使四旋翼完成起始点至终点的自主导航飞行。

2.根据权利要求1所述的地铁隧道内四旋翼飞行器自主导航方法，其特征在于，步骤1中，建图及定位传感器采用激光雷达HOKUYO UTM-30LX。

3.根据权利要求1所述的地铁隧道内四旋翼飞行器自主导航方法，其特征在于，步骤5中，根据人工势场法求解障碍物的斥力加速度，计算方法为：

其中，η为斥力系数，ρ_obs(q)为四旋翼到障碍物的最短距离，d为障碍物的作用范围阈值，

为斥力加速度的作用方向，为单位矢量，始终垂直于四旋翼前进方向，且背离障碍物。

4.根据权利要求1所述的地铁隧道内四旋翼飞行器自主导航方法，其特征在于，步骤6中，计算引力及斥力加速度的矢量和作为参考姿态角以控制四旋翼的位姿偏移，使其完成自主导航飞行。

Images