一种水下机器人轨迹上障碍物检测方法技术

本发明专利技术公开了一种水下机器人轨迹上障碍物检测方法，包括：(1)可行轨迹集生成；(2)脚印栅格地图集生成；(3)概率脚印栅格地图集生成；(4)概率脚印栅格地图集保存；(5)传感器栅格地图集构建；(6)传感器数据整合；(7)碰撞检测。本发明专利技术步骤(2)建模了不同形状机器人按轨迹移动的脚印形状，使得本发明专利技术方法适用于各种形状的机器人，步骤(3)考虑了机器人在水中移动的不确定性，步骤(5)考虑了传感器测量的不确定性，另外，本发明专利技术步骤(1)

【技术实现步骤摘要】
一种水下机器人轨迹上障碍物检测方法


[0001]本专利技术涉及一种水下机器人轨迹上障碍物检测方法。

技术介绍

[0002]水下机器人对于海底勘探，救援，深远海养殖，海上平台清污及检测具有重要的意义。要实现水下机器人安全可靠运行，机器人水下障碍物检测的能力必不可少。当机器人为人为操作时，水下障碍物检测能力可以为操作人员提供碰撞概率小的候选轨迹，操作人员只需要在其中选择一条即可避免碰撞，当机器人为自主运行时，障碍物检测能力更加重要，其需要实时为机器人提供无碰撞的候选轨迹，以供路径规划层选择安全的路径。
[0003]目前的障碍物检测方法大多没有考虑机器人的形状，或者简单假设机器人的形状为方形或圆形。另外，对于水下机器人，其障碍物识别传感器一般为声呐，测量不确定性高，同时，由于水流的影响，水下机器人的移动轨迹也具有很大的不确定性，这两方面是大多数避障方法没有同时考虑的。
[0004]总之，依据目前的水下机器人轨迹上障碍物检测方法，判断机器人是否能安全运行，具有较大的不确定性。

技术实现思路

