CN109992894B - 一种考虑感知信息误差的无人艇局部环境建模方法 - Google Patents

Abstract

本发明提供一种考虑感知信息误差的无人艇局部环境建模方法，获取无人艇运动状态信息和环境感知信息，对获取的环境感知信息进行处理，建立环境模型中的障碍物更新机制；依据障碍物出现的概率，决定其在环境模型中的标识与剔除，建立感知环境模型，对于环境的表示，采用便捷高效的栅格法。本发明考虑水面无人艇在实际航行过程中感知信息存在的误差，通过对无人艇获取的瞬时障碍物进行筛选，将大概率出现的较准确的障碍物位置标识在环境模型中，剔除了由于感知误差而误产生的障碍物点，提高了无人艇在实际航行过程中路径规划的稳定性和准确性，对于无人艇在实际情况下处于障碍物较多且较小的复杂环境中的精确路径规划有重要作用。

Description

一种考虑感知信息误差的无人艇局部环境建模方法

技术领域

本发明属于舰艇领域，具体涉及一种考虑感知信息误差的无人艇局部环境建模方法。

背景技术

无人水面艇是一个拥有自主运行能力的小型水面任务平台，主要用于执行危险以及不适于有人船只执行的任务。随着无人艇的快速发展，其在军事和民用领域都展现出了良好的发展前景。无人艇在运动过程中需要有与外界环境交互的能力，即通过各种传感器及感知设备获得外界环境的有用信息，并进行环境建模。环境建模是路径规划的重要组成部分，目的在于实现从无人艇作业的物理环境到计算机可以处理的抽象环境的映射，使得所建立的环境模型便于计算机实现路径规划方法，如大规模数据信息的存储、处理、更新等。

目前，绝大多数的无人艇静态路径规划都是在建立环境模型时无人艇能够稳定并且精确的获得障碍物位置等信息的前提下进行的。但是无人艇在实际航行过程中，需要通过激光雷达等感知设备得到障碍物在随船坐标系下的位置信息，同时，需要依据自身航向和位置信息将障碍物位置信息转换到绝对坐标系下进行环境建模，在这个过程中，由于导航传感器自身的误差，以及感知信息与导航传感器信息存在时延差等误差，无人艇并不是总能稳定且精确的获得障碍物位置信息。而获取障碍物信息的不稳定与不精确会对无人艇路径规划的准确性及无人艇航行的安全性产生巨大影响。

现阶段对水面无人艇环境建模的研究大都考虑的是障碍物在环境中的表示方法，并没有考虑无人艇在实际航行过程中获取障碍物信息的稳定性和准确性，也没有考虑无人艇在航行过程中环境的不断更新变化。本方法考虑了无人艇在实际航行过程中获取的感知信息存在误差，提高了环境建模过程中在环境模型中标记障碍物的稳定性，确保无人艇能够稳定准确的躲避障碍物。本方法对于无人艇在实际情况下处于障碍物较多且较小的复杂环境中的精确路径规划有重要作用。

发明内容

本发明的目的在于提供了一种考虑感知信息误差的无人艇局部环境建模方法，减少感知信息误差对环境建模造成的不稳定影响。

本发明的目的是这样实现的：

一种考虑感知信息误差的无人艇局部环境建模方法，具体的实现步骤为：

步骤1.获取无人艇运动状态信息和环境感知信息；

步骤2.对获取的环境感知信息进行处理；

步骤3.建立环境模型中的障碍物更新机制；依据障碍物出现的概率，决定其在环境模型中的标识与剔除；

步骤4.建立感知环境模型，对于环境的表示，采用便捷高效的栅格法。

所述步骤1的具体步骤为：

步骤1.1.通过串口接收GPS、电子罗盘TCM的数据信息，通过激光雷达获取环境感知信息；

步骤1.2.分别根据相应传感器的通讯协议，对接收到的数据串进行校验、解码，获得无人水面艇当前的位置坐标(s_x,s_y)、速度v、航向角

姿态角的运动状态信息，以及障碍物位置的环境信息；

步骤1.3.对原始数据中存在的野值进行识别、剔除与补正，采用多项式拟合法来对运动状态信息进行判断和剔除野值；

步骤1.4.用卡尔曼滤波方法进行数据处理；通过卡尔曼滤波算法得到系统输入的导航信息的最优估计值。

所述步骤2的具体步骤为：

将激光雷达得到的随体坐标依据无人艇当前首向进行坐标系旋转，得到以无人艇为原点的绝对坐标下的坐标，再结合无人艇自身位置得到障碍物的瞬时位置信息；集合O_t表示激光雷达第t次检测到的瞬时障碍物集合，O_t＝{O_1t,O_2t······O_nt}，

t表示当前无人艇接收激光雷达感知信息的总次数，n表示障碍物的个数，j表示集合中的第j个障碍物，

表示激光雷达第t次检测到的瞬时障碍物集合中的第j个障碍物的经纬度。

所述步骤3的具体步骤为：

步骤3.1.建立历史障碍物信息集合P_t＝{P_1t,P_2t·····P_mt}与环境障碍物集合R_t＝{R_1t,R_2t······R_lt}，集合P_t保存从第一次接收到第t次的感知信息，t表示当前无人艇接收激光雷达感知信息的总次数，集合P_t中的每一个元素P_it代表集合中第i个障碍物的信息，

其中，

代表该障碍物的经度值，

代表该障碍物的纬度值，

代表该障碍物被检测到的次数，

代表从该障碍物第一次出现到当前时刻之间激光雷达检测的次数，

代表当前该障碍物出现的概率，

集合R_t保存当前时刻需要标识在环境中的感知信息，R_t中元素R_it的属性与P_it相同；

步骤3.2.初始化集合P_t，当t＝1时，

即P₁中每个障碍物的位置为O₁中每个障碍物的位置，且初始时刻出现概率为0；集合P_t中元素P_it通过是否检测到新出现障碍物以及该元素中

值的大小进行添加与剔除；

步骤3.3.更新集合P_t；将第t次检测到的瞬时障碍物信息集合O_t中的每个元素O_jt中的位置

依次与第t-1次的历史障碍物集合P_t-1中每个元素P_i(t-1)中的位置

进行比较，比较公式为

其中，ΔLAT表示纬度与米的转换量，即距离变化1米的纬度值变化量，ΔLON表示经度与米的转换量，即距离变化1米的经度值变化量，ε为系数，依据无人艇所需检测的精度确定，ε越小表明障碍物位置越精确；

若

满足比较公式，则

当

时开始计算并更新

值，

当

时，

的值为0，其中σ为一常数，依据激光雷达频率决定其取值；当i＝m时，若

没有满足比较公式，将

添加到P_t；当j＝n时，若

没有满足比较公式，则

当

时开始计算并更新

值；当

时，

的值为0；

步骤3.4.得到环境障碍物集合R_t；依次将集合P_t中障碍物P_it的出现概率

与出现概率最大阈值ε_max和最小阈值ε_min进行比较，如果

将其添加到环境障碍集合R_t中，该障碍物不再会被剔除，同时

也停止更新；如果

则将其从P_t中剔除，如果该元素同时在环境障碍集合R_t中，同时将其从R_t中剔除；如果

则依据之更新的

判断其是否剔除。

所述步骤4的具体步骤为：

步骤4.1.以无人艇自身位置(s_x,s_y)为中心，以激光雷达的感知范围L为边长建立无人艇局部作业区域；

步骤4.2.对无人艇局部作业区域进行栅格化，栅格的边长依据无人艇的运动能力而定，本方法取栅格边长l＝v·Δt，v为无人艇的平均航速，Δt为无人艇的运动控制节拍；

步骤4.3.对作业区域的栅格进行编码；

步骤4.4.将集合R_t中的每个障碍物元素依次标识到感知栅格环境中。

本发明的有益效果在于：本方法考虑水面无人艇在实际航行过程中感知信息存在的误差，通过对无人艇获取的瞬时障碍物进行筛选，将大概率出现的较准确的障碍物位置标识在环境模型中，剔除了由于感知误差而误产生的障碍物点，提高了无人艇在实际航行过程中路径规划的稳定性和准确性，对于无人艇在实际情况下处于障碍物较多且较小的复杂环境中的精确路径规划有重要作用。

附图说明

图1为本发明的整体步骤流程示意图。

图2为本发明的简要整体步骤流程示意图。

具体实施方式

下面结合附图对本发明做进一步的描述：

实施例1

本方法考虑了无人艇在实际航行过程中获取的感知信息存在误差，提高了环境建模过程中在环境模型中标记障碍物的稳定性，确保无人艇能够稳定准确的躲避障碍物，提供了一种考虑感知信息误差的无人艇局部环境建模方法，具体的实现步骤如下：

步骤1.获取水面无人艇运动状态信息和环境感知信息；

