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 小米 华为 单反 装机 图拉丁
 
   -> 开发工具 -> Webots中获取机器人质心三维速度的一种简单方法 -> 正文阅读

[开发工具]Webots中获取机器人质心三维速度的一种简单方法

Webots中获取机器人质心三维速度的一种简单方法


0 前言

??最近实在是太忙了,有空把CLion和VSCode以及PyCharm配置Webots的文章写一下,我自己直接用的CLion配置的,能像VS一样直接在Webots外部运行extern控制器。
??Webots中常用的传感器有GPS获取机器人全局位置,加速度计(Accelerator)获取机器人全局加速度,磁力计(Gyro)获取刚体旋转角速度,IMU(Inertial Unit)获取机身欧拉角
??对于大多数Webots用户,主要工作还是开发控制算法,很多算法要求在仿真中能得到一些精确的机器人状态如质心速度等等,虽然在实际机器人开发中也是使用如上所列的多类传感器结合状态估计算法来获取这些状态,但为了提高控制算法开发速度,我这里给出一个简单的质心速度求取方法。这也是我看在pybullet中常用的一种方法。

1 旋转矩阵

??假设全局坐标系为 s {s} s,机身坐标系为 b {b} b,我们根据Webots中的IMU可以轻易求得从坐标系 s s s变换到坐标系 b b b旋转矩阵为 R s b ( t ) R^b_s(t) Rsb?(t),具体怎么求马上讲,别急。
??求得旋转矩阵后,我们需要利用GPS做差分,得到机身速度在全局坐标系下的表示记为 v s v_s vs?,同理机身速度在机身坐标系下的表示为 v b v_b vb?,那么可以很轻易的得到他们的关系如下:
v s = R s b ( t ) v b (1) v_s=R^b_s(t)v_b\tag{1} vs?=Rsb?(t)vb?(1)
??亦即:
v b = R s b ( t ) ? 1 v s = R s b ( t ) T v s (2) v_b = R^b_s(t)^{-1}v_s=R^b_s(t)^{T}v_s\tag{2} vb?=Rsb?(t)?1vs?=Rsb?(t)Tvs?(2)
??注意速度大小不随表示坐标系而改变,这是由旋转矩阵的性质决定的,保长度

1.1 Webots中旋转矩阵的求取

??首先我们看官方手册对IMU的描述:
Webots手册
??废话有点多,重点就是:

  • 按手册所述配置好IMU坐标轴方向
  • IMU旋转顺序是YZX,即先绕X轴旋转,再绕Z轴旋转,最后绕Y轴旋转
    ??我们知道了旋转顺序就好说了,由于欧拉角是绕固定坐标系旋转,所以左乘,乘积如下:

R s b = R o t ( y , β ) ? R o t ( z , γ ) ? R o t ( x , α ) (3) R^b_s=Rot(y,\beta)\cdot Rot(z, \gamma) \cdot Rot(x, \alpha)\tag{3} Rsb?=Rot(y,β)?Rot(z,γ)?Rot(x,α)(3)

??其中 α \alpha α为横滚角roll, γ \gamma γ为俯仰角pitch, β \beta β为偏航角yaw。很容易根据旋转矩阵的公式求得:
R s b = [ c α c γ ? c β c α s γ + s α s β c β s α s γ + c α s β s γ c γ c α ? c γ s α ? s β c γ s β c α s γ + s α s β ? s β s α s γ + c α c β ] (4) R^b_s=\begin{bmatrix} c_{\alpha}c_{\gamma} & -c_{\beta}c_{\alpha}s_{\gamma}+s_{\alpha}s_{\beta} & c_{\beta}s_{\alpha}s_{\gamma}+c_{\alpha}s_{\beta} \\ s_{\gamma} & c_{\gamma}c_{\alpha} & -c_{\gamma}s_{\alpha}\\ -s_{\beta}c_{\gamma} & s_{\beta}c_{\alpha}s_{\gamma}+s_{\alpha}s_{\beta} & -s_{\beta}s_{\alpha}s_{\gamma}+c_{\alpha}c_{\beta} \end{bmatrix}\tag{4} Rsb?=???cα?cγ?sγ??sβ?cγ???cβ?cα?sγ?+sα?sβ?cγ?cα?sβ?cα?sγ?+sα?sβ??cβ?sα?sγ?+cα?sβ??cγ?sα??sβ?sα?sγ?+cα?cβ?????(4)
??其中 s s s代表 s i n sin sin c c c代表 c o s cos cos,加个下标分别代表求对应角的正余弦值。有了这个东东后面就好办了,我弄了一个小车进行了测试,下面见代码。

2 代码展示

