机械臂末端运动稳定性评估（附linux实时波形客户端源码）

tinnu

机械臂末端运动稳定性评估（附linux实时波形客户端源码） [复制链接]

本帖最后由 tinnu 于 2020-7-6 20:26 编辑

（一）项目背景

常见的机械臂控制由数控发展而来，强调运动过程恒速稳定，很多控制器的速度控制采用混合式PID，在启动阶段产生极大的加速度以求快速到达所设定速度，由于固体本身的刚度，这种剧烈的速度变化对于数控加工场景或一般的固体搬运场景非常有效。但在液体搬运场景却会产生极大的影响，因此需要提出新的控制方法。这种控制方法必须对加速度有较高的要求，为了量化这种加速度性能，需要有一个用于测试机械臂末端运动稳定的器件。

（二）作品简介

验证算法的桌面机械臂是一种小型机械臂，因此末端加速度测量装置必须轻便且小型。此外为了避免线缆对运动的影响，测量装置必须拥有独立的供电体系以及基于无线传输数据，SensorTile.box完美切合了这个需求。只需要把读取到的数据上传到上位机即可分析数据，包括绘制出实时波形、最值分析等等。

（三）系统框图

1-SensorTile.box

作为下位机，获取六轴传感器lsm6dso 的OUTX_A、OUTZ_A数据，通过蓝牙spbtle-1s向上位机上报。

2-上位机

上位机接收X、Z两个轴的信息，与9.8m/s^2的标准加速度进行映射，可以计算出X轴加速度。只要测试条件保持X轴向前，水平放置即可测出加速度情况。上位机接收X、Z两个轴的信息，解析后映射，得出加速度，绘制实时波形，并记录每一次的最值，用于评估运动算法。图中靠下的绿线为零点，靠上的绿线为500mm/s2。由于纵轴自适应最大最小值显示波形，因此绘制两条参考下用于视觉直观对比。

（四）传感器数据

1-SensorTile.box

SensorTile.box 是基于例程BLESensors改写，把不需要的环境服务和电池服务去掉，保留运动学传感器服务

从初始化出分析

Init_BlueNRG_Custom_Services 函数 BlueNRG 的三个应用：
Add_HW_SW_ServW2ST_Service
Add_ConsoleW2ST_Service
Add_ConfigW2ST_Service
其中 Add_HW_SW_ServW2ST_Service 里面定义了 AccGyroMagCharHandle 这个 charhandle，蓝牙的UUID就是在这里定义的。

AccGyroMag即运动学相关传感器UUID信息：COPY_ACC_GYRO_MAG_W2ST_CHAR_UUID(uuid);
环境传感器：COPY_ENVIRONMENTAL_W2ST_CHAR_UUID(uuid);
电池传感器：COPY_BAT_W2ST_CHAR_UUID(uuid);

从每次发送分析：
1-调用 AccGyroMag_Update(&ACC_Value,&GYR_Value,&MAG_Value);
2-里面使用 ACI_GATT_UPDATE_CHAR_VALUE 即 aci_gatt_update_char_value（这是 BlueNRG 协议栈API）发送
3-发送的内容是2+3*3*2的buff，也就是一次信息20位，

  STORE_LE_16(buff   ,(HAL_GetTick()>>3));

  STORE_LE_16(buff+2 ,Acc->x);
  STORE_LE_16(buff+4 ,Acc->y);
  STORE_LE_16(buff+6 ,Acc->z);

  Gyro->x/=100;
  Gyro->y/=100;
  Gyro->z/=100;

  STORE_LE_16(buff+8 ,Gyro->x);
  STORE_LE_16(buff+10,Gyro->y);
  STORE_LE_16(buff+12,Gyro->z);

  STORE_LE_16(buff+14, Mag->x);
  STORE_LE_16(buff+16, Mag->y);
  STORE_LE_16(buff+18, Mag->z);

组织方式，前两位是时间戳，后是传感器信息。
小端模式

依次分析后发现：

电池信息是9个字节一次

环境信息是10个字节一次：uint8_t buff[2+4/*Press*/+2/*Hum*/+2/*Temp*/];

运动学传感器是20个字节一次：uint8_t buff[2+3*3*2];

2-linux上位机

在linux下操作蓝牙采用 bluetoothctl 工具

1-蓝牙适配器

首先要注意所使用的蓝牙硬件，有时候连接不上并不是操作有问题，而是蓝牙硬件本身驱动兼容的问题。上一篇帖子中我提到了我的 magicbook 在 deepin 系统下使用命令：

pair <SensorTile.box mac>

连接不上。后来经过测试证明，是因为 magicbook 本身的驱动有问题。deepin + magicbook 的 8822cu 一直以来以驱动差劲著称，但我一开始实在没想到会这么差。后来使用10块一个USB蓝牙解决问题，使用的芯片是英国的SRC系列蓝牙芯片。

2-连接命令

命令行输入 bluetoothctl 进入交互

power on

agent on

Defaultagent

scan on

pair <SensorTile.box mac>

connect <SensorTile.box mac>

select-attribute /org/bluez/hci1/dev_<SensorTile.box mac>/service000c/char000d

Notify on

输入Notify on之后就会有数据读出来。

3-解析

/service000c/char000d 这个是运动传感器的数据，组织格式如上所述。第一第二是时间戳。后面两个字节一个数据，小端模式。

通过代码解析：

long t_systime = (unsigned char) t_byteArray.at(0)|(unsigned char)t_byteArray.at(1)<<8;
short t_accx = (unsigned char) t_byteArray.at(2)|(unsigned char) t_byteArray.at(3)<<8;
short t_accz = (unsigned char) t_byteArray.at(6)|(unsigned char) t_byteArray.at(7)<<8;
int t_accxValue = 9800*t_accx/t_accz;
if(t_lastTime != t_systime)
    SN_newDataRec(t_systime, t_accxValue);
t_lastTime = t_systime;