步骤1.1.通过串口接收GPS、电子罗盘(TCM)的数据信息，通过激光雷达获取环境感知信息；

步骤1.2.分别根据相应传感器的通讯协议，对接收到的数据串进行校验、解码，获得无人水面艇当前的位置坐标(s_x,s_y)、速度v、航向角

姿态角等运动状态信息，以及障碍物位置等环境信息；

步骤1.3.GPS采用GPS0183通讯协议，该协议的解码示例如下：

GPS发送字符串示例：

$GPRMC,024813.640,A,3158.4608,N,11848.3737,E,10.05,324.27,150706,,,A*50

其中，字段0：$GPRMC,语句ID。

字段1：UTC时间，hhmmss.sss格式

字段2：状态，A＝定位，V＝为定位

字段3：纬度ddmm.mmmm，度分格式(前导位数不足则补0)

字段4：纬度N(北纬)或S(南纬)

字段5：经度dddmm.mmmm，度分格式(前导位数不足则补0)

字段6：经度E(东经)或W(西经)

字段7：速度，节，Knots

字段8：方位角，度

字段9：UTC日期，DDMMYY格式

字段10：磁偏角，(000-180)度(前导位数不足则补0)

字段11：磁偏角方向，E＝东W＝西

字段12：校验值

通过对串口数据进行读写，判断RMC语句状态字段地位的有效性，A为已定位，V为未定位，之后，在该语句中截取出对应的经纬度、速度以及航向角信息；

步骤1.4.电子罗盘(TCM),通讯协议，解码示例：

TCM发送信息示例：【00 15 05 03(05 43 37 98 06)(18 3F CB 61 75)(19 BF CE5F 77)9A E8】

第1-2两个字节表示总字节数；

第3字节为05表示返回数据标志数；

第4字节为03表示输出3种数据；

第5字节为05表示方位角标志；

第6-9字节为方位角数据；

第10字节为18表示俯仰角标志；

第11-14字节为俯仰角数据；

第15字节为19表示横滚角标志

第16-19字节为横滚角数据

第20-21字节为校验值

TCM发送的数据为一串十六进制数，一个十六进制数为四个二进制数，一个字节为八个二进制数，故两个十六进制数为一个字节。方位角、俯仰角和横滚角的数据表示方式为32位浮点数，在提取数据时将十六进制数转换为32位浮点数后，再将32位浮点数转换为十进制数，得到方位角、俯仰角和横滚角的数值；

步骤1.5.对上述原始数据中可能存在的野值进行识别、剔除与补正。采用多项式拟合法来对上述运动状态信息进行判断和剔除野值。为消除多项式拟合过程中新出现的野值干扰正常历史数据的缺点，利用七点二阶公式进行前推差分，按离散时间点顺序逐一计算观测数据为

则野值χ_k满足

步骤1.6.用卡尔曼滤波方法进行数据处理。通过卡尔曼滤波算法得到系统输入的导航信息的最优估计值。

步骤2.对获取的环境感知信息进行处理

步骤2.1.在环境建模时，无人艇位置与障碍物位置应处于同一坐标系下。在实际情况中，无人艇位置信息一般通过经纬度表示，而激光雷达等感知设备得到的位置信息是以其自身位置为中心的随船坐标系，因此，需要将激光雷达得到的随体坐标依据无人艇当前首向进行坐标系旋转，得到以无人艇为原点的绝对(北东)坐标下的坐标，再结合无人艇自身位置得到障碍物的瞬时位置信息。转化过程如下：

其中，T_x、T_y表示以激光雷达为坐标原点的随体坐标系下的坐标值，T_x表示垂直于船首方向的坐标值，T_y表示平行于船首方向的坐标值。

表示船首与正北方向的夹角，

S_x、S_y表示无人艇自身位置的经纬度坐标。ΔLAT表示纬度与米的转换量，即距离变化1米的纬度值变化量。ΔLON表示经度与米的转换量，即距离变化1米的经度值变化量。x、y表示转换后的障碍物的纬度、经度坐标值。

步骤2.2.集合O_t表示激光雷达第t次检测到的瞬时障碍物集合，O_t＝{O_1t,O_2t······O_nt}，

t表示当前无人艇接收激光雷达感知信息的总次数，n表示障碍物的个数，j表示集合中的第j个障碍物，

表示激光雷达第t次检测到的瞬时障碍物集合中的第j个障碍物的经纬度。

步骤3.建立环境模型中的障碍物更新机制。依据障碍物出现的概率，决定其在环境模型中的标识与剔除。

