IT数码 购物 网址 头条 软件 日历 阅读 图书馆
TxT小说阅读器
↓语音阅读,小说下载,古典文学↓
图片批量下载器
↓批量下载图片,美女图库↓
图片自动播放器
↓图片自动播放器↓
一键清除垃圾
↓轻轻一点,清除系统垃圾↓
开发: C++知识库 Java知识库 JavaScript Python PHP知识库 人工智能 区块链 大数据 移动开发 嵌入式 开发工具 数据结构与算法 开发测试 游戏开发 网络协议 系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑 笔记本 显卡 显示器 固态硬盘 硬盘 耳机 手机 iphone vivo oppo 小米 华为 单反 装机 图拉丁
 
   -> 数据结构与算法 -> python实现胡萝卜路径追踪算法 -> 正文阅读

[数据结构与算法]python实现胡萝卜路径追踪算法

胡萝卜路径追踪算法:
当针对关键任务启动无人系统时,需要遵循预定路径。这意味着无人系统需要遵循路径的算法来完成任务。该算法使用比例控制器形式的简单控制器来控制无人系统的移动。这篇文章基于python实现了该算法。可直接复制运行。
关于该算法的详细介绍,建议参考这篇论文:
Path Planning of Unmanned System using Carrot-chasing Algorithm

# coding=utf-8
from math import atan

import matplotlib.pyplot as plt
import numpy as np

# 速度
Va = 25
phi = 0.9
Vx = Va * np.cos(phi)
Vy = Va * np.sin(phi)
Wa = (6, 65)
Wb = (12, 35)
# 初始点
Px = 10
Py = 32
plt.plot(Px, Py, 'pr')
plt.plot(Wa, Wb, '-d')

Ru = np.sqrt((Wb[0] - Py) ** 2 + (Wa[0] - Px) ** 2)
theta = abs(atan(float(Wb[1] - Wb[0]) / float(Wa[1] - Wa[0])))
thetaU = abs(atan((Py - Wb[0]) / (Px - Wa[0])))
beta = abs(theta - thetaU)
R = Ru * np.cos(beta)
# cross-track eror
e = Ru * np.sin(beta)

plt.plot((Wa[0], Px), (Wb[0], Py), '-g')

# plt.show()
delta = 5
# print theta, thetaU
xt = Wa[0] + (R + delta) * np.cos(theta)
yt = Wb[0] + (R + delta) * np.sin(theta)
plt.plot(xt, yt, '^r')
# print xt, yt
K = 0.5
K2 = 35
while abs(e) > 0:
    t = 0.05
    # print xt, yt, Px, Py
    # 偏移角
    phiD = abs(atan((yt - Py) / (xt - Px)))
    u = K * (phiD - phi) * Va - K2 * e
    if u > 1:
        u = 1
    phi = phiD
    Vy = Va * np.sin(phi) + u * t
    Vx = np.sqrt(Va * Va - Vy * Vy)
    # 位置
    Px = Px + Vx * t
    Py = Py + Vy * t

    t = t + 0.1
    plt.plot(Px, Py, 'pr ')
    plt.pause(0.1)
    # plt.show()
    # 距离原点的距离
    Ru = np.sqrt((Wb[0] - Py) ** 2 + (Wa[0] - Px) ** 2)
    # 距离原点的角度
    thetaU = abs(atan((Py - Wb[0]) / (Px - Wa[0])))

    beta = abs(theta - thetaU)
    R = Ru * np.cos(beta)
    # 轨迹偏移误差
    e = Ru * np.sin(beta)
    plt.plot((Wa[0], Px), (Wb[0], Py), '--g ')
    # carrot position
    xt = Wa[0] + (R + delta) * np.cos(theta)
    yt = Wb[0] + (R + delta) * np.sin(theta)
    plt.plot(xt, yt, '^b')
    print xt, yt
plt.show()
  数据结构与算法 最新文章
【力扣106】 从中序与后续遍历序列构造二叉
leetcode 322 零钱兑换
哈希的应用:海量数据处理
动态规划|最短Hamilton路径
华为机试_HJ41 称砝码【中等】【menset】【
【C与数据结构】——寒假提高每日练习Day1
基础算法——堆排序
2023王道数据结构线性表--单链表课后习题部
LeetCode 之 反转链表的一部分
【题解】lintcode必刷50题<有效的括号序列
上一篇文章      下一篇文章      查看所有文章
加:2021-12-15 18:33:00  更:2021-12-15 18:33:27 
 
开发: C++知识库 Java知识库 JavaScript Python PHP知识库 人工智能 区块链 大数据 移动开发 嵌入式 开发工具 数据结构与算法 开发测试 游戏开发 网络协议 系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑 笔记本 显卡 显示器 固态硬盘 硬盘 耳机 手机 iphone vivo oppo 小米 华为 单反 装机 图拉丁

360图书馆 购物 三丰科技 阅读网 日历 万年历 2024年11日历 -2024/11/26 16:38:11-

图片自动播放器
↓图片自动播放器↓
TxT小说阅读器
↓语音阅读,小说下载,古典文学↓
一键清除垃圾
↓轻轻一点,清除系统垃圾↓
图片批量下载器
↓批量下载图片,美女图库↓
  网站联系: qq:121756557 email:121756557@qq.com  IT数码