[发明专利]一种基于到达角度测距的协作定位方法及装置

权利要求书

1.一种基于到达角度测距的协作定位方法，其特征在于，所述方法包括：

获取当前待定位节点的初始位置坐标，所述初始位置坐标为卫星定位坐标；

基于所述当前待定位节点与当前待定位节点的各邻居节点间的信号交互，获取所述当前待定位节点接收所述邻居节点信号时的到达角度；所述邻居节点包括：与所述当前待定位节点的通信半径小于预设阈值的，位置已知的节点和位置未知的节点；

获取所述当前待定位节点的方向角；

利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标；

当所述当前待定位节点的所述当前位置坐标达到第一预设收敛条件时，将所述当前位置坐标确定为所述当前待定位节点的目标定位坐标。

2.根据权利要求1所述的方法，其特征在于，所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标之后，所述方法还包括：

判断所述当前待定位节点的所述当前位置坐标是否达到第一预设收敛条件；

如果所述当前待定位节点的所述当前位置坐标没有达到第一预设收敛条件，则返回执行所述利用所获取的所述到达角度，所述初始位置坐标，以及所述方向角，使用梯度下降法更新所述当前待定位节点的初始位置坐标，得到所述当前待定位节点的当前位置坐标的步骤。

3.根据权利要求1所述的方法，其特征在于，所述获取所述当前待定位节点的方向角的步骤，包括：

利用所述当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到所述当前待定位节点的方向角；

所述当所述当前待定位节点的所述当前位置坐标达到第一预设收敛条件时，将所述当前位置坐标确定为所述当前待定位节点的目标定位坐标之后，所述方法还包括：

判断所述当前待定位节点的目标定位坐标是否达到第二预设收敛条件；

如果所述当前待定位节点的目标定位坐标达到第二预设收敛条件，则将所述当前待定位节点的目标定位坐标，确定为所述当前待定位节点的最终目标定位坐标；

如果所述当前待定位节点的目标定位坐标没有达到第二预设收敛条件，则将所述当前待定位节点的初始位置坐标更新为所述目标定位坐标，并执行利用所述当前待定位节点的初始位置坐标，及预设的最大似然估计算法，得到所述当前待定位节点的方向角的步骤。