[0005]本专利技术针对上述问题，提出一种能降低不确定性的水下机器人轨迹上障碍物检测方法，从而为水下机器人的安全运行提供更有力的支撑。
[0006]本专利技术通过如下技术方案解决其技术问题：一种水下机器人轨迹上障碍物检测方法，包括如下步骤：
[0007](1)可行轨迹集生成；
[0008](2)脚印栅格地图集生成
[0009]在栅格地图上标记可行轨迹集中每条轨迹的轨迹脚印，并记录每个脚印位置的警示级别，为每条轨迹分别构建一张脚印栅格地图，所述警示级别根据机器人避开对应脚印位置具有的反应时间设定；
[0010](3)概率脚印栅格地图集生成
[0011]在栅格地图上标记水下机器人沿某条轨迹移动时，实际移动轨迹可能经过的区域，并在每个栅格中保存轨迹经过该栅格的概率，构建轨迹概率栅格地图M
probTraj(j)
，然后根据该轨迹概率栅格地图M
probTraj(j)
生成相应的脚印栅格地图，称该地图为概率脚印栅格地图M
probfp(j)
，并最终获得概率脚印栅格地图集；
[0012](4)概率脚印栅格地图集保存；
[0013](5)传感器栅格地图集构建
[0014]给定传感器测量值，确定障碍物的真实位置在传感器坐标系中可能存在的区域，并由此标记坐标系中各个栅格处可能存在障碍物的概率，称该地图为传感器栅格地图，对应不同测量值的传感器栅格地图的集合，称为传感器栅格地图集；(6)传感器数据整合
[0015]采集机器人上各个传感器的测量值{d
sensorl
，d
sensor2
…
d
sensorN
}，并根据测量值索引到相应的传感器栅格地图{M
sensor(d1)
，M
sensor(d2)
…
M
sensor(dsensorN)
}，根据各个传感器坐标系在机器人坐标系下的位姿{X
sensor1
，X
sensor2
…
X
sensorN
}，将{M
sensor(d1)
，M
sensor(d2)
....M
sensor(dsensorN)
}转化到机器人坐标系，并且进行对应栅格的占据概率p
occ(x，y)
累加，形成一张传感器融合栅格地图M
AllSensorInRobot
，并对传感器融合栅格地图中的占据概率进行归一化操作；
[0016](7)碰撞检测
[0017]将传感器融合栅格地图和概率脚印栅格地图集中的每一张栅格地图的对应栅格的概率值相乘，获取每条轨迹每个轨迹脚印栅格可能发送碰撞的概率。
[0018]本专利技术中，步骤(1)可行轨迹集生成的具体步骤为：
[0019]将连续的轨迹离散化，表示机器人的第j条连续轨迹Tra
j
离散化后的样本点的集合，X
i
≡[x
i
，y
i
，z
i
，row
i
，pitch
i
，yaw
i
]为该离散轨迹上的第i个样本点，其包括三个位置坐标x
i
、y
i
、z
i
和三个姿态坐标row
i
、pitch
i
、yaw
i
，位置表示选定的机器人坐标系原点在指定坐标系下的坐标，姿态表示机器人坐标系的翻滚角，俯仰角，航向角在指定坐标系下的表示，其中，N
ps
为该离散轨迹上离散点的个数；可行轨迹集由一系列可行轨迹组成，即其中N
traj
表示可行轨迹集合的个数。
[0020]N
ps
建议通过如下方式确定：Nps＝Tbreak
·
N/ΔTps，其中，ΔTps表示相邻两个轨迹样本点X
i
和X
i+1
的时间间隔，ΔTps取值的大小，影响对离散轨迹近似的精确性，同时也影响计算量，Tbreak为机器人从最高速到刹停的时间，N为为留设安全余量而设置的对Tbreak的放大倍数，所以其取值应大于1。
[0021]Tra
j
通过如下方式确定：
[0022](1.a)对机器人的控制量离散化获得离散控制量集合N
Ntraj
为控制量个数；
[0023](1.b)根据控制量和机器人的运动模型f()获得离散轨迹点
[0024]当控制量为U
j
时，获得的离散轨迹为：
[0025]Tra
j
＝{X0，X1＝f(X0，U
j
)，X2＝f(X1，U
j
)，...，X
Nps
＝f(X
Nps
‑1，U
j
)}。
[0026]本专利技术中，步骤(2)脚印栅格地图集生成的具体步骤为：
[0027]2.a构建栅格数据结构，并确定栅格坐标系O
grid
，栅格坐标系O
grid
和机器人坐标系O
robot
重合
[0028]每个栅格中保存如下向量M
fb
(x，y，z)≡[p
fp
(x，y，z)，dl(x，y，z)]T
，其中p
fp
(x，y，z)为对应栅格坐标(x，y，z)中有脚印的概率，dl(x，y，z)为对应栅格坐标(x，y，z)的警示级别；每个栅格的初始化值均为M
fb
(x，y，z)＝[0，安全]T
；
[0029]2.b构建第j条轨迹的栅格地图<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种水下机器人轨迹上障碍物检测方法，其特征在于，包括如下步骤：(1)可行轨迹集生成；(2)脚印栅格地图集生成在栅格地图上标记可行轨迹集中每条轨迹的轨迹脚印，并记录每个脚印位置的警示级别，为每条轨迹分别构建一张脚印栅格地图，所述警示级别根据机器人避开对应脚印位置具有的反应时间设定；(3)概率脚印栅格地图集生成在栅格地图上标记水下机器人沿某条轨迹移动时，实际移动轨迹可能经过的区域，并在每个栅格中保存轨迹经过该栅格的概率，构建轨迹概率栅格地图M
probTraj(j)
，然后根据该轨迹概率栅格地图M
probTraj(j)
生成相应的脚印栅格地图，称该地图为概率脚印栅格地图M
probfp(j)
，并最终获得概率脚印栅格地图集；(4)概率脚印栅格地图集保存；(5)传感器栅格地图集构建给定传感器测量值，确定障碍物的真实位置在传感器坐标系中可能存在的区域，并由此标记坐标系中各个栅格处可能存在障碍物的概率，称该地图为传感器栅格地图，对应不同测量值的传感器栅格地图的集合，称为传感器栅格地图集；(6)传感器数据整合采集机器人上各个传感器的测量值{d
sensor1
，d
sensor2
…
d
sensorN
}，并根据测量值索引到相应的传感器栅格地图{M
sensor(d1)
，M
sensor(d2)
....M
sensor(dsensorN)
}，根据各个传感器坐标系在机器人坐标系下的位姿{X
sensor1
，X
sensor2
....X
sensorN
}，将{M
sensor(d1)
，M
sensor(d2)
....M
sensor(dsensorN)
}转化到机器人坐标系，并且进行对应栅格的占据概率p
occ(x，y)
累加，形成一张传感器融合栅格地图M
AllSensorInRobot
，并对传感器融合栅格地图中的占据概率进行归一化操作；(7)碰撞检测将传感器融合栅格地图和概率脚印栅格地图集中的每一张栅格地图的对应栅格的概率值相乘，获取每条轨迹每个轨迹脚印栅格可能发送碰撞的概率。2.根据权利要求1所述的水下机器人轨迹上障碍物检测方法，其特征在于，步骤(1)可行轨迹集生成的具体步骤为：将连续的轨迹离散化，表示机器人的第j条连续轨迹Tra
j
离散化后的样本点的集合，X
i
≡[x
i
，y
i
，z
i
，row
i
，pitch
i
，yaw
i
]为该离散轨迹上的第i个样本点，其包括三个位置坐标x
i
、y
i
、z
i
和三个姿态坐标row
i
、pitch
i
、yaw
i
，位置表示选定的机器人坐标系原点在指定坐标系下的坐标，姿态表示机器人坐标系的翻滚角，俯仰角，航向角在指定坐标系下的表示，其中，N
ps
为该离散轨迹上离散点的个数；可行轨迹集由一系列可行轨迹组成，即其中N
traj
表示可行轨迹集合的个数。3.根据权利要求2所述的水下机器人轨迹上障碍物检测方法，其特征在于，N
ps
通过如下方式确定：Nps＝Tbreak
·
N/ΔTps，其中，ΔTps表示相邻两个轨迹样本点X
i
和X
i+1
的时间间隔，Tbreak为机器人从最高速到刹停的时间，N为为留设安全余量而设置的对Tbreak的放大倍数。
4.根据权利要求3所述的水下机器人轨迹上障碍物检测方法，其特征在于，Tra
j
通过如下方式确定：(1.a)对机器人的控制量离散化获得离散控制量集合N
Ntraj
为控制量个数；(1.b)根据控制量和机器人的运动模型f()获得离散轨迹点当控制量为U
j
时，获得的离散轨迹为：Tra
j
＝{X0，X1＝f(X0，U
j
)，X2＝...

【专利技术属性】
技术研发人员：李根，黄小华，袁太平，庞国良，陶启友，胡昱，
申请(专利权)人：三亚热带水产研究院，
类型：发明
国别省市：