vc6环境下,一个多关节冗余机器人的避障模拟程序。无论debug还是release下,运行都会提示遇错需要关闭,但是程序还可以接着运行(因为做的是图形模拟,还可以看出在运行),但是运行结果不稳定,偶尔运行结果会很奇怪,大部分时候比较正常(但不敢保证是正确的)。通过排查,把问题代码压缩到一个局部,在断点调试下,结果竟然不报错了,但运行一段时间就会卡在一个断点下,过很长时间又能够跳到下一个断点处。观察程序,貌似运算量也不是很大,就是一些矩阵运算,无非乘法,求逆、伪逆之类。请问大家,这个是计算机运算能力不足造成的吗?多谢这是问题代码 if(flag_dis)
{
Manipulator.Jacobi(Jac);
Manipulator.Pseudo_Inverse(Pes_Inv,Jac);
Manipulator.Zero_Jacobi(Zero_Jac,Pes_Inv,Jac);
CVector F52(3); 
Manipulator.m_ArticulatedBody[2].T_temp=Manipulator.m_ArticulatedBody[2].T_w2l;
Matrix res(Manipulator.m_ArticulatedBody[2].T_temp.m[0][0],Manipulator.m_ArticulatedBody[2].T_temp.m[0][1],
   Manipulator.m_ArticulatedBody[2].T_temp.m[0][2],Manipulator.m_ArticulatedBody[2].T_temp.m[0][3],
   Manipulator.m_ArticulatedBody[2].T_temp.m[1][0],Manipulator.m_ArticulatedBody[2].T_temp.m[1][1],
   Manipulator.m_ArticulatedBody[2].T_temp.m[1][2],Manipulator.m_ArticulatedBody[2].T_temp.m[1][3],
   Manipulator.m_ArticulatedBody[2].T_temp.m[2][0],Manipulator.m_ArticulatedBody[2].T_temp.m[2][1],
   Manipulator.m_ArticulatedBody[2].T_temp.m[2][2],Manipulator.m_ArticulatedBody[2].T_temp.m[2][3],
   Manipulator.m_ArticulatedBody[2].T_temp.m[3][0],Manipulator.m_ArticulatedBody[2].T_temp.m[3][1],
   Manipulator.m_ArticulatedBody[2].T_temp.m[3][2],Manipulator.m_ArticulatedBody[2].T_temp.m[3][3]); double R02[9],Inv_R02[9];
double* pInv_R02;
for(int i=0;i<3;i++)
for(int j=0;j<3;j++)
R02[i*3+j]=res.M[i][j];

pInv_R02=res.MatrixInver(R02,3,3);
for(i=0;i<9;i++)
Inv_R02[i]=*(pInv_R02+i);

double temp=0;
for(i=0;i<3;i++)
{
temp = 0;
for(int j=0;j<3;j++)
temp+=Inv_R02[i*3+j]*ReactingForce[j];   //F52=[R02]-1 * F5
//获得避障力在2坐标系下的坐标
F52.p[i] = temp;   
}

//TRACE("F52=%f %f %f\n",F52.v[0],F52.v[1],F52.v[2]);
CVector V25(3);   //力臂r在{2}坐标
CMatrix CM_temp=Manipulator.m_ArticulatedBody[3].T_p2m*Manipulator.m_ArticulatedBody[4].T_p2m;
CM_temp=CM_temp*Manipulator.m_ArticulatedBody[5].T_p2m;
Matrix M_temp(CM_temp.m[0][0],CM_temp.m[0][1],
      CM_temp.m[0][2],CM_temp.m[0][3],
      CM_temp.m[1][0],CM_temp.m[1][1],
      CM_temp.m[1][2],CM_temp.m[1][3],
      CM_temp.m[2][0],CM_temp.m[2][1],
      CM_temp.m[2][2],CM_temp.m[2][3],
      CM_temp.m[3][0],CM_temp.m[3][1],
      CM_temp.m[3][2],CM_temp.m[3][3]);
res=M_temp;//R25
V25.p[0]=res.M[0][3];
V25.p[1]=res.M[1][3];
V25.p[2]=res.M[2][3];
//TRACE("V25=%f %f %f\n",V25.v[0],V25.v[1],V25.v[2]);

//力矩M2[3] = V25[3]×F52[3] //{2}坐标中的力矩
CVector M21;
M21 = V25.cross(F52);

double M2z1 = M21.p[2];    //对关节2,z向的力矩

double Joint2_W1=0;
Joint2_W1=M2z1;    //力矩产生避障角速度 double Z21[7]={0};
Z21[1] = Joint2_W1/100000.0;
if (fabs(Joint2_W1) <0.001)   //当力矩为零时,停止避障
{
Z21[1] = 0;
}
for(i=0;i<7;i++)
{
CVector V(6);
{
V.p[0]=Manipulator.m_ArticulatedBody[i].v[3];
V.p[1]=Manipulator.m_ArticulatedBody[i].v[4];
V.p[2]=Manipulator.m_ArticulatedBody[i].v[5];
V.p[3]=Manipulator.m_ArticulatedBody[i].v[0];
V.p[4]=Manipulator.m_ArticulatedBody[i].v[1];
V.p[5]=Manipulator.m_ArticulatedBody[i].v[2];
}
for(int j=0;j<6;j++)
Manipulator.m_ArticulatedBody[i].dq+=Pes_Inv[i][j]*V.p[j]; 
for(int k=0;k<7;k++)
{
Manipulator.m_ArticulatedBody[i].dq+=Zero_Jac[i][k]*Z21[k];
Manipulator.m_ArticulatedBody[i].dq=Manipulator.m_ArticulatedBody[i].dq/3.0;
}
}
}