一种基于多传感器的无人艇全局路径规划和局部避障方法

本发明涉及无人艇自主导航，具体涉及的一种基于多传感器的无人艇全局路径规划和局部避障方法。

背景技术：

1、在无人艇自主导航领域，全局路径规划和局部避障方法的有效实现至关重要。复杂的水域环境需要无人艇获取各种环境信息，包括准确的地理位置、障碍物位置等，这些信息对无人艇安全准确的自主导航具有关键影响。使用激光雷达、gps、imu等多种传感器来获取这些复杂的信息，有效增强无人艇的环境感知能力，进而因此选取基于多传感器的无人艇全局路径规划和局部避障方法对提升无人艇自主导航性能至关重要。

2、基于多传感器的无人艇全局路径规划和局部避障方法需要综合考虑各种传感器数据，通过激光雷达和imu数据融合处理生成可以用于导航的环境感知信息，并通过gps和imu数据融合处理提高定位精度。进行全局路径规划，同时利用人工势场方法避开全局路径上存在的障碍物，调整局部路径，从而实现无人艇高效、安全地在复杂水域环境中自主航行的目标。

技术实现思路

1、本发明的目的是为了实现复杂水域的无人艇自主导航，提供一种基于多传感器的无人艇全局路径规划和局部避障方法，通过多传感器的数据获取并融合，可以获得综合的环境感知信息以及提高定位精度等优点，是一种集成了多传感器融合、路径规划和避障技术，实现了无人艇高效、安全地在复杂环境中航行的方法设计。

2、本发明的目的可以通过采取如下技术方案达到：

3、一种基于多传感器的无人艇全局路径规划和局部避障方法，所述方法包括以下步骤：

4、s1、通过无人艇搭载的多传感器获取多传感器数据，其中，多传感器包括：激光雷达、惯性测量模块和卫星定位模块，以下惯性测量模块简称imu，卫星定位模块简称gps，激光雷达用于获取采集三维空间数据，imu用于采集加速度和角速度，gps用于采集实时经纬高数据；

5、s2、对多传感器数据进行融合，将激光雷达与imu的数据融合处理生成环境感知信息，将gps与imu的数据融合处理生成定位信息；

6、s3、通过外部设备获取得到目的地，进行全局路径规划，生成无人艇的全局路径；

7、s4、根据环境感知信息，对无人艇的全局路径进行障碍物检测，判断全局路径上是否存在新的障碍物；

8、s5、若存在新的障碍物，根据障碍物位置构建人工势场，引导并调整无人艇的局部路径，直到避开障碍物；若否，继续执行步骤s4在全局路径上的障碍物检测，直到达到指定目的地。

9、进一步地，激光雷达通过发射激光束扫描周围环境，得到的三维点云数据，点云中每一点包含在激光雷达坐标系下的三维信息；gps获取无人艇当前位置的导航系下的经纬度坐标信息；imu获取无人艇的加速度、角速度，由于激光雷达、imu、gps与无人艇刚性连接，并且三轴方向一致，所以激光雷达、imu、gps三者坐标系统一称为艇体固连坐标系简称b系，北东地坐标系简称ned系。获取多传感器的数据有利于无人艇获取各种环境信息，激光雷达、imu、gps与无人艇刚性连接有利于后续坐标转换和程序运行减小工作量。

10、进一步地，所述步骤s2中生成环境感知信息过程如下：

11、s211、通过激光雷达采集到每一帧的三维点云数据，以及对应时间戳的imu和gps数据，生成n、e、d轴上的三维点云信息以及无人艇在全局点云地图的位置和姿态信息，同时剔除d轴高于无人艇高度以及d轴方向为负数的点云信息，完成无人艇的即时定位，构建得到在ned系下的全局点云地图；提前剔除d轴高于无人艇高度以及d轴方向为负数的无效点云信息，有利于减小之后的计算量；