步骤3.1.建立历史障碍物信息集合P_t＝{P_1t,P_2t·····P_mt}，与环境障碍物集合R_t＝{R_1t,R_2t······R_lt}。集合P_t用于保存从第一次接收到第t次的的感知信息，t表示当前无人艇接收激光雷达感知信息的总次数，集合P_t中的每一个元素P_it代表集合中第i个障碍物的信息，

其中，

代表该障碍物的经度值，

代表该障碍物的纬度值，

代表该障碍物被检测到的次数，

代表从该障碍物第一次出现到当前时刻之间激光雷达检测的次数，

代表当前该障碍物出现的概率，

集合R_t用于保存当前时刻需要标识在环境中的感知信息，R_t中元素R_it的属性与P_it相同。

步骤3.2.初始化集合P_t。当t＝1时，

即P₁中每个障碍物的位置为O₁中每个障碍物的位置，由于初始时刻检测次数太少，故令其出现概率为0。之后集合P_t中元素P_it通过是否检测到新出现障碍物以及该元素中

值的大小进行添加与剔除。

步骤3.3.更新集合P_t。将第t次检测到的瞬时障碍物信息集合O_t中的每个元素O_jt中的位置

依次与第t-1次的历史障碍物集合P_t-1中每个元素P_i(t-1)中的位置

进行比较，

其中，ΔLAT表示纬度与米的转换量，即距离变化1米的纬度值变化量；ΔLON表示经度与米的转换量，即距离变化1米的经度值变化量；ε为系数，依据无人艇所需检测的精度确定，ε越小表明障碍物位置越精确。

若

满足式(3-1)中的关系，则认为同一障碍物又出现一次，即

当

时开始计算并更新

值，

当

时，

的值为0，其中σ为一常数，依据激光雷达频率决定其取值。当i＝m时，若

仍没有满足式(3-1)，则认为O_jt是P_t-1中没有的障碍物，即出现新检测到的障碍物，将O_jt的位置信息添加到P_t中，即将

添加到P_t。当j＝n时，若

仍没有满足式(3-1)，则认为该障碍物在第t次没有被检测到，即

当

时开始计算并更新

值；当

时，

的值为0。

步骤3.4.得到环境障碍物集合R_t。依次将集合P_t中障碍物P_it的出现概率

与出现概率最大阈值ε_max和最小阈值ε_min进行比较，如果

则该障碍物是准确存在的，将其添加到环境障碍集合R_t中，该障碍物不再会被剔除，同时

也停止更新；如果

则认为该障碍物是由于感知信息误差产生的，将其从P_t中剔除，如果该元素同时在环境障碍集合R_t中，同时将其从R_t中剔除；如果

则认为该障碍物可能是由感知信息误差产生的，依据之后更新后的

判断其是否剔除。

步骤4.建立感知环境模型，对于环境的表示，本方法采用便捷高效的栅格法。

步骤4.1.以无人艇自身位置(s_x,s_y)为中心，以激光雷达的感知范围L为边长建立无人艇局部作业区域。

步骤4.2.对无人艇局部作业区域进行栅格化，栅格的边长依据无人艇的运动能力而定，本方法取栅格边长l＝v·Δt，v为无人艇的平均航速，Δt为无人艇的运动控制节拍。

步骤4.3.通过(r_i,c_i)可以唯一标识栅格环境中的每一个栅格中心，

步骤4.4.将集合R_t中的每个障碍物元素依次标识到感知栅格环境中。

Claims (4)

1.一种考虑感知信息误差的无人艇局部环境建模方法，其特征在于，具体的实现步骤为：

步骤1获取无人艇运动状态信息和环境感知信息；

步骤2.对获取的环境感知信息进行处理；

步骤3.建立环境模型中的障碍物更新机制；依据障碍物出现的概率，决定其在环境模型中的标识与剔除；

步骤3.1：建立历史障碍物信息集合P_t＝{P_1t,P_2t……P_mt}与环境障碍物集合R_t＝{R_1t,R_2t……R_lt}；

其中，集合P_t用于保存从第一次接收到第t次的历史障碍物感知信息，t表示当前无人艇接收激光雷达感知信息的总次数，元素P_it代表集合P_t中第i个历史障碍物的信息，

代表第i个历史障碍物的经度值，

代表第i个历史障碍物的纬度值，

代表第i个历史障碍物被检测到的次数，

代表从第i个历史障碍物第一次出现到当前时刻之间激光雷达检测的次数，

代表当前第i个历史障碍物出现的概率，

