CN111174790B - 一种地形剖面跟踪路径的形成方法 - Google Patents

一种地形剖面跟踪路径的形成方法 Download PDF

Info

Publication number
CN111174790B
CN111174790B CN201911217550.8A CN201911217550A CN111174790B CN 111174790 B CN111174790 B CN 111174790B CN 201911217550 A CN201911217550 A CN 201911217550A CN 111174790 B CN111174790 B CN 111174790B
Authority
CN
China
Prior art keywords
points
point
curve
tracking
slope
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201911217550.8A
Other languages
English (en)
Other versions
CN111174790A (zh
Inventor
王汝鹏
徐鹏飞
骆佳成
程红霞
张弛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hohai University HHU
Original Assignee
Hohai University HHU
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hohai University HHU filed Critical Hohai University HHU
Priority to CN201911217550.8A priority Critical patent/CN111174790B/zh
Publication of CN111174790A publication Critical patent/CN111174790A/zh
Application granted granted Critical
Publication of CN111174790B publication Critical patent/CN111174790B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Processing Or Creating Images (AREA)

Abstract

本发明公开一种地形剖面跟踪路径的形成方法，包括如下过程：对于任一地形剖面线，将所述地形剖面线上的所有离散点进行法向偏移得到对应的偏移点；删除所有偏移点中不满足拟合条件的偏移点，将剩余偏移点进行样条曲线拟合得到极限跟踪测量路径；以所述极限跟踪测量路径上连续的三个点为一组将所述极限跟踪测量路径划分为若干曲线段；根据探测器的运动约束对每个曲线段中不满足约束条件的点进行调整，得到地形剖面跟踪测量路径；所述约束条件为上浮和下潜过程中对应的极限潜浮角、极限运动路径的曲率极限以及斜率跟踪极限。本发明通过跟踪地形的剖面信息进行快速而精确的底跟踪路径生成，从而有助于对海底探测的深入研究。

Description

一种地形剖面跟踪路径的形成方法
技术领域
本发明属于水下地形测绘技术领域，具体涉及一种地形剖面跟踪路径的形成方法。
背景技术
随着海底探测任务需求的增加，AUV的近海底定高跟踪能力越来越得到重视。由于水下探测传感器的感知范围约束，AUV必须保持距海底一定高度航行才能保证自身安全性和对底观测的精度，而且水下地形具有随机性且水下地形的探测范围有限，因此AUV需要根据地形剖面的局部探测信息和自身状态实时的、动态的规划底跟踪路径。而现有技术中，AUV规划跟踪路径存在误差较大，不够精确的问题，因此制约了对海底探测的更深一步的研究。
发明内容
本发明的目的在于克服现有技术中的不足，提供一种地形剖面跟踪路径的形成方法，通过跟踪地形的剖面信息进行快速而精确的底跟踪路径生成。
为解决现有技术问题，本发明公开了一种地形剖面跟踪路径的形成方法，包括如下过程：
对于任一地形剖面线，
将所述地形剖面线上的所有离散点进行法向偏移得到对应的偏移点；
删除所有偏移点中不满足拟合条件的偏移点，将剩余偏移点进行样条曲线拟合得到极限跟踪测量路径；
以所述极限跟踪测量路径上连续的三个点为一组将所述极限跟踪测量路径划分为若干曲线段；
根据探测器的运动约束对每个曲线段中不满足约束条件的点进行调整，得到地形剖面跟踪测量路径；所述约束条件为上浮和下潜过程中对应的极限潜浮角、极限运动路径的曲率极限以及斜率跟踪极限。
进一步地，
所述拟合条件为：对于任一偏移点，该偏移点与地形剖面线之间最小距离不小于该偏移点的偏移距离。
进一步地，
所述根据探测器的运动约束对每个曲线段中不满足约束条件的点进行调整的过程具体为：
当曲线段为凸曲线且中间点的曲率不符合曲率极限约束条件时，则对两个端点进行调整；
当曲线段为凹曲线且两个端点的曲率不符合曲率极限约束条件时，则对中间点进行调整；
当曲线段中相邻两个点的连线的斜率为负值且斜率不满足斜率跟踪约束条件时，则对后一个点进行调整；
当曲线段中相邻两个点的连线的斜率为正值且斜率不满足斜率跟踪约束条件时，则对前一个点进行调整。
进一步地，
所述当曲线段为凸曲线且中间点的曲率不符合曲率极限约束条件时，则对两个端点和进行调整的具体过程为：
对于平面坐标系x0y内的曲线段，其两个端点和/>的调整量分别为：
其中，mi表示端点的调整量；mi+2表示端点/>的调整量；ni+1表示曲线段/>在点的二阶导数；sign(ni+1)表示二阶导数ni+1的符号，表明曲线段/>在点/>的凹凸性；λκ表示控制点/>位置调整权重；/>表示曲线段/>满足约束条件时控制点/>的位置；/>表示点/>与/>点之间的距离；/>表示曲线段/>满足约束条件时控制点/>的位置；表示点/>与/>点之间的距离，平面坐标系x0y的x轴为地形剖面线的水平面投影轴，y轴为地形剖面线的高度轴。
进一步地，
所述当曲线段为凹曲线且两个端点的曲率不符合曲率极限约束条件时，则对中间点进行调整的具体过程为：
对于平面坐标系x0y内的曲线段，其中间点的调整量为：
其中，ni+1表示曲线段在点/>的二阶导数；sign(ni+1)表示二阶导数ni+1的符号，表明曲线段/>在点/>的凹凸性；λκ表示控制点/>位置调整权重；/>表示曲线段/>满足约束条件时的/>控制点位置；/>表示/>与/>控制点之间的距离，平面坐标系x0y的x轴为地形剖面线的水平面投影轴，y轴为地形剖面线的高度轴。
进一步地，
所述当曲线段中相邻两个点的连线的斜率为负值且斜率不满足斜率跟踪约束条件时，则对后一个点进行调整的具体过程为：
对于平面坐标系x0y内的曲线段，其后一个点的调整量为：
其中，zi+1表示曲线段在后一个点/>的y方向的分量，z方向为高度方向；d表示曲线段上相邻两个点/>和/>在x方向的距离，x方向为横坐标方向；λk表示控制点/>位置调整权重，平面坐标系x0y的x轴为地形剖面线的水平面投影轴，y轴为地形剖面线的高度轴。
进一步地，
所述当曲线段中相邻两个点的连线的斜率为正值且斜率不满足斜率跟踪约束条件时，则对前一个点进行调整的具体过程为：
对于平面坐标系x0y内的曲线段，其前一个点的调整量为：
其中，zi表示曲线段C在点的y方向的分量，z方向为高度方向；d表示曲线段上相邻两个点/>和/>在x方向的距离，x方向为横坐标方向；λk表示控制点/>位置调整权重，平面坐标系x0y的x轴为地形剖面线的水平面投影轴，y轴为地形剖面线的高度轴。
进一步地，
还包括如下步骤：
若所述极限跟踪测量路径上的任一极点与其相邻的拐点之间的间距超过设定值则在两者之间增加一个划分点，然后将划分后的得到的所有点以连续的三个点为一组将所述极限跟踪测量路径划分为若干曲线段。
本发明具有的有益效果：通过跟踪地形的剖面信息进行快速而精确的底跟踪路径生成，从而有助于对海底探测的深入研究。
附图说明
图1为本发明中极限跟踪测量路径的形成示意图；
图2为本发明中极限跟踪测量路径的进行等间距划分的示意图；
图3为本发明中极限跟踪测量路径的进行曲线段划分的示意图；
图4为AUV在初始平衡状态下下潜或上浮运动中的极限示意图；
图5为本发明中凸曲线的调整过程图；
图6为本发明中凹曲线的调整过程图；
图7为本发明中正斜率曲线的调整过程图；
图8为本发明中负斜率曲线的调整过程图；
图9为本发明中跟踪路径与地形剖面线的对比图。
具体实施方式
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
如图1至8所示，一种地形剖面跟踪路径的形成方法，包括如下过程：
以地形剖面线的水平面投影轴作为横坐标，高度作为纵坐标建立平面坐标系x0y，对于平面坐标系x0y内的任一地形剖面线C0作出如下处理：
S1、如图1所示，将地形剖面线C0上的所有离散点进行法向偏移得到对应的偏移点以及其表达式。以地形剖面线C0上的三个连续的离散点pi，pi+1，pi+2为例，离散点pi+1偏移后得到的对应的偏移点Xi+1的表达式为：其中，dopt表示偏移量，/>表示单位向量，由/>和/>合成得到，/>表示/>垂直方向上且位于地形剖面外侧的单位向量，/>表示/>垂直方向上且位于地形剖面外侧的单位向量，/>表示离散点pi和pi+1连线的斜率，/>表示离散点pi+1和pi+2连线的斜率。
S2、对于偏移后得到的任一离散点Xi＝[xi,yi]，若不满足拟合条件dm≥dopt，则将不满足该拟合条件的偏移点删除，将剩余偏移点计为通过样条曲线拟合方法拟合/>得到曲线/>即为极限跟踪测量路径。
S3、如图2所示，若极限跟踪测量路径上的任一极点与其相邻的拐点之间的间距超过设定值时则在两者之间增加一个划分点，例如超过其它任一相邻两个点之间的距离时，具体地，在划分时，按照等间距方式进行划分。这样做的目的是提高最终的规划路径的精度。如图3所示，将划分后的得到的所有点以连续的三个点为一组将极限跟踪测量路径划分为若干曲线段/>
S4、如图4所示，获取探测器前向运动可达集合的边界，也就是其极限操纵运动下的前向运动极限，获得其运动约束参数，上浮和下潜极限潜浮角和χ，极限运动路径的曲率极限/>和κ，斜率跟踪极限/>和k。然后对于S3中获得的曲线段/>判断其是否满足上述运动约束，对于不满足运动约束的点，按照如下方法进行调整：
S41、如图5所示，当曲线段为凸曲线且中间点的曲率不符合曲率极限约束条件时，则对两个端点进行调整，具体过程为：
对于由点构成的曲线段，在/>处不满足曲率约束，需要调整/>和两个点，/>和/>的调整量分别为：
其中，mi表示端点的调整量；mi+2表示端点/>的调整量；ni+1表示曲线段/>在点的二阶导数；sign(ni+1)表示二阶导数ni+1的符号，表明曲线段/>在点/>的凹凸性；λκ表示控制点/>位置调整权重；/>表示曲线段/>满足约束条件时控制点/>的位置；表示点/>与/>点之间的距离；/>表示曲线段/>满足约束条件时控制点/>的位置；/>表示点/>与/>点之间的距离。
S42、如图6所示，当曲线段为凹曲线且两个端点的曲率不符合曲率极限约束条件时，则对中间点进行调整，具体过程为：
对于由点构成的曲线段，需要调整/>点，/>的调整量为：
其中，ni+1表示曲线段在点/>的二阶导数；sign(ni+1)表示二阶导数ni+1的符号，表明曲线段/>在点/>的凹凸性；λκ表示控制点/>位置调整权重；/>表示曲线段/>满足约束条件时的/>控制点位置；/>表示/>与/>控制点之间的距离。
S43、如图7所示，对于不满足斜率约束的曲线段，同样的可以通过调整控制点的位置使其满足斜率约束。以当前点沿AUV航行方向为路径调整过程的约束监测和调整单元。当曲线段中相邻两个点的连线的斜率为负值且斜率不满足斜率跟踪约束条件时，则对后一个点进行调整，具体过程为：
对于由点构成的曲线段，在点/>位置不满足斜率约束，需要调整点/>的位置使其满足斜率约束，/>为斜率为k的直线，/>为线段/>与/>点的曲线分割线的交点，则线段/>的长度为控制线段/>满足斜率为k时的调整距离。因此，为控制曲线在点/>的斜率满足斜率约束k，点/>的调整距离为：
其中，zi+1表示曲线段在后一个点/>的y方向的分量，z方向为高度方向；d表示曲线段上相邻两个点/>和/>在x方向的距离，x方向为横坐标方向；λk表示控制点/>位置调整权重。
S44、如图8所示，当曲线段中相邻两个点的连线的斜率为正值且斜率不满足斜率跟踪约束条件时，则对前一个点进行调整，具体过程为：
对于由点构成的曲线段，需要调整点/>相应的调整量为：
其中，zi表示曲线段C在点的y方向的分量，z方向为高度方向；d表示曲线段上相邻两个点/>和/>在x方向的距离，x方向为横坐标方向；λk表示控制点/>位置调整权重。
如图9所示，经过调整后即可得到更为精确的跟踪测量路径。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (7)

1.一种地形剖面跟踪路径的形成方法，其特征在于：包括如下过程：
对于任一地形剖面线，
将所述地形剖面线上的所有离散点进行法向偏移得到对应的偏移点；
删除所有偏移点中不满足拟合条件的偏移点，将剩余偏移点进行样条曲线拟合得到极限跟踪测量路径；
以所述极限跟踪测量路径上连续的三个点为一组将所述极限跟踪测量路径划分为若干曲线段；
根据探测器的运动约束对每个曲线段中不满足约束条件的点进行调整，得到地形剖面跟踪测量路径；所述约束条件为上浮和下潜过程中对应的极限潜浮角、极限运动路径的曲率极限以及斜率跟踪极限；
所述根据探测器的运动约束对每个曲线段中不满足约束条件的点进行调整的过程具体为：
当曲线段为凸曲线且中间点的曲率不符合曲率极限约束条件时，则对两个端点进行调整；
当曲线段为凹曲线且两个端点的曲率不符合曲率极限约束条件时，则对中间点进行调整；
当曲线段中相邻两个点的连线的斜率为负值且斜率不满足斜率跟踪约束条件时，则对后一个点进行调整；
当曲线段中相邻两个点的连线的斜率为正值且斜率不满足斜率跟踪约束条件时，则对前一个点进行调整。
2.根据权利要求1所述的一种地形剖面跟踪路径的形成方法，其特征在于：
所述拟合条件为：对于任一偏移点，该偏移点与地形剖面线之间最小距离不小于该偏移点的偏移距离。
3.根据权利要求1所述的一种地形剖面跟踪路径的形成方法，其特征在于：
所述当曲线段为凸曲线且中间点的曲率不符合曲率极限约束条件时，则对两个端点和进行调整的具体过程为：
对于平面坐标系x0y内由点构成的曲线段，其两个端点/>和/>的调整量分别为：
其中，mi表示端点的调整量；mi+2表示端点/>的调整量；ni+1表示曲线段/>在点/>的二阶导数；sign(ni+1)表示二阶导数ni+1的符号，表明曲线段/>在点/>的凹凸性；λκ表示控制点/>位置调整权重；/>表示曲线段/>满足约束条件时控制点/>的位置；/>表示点/>与/>点之间的距离；/>表示曲线段/>满足约束条件时控制点/>的位置；表示点/>与/>点之间的距离，平面坐标系x0y的x轴为地形剖面线的水平面投影轴，y轴为地形剖面线的高度轴。
4.根据权利要求1所述的一种地形剖面跟踪路径的形成方法，其特征在于：
所述当曲线段为凹曲线且两个端点的曲率不符合曲率极限约束条件时，则对中间点进行调整的具体过程为：
对于平面坐标系x0y内由点构成的的曲线段，其中点/>的调整量为：
其中，ni+1表示曲线段在点/>的二阶导数；sign(ni+1)表示二阶导数ni+1的符号，表明曲线段/>在点/>的凹凸性；λκ表示控制点/>位置调整权重；/>表示曲线段/>满足约束条件时的/>控制点位置；/>表示/>与/>控制点之间的距离，平面坐标系x0y的x轴为地形剖面线的水平面投影轴，y轴为地形剖面线的高度轴。
5.根据权利要求1所述的一种地形剖面跟踪路径的形成方法，其特征在于：
所述当曲线段中相邻两个点的连线的斜率为负值且斜率不满足斜率跟踪约束条件时，则对后一个点进行调整的具体过程为：
对于平面坐标系x0y内由点构成的曲线段，其点/>的调整量为：
其中，zi+1表示曲线段在点/>的y方向的分量，z方向为高度方向；d表示曲线段上相邻两个点/>和/>在x方向的距离，x方向为横坐标方向；λk表示控制点/>位置调整权重，平面坐标系x0y的x轴为地形剖面线的水平面投影轴，y轴为地形剖面线的高度轴。
6.根据权利要求1所述的一种地形剖面跟踪路径的形成方法，其特征在于：
所述当曲线段中相邻两个点的连线的斜率为正值且斜率不满足斜率跟踪约束条件时，则对前一个点进行调整的具体过程为：
对于平面坐标系x0y内由点构成的曲线段，点/>的调整量为：
其中，zi表示曲线段C在点的y方向的分量，z方向为高度方向；d表示曲线段上相邻两个点/>和/>在x方向的距离，x方向为横坐标方向；λk表示控制点/>位置调整权重，平面坐标系x0y的x轴为地形剖面线的水平面投影轴，y轴为地形剖面线的高度轴。
7.根据权利要求1至6任一所述的一种地形剖面跟踪路径的形成方法，其特征在于：还包括如下步骤：
若所述极限跟踪测量路径上的任一极点与其相邻的拐点之间的间距超过设定值则在两者之间增加一个划分点，然后将划分后的得到的所有点以连续的三个点为一组将所述极限跟踪测量路径划分为若干曲线段。
CN201911217550.8A 2019-12-03 2019-12-03 一种地形剖面跟踪路径的形成方法 Active CN111174790B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911217550.8A CN111174790B (zh) 2019-12-03 2019-12-03 一种地形剖面跟踪路径的形成方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911217550.8A CN111174790B (zh) 2019-12-03 2019-12-03 一种地形剖面跟踪路径的形成方法

Publications (2)

Publication Number Publication Date
CN111174790A CN111174790A (zh) 2020-05-19
CN111174790B true CN111174790B (zh) 2023-08-01

Family

ID=70624588

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911217550.8A Active CN111174790B (zh) 2019-12-03 2019-12-03 一种地形剖面跟踪路径的形成方法

Country Status (1)

Country Link
CN (1) CN111174790B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112581558B (zh) * 2020-10-26 2024-04-19 南京师范大学 图切地质剖面中侵入构造的模型构建方法及系统

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8131415B2 (en) * 2005-05-24 2012-03-06 Trimble Navigation, Ltd Method and apparatus for automatic vehicle guidance using continuous 2-D poly-point path
CN106651987B (zh) * 2016-10-10 2018-12-07 腾讯科技（深圳）有限公司 路径规划方法及装置
CN107085438B (zh) * 2017-04-28 2020-02-07 中国船舶重工集团公司第七0九研究所 基于准均匀样条曲线的无人机路径修正方法及系统
CN108132675B (zh) * 2017-11-23 2020-02-21 东南大学 一种工厂巡视无人机自主路径巡航以及智能避障方法
US10816977B2 (en) * 2018-01-26 2020-10-27 Baidu Usa Llc Path and speed optimization fallback mechanism for autonomous vehicles
CN109141421A (zh) * 2018-07-13 2019-01-04 哈尔滨工程大学 欠驱动auv水下动态目标跟踪中的期望路径构造方法
CN109542106A (zh) * 2019-01-04 2019-03-29 电子科技大学 一种移动机器人多约束条件下的路径规划方法
CN109782779B (zh) * 2019-03-19 2020-11-06 电子科技大学 基于种群超启发式算法的洋流环境下auv路径规划方法

Also Published As

Publication number Publication date
CN111174790A (zh) 2020-05-19

Similar Documents

Publication Publication Date Title
CN109685858B (zh) 一种单目摄像头在线标定方法
CN111060135A (zh) 一种基于局部地图的地图修正方法及系统
CN109141427B (zh) 在非视距环境下基于距离和角度概率模型的ekf定位方法
CN103389095A (zh) 一种用于捷联惯性/多普勒组合导航系统的自适应滤波方法
CN110726415B (zh) 一种自适应的水下多波束同步定位与建图方法
CN109738902B (zh) 一种基于同步信标模式的水下高速目标高精度自主声学导航方法
CN110440743B (zh) 一种基于基线的隧道变形监测方法、系统、介质及设备
CN114136311A (zh) 一种基于imu预积分的激光slam定位方法
CN114396871B (zh) 一种基于三维激光扫描的预制墩柱安装位置姿态监测方法
CN111174790B (zh) 一种地形剖面跟踪路径的形成方法
CN110824430A (zh) 基于北斗定位系统的水下定位方法
CN110596715B (zh) 一种水线智能识别系统及定位方法
CN108562872B (zh) 一种超短基线水下声学定位校准时异常值探测的方法
CN108225286B (zh) 一种移动站车载全站仪位姿估计方法
CN113847884B (zh) 基于线扫描的精细化三维测量与建模方法
CN108387233A (zh) 一种基于模糊逻辑的行人运动模式判断方法
CN109397293B (zh) 一种基于移动机器人的地面水平误差建模及补偿方法
CN104949692B (zh) 多波束测深系统安装校正值的计算方法
CN111664801A (zh) 一种基于高斯曲率的光学靶标点投影规划方法
CN117237563A (zh) 一种钢轨空间三维几何形态构建方法及装置
CN116929336A (zh) 一种基于最小误差的激光反光柱slam建图方法
CN114355287B (zh) 一种超短基线水声测距方法及系统
CN103198229B (zh) 一种铁路既有曲线的拨距计算方法
CN110132195A (zh) 叶片截面接触式扫描测量的探针测球三维半径补偿方法
CN110986860B (zh) 一种基于智能机器人的塔柱倾斜度判定方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant