集成在机械臂中的运动检测系统（4 自由度）
2.77/5 (5投票s)
使用 PIC16F628A 和伺服电机构建一个具有 4 个自由度的机械臂
视频测试01
视频测试02
引言
在大多数必须能够识别用户的设备中，相信我们的项目在墨西哥仍然是发展程度最低的项目之一，这在一定程度上是为了实现使用此类验证方式识别一个人而带来的挑战，因为我们依赖许多来源来开展我们的研究，这甚至使我们能够将其作为此类设备开发中的早期研究之一。
在您想尝试寻找我们意识到使用虹膜识别更容易允许多人进入某个地方的部分，这是为了取代或提供用户识别的替代方案，因为该过程是通过无线电频率凭证或识别用户手部模式的手部密钥来完成的，而这些凭证已经在CIC（计算研究中心）中使用。
本项目进行的研究包括互联网上关于机器人手臂运动的许多理论来源，使用了数学公式来计算手臂的运动，以及互联网来源的代码，这些代码仅作了改编以使其符合我们的需求。
在项目开发中，我们寻求的目标之一是在这些项目的发展中取得突破，并以此促进IPN学生开展此类项目，因为有些人尚未意识到他们有能力做到这一点。
系统架构理论
逆运动学
运动学处理运动的描述，而不考虑其原因。逆运动学的目标是找到连杆末端到达特定位置所需的各个关节的姿态。
背景
2.3 项目开发设备理论
系统将尝试在一端放置摄像头，在另一端放置机器人手臂。铰接系统达到特定位置的姿态由逆运动学定义。为了准备标准舵机臂，我们将使用¼比例舵机力与效应，原因是它们具有反向旋转，因此两个舵机接收相同的脉冲宽度以放置在所需位置。
舵机基本上是一种电动机，其只能移动大约180度的角度（不能像普通发动机那样提供完整的转动）。它有三根线，红色是供电电压（+5伏），黑色是地线（GND），黄色线是通过其所需的电源线，用于将舵机调整到所需位置（0°至180°），即控制线。
舵机内部的控制器卡告诉直流电机旋转几圈以将箭头调整到所要求的的位置。可变电阻器（也称为“电位器”）连接到箭头，并随时测量其旋转到的位置。这样，控制器卡就知道在哪里移动引擎了。
通过脉冲（PWM）向舵机提供所需的位置。必须始终在控制线上存在脉冲信号，以使舵机保持其位置。这些“命令”是一系列脉冲。脉冲持续时间表示电机的旋转角度。每个舵机都有其工作范围，对应于舵机能理解的最大和最小脉冲宽度。
下表显示了一些对应于舵机角度的脉冲宽度。
标准舵机力
 | 工作电压：4.8-6.0伏 标准地址：顺时针反向/脉冲范围1500-1900微秒。 力矩：6.0V：≥3.5公斤·厘米（49盎司/英寸），4.8V：≥3.2公斤·厘米（44.8盎司/英寸） 工作速度：6.0V：0.19秒/60°（空载），4.8V：0.23秒/60°（空载） 重量：39.2克（1.37盎司。） 尺寸：40.6 x 20.0 x 38.9毫米 |
