python机器人编程——基于单目视觉、固定场景下的自动泊车(下)

2024-02-11 12:30

本文主要是介绍python机器人编程——基于单目视觉、固定场景下的自动泊车(下),希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

目录

  • 一、前言
  • 二、主要思路
    • step0 设定一个中间位置
    • step1 掉转马头
    • step2 直线匀速前进
    • step3 调整姿态
    • step4 视觉匹配
  • 三、效果
  • 四、全篇总结

一、前言

本篇来讨论一下在固定场景下,如何仅通过单目视觉,实现差速小车的自动停靠,这种方式实现成本比较低,可适应的范围比较广,比如可以应用于小车的自动充电桩寻找、工位的最准等应用场景。我们还是尝试应用有限、浅显的数学对固定场景下,通过坐标的变换、几何的分析、结合单目摄像头获取的图像,进行相机位置(或者小车的位置)的估计,并控制差速小车行驶至我们预设的位置停靠。本篇根据上篇已经识别的位置,进一步实现小车的控制部分。
在这里插入图片描述

二、主要思路

由于我们讨论的是简单场景,在知道了当前小车位置和方向角后,要行驶至固定点位,最直接粗暴的方法是走直线了,我们可以这样进行路线的控制:

step0 设定一个中间位置

由于视觉判断出来的位置毕竟准确的不高,我们有必要设立一个在目标停车区前面设置一个中间位置,用于最后停进去前进行缓冲,稍微精确的进行位置调整,类似两个空间站对接的场景。如上图绿色点,例如这个坐标如设为(0,-2500)单位mm,目标点坐标为(0,-1500)。

step1 掉转马头

这一步是直接原地转向,根据视觉计算的位置和中间点位置,将当前方向角原地旋转调整至朝向中间点的位置。如下图:
在这里插入图片描述
应该调整的角度dtheta=theta_t1-theta_t0,其中,theta_t1是根据向量Pt0---->Pt1计算获取,向量的旋转角度计算程序如下:

def single_vangle(v1):"""计算单个向量夹角,返回弧度和角度"""dx1 = v1[2] - v1[0]    dy1 = v1[3] - v1[1]    fangle1 = math.atan2(dy1, dx1)    angle1 = round(fangle1 * 180/math.pi,2)    return fangle1,angle1

用程序计算小车在Pt0点应该旋转的角度如下:

def move2ready(location,theta,clientID,TTHandle,speed=0.15,tspeed=0.05,main_local=(0,-2500),r=0.16):"""r:小车中心到轮子的距离"""p0x=location[0]p0y=location[1]p1x=main_local[0]p1y=main_local[1]orders=[]angle1=theta+90print("当前小车角度:",angle1)print("当前小车坐标:",p0x,p0y)f,angle0=single_vangle([p0x,p0y,p1x,p1y])print("当前中间点角度:",angle0)dangle=angle0-angle1print("转角角度:",dangle)if dangle>=0:#顺时针转rv>0>lvdangle=dangle/180*math.pi#转弧度vr=tspeedvl=-tspeed#旋转时间=角度/角速度,角速度=线速度/rtita=abs(dangle/(vr*10*0.164))orders.append([tita,(vl,vr),clientID,TTHandle])else:dangle=-dangle/180*math.pi#转弧度vr=-tspeedvl=tspeed#旋转时间=角度/角速度,角速度=线速度/rtita=abs(dangle/(vr*10*0.164))orders.append([tita,(vl,vr),clientID,TTHandle])######part1------接下部分###############

PS:如何控制小车按照既定的指令前进,比如:先让小车左转30度,再匀速直行1米,再右转20度…
这里我们可以通过设置一个独立的线程,将需要的一些先后顺序的控制指令封装在一个list里面,命名为orders,其结构如下:
orders=[[执行时间t,(执行的左右轮速度),小车的ID号,小车的反馈状态句柄],…]
然后编制一个顺序指令函数,来解释这个orders,并按一定的先后顺序根据计时时间下发到小车,这样就形成了一个顺序控制(CCS)过程:

