【机械臂】动作组+五次项插值位置速度(matlab+C)
我的理解
机械臂运动规划中，规划器通过运动学逆解或者遥控器输出单个关节的目标角度，在运动的过程中控制算法控制电机到达目标位置，为了足够平缓，可以采用插值算法，所以了解各种插值算法
机械臂运动空间
机械臂运动轨迹规划在空间可以分为两大类：关节空间轨迹规划与笛卡尔空间轨迹规划。机械臂关节空间的轨迹规划解决机械臂从起始位姿到终止位姿去取放物体的问题。机械臂末端移动的过程并不重要，只要求运动是平滑的且没有碰撞产生。而笛卡尔空间轨迹规划要解决的是机械臂末端确定的轨迹规划问题；在关节空间中进行轨迹规划时，算法简单、工具移动效率高、关节空间与直角坐标空间连续的对应关系是不存在的，因此机构的奇异性问题一般不会发生。对于无路径的要求，应尽量在关节空间进行轨迹规划。
三次多项式插值原理
三次多项式插值适用于起点和终点速度为零的情况。约束关节在起点和终点为零的角度值，规定轨迹端点位置角速度为定值。
设关节角满足下式：
MATLAB仿真测试
基于上面的信息，我们就可以用Matlab来写三项式插值的脚步代码，设机械臂某关节在5s内由初始点A经过中间点B到达目标点C的位置、速度、加速度变化情况，设
clear; % 清除工作区的所有变量
clc; % 清除命令行窗口
close all; % 关闭所有图形窗口
q_array=[25,70,50]; % 指定起止位置
t_array=[0,2,5]; % 指定起止时间
v_array=[30,20,30]; % 指定起止速度
a_array=[2,4,3]; % 指定起止加速度
t=t_array(1); % 初始时间
q=q_array(1); % 初始位置
v=v_array(1); % 初始速度
a=a_array(1); % 初始加速度
for i=1:1:length(q_array)-1 % 遍历每一段规划的时间
T=t_array(i+1)-t_array(i); % 当前段的时间间隔
a0=q_array(i); % 当前段的起始位置
a1=v_array(i); % 当前段的起始速度
a2=(q_array(i+1)-q_array(i))*3/(T^2)-(2*v_array(i)+v_array(i+1))/T; % 当前段的二次导数系数
a3=(q_array(i)-q_array(i+1))*2/(T^3)+(v_array(i)+v_array(i+1))/(T^2); % 当前段的三次导数系数
ti=t_array(i):0.02:t_array(i+1); % 在当前段内按步长生成时间序列
qi=a0+a1*(ti-t_array(i))+a2*(ti-t_array(i)).^2+a3*(ti-t_array(i)).^3; % 根据三次多项式插值公式计算位置
vi=a1+2*a2*(ti-t_array(i))+3*a3*(ti-t_array(i)).^2; % 计算速度
ai=2*a2+6*a3*(ti-t_array(i)); % 计算加速度
t=[t,ti(2:end)]; % 将当前段的时间序列添加到总时间序列中
q=[q,qi(2:end)]; % 将当前段的位置添加到总位置序列中
v=[v,vi(2:end)]; % 将当前段的速度添加到总速度序列中
a=[a,ai(2:end)]; % 将当前段的加速度添加到总加速度序列中
end
subplot(3,1,1),plot(t,q,'r'),xlabel('t'),ylabel('position'); % 在第一个子图中绘制时间-位置曲线
hold on; % 保持绘图环境
plot(t_array,q_array,'*','color','r'),grid on; % 绘制起止位置的数据点，并打开网格线
subplot(3,1,2),plot(t,v,'b'),xlabel('t'),ylabel('velocity'); % 在第二个子图中绘制时间-速度曲线
hold on; % 保持绘图环境
plot(t_array,v_array,'o','color','g'),grid on; % 绘制起止速度的数据点，并打开网格线
subplot(3,1,3),plot(t,a,'g'),xlabel('t'),ylabel('acceleration'); % 在第三个子图中绘制时间-加速度曲线
hold on; % 保持绘图环境
plot(t_array,a_array,'^','color','b'),grid on; % 绘制起止加速度的数据点，并打开网格线
————————————————
版权声明：本文为CSDN博主「子_非云」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。
原文链接：https://blog.csdn.net/qq_43412584/article/details/109143103
机械臂控制中
五次项插值位置+速度
动作组控制思路
/**
* @brief 连续动作组解决器
* @param[in] *pose 目标动作组
* Time 插值密度
* @retval none
*/
void ArmControl_base::ArmMoveGroupSolver(float *pose,int Time)
{
//判断是否到达目标
if(Is_Reach_aim((float *)CAN_Cmd.Arm.POS_,pose,0.02))
{
return;
}
joint1_slover = polynomial_trajectory_slover(aim_pose[1],aim_vel[1],Time,CAN_Cmd.Arm.POS_[0],CAN_Cmd.Arm.VEl_[0],pose[0],0);
joint2_slover = polynomial_trajectory_slover(aim_pose[2],aim_vel[2],Time,CAN_Cmd.Arm.POS_[1],CAN_Cmd.Arm.VEl_[1],pose[1],0);
joint3_slover = polynomial_trajectory_slover(aim_pose[3],aim_vel[3],Time,CAN_Cmd.Arm.POS_[2],CAN_Cmd.Arm.VEl_[2],pose[2],0);
// joint1_slover = polynomial_trajectory_slover(aim_pose[1],aim_vel[1],Time,CAN_Cmd.Arm.POS_[0],CAN_Cmd.Arm.VEl_[0],0.436,pose[1],0,-0.436);
// joint2_slover = polynomial_trajectory_slover(aim_pose[2],aim_vel[2],Time,CAN_Cmd.Arm.POS_[1],CAN_Cmd.Arm.VEl_[1],0.436,pose[2],0,-0.436);
// joint3_slover = polynomial_trajectory_slover(aim_pose[3],aim_vel[3],Time,CAN_Cmd.Arm.POS_[2],CAN_Cmd.Arm.VEl_[2],0.436,pose[3],0,-0.436);
// joint4_slover = polynomial_trajectory_slover(aim_pose[0],aim_vel[0],Time,CAN_Cmd.Arm.POS_[0],CAN_Cmd.Arm.VEl_[3],pose[2],0);
for(int i=0;i<=Time;i++)
{
//限幅
aim_pose[2][i]=constrain(aim_pose[2][i],-3.14,3.14);
aim_pose[1][i]=constrain(aim_pose[1][i],-1.75,1.75);
aim_pose[0][i]=constrain(aim_pose[0][i],-3.14,3.14);
//输出
ctrl_motor_pv(2,(float)aim_pose[3][i],(float)aim_vel[3][i]);
ctrl_motor_pv(1,(float)aim_pose[2][i],(float)aim_vel[2][i]);
ctrl_motor_pv(0,(float)aim_pose[1][i],(float)aim_vel[1][i]);
//串口输出
send_float(aim_pose[0][i],USART6);
vTaskDelayUntil( &xLastWakeTime, xDelay1s );
}
}
/**
* @brief 五次多项式插值
* @param[in] &rad 存储目标弧度
* &vel存储目标速度
* &插值密度
* &当前角度 速度 加速度
* &目标角度 速度 加速度
* @retval none
*/
bool Kinematics_base::polynomial_trajectory_slover(float* rad,float* vel,const int &T,float theat,float dot_theta,float dott_theta,float aim,float dot_aim,float dott_aim)
{
int times = T ;
int param_times = T;
Qptpf.c0 = theat;
Qptpf.c1 = dot_theta;
Qptpf.c2 = dott_theta/2;
Qptpf.c3 = (10*aim)/powf(param_times,3) - (4*dot_aim)/powf(param_times,2) + dott_aim/(2*param_times) - (6*dot_theta)/powf(param_times,2) - (3*dott_theta)/(2*param_times) - (10*theat)/powf(param_times,3);
Qptpf.c4 = (7*dot_aim)/powf(param_times,3) - (15*aim)/powf(param_times,4) - dott_aim/powf(param_times,2) + (8*dot_theta)/powf(param_times,3) + (3*dott_theta)/(2*powf(param_times,2)) + (15*theat)/powf(param_times,4);
Qptpf.c5 = (6*aim)/powf(param_times,5) - (3*dot_aim)/powf(param_times,4) + dott_aim/(2*powf(param_times,3)) - (3*dot_theta)/powf(param_times,4) - dott_theta/(2*powf(param_times,3)) - (6*theat)/powf(param_times,5);
if(!rad||!vel)
{return error;}
for(int i=0;i<=times;i++)
{
rad[i] = Qptpf.c0 + Qptpf.c1*i + Qptpf.c2*powf(i,2) + Qptpf.c3*powf(i,3) + Qptpf.c4*powf(i,4) + Qptpf.c5*powf(i,5);
vel[i] = Qptpf.c1 + 2*Qptpf.c2*i + 3*Qptpf.c3*powf(i,2) + 4*Qptpf.c4*powf(i,3) + 5*Qptpf.c5*powf(i,4);
if(((rad+1== NULL)||(vel+1== NULL))&&(i!= times))
{return fault;}
}
return success;
}
动作组
void ArmControl_base::ArmModeGoing()
{
#if USE_RC==1
if(message_bag.RobotBehavior.RobotMode==RobotArm_m)//遥控器控制机械臂
{
RCArmContorl();
}else
{
switch(message_bag.RobotBehavior.ActionGroup)//判断要执行的动作组
{
case AirCatch:
{
ArmMoveGroupSolver(AirCatchGroup,this->Times__);
break;
}
case Init:
{
ArmMoveGroupSolver(InitGroup,this->Times__);
break;
}
case PutBack:
{
ArmMoveGroupSolver(PutBackGroup,this->Times__);
break;
}
case CatchGround:
{
ArmMoveGroupSolver(CatchGroundGroup,this->Times__);
break;
}
case CatchMid:
{
ArmMoveGroupSolver(CatchMidGroup,this->Times__);
break;
}
case Stop:
{
ArmMoveGroupSolver(StopGroup,this->Times__);
break;
}
case KEEP:
{
float temppose[4] = {JointPose[0],CAN_Cmd.Arm.POS_[1],CAN_Cmd.Arm.POS_[2],CAN_Cmd.Arm.POS_[3]};
ArmControl(temppose);//保持当前数据不变
break;
}
}
}
#else
PCArmContorl();
#endif
}