通过控制我们的舵机以不同的手臂角度，我们可以让它们在不同的检测地点工作。
运动轴由伺服电机设置，由安装的领先卡R/C Servo Controller II控制。该卡基于PIC微控制器，连接到PC的串行端口。 在这种情况下，我们将使用PIC16F628A微控制器，与PIC16F84相比，它具有两倍的USART程序内存和4MHz内部时钟，这简化了电路并降低了成本。
它是一款8位RISC架构的CMOS闪存微控制器，能够以高达20 MHz的时钟频率运行，易于编程，并提供18引脚的DIP（双排引脚）和SOIC（方形表面贴装）封装。它内部有一个4 MHz的振荡器和一个上电复位电路，无需外部组件，并将可作为通用I/O（输入/输出）线的引脚数量扩展到16个，与PIC16F84相比，PIC16F84在其两个端口中只能使用13个引脚作为输入或输出。此外，不要忘记其哈佛架构RISC指令，PIC16F628提供128x8（128字节）的EEPROM数据内存，2024x14（2K，每地址14位）的闪存程序内存，224x8的通用数据RAM内存，一个CCP（捕获/比较/PWM）模块，一个USART，3个模拟比较器，一个可编程电压参考和三个定时器。这些和其他功能使其非常适合汽车、工业和消费电子产品应用，以及各种可编程工具和设备。PIC16F628的引脚分配与PIC16F627相同，只是后者具有1024x14的闪存程序内存。它也与PIC16F84相同，只是PIC16F628可以为端口A提供三个额外的I/O线（RA7, RA6, RA5，后者只能是输入），并且一些I/O引脚与芯片支持的各种外设功能交替复用。例如，RB1也作为USART的接收线（RX）工作。通常，当外围设备启用时，该线不能用作通用I/O引脚。
使用代码
使用PICBASIC，我们为PIC16F628A编写了几行代码，这只会将一个串行信号发送到伺服电机的端口
INCLUDE "bs2defs.bas"
symbol pos = b3
symbol servo = b4
trisa = %11111111
trisb = %00000000
serpin VAR porta.0 'serial input pin
start:
serin serpin,N2400,pos 'get serial input from PC
move:
pulsout portb.0,pos ' send data to position servos
pause 10
goto start ' do it again
这里有一个图，描述了伺服电机和pic集成的物理模型。
在理解了逆运动学后，我们编写了一些代码来解决运动学的公式
private void Dibujar(object sender, PaintEventArgs e)
{
Graphics g = e.Graphics;
Pen blackPen = new Pen(Color.Black, 3);
g.DrawEllipse(blackPen, PuntoLinea01.X, PuntoLinea01.Y - 5, 10, 10);
g.DrawLine(Pens.Gray, 0, PuntoMedio.Y, PuntoInicio.X, PuntoMedio.Y);
g.DrawLine(Pens.Blue, PuntoMedio.X, 0, PuntoMedio.X, PuntoInicio.Y);
g.DrawLine(Pens.Purple, PuntoMedio, PuntoLinea01);
double angulo = Math.Atan2(PuntoLinea02.Y - 0, PuntoLinea02.X - PuntoMedio.X) * 180 / Math.PI;
label1.Text = angulo.ToString();
double angulo2 = Math.Atan2(PuntoLinea01.Y-PuntoLinea02.Y ,PuntoLinea01.X-PuntoLinea02.X) * 180 / Math.PI;
label2.Text = angulo2.ToString();
#region gradocentral
double hipotenusaAnguloCentral= (PuntoLinea01.Y - PuntoMedio.Y) / -(PuntoLinea01.X - PuntoMedio.X);
double gradosangulo = Math.Atan2((PuntoLinea01.Y - PuntoMedio.Y), (PuntoLinea01.X - PuntoMedio.X)) * 180 / Math.PI;
gradosangulo = -gradosangulo;
label3.Text = gradosangulo.ToString();
#endregion
double hipotenusa = Math.Sqrt(Math.Pow(getXReal(PuntoLinea01.X),2)+Math.Pow(getYReal(PuntoLinea01.Y),2));
double MitadHipotenusa = hipotenusa / 2;
double LongitudMotor01;
double LongitudMotor02;
double LongitudMotor03;
if (!checkBox3.Checked)
{
LongitudMotor01 = hipotenusa * PorcentajeMotor01;
LongitudMotor02 = hipotenusa * PorcentajeMotor02;
LongitudMotor03 = hipotenusa * PorcentajeMotor03;
}
else
{
LongitudMotor01 = 100;
LongitudMotor02 = 50;
LongitudMotor03 = 15;
}
double alturaMotor01 = LongitudMotor01 * (Math.Sin((gradosangulo*Math.PI)/180));
double baseMotor01 =Math.Sqrt( Math.Pow(LongitudMotor01,2)-Math.Pow(alturaMotor01,2));
double alturaMotor02 = LongitudMotor02 * (Math.Sin((gradosangulo * Math.PI) / 180));
double baseMotor02 = Math.Sqrt(Math.Pow(LongitudMotor02, 2) - Math.Pow(alturaMotor02, 2));
double alturaMotor03 = LongitudMotor03 * (Math.Sin((gradosangulo * Math.PI) / 180));
double baseMotor03 = Math.Sqrt(Math.Pow(LongitudMotor03, 2) - Math.Pow(alturaMotor03, 2));
float Motor01Medio = PuntoMedio.X + (float)baseMotor01;
float Motor02Medio = Motor01Medio + (float)baseMotor02;
float Motor03Medio = Motor02Medio + (float)baseMotor03;
label4.Text = (alturaMotor01/LongitudMotor01).ToString();
g.DrawLine(Pens.Red, Motor01Medio, PuntoMedio.Y, Motor01Medio, PuntoMedio.Y - (float)alturaMotor01);
g.DrawLine(Pens.RoyalBlue, Motor02Medio, PuntoMedio.Y, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02+alturaMotor01));
g.DrawLine(Pens.Plum, Motor03Medio, PuntoMedio.Y, Motor03Medio, PuntoMedio.Y - (float)(alturaMotor03+alturaMotor02+alturaMotor01));
double anguloMotor01 = Math.Asin(LongitudMotor01/hipotenusa)*180/Math.PI;
double anguloMotoro01Base;
if(checkBox2.Checked)
anguloMotoro01Base = anguloMotor01+gradosangulo;
else
anguloMotoro01Base = anguloMotor01;
double alturaAnguloMotor01 = LongitudMotor01 * (Math.Sin((anguloMotoro01Base * Math.PI) / 180));
double baseAnguloMotor01 = Math.Sqrt(Math.Pow(LongitudMotor01, 2) - Math.Pow(alturaAnguloMotor01, 2));
PointFloat PuntoFinalMotor01 = new PointFloat((float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01);
g.DrawLine(Pens.Black, PuntoMedio.X, PuntoMedio.Y, PuntoFinalMotor01.X, PuntoFinalMotor01.Y);
g.DrawLine(Pens.Green, (float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor01Medio, PuntoMedio.Y - (float)alturaMotor01);
g.DrawLine(Pens.Yellow, (float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01));
g.DrawLine(Pens.Tomato, (float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor01Medio, PuntoMedio.Y);
g.DrawLine(Pens.IndianRed, Motor01Medio, PuntoMedio.Y - (float)alturaMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01));
double pendiente01 = getPendiente(Motor01Medio, PuntoMedio.Y - (float)alturaMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01));
g.DrawLine(Pens.Green, (float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor01Medio, PuntoMedio.Y - (float)alturaMotor01);
double pendiente02 = getPendiente((float)baseAnguloMotor01 + PuntoMedio.X, PuntoMedio.Y - (float)alturaAnguloMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01));
label9.Text = getAnguloEntreRecta(pendiente01, pendiente02).ToString();
double distancia = getDistanciaEntreDosPuntos(PuntoFinalMotor01.X,PuntoFinalMotor01.Y,PuntoLinea01.X,PuntoLinea01.Y);
double distancia2 = getDistanciaEntreDosPuntos(PuntoMedio.X, PuntoMedio.Y, PuntoFinalMotor01.X, PuntoFinalMotor01.Y);
double anguloMotor02 = getAnguloEntreRecta( pendiente02,pendiente01);
double alturaAnguloMotor02 = LongitudMotor02 * CalcularSeno(anguloMotor02);
double baseAnguloMotor02 = Math.Sqrt(Math.Pow(LongitudMotor02, 2) - Math.Pow(alturaAnguloMotor02, 2));
PointFloat PuntoFinalMotor02 = new PointFloat((float)baseAnguloMotor02 + PuntoMedio.X + (float)baseAnguloMotor01, PuntoMedio.Y - (float)alturaAnguloMotor02-(float)alturaAnguloMotor01);
label6.Text = getDistanciaEntreDosPuntos(PuntoFinalMotor01.X, PuntoFinalMotor01.Y, PuntoFinalMotor02.X, PuntoFinalMotor02.Y).ToString();
label4.Text = getDistanciaEntreDosPuntos(Motor01Medio, PuntoMedio.Y - (float)alturaMotor01, Motor02Medio, PuntoMedio.Y - (float)(alturaMotor02 + alturaMotor01)).ToString();
//change servo motor broken
// g.DrawLine(Pens.Black, PuntoFinalMotor01.X, PuntoFinalMotor01.Y,PuntoFinalMotor02.X,PuntoFinalMotor02.Y);
// g.DrawLine(Pens.Black, PuntoFinalMotor02.X, PuntoFinalMotor02.Y,PuntoLinea01.X,PuntoLinea01.Y);
g.DrawLine(Pens.Black, PuntoFinalMotor01.X, PuntoFinalMotor01.Y, PuntoLinea01.X, PuntoLinea01.Y);
lblAnguloMotor01.Text = getAnguloEntreDosRectas(PuntoFinalMotor01.X, PuntoFinalMotor01.Y, PuntoMedio.X, PuntoMedio.Y) + "";
lblAnguloMotor02.Text = anguloMotor02 + "";
double dLineaCentral = getPendiente(PuntoFinalMotor02.X, PuntoFinalMotor02.Y, PuntoLinea01.X, PuntoLinea01.Y);
double dLineaMotor01 = getPendiente(PuntoMedio.X,PuntoMedio.Y, PuntoLinea01.X,PuntoLinea01.Y);
MotorDeAngulos.setAnguloMotor01( AnguloMotor.ConvertirAngulo01RealAEstandar(Convert.ToDouble(lblAnguloMotor01.Text)));
MotorDeAngulos.setAnguloMotor02( AnguloMotor.ConvertirAngulo02RealAEstandar(Convert.ToDouble(lblAnguloMotor02.Text)));
label12.Text = MotorDeAngulos.getAnguloMotor03().ToString();
ActualizarMotor();
}
public double getPendiente(double x1, double y1, double x2, double y2)
{
return (y2 - y1) / (x2 - x1);
}
public double getAnguloEntreRecta(double m1, double m2){
double div = (m2 - m1) / (1 + (m1 * m2));
return CalcularATan(div);
}
public double getAnguloEntreDosRectas(double x1, double y1, double x2, double y2) {
return Math.Atan2((y1 - y2), (x1 - x2)) * 180 / Math.PI;
}
public double getDistanciaEntreDosPuntos(double x1, double y1, double x2, double y2)
{
return Math.Sqrt(Math.Pow((x1 - x2), 2.0) + Math.Pow((y1 - y2), 2.0));
}
public double CalcularSeno(double valor)
{
double temp = (valor * Math.PI)/180;
return Math.Sin(temp);
}
public double CalcularASeno(double valor)
{
double temp = Math.Asin(valor);
return (valor * 180) / Math.PI;
}
public double CalcularATan(double valor)
{
double temp = Math.Atan(valor);
return (valor * 180) / Math.PI;
}
public double getXReal(double valor)
{
return valor - PuntoMedio.X;
}
public double getYReal(double valor)
{
return valor - PuntoMedio.Y;
}
这是C#中逆运动学的结果
获得角度后，我们应该使用RS232协议将此测量转换为信号：
using System;
using System.Collections.Generic;
using System.Text;
using System.Collections;
namespace puertoserie.xiller.serie.angulo
{
public class Angulos
{
private static double ConvertirRangoAnguloASenal(double angulo){
double maxRangoAngulo = (double)Rangos.RangoAngulo;
double maxRangoSenal = (double)Rangos.RangoSenal;
double temp= maxRangoAngulo / maxRangoSenal;
int ciclo = 0;
for (double x = (double)Rangos.AnguloInicio; x <= (double)Rangos.AnguloFin; x+=temp)
{
if (x == angulo)return ciclo;
if (x > angulo)return ciclo;
ciclo++;
}
return ciclo;
}
public static double ConvertirAnguloASenal(double angulo)
{
double valor=ConvertirRangoAnguloASenal(angulo);
return valor + 50;
}
public static double ConvertirSenalAAngulo(double senal)
{
double temp = (double)Rangos.RangoSenal / (double)Rangos.RangoAngulo;
int ciclo=0;
for (double x = (double)Rangos.SenalInicio; x <= (double)Rangos.SenalFin; x += temp)
{
if (x == senal) return ciclo;
if (x > senal) return ciclo;
ciclo++;
}
return ciclo;
}
}
}
using System;
using System.Collections.Generic;
using System.Text;
namespace puertoserie.xiller.serie
{
public class Motor
{
private double ConvertirRangoAnguloASenal(double angulo)
{
double temp = this.rangoAngulo / this.rangoSenal;
int ciclo = 0;
for (double x = this.anguloInicio; x <= this.anguloFin; x += temp)
{
if (x == angulo) return ciclo;
if (x > angulo) return ciclo;
ciclo++;
}
return ciclo;
}
public double ConvertirAnguloASenal(double angulo)
{
double valor = ConvertirRangoAnguloASenal(angulo);
return valor + 50;
}
public double ConvertirSenalAAngulo(double senal)
{
double temp = this.rangoSenal / this.rangoAngulo;
int ciclo = 0;
for (double x = this.senalInicio; x <= this.senalFin; x += temp)
{
if (x == senal) return ciclo;
if (x > senal) return ciclo;
ciclo++;
}
return ciclo;
}
public Motor(double aInicio,double aRango,double aFin,double sInicio,double sRango,double sFin) {
this.anguloInicio = aInicio;
this.rangoAngulo = aRango;
this.anguloFin = aFin;
this.senalInicio = sInicio;
this.rangoSenal = sRango;
this.senalFin = sFin;
}
public Motor() { }
private double anguloInicio = 0;
public double AnguloInicio
{
get { return anguloInicio; }
set { anguloInicio = value; }
}
private double rangoAngulo = 180;
public double RangoAngulo
{
get { return rangoAngulo; }
set { rangoAngulo = value; }
}
private double anguloFin = 180;
public double AnguloFin
{
get { return anguloFin; }
set { anguloFin = value; }
}
private double senalInicio = 50;
public double SenalInicio
{
get { return senalInicio; }
set { senalInicio = value; }
}
private double rangoSenal = 200;
public double RangoSenal
{
get { return rangoSenal; }
set { rangoSenal = value; }
}
private double senalFin = 250;
public double SenalFin
{
get { return senalFin; }
set { senalFin = value; }
}
}
}
另一种可能性是使用Wiimote实现，因此利用控制器的一些特性，我们编写了一些代码
private void panel1_Paint(object sender, PaintEventArgs e)
{
if (wiiEstados.B == true)
{
Graphics g = e.Graphics;
control.MaxPanelX = panel1.Width;
control.MaxPanelY = panel1.Height;
label1.Text = wiiEstados.X.ToString();
label2.Text = wiiEstados.Y.ToString();
label3.Text = wiiEstados.Z.ToString();
control.dWiiMoteSetSpeedX = wiiEstados.X - control.dWiiOffsetX;
control.nSign = Math.Sign(control.dWiiMoteSetSpeedX);
control.dWiiMoteSSXQuadratic = Math.Pow(control.dWiiMoteSetSpeedX, 2.0) * control.nSign;
control.dWiiMotePosX = (control.dWiiMotePosX + (control.dWiiMoteSSXQuadratic * control.dSamplePeriod) * control.nSpeedGain);
if (control.dWiiMotePosX >= 1) control.dWiiMotePosX = 1;
if (control.dWiiMotePosX <= -1) control.dWiiMotePosX = -1;
control.dMousePosX = control.dWiiMotePosX;
control.CoordenadaXFinal = (control.MaxPanelX / 2) * (control.dMousePosX + 1);
// COORDENADA Y
control.dWiiMoteSetSpeedY = wiiEstados.Y - control.dWiiOffsetY; // CURRENT position Y from WiiMote
//lblVy.Text = dWiiMoteSetSpeedY.ToString(); // Just for debug visualisation
// Using the Signed Square function:
control.nSign = Math.Sign(control.dWiiMoteSetSpeedY); // Mind the Wiimote directions ...
control.dWiiMoteSSYQuadratic = Math.Pow(control.dWiiMoteSetSpeedY, 2.0) * control.nSign;
control.dWiiMotePosY = (control.dWiiMotePosY + (control.dWiiMoteSSYQuadratic * control.dSamplePeriod) * control.nSpeedGain); // INTEGRATOR
// Do some limitations ...
if (control.dWiiMotePosY >= 1) control.dWiiMotePosY = 1;
if (control.dWiiMotePosY <= -1) control.dWiiMotePosY = -1;
control.dMousePosY = control.dWiiMotePosY;
control.CoordenadaYFinal = (control.MaxPanelY / 2) * (control.dMousePosY + 1); // Mid screen
Pen blackPen = new Pen(Color.Black, 3);
//trackBar2.Value = Convert.ToInt16(control.CoordenadaXFinal);
setValorTrack02(control.CoordenadaXFinal);
// trackBar1.Value = Convert.ToInt16();
setValorTrack01(control.CoordenadaYFinal);
g.DrawEllipse(blackPen, (float)control.CoordenadaXFinal, (float)control.CoordenadaYFinal, 10 + (wiiEstados.Z * 10), 10 + (wiiEstados.Z * 10));
}
}
private void setValorTrack01(double valor) {
int conv = Convert.ToInt16(Math.Round(valor));
if (conv >= 0 && conv <= wPanel.PanelWidth) {
trackBar1.Value = conv + 50;
setValorMotor(1, trackBar1.Value);
}
}
private void setValorTrack02(double valor)
{
int conv = Convert.ToInt16(Math.Round(valor));
if (conv >= 0 && conv <= wPanel.PanelHeight)
{
trackBar2.Value = conv + 50;
setValorMotor(3,trackBar2.Value);
}
}
private void setValorMotor(int motor , int valor) {
almacen.setCantidad(motor, valor);
switch (motor) {
case 0:
this.label15.Text = Angulos.ConvertirSenalAAngulo((double)trackBar3.Value).ToString() + "°";
break;
case 1:
//almacen.setCantidad(1, valor);
this.label14.Text = Angulos.ConvertirSenalAAngulo((double)trackBar1.Value).ToString() + "°";
break;
case 2:
this.label14.Text = Angulos.ConvertirSenalAAngulo((double)trackBar1.Value).ToString() + "°";
break;
case 3:
this.label8.Text = Angulos.ConvertirSenalAAngulo((double)trackBar2.Value).ToString() + "°";
break;
}
}
参考文献
- 图像处理手册，John C. Russ，第五版，2007年 Publisher Taylor & Francis。
- OpenCV图书销售商，http://opencvlibrary.sourceforge.net/FaceDetection
- OpenCVDotNet图书销售商，http://code.google.com/p/opencvdotnet/
- AForge.NET，http://code.google.com/p/aforge/
- Speech SDK 5.1
- 人脸检测级联，作者：Modesto Castrillón Santana，（OpenCV开发者社区）
大学
墨西哥国家理工学院-跨学科工程、社会与行政科学学院（UPIICSA – 墨西哥国家理工学院跨学科工程、社会与行政科学学院）
历史
- 初稿 2013/07/28
- 二稿：2021/05/11