def tick_tack(orders,circle=True):"""按时间执行控制,orders=[[tita,(leftspeed,rightpeed),clientID]]"""x,y,xe,ye,ang0,ang1=0,0,0,0,0,0start_time=time.time()for i in range(len(orders)):          tita=orders[i][0]lv=orders[i][1][0]rv=orders[i][1][1]clientID=orders[i][2]TTHandle=orders[i][3]if circle:ret, arr = sim.simxGetObjectOrientation(clientID, TTHandle, -1, sim.simx_opmode_blocking)if ret==sim.simx_return_ok:x=round(arr[0]*1000,2)y=round(arr[1]*1000,2)  ang0=round(arr[2],2) print("角度:",arr)if ang0<0:ang0=math.pi*2+ang0else:ret, arr = sim.simxGetObjectPosition(clientID, TTHandle, -1, sim.simx_opmode_blocking)if ret==sim.simx_return_ok:x=round(arr[0]*1000,2)y=round(arr[1]*1000,2)  ang0=round(arr[2],2)  send_LR(clientID,lv,rv)            time.sleep(tita)if circle:ret, arr = sim.simxGetObjectOrientation(clientID, TTHandle, -1, sim.simx_opmode_blocking)if ret==sim.simx_return_ok:xe=round(arr[0]*1000,2)ye=round(arr[1]*1000,2)ang1=round(arr[2],2)  if ang1<0:ang1=math.pi*2+ang1if ang1<ang0:aspeed=(ang1+math.pi*2-ang0)/tita                    #speed=math.sqrt((xe-x)**2+(ye-y)**2)/tita#print("线速度(mm):",speed)else:aspeed=(ang1-ang0)/titaprint("角速度(mm):",aspeed)print("角度:",arr)else:ret, arr = sim.simxGetObjectPosition(clientID, TTHandle, -1, sim.simx_opmode_blocking)if ret==sim.simx_return_ok:xe=round(arr[0]*1000,2)ye=round(arr[1]*1000,2)                     speed=math.sqrt((xe-x)**2+(ye-y)**2)/titaprint("线速度(mm):",speed)send_LR(clientID,0,0)send_LR(clientID,0,0)time.sleep(0.01)end_time=time.time()   print("用时:",end_time-start_time)return True,end_time-start_time

step2 直线匀速前进

掉转朝向完成后,我们就控制小车左右轮速为相等,计算好直线移动至中间位置的时间,利用计算机监控时间,倒计时停止后,即为中间位置。
此处比较好理解,只要计算一下当前点Pt0到中间点Pt1的距离,并且设定好合适的速度,就可以计算小车应该行进的时间,于是生成指令加入orders序列:

######part2------接part1部分################计算前进时间distance=math.sqrt((p0x-p1x)**2+(p0y-p1y)**2)vl=speedvr=speedtita=distance/(vl*10*24)orders.append([tita,(vl,vr),clientID,TTHandle])######part2------接下部分###############

step3 调整姿态

在中间位置附近了,就可以根据当前的朝向,原地旋转小车,将小车朝向调整至垂直面向目的区域,并且下车的控制由模型计算控制转为视觉导航为主的位置精调控制。
这一步跟第一步是一样的只要调整好小车方向至中间点至原点向量的角度就可以:

######part3------接part2部分###############
#计算中间点位置旋转dangle=90-angle0if dangle>=0:#顺时针转rv>0>lvdangle=dangle/180*math.pi#转弧度vr=tspeedvl=-tspeed#旋转时间=角度/角速度,角速度=线速度/rtita=abs(dangle/(vr*10*0.164))orders.append([tita,(vl,vr),clientID,TTHandle])else:dangle=-dangle/180*math.pi#转弧度vr=-tspeedvl=tspeed#旋转时间=角度/角速度,角速度=线速度/rtita=abs(dangle/(vr*10*0.164))orders.append([tita,(vl,vr),clientID,TTHandle])print("bigen move:",i,orders) t=threading.Thread(target=tick_tack,args=(orders,))t.setDaemon(True)t.start()return t

最后创建一个新的线程,将生成的顺序控制指令orders用我们的顺序控制函数tick_tack执行。小车就会执行一些列的动作,移动到中间点附近了!
在这里插入图片描述

step4 视觉匹配

类似于导弹的地形匹配,我们利用单目视觉获得的图像,在小车前进中进行位置角度的调整,原则上是使得固定二维码中心始终在图像的中心位置,且其它参考点P1、P2相对中心点位置距离比例是相等的,垂直、横向二维码点不产生比例偏移。直至小车运动至目标位置停止。
这个问题其实就是一个阶跃的响应控制问题:
在这里插入图片描述
这里我们还是利用之前博文中所用到的预测控制思想去控制小车至目标设定值,上图红线所示。这里也可以通过PID等其它控制手段,实现小车位置的精调回正。

三、效果

移动到中间点:
在这里插入图片描述

最后精细控制至目标点:
在这里插入图片描述

四、全篇总结

至此,初步完成了在简单固定场景仿真环境下,基于单目视觉的自动停靠控制,通过测试,即使在理想的仿真环境下面,基于单目视觉简单的定位也存在着一定的误差,需要反复的进行调整参数,由此可见应用于实际场景,相对情况要复杂很多,单单利用单目视觉和简单的特征识别处理在实际应用场景还是不够的,要引入更多的信息源(如激光传感、深度传感)、使用优化算法来提高系统的可靠性和准确性。
后续还有很多改进的地方,让我们继续深入研究吧!

这篇关于python机器人编程——基于单目视觉、固定场景下的自动泊车(下)的文章就介绍到这儿,希望我们推荐的文章对编程师们有所帮助!



http://www.chinasem.cn/article/699774

相关文章

python: 多模块(.py)中全局变量的导入