12、s212、将激光雷达扫描到的区域进行栅格化处理，每个栅格点云代表空间的一小块区域，内含一部分点云，每一个栅格的大小根据无人艇的大小决定，取无人艇的艇长作为栅格的边长；存储栅格化之后的点云信息，以便后续路径规划和障碍物检测的分析和使用；

13、s213、将d轴方向上的点云投影至n轴和e轴的栅格平面，计算每个栅格中d轴方向的最高点云和最低点云之间的差值，若差值不为0则视为障碍物栅格，反之则视为非障碍物栅格，得到二维的环境感知信息；有效地提取出二维环境感知信息，快速识别出存在障碍物的区域，为无人艇的路径规划和避障提供重要参考数据。

14、进一步地，所述步骤s2中生成定位信息过程如下：

15、s221、构建系统状态方程，通过数学模型描述imu中各种误差项的演化规律；imu的误差微分方程包括姿态误差微分速度误差微分位移误差微分分别如下：

16、

17、其中,分别为姿态误差微分在ned系下n,e,d轴上的分量；分别为速度误差微分在ned系下n,e,d轴上的分量；分别为速度误差微分在ned系下n,e,d轴上的分量；向量φ＝[φn φe φd]t为失准角，通过ned系下n,e,d轴上的轴角φn,φe,φd表示旋转的误差；向量为地球自转角速度的分量，ω为地球自转角速度，l为当前的纬度，用弧度制表示；向量为imu中陀螺仪测量的角速度在ned系下的偏差，εx，εy，εz分别为该偏差在imu的坐标系下的x,y,z轴上的分量，为imu的坐标系到ned系的变换矩阵；向量fn＝[fn fe fd]t为imu中加速度计测量的加速度在ned系下的表示，fn，fe，fd分别为加速度计测量的加速度在ned系下n,e,d轴上的分量；为imu中加速度计测量的加速度的偏差在导航系下的表示，分别为加速度计的偏差在imu的坐标系下的x,y,z轴上的分量；

18、带入公式(1)，得到如下imu的完整误差方程：

19、

20、

21、

22、在滤波器中，状态估计方程写为如下形式：

23、

24、为系统状态量微分，x为系统状态量，都包括：位移误差δpt、速度误差δvt、姿态误差δφt、陀螺仪的偏差误差εt、加速度计的偏差误差写成向量如下：

25、x＝[δpt δvt δφt εt ▽t]t  (6)

26、其中，δpt＝[δpn δpe δpd]t,δpn,δpe,δpd分别为位置误差在ned系下n,e,d轴上的分量；δvt＝[δvn δve δvd]t,δvn,δve,δvd分别为速度误差在ned系下n,e,d轴上的分量；δφt＝[δφn δφe δφd]t,δvn,δve,δvd分别为姿态误差在ned系下n,e,d轴上的分量；

27、将状态量x带入式(5)，得到状态转移矩阵ft：

28、

29、其中，03×3为3x3的全零矩阵，i3×3为3x3的单位矩阵；

30、噪声增益矩阵

31、偏差噪声矩阵w＝[ωgx ωgy ωgz ωax ωay ωaz]t,ωgx,ωgy,ωgz分别为角速度偏差在b系下x,y,z轴上的分量,ωax,ωay,ωaz分别为加速度偏差在b系下x,y,z轴上的分量；

32、状态估计方程描述系统状态量中误差项之间的相互影响，以及它们随时间的演化规律，可以对imu误差进行实时估计和校正；

33、s222、构建系统观测方程，将滤波器中观测方程的形式写为：

34、y＝gtx+ctn  (8)

35、其中，观测量y＝[δpn δpe δpd]t，观测矩阵gt＝[i3×3 03×12]，描述系统状态如何映射到观测值，03×12为3x12的全零矩阵，观测噪声增益矩阵ct＝[i3×3]，由于gps传感器不准确产生的观测噪声向量分别为位置观测噪声在ned系下n,e,d轴上的分量；系统观测方程的作用在于描述系统状态量与观测量之间的联系，可以从观测数据中推断系统状态，从而实现对系统的监测和控制；