??小车IMU坐标系和全局坐标系如下图所示:
IMU坐标系
??我们可以测试我们刚刚求取的旋转矩阵是否正确,如下图所示:
在这里插入图片描述
??由此可见旋转矩阵求取正确,机身 x x x轴在全局坐标系下的表示为 [ 0 0 1 ] T [0\quad 0 \quad 1]^T [001]T,其他两轴也都正确。
??下面我们测试我们得到的机身坐标系下的速度和全局坐标系下的速度是否相同,相同岂不是我们白做工作了。利用坡道测试,当小车上坡时,全局坐标系下的 y y y方向和 z z z方向速度分量肯定不小,但机身坐标系下 y y y和z方向速度肯定很小。结果如下:
在这里插入图片描述
在这里插入图片描述
??其中由GPS求出的全局坐标系下速度的幅值为0.15左右,机身坐标系下的速度(local speed)只有x方向有较大速度,而全局坐标系下的速度表示在 y y y方向也有一个不可忽略的值。
??代码如下,实际上你可以对这个求GPS微分的过程进行一个滤波处理,我就懒得弄了,你也可以把小车悬空,测试小车下降速度,这里就不展示了,总的来说,我打算就用这个方法来估计机器人速度了,导航滤波机器人状态估计啥的也来不及去学去弄了。

2.1 代码说明与展示

??函数com_velocity_estimate()返回三个值,可以打印返回的旋转矩阵,看看IMU坐标轴在全局坐标轴的位置,是不是正确的以验证旋转矩阵求取的正确性,最后用求得的速度和GPS自带的一维速度获取函数进行比较,以及与直接微分求得的全局速度进行比较,发现误差还是不大的,希望大家能弄出更好的办法,到时候踢我一下,我直接用了,谢谢。

from controller import Robot
import numpy as np
import math
import copy

robot = Robot()
timeStep = int(robot.getBasicTimeStep())

motor = [robot.createMotor('LF'), robot.createMotor('RF'), robot.createMotor('RH'), robot.createMotor('LH')]

GPS = robot.createGPS("gps")
IMU = robot.createInertialUnit("IMU")

IMU.enable(timeStep)
GPS.enable(timeStep)

for i in range(4):
    motor[i].setPosition(float('+inf'))
    motor[i].setVelocity(-3)

pre_pos = np.array(GPS.getValues())


def com_velocity_estimate():
    global pre_pos

    euler = IMU.getRollPitchYaw()

    s_r = math.sin(euler[0])
    c_r = math.cos(euler[0])
    s_p = math.sin(euler[1])
    c_p = math.cos(euler[1])
    s_y = math.sin(euler[2])
    c_y = math.cos(euler[2])

    pos = np.array(GPS.getValues())

    velocity = (pos - pre_pos) / (0.001 * timeStep)

    pre_pos = copy.deepcopy(pos)
    rotate = np.zeros((3, 3), dtype=float)
    rotate[0, 0] = c_y * c_p
    rotate[0, 1] = s_r * s_y - c_y * c_r * s_p
    rotate[0, 2] = c_y * s_r * s_p + s_y * c_r
    rotate[1, 0] = s_p
    rotate[1, 1] = c_p * c_r
    rotate[1, 2] = -c_p * s_r
    rotate[2, 0] = -s_y * c_p
    rotate[2, 1] = c_r * s_y * s_p + s_r * c_y
    rotate[2, 2] = -s_y * s_r * s_p + c_r * c_y

    return rotate.T @ velocity, rotate, velocity


while robot.step(timeStep) != -1:
    # Read the sensors:
    # Enter here functions to read sensor data, like:
    #  val = ds.getValue()
    print("gps speed:{0}".format(GPS.getSpeed()))
    a, b, c = com_velocity_estimate()
    # print("gps_pos:{0},{1},{2}".format(gps_pos[0], gps_pos[1], gps_pos[2]))
    # print("acc:{0},{1},{2}".format(acc[0], acc[1], acc[2]))
    # print("euler:{0},{1},{2}".format(euler[0], euler[1], euler[2]))
    # print(b)
    print("global speed:{0}".format(c))
    print("local speed:{0}".format(a))

小车模型和整个项目代码百度网盘自取:
链接:https://pan.baidu.com/s/1Kw4f4k0Rorj5N09dUqL83A
提取码:znb5

  开发工具 最新文章
Postman接口测试之Mock快速入门
ASCII码空格替换查表_最全ASCII码对照表0-2
如何使用 ssh 建立 socks 代理
Typora配合PicGo阿里云图床配置
SoapUI、Jmeter、Postman三种接口测试工具的
github用相对路径显示图片_GitHub 中 readm
Windows编译g2o及其g2o viewer
解决jupyter notebook无法连接/ jupyter连接
Git恢复到之前版本
VScode常用快捷键
上一篇文章      下一篇文章      查看所有文章
加:2021-07-15 16:24:23  更:2021-07-15 16:24:51 
 
开发: 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年3日历 -2024/3/29 22:18:46-

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