集合R_t用于保存当前时刻需要标识在环境中的障碍物感知信息，元素R_kt代表集合R_t中第k个障碍物的信息，

代表第k个障碍物的经度值，

代表第k个障碍物的纬度值；

代表第k个障碍物被检测到的次数，

代表从第k个障碍物第一次出现到当前时刻之间激光雷达检测的次数，

代表当前第k个障碍物出现的概率，

步骤3.2：初始化集合P_t；当t＝1时，

即P₁中每个障碍物的位置为O₁中每个障碍物的位置；

步骤3.3：更新集合P_t；将第t次检测到的瞬时障碍物信息集合O_t中的每个元素O_jt中的位置

依次与第t-1次的历史障碍物集合P_t-1中每个元素P_i(t-1)中的位置

进行比较，j＝1,2…n；

若

满足

则认为同一障碍物又出现一次，即

当

时更新

值；当

时，

的值为0；

其中，ΔLAT表示纬度与米的转换量，即距离变化1米的纬度值变化量；ΔLON表示经度与米的转换量，即距离变化1米的经度值变化量；ε为系数，依据无人艇所需检测的精度确定，ε越小表明障碍物位置越精确；σ为一常数，依据激光雷达频率决定其取值

当i＝m时，若

仍没有满足

则认为O_jt是P_t-1中没有的障碍物，即出现新检测到的障碍物，将O_jt的位置信息添加到P_t中，即将

添加到P_t；

当j＝n时，若

仍没有满足

则认为该障碍物在第t次没有被检测到，即

当

时更新

值；当

时，

的值为0；

步骤3.4：得到环境障碍物集合R_t；依次将集合P_t中障碍物P_it的出现概率

与出现概率最大阈值ε_max和最小阈值ε_min进行比较，如果

则该障碍物是准确存在的，将其添加到环境障碍集合R_t中，该障碍物不再会被剔除，同时

也停止更新；如果

则认为该障碍物是由于感知信息误差产生的，将其从P_t中剔除，如果该元素同时在环境障碍集合R_t中，同时将其从R_t中剔除；如果

则认为该障碍物可能是由感知信息误差产生的，依据之后更新后的

判断其是否剔除；

步骤4.建立感知环境模型，对于环境的表示，采用栅格法。

2.根据权利要求1所述的一种考虑感知信息误差的无人艇局部环境建模方法，其特征在于，所述步骤1的具体步骤为：

步骤1.1.通过串口接收GPS、电子罗盘TCM的数据信息，通过激光雷达获取环境感知信息；

步骤1.2.分别根据相应传感器的通讯协议，对接收到的数据串进行校验、解码，获得无人水面艇当前的位置坐标(s_x,s_y)、速度v、航向角

姿态角的运动状态信息，以及障碍物位置的环境信息；

步骤1.3.对原始数据中存在的野值进行识别、剔除与补正，采用多项式拟合法来对运动状态信息进行判断和剔除野值；

步骤1.4.用卡尔曼滤波方法进行数据处理；通过卡尔曼滤波算法得到系统输入的导航信息的最优估计值。

3.根据权利要求1所述的一种考虑感知信息误差的无人艇局部环境建模方法，其特征在于，所述步骤2的具体步骤为：

将激光雷达得到的随体坐标依据无人艇当前首向进行坐标系旋转，得到以无人艇为原点的绝对坐标下的坐标，再结合无人艇自身位置得到障碍物的瞬时位置信息；集合O_t表示激光雷达第t次检测到的瞬时障碍物集合，O_t＝{O_1t,O_2t……O_nt}，

t表示当前无人艇接收激光雷达感知信息的总次数，n表示障碍物的个数，j表示集合中的第j个障碍物，

表示激光雷达第t次检测到的瞬时障碍物集合中的第j个障碍物的经纬度。

4.根据权利要求1所述的一种考虑感知信息误差的无人艇局部环境建模方法，其特征在于，所述步骤4的具体步骤为：

步骤4.1.以无人艇自身位置(s_x,s_y)为中心，以激光雷达的感知范围L为边长建立无人艇局部作业区域；

步骤4.2.对无人艇局部作业区域进行栅格化，栅格的边长依据无人艇的运动能力而定，本方法取栅格边长l＝v·Δt，v为无人艇的平均航速，Δt为无人艇的运动控制节拍；

步骤4.3.对作业区域的栅格进行编码；

步骤4.4.将集合R_t中的每个障碍物元素依次标识到感知栅格环境中。

Images