36、s223、构建卡尔曼滤波器，卡尔曼滤波器的模型经典公式定义如下：

37、(a)预测系统状态变量：

38、

39、其中，xk为k时刻预测的误差状态估计值，为k-1时刻更新后的误差状态估计值，f(,)为状态转移函数，vk为k时刻系统的控制输入的误差；

40、(b)预测误差协方差矩阵：

41、

42、其中，pk为k时刻预测的误差协方差矩阵，为k-1时刻更新后的误差协方差矩阵，fk-1为k-1时刻状态转移矩阵的雅可比矩阵，bk-1为k-1时刻噪声增益矩阵的雅可比矩阵，qk为k时刻过程噪声协方差矩阵；

43、(c)计算卡尔曼增益：

44、

45、其中，kk为k时刻的卡尔曼增益，gk为k时刻的观测矩阵的雅可比矩阵，ck为k时刻的观测噪声增益矩阵的雅可比矩阵，rk为k时刻的观测噪声协方差矩阵；

46、(d)更新误差协方差矩阵：

47、

48、其中，为k时刻更新后的误差协方差矩阵；

49、(e)更新误差状态变量：

50、

51、其中，yk为k时刻的观测值，g(，)为观测模型函数；

52、已知卡尔曼滤波器的模型是工作在离散时间下的，观测方程是离散化的，将状态转移方程公式(5)进行离散化，对状态转移矩阵ft和噪声增益矩阵bt进行的一阶泰勒近似得到：

53、

54、将公式(10)代入公式(9-1)至(9-5)得到最终的卡尔曼滤波器的方程为：

55、

56、其中，yk为位置测量误差，在该系统中计算方法为gps的测量值减去imu的预测值；

57、s24、式(11)前两个公式对应预测过程，根据式(11)中前两个公式预测状态变量和状态变量对应的协方差矩阵，式(11)后三个公式对应状态更新过程，根据式(11)中后三个公式计算卡尔曼增益，决策当前时刻更倾向于imu的预测值还是gps的测量值，进而校正式(11)前两个公式预测产生的误差，直至gps与imu数据均为空，实现对定位信息的精确修正和更新，提高了准确性和鲁棒性。

58、进一步地，所述步骤s3中全局路径规划过程如下：

59、s31、初始化两个空的队列，分别定义为开启队列qopen和关闭队列qclose，将无人艇的当前位置作为起点s，加入qopen；设置任意节点a的移动代价值计算函数f(a)＝g(a)+h(a)，其中，g(a)为起点s到节点a的曼哈顿距离，其计算方式为das＝|xa-xs|+|ya-ys|，(xa,ya)为节点a在二维的环境感知信息中的横纵坐标值，(xs,ys)为起点s在二维的环境感知信息中的横纵坐标值，h(a)为节点a到目标点e的曼哈顿距离；特别是在栅格化的环境信息中，曼哈顿距离更容易计算；设置起点的移动代价值为0；建立起搜索的起点和基本的移动代价函数，进而进行全局路径规划的搜索和扩展；

60、s32、遍历开启队列qopen，为提高搜索效率，只查找移动代价值最小的节点，把它作为当前要处理的节点b，并将节点b移动至关闭队列qclose；对节点b的周围相邻的8个节点j，j＝1…8行遍历，步骤如下：

61、s321、检测判断节点j，如果节点j是不可抵达的或者在关闭队列qclose中，跳过该节点j；排除不可抵达与在关闭队列qclose中的点以减小运行时间，提高全局路径的工作效率；

62、s322、检测判断节点j，如果节点j不在开启队列qopen中，则将节点j加入qopen，并且把节点b设置为节点j的父节点，记录节点j的f(j)、g(j)、h(j)值；

63、s323、检测判断节点j，如果节点j若在开启队列qopen中，计算当前节点b与节点j的曼哈顿距离dbj，判断g(b)+dbj＜g(j)；若不等式成立，起点s经过节点b再到节点j比起点s直接到节点j的曼哈顿距离要小，则意味着路径更优，所以把节点b设置为节点j的父节点，并且令g(j)＝g(b)+dbj，并更新f(j)值；