文章目录 global关键字可变类型和不可变类型数据的内存地址单模块(单个py文件)的全局变量示例总结 多模块(多个py文件)的全局变量from x import x导入全局变量示例 import x导入全局变量示例 总结 global关键字 global 的作用范围是模块(.py)级别: 当你在一个模块(文件)中使用 global 声明变量时,这个变量只在该模块的全局命名空

Hadoop企业开发案例调优场景

需求 (1)需求:从1G数据中,统计每个单词出现次数。服务器3台,每台配置4G内存,4核CPU,4线程。 (2)需求分析: 1G / 128m = 8个MapTask;1个ReduceTask;1个mrAppMaster 平均每个节点运行10个 / 3台 ≈ 3个任务(4    3    3) HDFS参数调优 (1)修改:hadoop-env.sh export HDFS_NAMENOD

Linux 网络编程 --- 应用层

一、自定义协议和序列化反序列化 代码: 序列化反序列化实现网络版本计算器 二、HTTP协议 1、谈两个简单的预备知识 https://www.baidu.com/ --- 域名 --- 域名解析 --- IP地址 http的端口号为80端口,https的端口号为443 url为统一资源定位符。CSDNhttps://mp.csdn.net/mp_blog/creation/editor

【Python编程】Linux创建虚拟环境并配置与notebook相连接

1.创建 使用 venv 创建虚拟环境。例如,在当前目录下创建一个名为 myenv 的虚拟环境: python3 -m venv myenv 2.激活 激活虚拟环境使其成为当前终端会话的活动环境。运行: source myenv/bin/activate 3.与notebook连接 在虚拟环境中,使用 pip 安装 Jupyter 和 ipykernel: pip instal

【机器学习】高斯过程的基本概念和应用领域以及在python中的实例

引言 高斯过程(Gaussian Process,简称GP)是一种概率模型,用于描述一组随机变量的联合概率分布,其中任何一个有限维度的子集都具有高斯分布 文章目录 引言一、高斯过程1.1 基本定义1.1.1 随机过程1.1.2 高斯分布 1.2 高斯过程的特性1.2.1 联合高斯性1.2.2 均值函数1.2.3 协方差函数(或核函数) 1.3 核函数1.4 高斯过程回归(Gauss

【学习笔记】 陈强-机器学习-Python-Ch15 人工神经网络(1)sklearn

系列文章目录 监督学习:参数方法 【学习笔记】 陈强-机器学习-Python-Ch4 线性回归 【学习笔记】 陈强-机器学习-Python-Ch5 逻辑回归 【课后题练习】 陈强-机器学习-Python-Ch5 逻辑回归(SAheart.csv) 【学习笔记】 陈强-机器学习-Python-Ch6 多项逻辑回归 【学习笔记 及 课后题练习】 陈强-机器学习-Python-Ch7 判别分析 【学

nudepy,一个有趣的 Python 库!

更多资料获取 📚 个人网站:ipengtao.com 大家好,今天为大家分享一个有趣的 Python 库 - nudepy。 Github地址:https://github.com/hhatto/nude.py 在图像处理和计算机视觉应用中,检测图像中的不适当内容(例如裸露图像)是一个重要的任务。nudepy 是一个基于 Python 的库,专门用于检测图像中的不适当内容。该

pip-tools:打造可重复、可控的 Python 开发环境,解决依赖关系,让代码更稳定

在 Python 开发中,管理依赖关系是一项繁琐且容易出错的任务。手动更新依赖版本、处理冲突、确保一致性等等,都可能让开发者感到头疼。而 pip-tools 为开发者提供了一套稳定可靠的解决方案。 什么是 pip-tools? pip-tools 是一组命令行工具,旨在简化 Python 依赖关系的管理,确保项目环境的稳定性和可重复性。它主要包含两个核心工具:pip-compile 和 pip

基于51单片机的自动转向修复系统的设计与实现

文章目录 前言资料获取设计介绍功能介绍设计清单具体实现截图参考文献设计获取 前言 💗博主介绍:✌全网粉丝10W+,CSDN特邀作者、博客专家、CSDN新星计划导师,一名热衷于单片机技术探索与分享的博主、专注于 精通51/STM32/MSP430/AVR等单片机设计 主要对象是咱们电子相关专业的大学生,希望您们都共创辉煌!✌💗 👇🏻 精彩专栏 推荐订阅👇🏻 单片机

【编程底层思考】垃圾收集机制,GC算法,垃圾收集器类型概述

Java的垃圾收集(Garbage Collection,GC)机制是Java语言的一大特色,它负责自动管理内存的回收,释放不再使用的对象所占用的内存。以下是对Java垃圾收集机制的详细介绍: 一、垃圾收集机制概述: 对象存活判断:垃圾收集器定期检查堆内存中的对象,判断哪些对象是“垃圾”,即不再被任何引用链直接或间接引用的对象。内存回收:将判断为垃圾的对象占用的内存进行回收,以便重新使用。