# ARM_Controller
**Repository Path**: ProtoDrive000/arm_controller
## Basic Information
- **Project Name**: ARM_Controller
- **Description**: 课程作业:步进电机4自由度机械臂上位机设计
- **Primary Language**: Unknown
- **License**: MIT
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No
## Statistics
- **Stars**: 19
- **Forks**: 0
- **Created**: 2021-05-30
- **Last Updated**: 2025-06-26
## Categories & Tags
**Categories**: Uncategorized
**Tags**: None
## README
# HugeSmart_ARM V1.0 Pro
## 简介
在原有项目基础上增加了```逆运动学求解```以及他的几个小应用
开发平台: **QT** and **Python 3.7+**
[BiliBili视频演示](https://www.bilibili.com/video/BV1eL411p7uM?from=search&seid=3153028964113630701)
[TOC]
## 运动学逆解
对于正运动学表示为 ![[公式]](https://www.zhihu.com/equation?tex=T(\theta)%2C\theta\in+R^n) 一般的n自由度开链机器人,其逆运动学可以表示为给定一个线性变换 ![[公式]](https://www.zhihu.com/equation?tex=X\in+SE(3)) ,解出 ![[公式]](https://www.zhihu.com/equation?tex=\theta) 满足 ![[公式]](https://www.zhihu.com/equation?tex=T(\theta)%3DX) ，为了突出逆运动学的主要特征，依旧选用2R平面机器人为例。
只考虑末端执行器的位置，就有：
假设 ![[公式]](https://www.zhihu.com/equation?tex=L_1>L_2) ,那么机器人的工作空间就是一个 ![[公式]](https://www.zhihu.com/equation?tex=L_1-L_2) 到 ![[公式]](https://www.zhihu.com/equation?tex=L_1%2BL_2) 的圆环，给出一些末端执行器的坐标 ![[公式]](https://www.zhihu.com/equation?tex=（x，y）) ，很容易看出给出坐标是在圆环的外部，边界，或内部，对于一个末端执行器坐标有两个解时，第二个关节处的角度可以是正的，也可以是负的，对应的两种情况就被称为lefty和righty，也被称为elbow-up和elbow-down。
求出给定坐标x，y所对应的 ![[公式]](https://www.zhihu.com/equation?tex=\theta_1) 和 ![[公式]](https://www.zhihu.com/equation?tex=\theta_2)是不难的，在这之前需要引入atan2函数，它返回的是原点至点(x,y)的方位角，即与 x 轴的夹角，也可以理解为复数 x+yi 的辐角，返回值的单位为弧度，取值范围为 ![[公式]](https://www.zhihu.com/equation?tex=（-\pi%2C\pi]) ，因此，atan2有时被称为四象限反正切。
图2 求对应坐标的角度
根据余弦定理，就有：
同理，解出另一个：
用atan2函数求出 ![[公式]](https://www.zhihu.com/equation?tex=\gamma) ，这样上面的righty解就为：
上面的lefty解就为：
如果![[公式]](https://www.zhihu.com/equation?tex=x^2%2By^2) 在 ![[公式]](https://www.zhihu.com/equation?tex=L_1-L_2) 到 ![[公式]](https://www.zhihu.com/equation?tex=L_1%2BL_2) 的圆环之外，则无解。
## 汉诺塔
### 什么是汉诺塔
汉诺塔：汉诺塔（又称河内塔）问题是源于印度一个古老传说的益智玩具。大梵天创造世界的时候做了三根金刚石柱子，在一根柱子上从下往上按照大小顺序摞着64片黄金圆盘。大梵天命令婆罗门把圆盘从下面开始按大小顺序重新摆放在另一根柱子上。并且规定，在小圆盘上不能放大圆盘，在三根柱子之间一次只能移动一个圆盘。
### 汉诺塔移动过程
下面分三种情况分别演示每个盘子的移动过程（n 代表圆盘数量）：
1个圆盘的情况：
2个圆盘的情况：
3个圆盘的情况：
### 汉诺塔算法思想
当 n 等于 1 的时候:
直接把圆盘从 A 移动到 C；
当 n > 1 的时候：
把 A 柱子上面的 (n-1) 个盘子，从 A 移动到 B；
把 A 柱子上面的第 n 个盘子由 A 移动到 C；
把第一步 B 柱子上的 (n-1) 个盘子由 B 移动到 C
在算法的实现过程中，我们利用**递归**的思想。来模拟移动过程以及总共移动的次数。
## 图像识别
### 通用物体识别
---
### 单主体坐标识别
---
## APP开发
### 控件设置
使用**openjumper ble串口模块** , 因为蓝牙2.0是已淘汰的技术，新手机已经不支持蓝牙2.0，blinker也不支 持, **将串口BLE模块的 TXD连接到UNO的2号引脚，RXD连接到UNO的3号引脚**
1. 确保蓝牙模块已通电
2. 进入App，点击右上角的“+”号，然后选择 **添加设备**
3. 点击选择**Arduino** > **蓝牙接入**
4. 等待搜索设备
5. 点击选择要接入的设备
### App语音控制
---
目前**blinker**提供了2种形式的语音及控制方案：
1. blinker App中语音控制
2. 接入智能音响/语音助手进行语音控制
这里我主要用了第一种blinker App中语音控制设备可以设置的语音指令，说出指令即可控制对应的设备
> ```none
> 使用注意事项：
> 如果多个设备拥有相同的语音指令，将只能控制系统首先发现的设备，因此请自行确认指令没有重复
> ```
点击**我的设备**页面右下角的麦克风图标，即可进入语音控制界面，然后说出指令即可
#### 设置方法
blinker APP自带的语音控制功能，可以将语音指令绑定到设备控制界面的按键上，设置方法如下：
在编辑模式下，点击按键组件，进入组件编辑页面，选择 **语音设置>添加语音指令** 即可添加一条语音指令。
语音指令可对应“on”、“off”、“tap”三种动作。如，选择动作"on"，该按键组件key为“btn-abc”，则触发该语音指令时，APP会向设备发送指令：
```none
{"btn-abc":"on"}
```
#### 语音变量支持
变量以“?”开头，当前支持的变量如下
```none
?name 设备名
```
如一个名为**插座**的设备，且动作指令设置为 **打开?name**，则该指令将被解析为 **打开插座**
如果您的设备通过区域管理功能设置了区域，则在语音动作指令解析时，会多解析出一条带区域信息的语音指令，如**打开办公室的插座**
## HID通讯协议
添加了HID协议用于**手柄控制**,并支持大部分人机接口设备
### 介绍
USB设备中有一大类就是HID设备，即Human Interface Devices，人机接口设备。这类设备包括鼠标、键盘等，主要用于人与计算机进行交互。它是USB协议最早支持的一种设备类。HID设备可以作为低速、 全速、高速设备用。由于HID设备要求用户输入能得到及时响应，故其传输方式通常采用中断方式。
#### 请求包格式
| 偏移量 | 域 | 大小 | 说明 |
| ------ | ------------- | ---- | ------------------------------------------------------------ |
| 0 | bmRequestType | 1 | HID设备类请求特性如下： 位7： 0＝从USB HOST到USB设备 1＝从USB设备到USB HOST 位6~5： 01＝请求类型为设备类请求 位4~0： 0001＝请求对象为接口（interface）因而，针对HID的设备类请求，仅仅10100001和00100001有效 |
| 1 | bRequest | 1 | HID类请求（参考下表） |
| 2 | wValue | 2 | 高字节说明描述符的类型0x21：HID描述符0x22：报告描述符0x23：物理描述符低字节为非0值时被用来选定实体描述符。 |
| 4 | wIndex | 2 | 2字节数值，根据不同的bRequest有不同的意义 |
| 6 | wLength | 2 | 该请求的数据段长度 |
#### HID类请求
| 数值 | HID类请求描述符 | 注释 |
| ---- | --------------- | ------------------------------------------------------------ |
| 0x01 | GET_REPORT | 主机用控制传输从设备接收数据，所有HID类设备都要支持这个请求； |
| 0x02 | GET_IDLE | 主机读取设备当前的空闲速率，设备可以不支持此请求； |
| 0x03 | GET_PROTOCOL | 仅仅适应于支持启动功能的HID设备（Boot Device） |
| 0x09 | SET_REPORT | 设备用控制传输接收主机的数据，设备可以不支持此请求； |
| 0x0A | SET_IDLE | 设置闲置状态，设备可不支持此请求； |
| 0x0B | SET_PROTOCOL | 仅仅适应于支持启动功能的HID设备（Boot Device） |
### 具体步骤
---
1. 编译开源库获得```hidapi.lib```,```hidapi.dll```两个文件,[开源镜像库](https://codechina.csdn.net/mirrors/signal11/hidapi?utm_source=csdn_github_accelerator)
2. 在QT项目的头文件中将```hidapi.h```添加进去
```c++
#include "hidapi.h"
```
3. 在```.pro```文件中添加下列语句，即引用外部库文件**```hidapi.lib```**，**```_PRO_FILE_PWD_```**是当前工程所在目录的意思，所以这个语句成功的前提是**```hidapi.lib文件```**放在当前QT工程所在目录下，如果放在别的地方那么路径名也要相应的改变
[注]:此步骤可以用```项目```->```右键```->```添加库``` 处理
```cpp
SOURCES += \
main.cpp \
mainwindow.cpp \
qcustomplot.cpp
HEADERS += \
connect.h \
mainwindow.h \
hidapi.h \
qcustomplot.h
FORMS += \
mainwindow.ui
LIBS += -L$$_PRO_FILE_PWD_/ -lhidapi
```
4. 构建项目（这里要注意选用的构建套件是64位还是32位，要和```VS```编译```hidapi```库时选择的一致），构建项目后QT会影子生成一个“build xxxxxx”的类似文件夹，点击进去，将```hidapi.dll```文件拷贝在里面的debug目录下
5. 声明对象
```c++
hid_device *handle;
```
6. 读取函数
```c++
unsigned char buf[8];
res = hid_read(handle, buf, 8);
for (int i = 0; i < res; i++)
{
//qDebug("buf[%d]: %x ", i, buf[i]);
gamedata[i] = buf[i];
}
```
7. 定时刷新
```c++
//打开hid设备,输入id
handle = hid_open(0x081F, 0xE401, NULL);
//添加手柄扫描5ms一次
QTimer *timer = new QTimer(this);
connect(timer, SIGNAL(timeout()), this, SLOT(read_gameController()));
timer->start(5); // 每隔0.005s
```
8. 界面显示
## ESP32语音识别
### 工作原理
语音识别是怎么工作的呢？实际上一点都不神秘，无非是：
1. 把帧识别成状态(难点)。
2. 把状态组合成音素。
3. 把音素组合成单词。
如下图所示：
图中，每个小竖条代表一帧，若干帧语音对应一个态，每三个状态组合成一个音素，若干个音素组合成一个单词。也就是说，只要知道每帧语音对应哪个状态了，语音识别的结果也就出来了。
那每帧音素对应哪个状态呢？有个容易想到的办法，看某帧对应哪个状态的概率最大，那这帧就属于哪个状态。比如下面的示意图，这帧在状态S3上的条件概率最大，因此就猜这帧属于状态S3。
### 硬件
---
#### 原理图
#### PCB图
### 软件
---
用Mu在ESP32模块上编写Micro Python代码,详细代码见附件
### 实物图
---
## 陀螺仪使用
走的是**```MPU6050```+```卡尔曼滤波```**的方案,放机械臂末端实现```末端姿态稳定```,他自带```mcu```,所以我只要根据协议读取就好
### 数据解算
1. 姿态角结算时所使用的坐标系为东北天坐标系，正方向放置模块,如下图所示向左 为 X 轴，向前为 Y 轴，向上为 Z 轴。欧拉角表示姿态时的坐标系旋转顺序定义为为 z-y-x,即先绕 z 轴转，再绕 y 轴转，再绕 x 轴转。
2. 滚转角的范围虽然是±180 度，但实际上由于坐标旋转顺序是 Z-Y-X，在表示姿态 的时候，俯仰角(Y 轴)的范围只有±90 度，超过 90 度后会变换到小于 90 度，同时让X轴的角度大于180度。详细原理请大家自行百度欧拉角及姿态表示的相关信息。
3. 由于三轴是耦合的，只有在小角度的时候会表现出独立变化，在大角度的时候姿态 角度会耦合变化，比如当 Y 轴接近 90 度时，即使姿态只绕 Y 轴转动，X 轴的角度 也会跟着发生较大变化，这是欧拉角表示姿态的固有问题。
#### 协议格式
#### 计算方法
**滚转角（x 轴）**
$$
Roll=((RollH<<8)|RollL)/32768*180(°)
$$
**俯仰角（y 轴）**
$$
Pitch=((PitchH<<8)|PitchL)/32768*180(°)
$$
**偏航角（z 轴）**
$$
Yaw=((YawH<<8)|YawL)/32768*180(°)
$$
**温度计算公式：**
$$
T=((TH<<8)|TL) /100 ℃
$$
**校验和：**
$$
Sum=0x55+0x53+RollH+RollL+PitchH+PitchL+YawH+YawL+TH+TL
$$
#### 实现代码
```c++
QString zitai = mytemp.toHex();
QString RollL = zitai.mid(4,2);
//ui->textEdit->append(RollL);
QString RollH = zitai.mid(6,2);
QString PitchL = zitai.mid(8,2);
QString PitchH = zitai.mid(10,2);
QString YawL = zitai.mid(12,2);
QString YawH = zitai.mid(14,2);
QString TL = zitai.mid(16,2);
QString TH = zitai.mid(18,2);
QString SUM = zitai.mid(20,2);
bool ok_1,ok_2,ok_3,ok_4;
x_data = float((RollH.toInt(&ok_1,16)<<8)|RollL.toInt(&ok_1,16))/32768.0*180.0 - 180.0;
y_data = float((PitchH.toInt(&ok_2,16)<<8)|PitchL.toInt(&ok_2,16))/32768.0*180.0 - 180.0;
z_data = float((YawH.toInt(&ok_3,16)<<8)|YawL.toInt(&ok_3,16))/32768.0*180.0 - 180.0;
Temper = float((TH.toInt(&ok_4,16)<<8)|TL.toInt(&ok_4,16))/100.0 - 118.0;
```
### 波形图
---
采用**```qcustomplot```**第三方库,这个加载较为简单,网上教程很多,不多赘述,主要来说说他的调用,因为这个库的数据格式要求是```QVector```,也就是vector的变形,关键是长时间的数据采集,vector必然内存爆掉,我也是刚接触STL库,记录一下我清内存的方法
#### 内存清空
```c++
void MainWindow::clear_data()
{
//数据清零//
x.clear();
y.clear();
z.clear();
time_count.clear();
x.resize(1);
y.resize(1);
z.resize(1);
time_count.resize(1);
//内存清零//
x.capacity();
y.capacity();
z.capacity();
time_count.capacity();
zitai_count=1;
}
```
#### 实现函数
```c++
//画窗体
void MainWindow::setupQuadraticDemo(QCustomPlot *customPlot)
{
//每条曲线都会独占一个graph()
customPlot->addGraph();
customPlot->graph(0)->setPen(QPen(Qt::blue)); // 曲线的颜色
customPlot->addGraph();//添加graph等价于添加新曲线
customPlot->graph(1)->setPen(QPen(Qt::red)); // 曲线的颜色
customPlot->addGraph();//添加graph等价于添加新曲线
customPlot->graph(2)->setPen(QPen(Qt::black)); // 曲线的颜色
// 边框右侧和上侧均显示刻度线，但不显示刻度值:
// (参见 QCPAxisRect::setupFullAxesBox for a quicker method to do this)
customPlot->xAxis2->setVisible(true);
customPlot->xAxis2->setTickLabels(false);
customPlot->yAxis2->setVisible(true);
customPlot->yAxis2->setTickLabels(false);
customPlot->graph(0)->rescaleAxes(true);
//自动调整XY轴的范围，以便显示出graph(1)中所有的点
customPlot->graph(1)->rescaleAxes(true);
//自动调整XY轴的范围，以便显示出graph(2)中所有的点
customPlot->graph(2)->rescaleAxes(true);
// 使上下两个X轴的范围总是相等，使左右两个Y轴的范围总是相等
connect(customPlot->xAxis, SIGNAL(rangeChanged(QCPRange)), customPlot->xAxis2, SLOT(setRange(QCPRange)));
connect(customPlot->yAxis, SIGNAL(rangeChanged(QCPRange)), customPlot->yAxis2, SLOT(setRange(QCPRange)));
// 把已存在的数据填充进graph的数据区
customPlot->graph(0)->setData(time_count, x);
customPlot->graph(1)->setData(time_count, y);
customPlot->graph(2)->setData(time_count, z);
// 支持鼠标拖拽轴的范围、滚动缩放轴的范围，左键点选图层（每条曲线独占一个图层）
customPlot->setInteractions(QCP::iRangeDrag | QCP::iRangeZoom | QCP::iSelectPlottables);
// 立即刷新图像
ui->customPlot->replot();
}
```
### 卡尔曼滤波
---
```c++
float Kalman_Filter(_KALMAN_ Kalman,double z){
float e_EST,Kk,hat_x;
Kk= Kalman.e_EST_last / (Kalman.e_EST_last + Kalman.e_MEA);
hat_x=Kalman.hat_x_last+Kk*(z-Kalman.hat_x_last);
e_EST=(1-Kk)*Kalman.e_EST_last;
Kalman.e_EST_last=e_EST;
Kalman.hat_x_last=hat_x;
return hat_x;
}
```
#### 效果