64、s33、重复步骤s31和步骤s32，直到目标点e加入开启队列qopen，表示找到路径；如果开启队列qopen遍历到空，表示没有找到路径；

65、s34、从目标点e开始，每个节点沿着父节点遍历至起点，遍历得到路径；通过不断评估节点的代价值和路径估计值以找到了一条最优路径，路径规划能够有效地帮助无人艇在复杂环境中进行安全、高效的导航。

66、进一步地，所述步骤s5中使用人工势场法进行局部避障过程如下：

67、s51、构建一个人工势场，由两部分组成，一部分是目标点对无人艇产生的引力势场，方向是由无人艇指向目标点，另一部分是障碍物节点对无人艇产生的斥力势场，方向是障碍物节点由指向无人艇，人工势场为引力势场和斥力势场共同叠加作用，通过引力和斥力的合力控制无人艇的移动，躲避障碍物；

68、s52、定义引力势函数如下：

69、

70、其中，η为正比例位置增益系数，ρ(q,qg)表示一个矢量，矢量大小为当前无人艇位置q和目标点qg两个位置之间的欧几里得距离|q-qg|，矢量方向为从q指向qg，则由引力势场产生的引力为：

71、

72、其中，符号为负梯度计算，引力势函数受无人艇与目标点的距离影响，当目标点与无人艇的距离越远时，无人艇所受的势能越大；当目标点与无人艇的距离越近时，无人艇所受的势能越小；当势能为零时，则表明无人艇到达目标点位置；引力势函数的引入可以帮助无人艇根据目标点的位置自动调整路径规划，在目标点附近能够减小无人艇的速度，有助于更精确地到达目标；

73、s53、定义斥力势函数如下：

74、

75、其中k为正比例位置增益系数；ρ0为正常数，表示在无人艇周围障碍物区域对无人艇产生作用的最大距离；ρ(q,q0)表示一个矢量，矢量大小为q和无人艇周围障碍物区域中离q最近的点q0两个位置之间的欧几里得距离|q-q0|，矢量方向为从q0指向q，则由斥力场产生的斥力为：

76、

77、其中，斥力势函数受无人艇与障碍物的距离影响，当障碍物与无人艇的距离越远时，无人艇所受的势能越小；当障碍物与无人艇的距离越近时，无人艇所受的势能越大，当势能为零时，则表明无人艇已经脱离障碍物的影响范围；引入斥力势函数可以帮助无人艇在路径规划过程中避开障碍物，降低碰撞的风险，确保生成的局部路径的安全性；

78、s54、根据引力势函数和斥力势函数，得到全局势场函数u(q)为无人艇所受的斥力势场和引力势场之和：

79、u(q)＝urep(q)+uatt(q) (16)

80、无人艇所受合力为：

81、

82、s55、重复步骤s51至s54，无人艇持续按照合力方向进行移动，直到达到目标点；局部避障算法能够同时考虑到目标点的吸引力和障碍物的排斥作用，从而生成安全、有效的路径，提高无人艇的导航效率和安全性。

83、本发明相对于现有技术具有如下的优点及效果：

84、(1)本发明通过对多种传感器数据进行综合分析和融合，无人艇能够获取全面的环境信息，有利于航行路径的规划和障碍物的检测。

85、(2)本发明通过gps和imu数据融合处理生成定位信息，相对于直接使用gps数据作为定位信息，有效提高了无人艇的定位精度和系统的鲁棒性，确保航行的高效性和安全性。

86、(3)本发明进行了全局路径规划，在环境感知信息的基础上快速规划出当前位置到目标位置的全局路径。此外，还引入了人工势场法，通过对环境中的障碍物施加虚拟力场，使得在全局路径的基础上规划出一条能够躲避未知障碍物的局部路径，实现了无人艇导航的全局路径与局部避障功能，从而可以实现无人艇高效、安全地在复杂水域环境中自主航行的目标。